From d8c5bd125e7bb8af22f9c369aa01e61b501a8651 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 2 Feb 2022 12:02:58 +0100 Subject: [PATCH 01/81] All EIVE changes --- .../fsfw_hal/common/gpio/gpioDefinitions.h | 20 +++--- .../devicehandlers/GyroL3GD20Handler.cpp | 2 +- .../devicehandlers/MgmLIS3MDLHandler.cpp | 8 ++- .../devicehandlers/MgmRM3100Handler.cpp | 2 +- .../fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp | 14 ++-- hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp | 5 +- hal/src/fsfw_hal/linux/rpi/GpioRPi.cpp | 2 +- hal/src/fsfw_hal/linux/rpi/GpioRPi.h | 3 +- hal/src/fsfw_hal/linux/uart/UartComIF.cpp | 3 +- hal/src/fsfw_hal/linux/uart/UartCookie.cpp | 2 +- hal/src/fsfw_hal/linux/uio/CMakeLists.txt | 3 + hal/src/fsfw_hal/linux/uio/UioMapper.cpp | 66 +++++++++++++++++++ hal/src/fsfw_hal/linux/uio/UioMapper.h | 58 ++++++++++++++++ .../datapoollocal/LocalPoolDataSetBase.cpp | 2 + src/fsfw/datapoollocal/LocalPoolDataSetBase.h | 5 ++ .../datapoollocal/LocalPoolObjectBase.cpp | 11 ++-- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 7 ++ src/fsfw/devicehandlers/DeviceHandlerBase.h | 6 ++ src/fsfw/osal/linux/CMakeLists.txt | 34 +++++----- src/fsfw/timemanager/Clock.h | 7 ++ src/fsfw/timemanager/ClockCommon.cpp | 21 +++++- src/fsfw/tmtcservices/TmTcBridge.cpp | 15 +++-- src/fsfw/tmtcservices/TmTcBridge.h | 2 + 23 files changed, 237 insertions(+), 61 deletions(-) create mode 100644 hal/src/fsfw_hal/linux/uio/CMakeLists.txt create mode 100644 hal/src/fsfw_hal/linux/uio/UioMapper.cpp create mode 100644 hal/src/fsfw_hal/linux/uio/UioMapper.h diff --git a/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h b/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h index b429449b..0ef5b2ea 100644 --- a/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h +++ b/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h @@ -11,7 +11,7 @@ namespace gpio { enum Levels : uint8_t { LOW = 0, HIGH = 1, NONE = 99 }; -enum Direction : uint8_t { IN = 0, OUT = 1 }; +enum Direction : uint8_t { DIR_IN = 0, DIR_OUT = 1 }; enum GpioOperation { READ, WRITE }; @@ -20,7 +20,7 @@ enum class GpioTypes { GPIO_REGULAR_BY_CHIP, GPIO_REGULAR_BY_LABEL, GPIO_REGULAR_BY_LINE_NAME, - CALLBACK + TYPE_CALLBACK }; static constexpr gpioId_t NO_GPIO = -1; @@ -57,7 +57,7 @@ class GpioBase { // Can be used to cast GpioBase to a concrete child implementation gpio::GpioTypes gpioType = gpio::GpioTypes::NONE; std::string consumer; - gpio::Direction direction = gpio::Direction::IN; + gpio::Direction direction = gpio::Direction::DIR_IN; gpio::Levels initValue = gpio::Levels::NONE; }; @@ -79,8 +79,8 @@ class GpiodRegularBase : public GpioBase { class GpiodRegularByChip : public GpiodRegularBase { public: GpiodRegularByChip() - : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP, std::string(), gpio::Direction::IN, - gpio::LOW, 0) {} + : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP, std::string(), + gpio::Direction::DIR_IN, gpio::LOW, 0) {} GpiodRegularByChip(std::string chipname_, int lineNum_, std::string consumer_, gpio::Direction direction_, gpio::Levels initValue_) @@ -89,7 +89,7 @@ class GpiodRegularByChip : public GpiodRegularBase { chipname(chipname_) {} GpiodRegularByChip(std::string chipname_, int lineNum_, std::string consumer_) - : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP, consumer_, gpio::Direction::IN, + : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP, consumer_, gpio::Direction::DIR_IN, gpio::LOW, lineNum_), chipname(chipname_) {} @@ -105,7 +105,7 @@ class GpiodRegularByLabel : public GpiodRegularBase { label(label_) {} GpiodRegularByLabel(std::string label_, int lineNum_, std::string consumer_) - : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LABEL, consumer_, gpio::Direction::IN, + : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LABEL, consumer_, gpio::Direction::DIR_IN, gpio::LOW, lineNum_), label(label_) {} @@ -126,8 +126,8 @@ class GpiodRegularByLineName : public GpiodRegularBase { lineName(lineName_) {} GpiodRegularByLineName(std::string lineName_, std::string consumer_) - : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME, consumer_, gpio::Direction::IN, - gpio::LOW), + : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME, consumer_, + gpio::Direction::DIR_IN, gpio::LOW), lineName(lineName_) {} std::string lineName; @@ -137,7 +137,7 @@ class GpioCallback : public GpioBase { public: GpioCallback(std::string consumer, gpio::Direction direction_, gpio::Levels initValue_, gpio::gpio_cb_t callback, void* callbackArgs) - : GpioBase(gpio::GpioTypes::CALLBACK, consumer, direction_, initValue_), + : GpioBase(gpio::GpioTypes::TYPE_CALLBACK, consumer, direction_, initValue_), callback(callback), callbackArgs(callbackArgs) {} diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp index be4c9aa9..f92f5f1f 100644 --- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp @@ -10,7 +10,7 @@ GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceC transitionDelayMs(transitionDelayMs), dataset(this) { #if FSFW_HAL_L3GD20_GYRO_DEBUG == 1 - debugDivider = new PeriodicOperationDivider(3); + debugDivider = new PeriodicOperationDivider(10); #endif } diff --git a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp index f3ea9942..cf1d934a 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp @@ -13,7 +13,7 @@ MgmLIS3MDLHandler::MgmLIS3MDLHandler(object_id_t objectId, object_id_t deviceCom dataset(this), transitionDelay(transitionDelay) { #if FSFW_HAL_LIS3MDL_MGM_DEBUG == 1 - debugDivider = new PeriodicOperationDivider(3); + debugDivider = new PeriodicOperationDivider(10); #endif // Set to default values right away registers[0] = MGMLIS3MDL::CTRL_REG1_DEFAULT; @@ -264,7 +264,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 * @@ -462,7 +462,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; } diff --git a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp b/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp index 4c6e09b1..a2e2e85f 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp @@ -12,7 +12,7 @@ MgmRM3100Handler::MgmRM3100Handler(object_id_t objectId, object_id_t deviceCommu primaryDataset(this), transitionDelay(transitionDelay) { #if FSFW_HAL_RM3100_MGM_DEBUG == 1 - debugDivider = new PeriodicOperationDivider(3); + debugDivider = new PeriodicOperationDivider(10); #endif } diff --git a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp index a33305b6..fa9eeb5a 100644 --- a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp +++ b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp @@ -74,7 +74,7 @@ ReturnValue_t LinuxLibgpioIF::configureGpios(GpioMap& mapToAdd) { configureGpioByLineName(gpioConfig.first, *regularGpio); break; } - case (gpio::GpioTypes::CALLBACK): { + case (gpio::GpioTypes::TYPE_CALLBACK): { auto gpioCallback = dynamic_cast(gpioConfig.second); if (gpioCallback->callback == nullptr) { return GPIO_INVALID_INSTANCE; @@ -161,11 +161,11 @@ ReturnValue_t LinuxLibgpioIF::configureRegularGpio(gpioId_t gpioId, struct gpiod consumer = regularGpio.consumer; /* Configure direction and add a description to the GPIO */ switch (direction) { - case (gpio::OUT): { + case (gpio::DIR_OUT): { result = gpiod_line_request_output(lineHandle, consumer.c_str(), regularGpio.initValue); break; } - case (gpio::IN): { + case (gpio::DIR_IN): { result = gpiod_line_request_input(lineHandle, consumer.c_str()); break; } @@ -326,7 +326,7 @@ ReturnValue_t LinuxLibgpioIF::checkForConflicts(GpioMap& mapToAdd) { } break; } - case (gpio::GpioTypes::CALLBACK): { + case (gpio::GpioTypes::TYPE_CALLBACK): { auto callbackGpio = dynamic_cast(gpioConfig.second); if (callbackGpio == nullptr) { return GPIO_TYPE_FAILURE; @@ -366,13 +366,13 @@ ReturnValue_t LinuxLibgpioIF::checkForConflictsById(gpioId_t gpioIdToCheck, case (gpio::GpioTypes::GPIO_REGULAR_BY_CHIP): case (gpio::GpioTypes::GPIO_REGULAR_BY_LABEL): case (gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME): { - if (gpioType == gpio::GpioTypes::NONE or gpioType == gpio::GpioTypes::CALLBACK) { + if (gpioType == gpio::GpioTypes::NONE or gpioType == gpio::GpioTypes::TYPE_CALLBACK) { eraseDuplicateDifferentType = true; } break; } - case (gpio::GpioTypes::CALLBACK): { - if (gpioType != gpio::GpioTypes::CALLBACK) { + case (gpio::GpioTypes::TYPE_CALLBACK): { + if (gpioType != gpio::GpioTypes::TYPE_CALLBACK) { eraseDuplicateDifferentType = true; } } diff --git a/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp index dc23542d..d7e022dd 100644 --- a/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp +++ b/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp @@ -8,8 +8,7 @@ #include -#include "fsfw/FSFW.h" -#include "fsfw/serviceinterface.h" +#include "fsfw/serviceinterface/ServiceInterface.h" #include "fsfw_hal/linux/UnixFileGuard.h" #include "fsfw_hal/linux/utility.h" @@ -104,7 +103,7 @@ ReturnValue_t I2cComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s return result; } - if (write(fd, sendData, sendLen) != static_cast(sendLen)) { + if (write(fd, sendData, sendLen) != (int)sendLen) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "I2cComIF::sendMessage: Failed to send data to I2C " "device with error code " diff --git a/hal/src/fsfw_hal/linux/rpi/GpioRPi.cpp b/hal/src/fsfw_hal/linux/rpi/GpioRPi.cpp index c005e24f..d3c0a577 100644 --- a/hal/src/fsfw_hal/linux/rpi/GpioRPi.cpp +++ b/hal/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/hal/src/fsfw_hal/linux/rpi/GpioRPi.h index 499f984b..8ca7065a 100644 --- a/hal/src/fsfw_hal/linux/rpi/GpioRPi.h +++ b/hal/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/hal/src/fsfw_hal/linux/uart/UartComIF.cpp b/hal/src/fsfw_hal/linux/uart/UartComIF.cpp index a648df3a..ba970e3e 100644 --- a/hal/src/fsfw_hal/linux/uart/UartComIF.cpp +++ b/hal/src/fsfw_hal/linux/uart/UartComIF.cpp @@ -7,7 +7,6 @@ #include -#include "fsfw/FSFW.h" #include "fsfw/serviceinterface.h" #include "fsfw_hal/linux/utility.h" @@ -313,7 +312,7 @@ ReturnValue_t UartComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, fd = uartDeviceMapIter->second.fileDescriptor; - if (write(fd, sendData, sendLen) != static_cast(sendLen)) { + if (write(fd, sendData, sendLen) != (int)sendLen) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "UartComIF::sendMessage: Failed to send data with error code " << errno << ": Error description: " << strerror(errno) << std::endl; diff --git a/hal/src/fsfw_hal/linux/uart/UartCookie.cpp b/hal/src/fsfw_hal/linux/uart/UartCookie.cpp index 99e80a8e..0db961dc 100644 --- a/hal/src/fsfw_hal/linux/uart/UartCookie.cpp +++ b/hal/src/fsfw_hal/linux/uart/UartCookie.cpp @@ -1,6 +1,6 @@ #include "fsfw_hal/linux/uart/UartCookie.h" -#include +#include UartCookie::UartCookie(object_id_t handlerId, std::string deviceFile, UartModes uartMode, uint32_t baudrate, size_t maxReplyLen) diff --git a/hal/src/fsfw_hal/linux/uio/CMakeLists.txt b/hal/src/fsfw_hal/linux/uio/CMakeLists.txt new file mode 100644 index 00000000..e98a0865 --- /dev/null +++ b/hal/src/fsfw_hal/linux/uio/CMakeLists.txt @@ -0,0 +1,3 @@ +target_sources(${LIB_FSFW_NAME} PUBLIC + UioMapper.cpp +) diff --git a/hal/src/fsfw_hal/linux/uio/UioMapper.cpp b/hal/src/fsfw_hal/linux/uio/UioMapper.cpp new file mode 100644 index 00000000..11430915 --- /dev/null +++ b/hal/src/fsfw_hal/linux/uio/UioMapper.cpp @@ -0,0 +1,66 @@ +#include "UioMapper.h" + +#include +#include + +#include +#include +#include + +#include "fsfw/serviceinterface/ServiceInterface.h" + +UioMapper::UioMapper(std::string uioFile, int mapNum) : uioFile(uioFile), mapNum(mapNum) {} + +UioMapper::~UioMapper() {} + +ReturnValue_t UioMapper::getMappedAdress(uint32_t** address, Permissions permissions) { + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + int fd = open(uioFile.c_str(), O_RDWR); + if (fd < 1) { + sif::warning << "PtmeAxiConfig::initialize: Invalid UIO device file" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + size_t size = 0; + result = getMapSize(&size); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + *address = static_cast( + mmap(NULL, size, static_cast(permissions), MAP_SHARED, fd, mapNum * getpagesize())); + + if (*address == MAP_FAILED) { + sif::warning << "UioMapper::getMappedAdress: Failed to map physical address of uio device " + << uioFile.c_str() << " and map" << static_cast(mapNum) << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t UioMapper::getMapSize(size_t* size) { + std::stringstream namestream; + namestream << UIO_PATH_PREFIX << uioFile.substr(5, std::string::npos) << MAP_SUBSTR << mapNum + << SIZE_FILE_PATH; + FILE* fp; + fp = fopen(namestream.str().c_str(), "r"); + if (fp == nullptr) { + sif::warning << "UioMapper::getMapSize: Failed to open file " << namestream.str() << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + char hexstring[SIZE_HEX_STRING] = ""; + int items = fscanf(fp, "%s", hexstring); + if (items != 1) { + sif::warning << "UioMapper::getMapSize: Failed with error code " << errno + << " to read size " + "string from file " + << namestream.str() << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + items = sscanf(hexstring, "%x", size); + if (items != 1) { + sif::warning << "UioMapper::getMapSize: Failed with error code " << errno << "to convert " + << "size of map" << mapNum << " to integer" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + fclose(fp); + return HasReturnvaluesIF::RETURN_OK; +} diff --git a/hal/src/fsfw_hal/linux/uio/UioMapper.h b/hal/src/fsfw_hal/linux/uio/UioMapper.h new file mode 100644 index 00000000..d58a4c52 --- /dev/null +++ b/hal/src/fsfw_hal/linux/uio/UioMapper.h @@ -0,0 +1,58 @@ +#ifndef FSFW_HAL_SRC_FSFW_HAL_LINUX_UIO_UIOMAPPER_H_ +#define FSFW_HAL_SRC_FSFW_HAL_LINUX_UIO_UIOMAPPER_H_ + +#include + +#include + +#include "fsfw/returnvalues/HasReturnvaluesIF.h" + +/** + * @brief Class to help opening uio device files and mapping the physical addresses into the user + * address space. + * + * @author J. Meier + */ +class UioMapper { + public: + enum class Permissions : int { + READ_ONLY = PROT_READ, + WRITE_ONLY = PROT_WRITE, + READ_WRITE = PROT_READ | PROT_WRITE + }; + + /** + * @brief Constructor + * + * @param uioFile The device file of the uiO to open + * @param uioMap Number of memory map. Most UIO drivers have only one map which has than 0. + */ + UioMapper(std::string uioFile, int mapNum = 0); + virtual ~UioMapper(); + + /** + * @brief Maps the physical address into user address space and returns the mapped address + * + * @address The mapped user space address + * @permissions Specifies the read/write permissions of the address region + */ + ReturnValue_t getMappedAdress(uint32_t** address, Permissions permissions); + + private: + static constexpr char UIO_PATH_PREFIX[] = "/sys/class/uio/"; + static constexpr char MAP_SUBSTR[] = "/maps/map"; + static constexpr char SIZE_FILE_PATH[] = "/size"; + static constexpr int SIZE_HEX_STRING = 10; + + std::string uioFile; + int mapNum = 0; + + /** + * @brief Reads the map size from the associated sysfs size file + * + * @param size The read map size + */ + ReturnValue_t getMapSize(size_t* size); +}; + +#endif /* FSFW_HAL_SRC_FSFW_HAL_LINUX_UIO_UIOMAPPER_H_ */ diff --git a/src/fsfw/datapoollocal/LocalPoolDataSetBase.cpp b/src/fsfw/datapoollocal/LocalPoolDataSetBase.cpp index e2fe7d39..4a076212 100644 --- a/src/fsfw/datapoollocal/LocalPoolDataSetBase.cpp +++ b/src/fsfw/datapoollocal/LocalPoolDataSetBase.cpp @@ -291,3 +291,5 @@ float LocalPoolDataSetBase::getCollectionInterval() const { return 0.0; } } + +void LocalPoolDataSetBase::printSet() { return; } diff --git a/src/fsfw/datapoollocal/LocalPoolDataSetBase.h b/src/fsfw/datapoollocal/LocalPoolDataSetBase.h index c2de2c54..17cf8be2 100644 --- a/src/fsfw/datapoollocal/LocalPoolDataSetBase.h +++ b/src/fsfw/datapoollocal/LocalPoolDataSetBase.h @@ -171,6 +171,11 @@ class LocalPoolDataSetBase : public PoolDataSetBase, public MarkChangedIF { */ float getCollectionInterval() const; + /** + * @brief Can be overwritten by a specific implementation of a dataset to print the set. + */ + virtual void printSet(); + protected: sid_t sid; //! This mutex is used if the data is created by one object only. diff --git a/src/fsfw/datapoollocal/LocalPoolObjectBase.cpp b/src/fsfw/datapoollocal/LocalPoolObjectBase.cpp index c974601c..82aefc18 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/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 08d7c1d8..ea1fcdf1 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -665,6 +665,11 @@ void DeviceHandlerBase::doGetWrite() { void DeviceHandlerBase::doSendRead() { ReturnValue_t result; + result = doSendReadHook(); + if (result != RETURN_OK) { + return; + } + size_t replyLen = 0; if (cookieInfo.pendingCommand != deviceCommandMap.end()) { replyLen = getNextReplyLength(cookieInfo.pendingCommand->first); @@ -920,6 +925,8 @@ void DeviceHandlerBase::commandSwitch(ReturnValue_t onOff) { } } +ReturnValue_t DeviceHandlerBase::doSendReadHook() { return RETURN_OK; } + ReturnValue_t DeviceHandlerBase::getSwitches(const uint8_t** switches, uint8_t* numberOfSwitches) { return DeviceHandlerBase::NO_SWITCH; } diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index f3dda5c8..037f4bd7 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -1082,6 +1082,12 @@ class DeviceHandlerBase : public DeviceHandlerIF, */ void commandSwitch(ReturnValue_t onOff); + /** + * @brief This function can be used to insert device specific code during the do-send-read + * step. + */ + virtual ReturnValue_t doSendReadHook(); + private: /** * State a cookie is in. diff --git a/src/fsfw/osal/linux/CMakeLists.txt b/src/fsfw/osal/linux/CMakeLists.txt index dcdade67..679b2931 100644 --- a/src/fsfw/osal/linux/CMakeLists.txt +++ b/src/fsfw/osal/linux/CMakeLists.txt @@ -1,29 +1,29 @@ 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 + 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 + ${CMAKE_THREAD_LIBS_INIT} + rt ) target_link_libraries(${LIB_FSFW_NAME} INTERFACE - ${CMAKE_THREAD_LIBS_INIT} + ${CMAKE_THREAD_LIBS_INIT} ) diff --git a/src/fsfw/timemanager/Clock.h b/src/fsfw/timemanager/Clock.h index 99e8a56a..e9afff2e 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 diff --git a/src/fsfw/timemanager/ClockCommon.cpp b/src/fsfw/timemanager/ClockCommon.cpp index e5749b19..18407362 100644 --- a/src/fsfw/timemanager/ClockCommon.cpp +++ b/src/fsfw/timemanager/ClockCommon.cpp @@ -1,7 +1,9 @@ +#include + #include "fsfw/ipc/MutexGuard.h" #include "fsfw/timemanager/Clock.h" -ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval *tt) { +ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval* tt) { uint16_t leapSeconds; ReturnValue_t result = getLeapSeconds(&leapSeconds); if (result != HasReturnvaluesIF::RETURN_OK) { @@ -31,7 +33,7 @@ ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) { return HasReturnvaluesIF::RETURN_OK; } -ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_) { +ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) { if (timeMutex == nullptr) { return HasReturnvaluesIF::RETURN_FAILED; } @@ -42,9 +44,22 @@ ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_) { return HasReturnvaluesIF::RETURN_OK; } +ReturnValue_t Clock::convertTimevalToTimeOfDay(const timeval* from, TimeOfDay_t* to) { + struct tm* timeInfo; + 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/tmtcservices/TmTcBridge.cpp b/src/fsfw/tmtcservices/TmTcBridge.cpp index 8ea67119..6fd6bff3 100644 --- a/src/fsfw/tmtcservices/TmTcBridge.cpp +++ b/src/fsfw/tmtcservices/TmTcBridge.cpp @@ -172,15 +172,18 @@ ReturnValue_t TmTcBridge::storeDownlinkData(TmTcMessage* message) { } if (tmFifo->full()) { + if (warningLatch) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "TmTcBridge::storeDownlinkData: TM downlink max. number " - "of stored packet IDs reached!" - << std::endl; + sif::warning << "TmTcBridge::storeDownlinkData: TM downlink max. number " + "of stored packet IDs reached!" + << std::endl; #else - sif::printWarning( - "TmTcBridge::storeDownlinkData: TM downlink max. number " - "of stored packet IDs reached!\n"); + sif::printWarning( + "TmTcBridge::storeDownlinkData: TM downlink max. number " + "of stored packet IDs reached!\n"); #endif + warningLatch = true; + } if (overwriteOld) { tmFifo->retrieve(&storeId); tmStore->deleteData(storeId); diff --git a/src/fsfw/tmtcservices/TmTcBridge.h b/src/fsfw/tmtcservices/TmTcBridge.h index 237f1f3e..1caf7b7f 100644 --- a/src/fsfw/tmtcservices/TmTcBridge.h +++ b/src/fsfw/tmtcservices/TmTcBridge.h @@ -72,6 +72,8 @@ class TmTcBridge : public AcceptsTelemetryIF, virtual uint16_t getIdentifier() override; virtual MessageQueueId_t getRequestQueue() override; + bool warningLatch = true; + protected: //! Cached for initialize function. object_id_t tmStoreId = objects::NO_OBJECT; From 07cb980e0645e44db72ceecff750d391d7a318b0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 2 Feb 2022 12:05:03 +0100 Subject: [PATCH 02/81] apply clang script --- src/fsfw/datalinklayer/DataLinkLayer.h | 2 +- .../datalinklayer/VirtualChannelReception.cpp | 4 ++-- src/fsfw/globalfunctions/CRC.cpp | 4 ++-- src/fsfw/modes/ModeMessage.h | 16 +++++++-------- src/fsfw/osal/linux/PeriodicPosixTask.h | 4 ++-- src/fsfw/osal/rtems/PeriodicTask.h | 4 ++-- src/fsfw/osal/rtems/RTEMSTaskBase.h | 4 ++-- src/fsfw/rmap/RMAP.h | 18 ++++++++--------- src/fsfw/rmap/RMAPChannelIF.h | 20 +++++++++---------- src/fsfw/rmap/rmapStructs.h | 6 +++--- 10 files changed, 41 insertions(+), 41 deletions(-) diff --git a/src/fsfw/datalinklayer/DataLinkLayer.h b/src/fsfw/datalinklayer/DataLinkLayer.h index edfba8a6..8735feb6 100644 --- a/src/fsfw/datalinklayer/DataLinkLayer.h +++ b/src/fsfw/datalinklayer/DataLinkLayer.h @@ -30,7 +30,7 @@ class DataLinkLayer : public CCSDSReturnValuesIF { //! [EXPORT] : [COMMENT] A previously found Bit Lock signal was lost. P1: raw BLO state, P2: 0 static const Event BIT_LOCK_LOST = MAKE_EVENT(3, severity::INFO); // static const Event RF_CHAIN_LOST = MAKE_EVENT(4, severity::INFO); //!< The CCSDS Board - //detected that either bit lock or RF available or both are lost. No parameters. + // detected that either bit lock or RF available or both are lost. No parameters. //! [EXPORT] : [COMMENT] The CCSDS Board could not interpret a TC static const Event FRAME_PROCESSING_FAILED = MAKE_EVENT(5, severity::LOW); /** diff --git a/src/fsfw/datalinklayer/VirtualChannelReception.cpp b/src/fsfw/datalinklayer/VirtualChannelReception.cpp index 4ee0c008..258bc1e6 100644 --- a/src/fsfw/datalinklayer/VirtualChannelReception.cpp +++ b/src/fsfw/datalinklayer/VirtualChannelReception.cpp @@ -30,9 +30,9 @@ ReturnValue_t VirtualChannelReception::mapDemultiplexing(TcTransferFrame* frame) mapChannelIterator iter = mapChannels.find(mapId); if (iter == mapChannels.end()) { // error << "VirtualChannelReception::mapDemultiplexing on VC " << std::hex << (int) - //channelId + // channelId // << ": MapChannel " << (int) mapId << std::dec << " not found." << - //std::endl; + // std::endl; return VC_NOT_FOUND; } else { return (iter->second)->extractPackets(frame); diff --git a/src/fsfw/globalfunctions/CRC.cpp b/src/fsfw/globalfunctions/CRC.cpp index 942e4a1a..6b8140c5 100644 --- a/src/fsfw/globalfunctions/CRC.cpp +++ b/src/fsfw/globalfunctions/CRC.cpp @@ -116,8 +116,8 @@ uint16_t CRC::crc16ccitt(uint8_t const input[], uint32_t length, uint16_t starti // for (int i=0; i<16 ;i++) // { // if (xor_out[i] == true) - // crc_value = crc_value + pow(2,(15 -i)); // reverse CrC result before Final - //XOR + // crc_value = crc_value + pow(2,(15 -i)); // reverse CrC result before + //Final XOR // } // // crc_value = 0;// for debug mode diff --git a/src/fsfw/modes/ModeMessage.h b/src/fsfw/modes/ModeMessage.h index e3638ee0..84429e84 100644 --- a/src/fsfw/modes/ModeMessage.h +++ b/src/fsfw/modes/ModeMessage.h @@ -14,30 +14,30 @@ class ModeMessage { static const uint8_t MESSAGE_ID = messagetypes::MODE_COMMAND; static const Command_t CMD_MODE_COMMAND = MAKE_COMMAND_ID(0x01); //!> Command to set the specified Mode, replies are: REPLY_MODE_REPLY, - //!REPLY_WRONG_MODE_REPLY, and REPLY_REJECTED; don't add any replies, - //!as this will break the subsystem mode machine!! + //! REPLY_WRONG_MODE_REPLY, and REPLY_REJECTED; don't add any replies, + //! as this will break the subsystem mode machine!! static const Command_t CMD_MODE_COMMAND_FORCED = MAKE_COMMAND_ID( 0xF1); //!> Command to set the specified Mode, regardless of external control flag, replies - //!are: REPLY_MODE_REPLY, REPLY_WRONG_MODE_REPLY, and REPLY_REJECTED; don't add any - //!replies, as this will break the subsystem mode machine!! + //! are: REPLY_MODE_REPLY, REPLY_WRONG_MODE_REPLY, and REPLY_REJECTED; don't add any + //! replies, as this will break the subsystem mode machine!! static const Command_t REPLY_MODE_REPLY = MAKE_COMMAND_ID(0x02); //!> Reply to a CMD_MODE_COMMAND or CMD_MODE_READ static const Command_t REPLY_MODE_INFO = MAKE_COMMAND_ID(0x03); //!> Unrequested info about the current mode (used for composites to - //!inform their container of a changed mode) + //! inform their container of a changed mode) static const Command_t REPLY_CANT_REACH_MODE = MAKE_COMMAND_ID( 0x04); //!> Reply in case a mode command can't be executed. Par1: returnCode, Par2: 0 static const Command_t REPLY_WRONG_MODE_REPLY = MAKE_COMMAND_ID(0x05); //!> Reply to a CMD_MODE_COMMAND, indicating that a mode was commanded - //!and a transition started but was aborted; the parameters contain - //!the mode that was reached + //! and a transition started but was aborted; the parameters contain + //! the mode that was reached static const Command_t CMD_MODE_READ = MAKE_COMMAND_ID( 0x06); //!> Command to read the current mode and reply with a REPLY_MODE_REPLY static const Command_t CMD_MODE_ANNOUNCE = MAKE_COMMAND_ID( 0x07); //!> Command to trigger an ModeInfo Event. This command does NOT have a reply. static const Command_t CMD_MODE_ANNOUNCE_RECURSIVELY = MAKE_COMMAND_ID(0x08); //!> Command to trigger an ModeInfo Event and to send this command to - //!every child. This command does NOT have a reply. + //! every child. This command does NOT have a reply. static Mode_t getMode(const CommandMessage* message); static Submode_t getSubmode(const CommandMessage* message); diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.h b/src/fsfw/osal/linux/PeriodicPosixTask.h index e2db042d..3c9a3a0d 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.h +++ b/src/fsfw/osal/linux/PeriodicPosixTask.h @@ -65,8 +65,8 @@ class PeriodicPosixTask : public PosixThread, public PeriodicTaskIF { /** * @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 + * 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); diff --git a/src/fsfw/osal/rtems/PeriodicTask.h b/src/fsfw/osal/rtems/PeriodicTask.h index e2643ec7..ff8617fc 100644 --- a/src/fsfw/osal/rtems/PeriodicTask.h +++ b/src/fsfw/osal/rtems/PeriodicTask.h @@ -13,8 +13,8 @@ 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 */ diff --git a/src/fsfw/osal/rtems/RTEMSTaskBase.h b/src/fsfw/osal/rtems/RTEMSTaskBase.h index 784d3594..9ae9e755 100644 --- a/src/fsfw/osal/rtems/RTEMSTaskBase.h +++ b/src/fsfw/osal/rtems/RTEMSTaskBase.h @@ -25,8 +25,8 @@ class RTEMSTaskBase { * all other attributes are set with default values. * @param priority Sets the priority of a task. Values range from a low 0 to a high 99. * @param stack_size The stack size reserved by the operating system for the task. - * @param nam The name of the Task, as a null-terminated String. Currently max 4 chars - * supported (excluding Null-terminator), rest will be truncated + * @param nam The name of the Task, as a null-terminated String. Currently max 4 + * chars supported (excluding Null-terminator), rest will be truncated */ RTEMSTaskBase(rtems_task_priority priority, size_t stack_size, const char *name); /** diff --git a/src/fsfw/rmap/RMAP.h b/src/fsfw/rmap/RMAP.h index d46fc318..42ee1ac5 100644 --- a/src/fsfw/rmap/RMAP.h +++ b/src/fsfw/rmap/RMAP.h @@ -51,9 +51,9 @@ class RMAP : public HasReturnvaluesIF { // MAKE_RETURN_CODE(0xE4); //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 - // replaced by DeviceCommunicationIF::NULLPOINTER static const ReturnValue_t COMMAND_NULLPOINTER - // = MAKE_RETURN_CODE(0xE5); //datalen was != 0 but data was == NULL in write command, or - // nullpointer in read command + // replaced by DeviceCommunicationIF::NULLPOINTER static const ReturnValue_t + // COMMAND_NULLPOINTER = MAKE_RETURN_CODE(0xE5); //datalen was != 0 but data was == NULL in write + // command, or nullpointer in read command static const ReturnValue_t COMMAND_CHANNEL_DEACTIVATED = MAKE_RETURN_CODE(0xE6); // the channel has no port set static const ReturnValue_t COMMAND_PORT_OUT_OF_RANGE = @@ -73,8 +73,8 @@ class RMAP : public HasReturnvaluesIF { static const ReturnValue_t REPLY_MISSMATCH = MAKE_RETURN_CODE( 0xD3); // a read command was issued, but get_write_rply called, or other way round static const ReturnValue_t REPLY_TIMEOUT = MAKE_RETURN_CODE(0xD4); // timeout - // replaced by DeviceCommunicationIF::NULLPOINTER static const ReturnValue_t REPLY_NULLPOINTER = - // MAKE_RETURN_CODE(0xD5);//one of the arguments in a read reply was NULL return values for + // replaced by DeviceCommunicationIF::NULLPOINTER static const ReturnValue_t REPLY_NULLPOINTER + // = MAKE_RETURN_CODE(0xD5);//one of the arguments in a read reply was NULL return values for // get_reply static const ReturnValue_t REPLY_INTERFACE_BUSY = MAKE_RETURN_CODE(0xC0); // Interface is busy (transmission buffer still being processed) @@ -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 77b56eb9..20dfd5f8 100644 --- a/src/fsfw/rmap/RMAPChannelIF.h +++ b/src/fsfw/rmap/RMAPChannelIF.h @@ -73,16 +73,16 @@ class RMAPChannelIF { * @param datalen length of data * @return * - @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) + * - @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_CHANNEL_DEACTIVATED the channel has no port set - * - @c NOT_SUPPORTED if you dont feel like implementing - * something... + * - @c NOT_SUPPORTED if you dont feel like + * implementing something... */ virtual ReturnValue_t sendCommand(RMAPCookie *cookie, uint8_t instruction, const uint8_t *data, size_t datalen) = 0; @@ -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/rmap/rmapStructs.h b/src/fsfw/rmap/rmapStructs.h index 44eedc16..55e32606 100644 --- a/src/fsfw/rmap/rmapStructs.h +++ b/src/fsfw/rmap/rmapStructs.h @@ -32,10 +32,10 @@ static const uint8_t RMAP_COMMAND_READ = ((1 << RMAP_COMMAND_BIT) | (1 << RMAP_C static const uint8_t RMAP_REPLY_WRITE = ((1 << RMAP_COMMAND_BIT_WRITE) | (1 << RMAP_COMMAND_BIT_REPLY)); static const uint8_t RMAP_REPLY_READ = ((1 << RMAP_COMMAND_BIT_REPLY)); -//#define RMAP_COMMAND_WRITE ((1< Date: Wed, 2 Feb 2022 12:10:39 +0100 Subject: [PATCH 03/81] include fixes --- hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp | 9 +++++---- hal/src/fsfw_hal/linux/uart/UartComIF.cpp | 6 +++--- hal/src/fsfw_hal/linux/uart/UartCookie.cpp | 4 ++-- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp index d7e022dd..7c4dee67 100644 --- a/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp +++ b/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp @@ -1,4 +1,8 @@ -#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" #include #include @@ -8,9 +12,6 @@ #include -#include "fsfw/serviceinterface/ServiceInterface.h" -#include "fsfw_hal/linux/UnixFileGuard.h" -#include "fsfw_hal/linux/utility.h" I2cComIF::I2cComIF(object_id_t objectId) : SystemObject(objectId) {} diff --git a/hal/src/fsfw_hal/linux/uart/UartComIF.cpp b/hal/src/fsfw_hal/linux/uart/UartComIF.cpp index ba970e3e..d9b8f235 100644 --- a/hal/src/fsfw_hal/linux/uart/UartComIF.cpp +++ b/hal/src/fsfw_hal/linux/uart/UartComIF.cpp @@ -1,4 +1,7 @@ #include "UartComIF.h" +#include "fsfw/FSFW.h" +#include "fsfw/serviceinterface.h" +#include "fsfw_hal/linux/utility.h" #include #include @@ -7,9 +10,6 @@ #include -#include "fsfw/serviceinterface.h" -#include "fsfw_hal/linux/utility.h" - UartComIF::UartComIF(object_id_t objectId) : SystemObject(objectId) {} UartComIF::~UartComIF() {} diff --git a/hal/src/fsfw_hal/linux/uart/UartCookie.cpp b/hal/src/fsfw_hal/linux/uart/UartCookie.cpp index 0db961dc..63d90766 100644 --- a/hal/src/fsfw_hal/linux/uart/UartCookie.cpp +++ b/hal/src/fsfw_hal/linux/uart/UartCookie.cpp @@ -1,6 +1,6 @@ -#include "fsfw_hal/linux/uart/UartCookie.h" +#include "UartCookie.h" -#include +#include UartCookie::UartCookie(object_id_t handlerId, std::string deviceFile, UartModes uartMode, uint32_t baudrate, size_t maxReplyLen) From 41614303d7036d2b4b15bd965d898ba1272743f1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 2 Feb 2022 12:11:39 +0100 Subject: [PATCH 04/81] renamed variable --- src/fsfw/tmtcservices/TmTcBridge.cpp | 4 ++-- src/fsfw/tmtcservices/TmTcBridge.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fsfw/tmtcservices/TmTcBridge.cpp b/src/fsfw/tmtcservices/TmTcBridge.cpp index 6fd6bff3..bacadd6d 100644 --- a/src/fsfw/tmtcservices/TmTcBridge.cpp +++ b/src/fsfw/tmtcservices/TmTcBridge.cpp @@ -172,7 +172,7 @@ ReturnValue_t TmTcBridge::storeDownlinkData(TmTcMessage* message) { } if (tmFifo->full()) { - if (warningLatch) { + if (warningSwitch) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "TmTcBridge::storeDownlinkData: TM downlink max. number " "of stored packet IDs reached!" @@ -182,7 +182,7 @@ ReturnValue_t TmTcBridge::storeDownlinkData(TmTcMessage* message) { "TmTcBridge::storeDownlinkData: TM downlink max. number " "of stored packet IDs reached!\n"); #endif - warningLatch = true; + warningSwitch = true; } if (overwriteOld) { tmFifo->retrieve(&storeId); diff --git a/src/fsfw/tmtcservices/TmTcBridge.h b/src/fsfw/tmtcservices/TmTcBridge.h index 1caf7b7f..81d8e5d8 100644 --- a/src/fsfw/tmtcservices/TmTcBridge.h +++ b/src/fsfw/tmtcservices/TmTcBridge.h @@ -72,7 +72,7 @@ class TmTcBridge : public AcceptsTelemetryIF, virtual uint16_t getIdentifier() override; virtual MessageQueueId_t getRequestQueue() override; - bool warningLatch = true; + bool warningSwitch = true; protected: //! Cached for initialize function. From 3a65c0db914bad3f47999f54747ed58a66168bee Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 2 Feb 2022 12:13:42 +0100 Subject: [PATCH 05/81] use C++ casts --- hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp | 2 +- hal/src/fsfw_hal/linux/uart/UartComIF.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp index 7c4dee67..0bf9ff32 100644 --- a/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp +++ b/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp @@ -104,7 +104,7 @@ ReturnValue_t I2cComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s return result; } - if (write(fd, sendData, sendLen) != (int)sendLen) { + if (write(fd, sendData, sendLen) != static_cast(sendLen)) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "I2cComIF::sendMessage: Failed to send data to I2C " "device with error code " diff --git a/hal/src/fsfw_hal/linux/uart/UartComIF.cpp b/hal/src/fsfw_hal/linux/uart/UartComIF.cpp index d9b8f235..d4096608 100644 --- a/hal/src/fsfw_hal/linux/uart/UartComIF.cpp +++ b/hal/src/fsfw_hal/linux/uart/UartComIF.cpp @@ -312,7 +312,7 @@ ReturnValue_t UartComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, fd = uartDeviceMapIter->second.fileDescriptor; - if (write(fd, sendData, sendLen) != (int)sendLen) { + if (write(fd, sendData, sendLen) != static_cast(sendLen)) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "UartComIF::sendMessage: Failed to send data with error code " << errno << ": Error description: " << strerror(errno) << std::endl; From 33386550cf81e47f4a3c95cd063349442dd83d09 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 2 Feb 2022 12:17:42 +0100 Subject: [PATCH 06/81] add uio subdir --- hal/src/fsfw_hal/linux/CMakeLists.txt | 2 ++ 1 file changed, 2 insertions(+) diff --git a/hal/src/fsfw_hal/linux/CMakeLists.txt b/hal/src/fsfw_hal/linux/CMakeLists.txt index 5ec8af06..0fb2d385 100644 --- a/hal/src/fsfw_hal/linux/CMakeLists.txt +++ b/hal/src/fsfw_hal/linux/CMakeLists.txt @@ -14,3 +14,5 @@ if(FSFW_HAL_LINUX_ADD_PERIPHERAL_DRIVERS) add_subdirectory(i2c) add_subdirectory(uart) endif() + +add_subdirectory(uio) From 6698d283b6576bc867b28d06ff772e4b3bb5c33f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 2 Feb 2022 16:04:36 +0100 Subject: [PATCH 07/81] device wants hard reboot event added --- src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp | 1 + src/fsfw/devicehandlers/DeviceHandlerIF.h | 1 + 2 files changed, 2 insertions(+) diff --git a/src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp b/src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp index 48783c20..88fba19f 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp @@ -29,6 +29,7 @@ ReturnValue_t DeviceHandlerFailureIsolation::eventReceived(EventMessage* event) switch (event->getEvent()) { case HasModesIF::MODE_TRANSITION_FAILED: case HasModesIF::OBJECT_IN_INVALID_MODE: + case DeviceHandlerIF::DEVICE_WANTS_HARD_REBOOT: // We'll try a recovery as long as defined in MAX_REBOOT. // Might cause some AssemblyBase cycles, so keep number low. handleRecovery(event->getEvent()); diff --git a/src/fsfw/devicehandlers/DeviceHandlerIF.h b/src/fsfw/devicehandlers/DeviceHandlerIF.h index 1fc63d3b..1ea742f7 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerIF.h +++ b/src/fsfw/devicehandlers/DeviceHandlerIF.h @@ -109,6 +109,7 @@ class DeviceHandlerIF { static const Event INVALID_DEVICE_COMMAND = MAKE_EVENT(8, severity::LOW); static const Event MONITORING_LIMIT_EXCEEDED = MAKE_EVENT(9, severity::LOW); static const Event MONITORING_AMBIGUOUS = MAKE_EVENT(10, severity::HIGH); + static const Event DEVICE_WANTS_HARD_REBOOT = MAKE_EVENT(11, severity::HIGH); static const uint8_t INTERFACE_ID = CLASS_ID::DEVICE_HANDLER_IF; From 992c05df566e0313a0010c7d33f4ed13f931ff2b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 2 Feb 2022 17:52:09 +0100 Subject: [PATCH 08/81] added cpp printout preprocessor guards --- hal/src/fsfw_hal/linux/uio/UioMapper.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/hal/src/fsfw_hal/linux/uio/UioMapper.cpp b/hal/src/fsfw_hal/linux/uio/UioMapper.cpp index 11430915..bdf126bf 100644 --- a/hal/src/fsfw_hal/linux/uio/UioMapper.cpp +++ b/hal/src/fsfw_hal/linux/uio/UioMapper.cpp @@ -7,7 +7,7 @@ #include #include -#include "fsfw/serviceinterface/ServiceInterface.h" +#include "fsfw/serviceinterface.h" UioMapper::UioMapper(std::string uioFile, int mapNum) : uioFile(uioFile), mapNum(mapNum) {} @@ -17,7 +17,9 @@ ReturnValue_t UioMapper::getMappedAdress(uint32_t** address, Permissions permiss ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; int fd = open(uioFile.c_str(), O_RDWR); if (fd < 1) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "PtmeAxiConfig::initialize: Invalid UIO device file" << std::endl; +#endif return HasReturnvaluesIF::RETURN_FAILED; } size_t size = 0; @@ -29,8 +31,10 @@ ReturnValue_t UioMapper::getMappedAdress(uint32_t** address, Permissions permiss mmap(NULL, size, static_cast(permissions), MAP_SHARED, fd, mapNum * getpagesize())); if (*address == MAP_FAILED) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UioMapper::getMappedAdress: Failed to map physical address of uio device " << uioFile.c_str() << " and map" << static_cast(mapNum) << std::endl; +#endif return HasReturnvaluesIF::RETURN_FAILED; } return HasReturnvaluesIF::RETURN_OK; @@ -43,22 +47,28 @@ ReturnValue_t UioMapper::getMapSize(size_t* size) { FILE* fp; fp = fopen(namestream.str().c_str(), "r"); if (fp == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UioMapper::getMapSize: Failed to open file " << namestream.str() << std::endl; +#endif return HasReturnvaluesIF::RETURN_FAILED; } char hexstring[SIZE_HEX_STRING] = ""; int items = fscanf(fp, "%s", hexstring); if (items != 1) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UioMapper::getMapSize: Failed with error code " << errno << " to read size " "string from file " << namestream.str() << std::endl; +#endif return HasReturnvaluesIF::RETURN_FAILED; } items = sscanf(hexstring, "%x", size); if (items != 1) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UioMapper::getMapSize: Failed with error code " << errno << "to convert " << "size of map" << mapNum << " to integer" << std::endl; +#endif return HasReturnvaluesIF::RETURN_FAILED; } fclose(fp); From 8030d9ac1b0ecd94da61f4cb8515b0c1447590f1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 2 Feb 2022 19:47:58 +0100 Subject: [PATCH 09/81] this fixes the warning --- hal/src/fsfw_hal/linux/uio/UioMapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hal/src/fsfw_hal/linux/uio/UioMapper.cpp b/hal/src/fsfw_hal/linux/uio/UioMapper.cpp index bdf126bf..546fb2cc 100644 --- a/hal/src/fsfw_hal/linux/uio/UioMapper.cpp +++ b/hal/src/fsfw_hal/linux/uio/UioMapper.cpp @@ -63,7 +63,7 @@ ReturnValue_t UioMapper::getMapSize(size_t* size) { #endif return HasReturnvaluesIF::RETURN_FAILED; } - items = sscanf(hexstring, "%x", size); + items = sscanf(hexstring, "%lx", size); if (items != 1) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UioMapper::getMapSize: Failed with error code " << errno << "to convert " From 22cd38fffde764f38f5677dd7427f48da8010658 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 2 Feb 2022 20:05:46 +0100 Subject: [PATCH 10/81] this should work for c++11 --- hal/src/fsfw_hal/linux/uio/UioMapper.cpp | 4 ++++ hal/src/fsfw_hal/linux/uio/UioMapper.h | 6 +++--- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/hal/src/fsfw_hal/linux/uio/UioMapper.cpp b/hal/src/fsfw_hal/linux/uio/UioMapper.cpp index 546fb2cc..bc2d06eb 100644 --- a/hal/src/fsfw_hal/linux/uio/UioMapper.cpp +++ b/hal/src/fsfw_hal/linux/uio/UioMapper.cpp @@ -9,6 +9,10 @@ #include "fsfw/serviceinterface.h" +const char UioMapper::UIO_PATH_PREFIX[] = "/sys/class/uio/"; +const char UioMapper::MAP_SUBSTR[] = "/maps/map"; +const char UioMapper::SIZE_FILE_PATH[] = "/size"; + UioMapper::UioMapper(std::string uioFile, int mapNum) : uioFile(uioFile), mapNum(mapNum) {} UioMapper::~UioMapper() {} diff --git a/hal/src/fsfw_hal/linux/uio/UioMapper.h b/hal/src/fsfw_hal/linux/uio/UioMapper.h index d58a4c52..20c90b4d 100644 --- a/hal/src/fsfw_hal/linux/uio/UioMapper.h +++ b/hal/src/fsfw_hal/linux/uio/UioMapper.h @@ -39,9 +39,9 @@ class UioMapper { ReturnValue_t getMappedAdress(uint32_t** address, Permissions permissions); private: - static constexpr char UIO_PATH_PREFIX[] = "/sys/class/uio/"; - static constexpr char MAP_SUBSTR[] = "/maps/map"; - static constexpr char SIZE_FILE_PATH[] = "/size"; + static const char UIO_PATH_PREFIX[]; + static const char MAP_SUBSTR[]; + static const char SIZE_FILE_PATH[]; static constexpr int SIZE_HEX_STRING = 10; std::string uioFile; From 8f95b03e6a6dd321488aa5d60d28f20dddf7549a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 3 Feb 2022 11:13:26 +0100 Subject: [PATCH 11/81] fixes warning for good --- hal/src/fsfw_hal/linux/uio/UioMapper.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/hal/src/fsfw_hal/linux/uio/UioMapper.cpp b/hal/src/fsfw_hal/linux/uio/UioMapper.cpp index bc2d06eb..47939044 100644 --- a/hal/src/fsfw_hal/linux/uio/UioMapper.cpp +++ b/hal/src/fsfw_hal/linux/uio/UioMapper.cpp @@ -67,7 +67,11 @@ ReturnValue_t UioMapper::getMapSize(size_t* size) { #endif return HasReturnvaluesIF::RETURN_FAILED; } - items = sscanf(hexstring, "%lx", size); + uint32_t sizeTmp = 0; + items = sscanf(hexstring, "%x", &sizeTmp); + if(size != nullptr) { + *size = sizeTmp; + } if (items != 1) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UioMapper::getMapSize: Failed with error code " << errno << "to convert " From fca48257b7a156f4563d5a5a6ca71b0ab98bb9fc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 3 Feb 2022 12:02:08 +0100 Subject: [PATCH 12/81] zero initialize array --- src/fsfw/datapoollocal/StaticLocalDataSet.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/datapoollocal/StaticLocalDataSet.h b/src/fsfw/datapoollocal/StaticLocalDataSet.h index dd264e58..59047d67 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_ */ From b3151a0ba033e7c72c3ead1e8958d7be596baa45 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 3 Feb 2022 13:37:28 +0100 Subject: [PATCH 13/81] added i2c wiretapping --- hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp | 14 ++++++++++++++ src/fsfw/FSFW.h.in | 5 +++++ 2 files changed, 19 insertions(+) diff --git a/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp index 0bf9ff32..6390b121 100644 --- a/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp +++ b/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp @@ -4,6 +4,10 @@ #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 #include @@ -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/src/fsfw/FSFW.h.in b/src/fsfw/FSFW.h.in index 88ad10cf..7e8bcd79 100644 --- a/src/fsfw/FSFW.h.in +++ b/src/fsfw/FSFW.h.in @@ -57,6 +57,11 @@ #define FSFW_HAL_SPI_WIRETAPPING 0 #endif +// Can be used for low-level debugging of the I2C bus +#ifndef FSFW_HAL_I2C_WIRETAPPING +#define FSFW_HAL_I2C_WIRETAPPING 0 +#endif + #ifndef FSFW_HAL_L3GD20_GYRO_DEBUG #define FSFW_HAL_L3GD20_GYRO_DEBUG 0 #endif /* FSFW_HAL_L3GD20_GYRO_DEBUG */ From b3aee76d91cdc8da568d844a83fcfde4faadce81 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 3 Feb 2022 17:06:18 +0100 Subject: [PATCH 14/81] fixes for event definitoons for parser --- hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp | 2 +- hal/src/fsfw_hal/linux/uart/UartComIF.cpp | 7 +- hal/src/fsfw_hal/linux/uio/UioMapper.cpp | 4 +- src/fsfw/globalfunctions/CRC.cpp | 2 +- src/fsfw/health/HasHealthIF.h | 22 +++--- src/fsfw/modes/HasModesIF.h | 41 +++++------ src/fsfw/osal/linux/PeriodicPosixTask.h | 7 +- src/fsfw/osal/rtems/PeriodicTask.h | 4 +- src/fsfw/power/Fuse.h | 16 ++--- src/fsfw/rmap/RMAP.h | 8 +-- src/fsfw/rmap/RMAPChannelIF.h | 12 ++-- src/fsfw/tmstorage/TmStoreBackendIF.h | 85 +++++++++++------------ 12 files changed, 101 insertions(+), 109 deletions(-) diff --git a/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp index 6390b121..4f53dc1f 100644 --- a/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp +++ b/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp @@ -1,4 +1,5 @@ #include "I2cComIF.h" + #include "fsfw/FSFW.h" #include "fsfw/serviceinterface.h" #include "fsfw_hal/linux/UnixFileGuard.h" @@ -16,7 +17,6 @@ #include - I2cComIF::I2cComIF(object_id_t objectId) : SystemObject(objectId) {} I2cComIF::~I2cComIF() {} diff --git a/hal/src/fsfw_hal/linux/uart/UartComIF.cpp b/hal/src/fsfw_hal/linux/uart/UartComIF.cpp index d4096608..a648df3a 100644 --- a/hal/src/fsfw_hal/linux/uart/UartComIF.cpp +++ b/hal/src/fsfw_hal/linux/uart/UartComIF.cpp @@ -1,7 +1,4 @@ #include "UartComIF.h" -#include "fsfw/FSFW.h" -#include "fsfw/serviceinterface.h" -#include "fsfw_hal/linux/utility.h" #include #include @@ -10,6 +7,10 @@ #include +#include "fsfw/FSFW.h" +#include "fsfw/serviceinterface.h" +#include "fsfw_hal/linux/utility.h" + UartComIF::UartComIF(object_id_t objectId) : SystemObject(objectId) {} UartComIF::~UartComIF() {} diff --git a/hal/src/fsfw_hal/linux/uio/UioMapper.cpp b/hal/src/fsfw_hal/linux/uio/UioMapper.cpp index 47939044..33c1b0f2 100644 --- a/hal/src/fsfw_hal/linux/uio/UioMapper.cpp +++ b/hal/src/fsfw_hal/linux/uio/UioMapper.cpp @@ -69,8 +69,8 @@ ReturnValue_t UioMapper::getMapSize(size_t* size) { } uint32_t sizeTmp = 0; items = sscanf(hexstring, "%x", &sizeTmp); - if(size != nullptr) { - *size = sizeTmp; + if (size != nullptr) { + *size = sizeTmp; } if (items != 1) { #if FSFW_CPP_OSTREAM_ENABLED == 1 diff --git a/src/fsfw/globalfunctions/CRC.cpp b/src/fsfw/globalfunctions/CRC.cpp index 6b8140c5..033920d0 100644 --- a/src/fsfw/globalfunctions/CRC.cpp +++ b/src/fsfw/globalfunctions/CRC.cpp @@ -117,7 +117,7 @@ uint16_t CRC::crc16ccitt(uint8_t const input[], uint32_t length, uint16_t starti // { // if (xor_out[i] == true) // crc_value = crc_value + pow(2,(15 -i)); // reverse CrC result before - //Final XOR + // Final XOR // } // // crc_value = 0;// for debug mode diff --git a/src/fsfw/health/HasHealthIF.h b/src/fsfw/health/HasHealthIF.h index 41abeef3..bdd1ce96 100644 --- a/src/fsfw/health/HasHealthIF.h +++ b/src/fsfw/health/HasHealthIF.h @@ -23,19 +23,15 @@ class HasHealthIF { static const Event HEALTH_INFO = MAKE_EVENT(6, severity::INFO); static const Event CHILD_CHANGED_HEALTH = MAKE_EVENT(7, severity::INFO); static const Event CHILD_PROBLEMS = MAKE_EVENT(8, severity::LOW); - static const Event OVERWRITING_HEALTH = - MAKE_EVENT(9, severity::LOW); //!< Assembly overwrites health information of children to keep - //!< satellite alive. - static const Event TRYING_RECOVERY = - MAKE_EVENT(10, severity::MEDIUM); //!< Someone starts a recovery of a component (typically - //!< power-cycle). No parameters. - static const Event RECOVERY_STEP = - MAKE_EVENT(11, severity::MEDIUM); //!< Recovery is ongoing. Comes twice during recovery. P1: - //!< 0 for the first, 1 for the second event. P2: 0 - static const Event RECOVERY_DONE = MAKE_EVENT( - 12, - severity::MEDIUM); //!< Recovery was completed. Not necessarily successful. No parameters. - + //! Assembly overwrites health information of children to keep satellite alive. + static const Event OVERWRITING_HEALTH = MAKE_EVENT(9, severity::LOW); + //! Someone starts a recovery of a component (typically power-cycle). No parameters. + static const Event TRYING_RECOVERY = MAKE_EVENT(10, severity::MEDIUM); + //! Recovery is ongoing. Comes twice during recovery. + //! P1: 0 for the first, 1 for the second event. P2: 0 + static const Event RECOVERY_STEP = MAKE_EVENT(11, severity::MEDIUM); + //! Recovery was completed. Not necessarily successful. No parameters. + static const Event RECOVERY_DONE = MAKE_EVENT(12, severity::MEDIUM); virtual ~HasHealthIF() {} virtual MessageQueueId_t getCommandQueue() const = 0; diff --git a/src/fsfw/modes/HasModesIF.h b/src/fsfw/modes/HasModesIF.h index 0ebe77d8..850d4349 100644 --- a/src/fsfw/modes/HasModesIF.h +++ b/src/fsfw/modes/HasModesIF.h @@ -19,32 +19,29 @@ class HasModesIF { static const ReturnValue_t INVALID_SUBMODE = MAKE_RETURN_CODE(0x04); static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SYSTEM_MANAGER; - static const Event CHANGING_MODE = - MAKE_EVENT(0, severity::INFO); //!< An object announces changing the mode. p1: target mode. - //!< p2: target submode - static const Event MODE_INFO = MAKE_EVENT( - 1, - severity::INFO); //!< An Object announces its mode; parameter1 is mode, parameter2 is submode + //! An object announces changing the mode. p1: target mode. p2: target submode + static const Event CHANGING_MODE = MAKE_EVENT(0, severity::INFO); + //! An Object announces its mode; parameter1 is mode, parameter2 is submode + static const Event MODE_INFO = MAKE_EVENT(1, severity::INFO); static const Event FALLBACK_FAILED = MAKE_EVENT(2, severity::HIGH); static const Event MODE_TRANSITION_FAILED = MAKE_EVENT(3, severity::LOW); static const Event CANT_KEEP_MODE = MAKE_EVENT(4, severity::HIGH); - static const Event OBJECT_IN_INVALID_MODE = - MAKE_EVENT(5, severity::LOW); //!< Indicates a bug or configuration failure: Object is in a - //!< mode it should never be in. - static const Event FORCING_MODE = MAKE_EVENT( - 6, severity::MEDIUM); //!< The mode is changed, but for some reason, the change is forced, - //!< i.e. EXTERNAL_CONTROL ignored. p1: target mode. p2: target submode - static const Event MODE_CMD_REJECTED = - MAKE_EVENT(7, severity::LOW); //!< A mode command was rejected by the called object. Par1: - //!< called object id, Par2: return code. + //! Indicates a bug or configuration failure: Object is in a mode it should never be in. + static const Event OBJECT_IN_INVALID_MODE = MAKE_EVENT(5, severity::LOW); + //! The mode is changed, but for some reason, the change is forced, i.e. EXTERNAL_CONTROL ignored. + //! p1: target mode. p2: target submode + static const Event FORCING_MODE = MAKE_EVENT(6, severity::MEDIUM); + //! A mode command was rejected by the called object. Par1: called object id, Par2: return code. + static const Event MODE_CMD_REJECTED = MAKE_EVENT(7, severity::LOW); - static const Mode_t MODE_ON = - 1; //!< The device is powered and ready to perform operations. In this mode, no commands are - //!< sent by the device handler itself, but direct commands van be commanded and will be - //!< interpreted - static const Mode_t MODE_OFF = 0; //!< The device is powered off. The only command accepted in - //!< this mode is a mode change to on. - static const Submode_t SUBMODE_NONE = 0; //!< To avoid checks against magic number "0". + //! The device is powered and ready to perform operations. In this mode, no commands are + //! sent by the device handler itself, but direct commands van be commanded and will be + //! interpreted + static const Mode_t MODE_ON = 1; + //! The device is powered off. The only command accepted in this mode is a mode change to on. + static const Mode_t MODE_OFF = 0; + //! To avoid checks against magic number "0". + static const Submode_t SUBMODE_NONE = 0; virtual ~HasModesIF() {} virtual MessageQueueId_t getCommandQueue() const = 0; diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.h b/src/fsfw/osal/linux/PeriodicPosixTask.h index 3c9a3a0d..1c3a52c7 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.h +++ b/src/fsfw/osal/linux/PeriodicPosixTask.h @@ -65,9 +65,10 @@ class PeriodicPosixTask : public PosixThread, public PeriodicTaskIF { /** * @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); /** diff --git a/src/fsfw/osal/rtems/PeriodicTask.h b/src/fsfw/osal/rtems/PeriodicTask.h index ff8617fc..119329f2 100644 --- a/src/fsfw/osal/rtems/PeriodicTask.h +++ b/src/fsfw/osal/rtems/PeriodicTask.h @@ -13,8 +13,8 @@ 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 */ diff --git a/src/fsfw/power/Fuse.h b/src/fsfw/power/Fuse.h index e6f9c149..0b57a6a9 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, diff --git a/src/fsfw/rmap/RMAP.h b/src/fsfw/rmap/RMAP.h index 42ee1ac5..d274fb15 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 20dfd5f8..0c937dc8 100644 --- a/src/fsfw/rmap/RMAPChannelIF.h +++ b/src/fsfw/rmap/RMAPChannelIF.h @@ -75,10 +75,10 @@ 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 + * - @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 @@ -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/tmstorage/TmStoreBackendIF.h b/src/fsfw/tmstorage/TmStoreBackendIF.h index d9f1a17b..1e08342a 100644 --- a/src/fsfw/tmstorage/TmStoreBackendIF.h +++ b/src/fsfw/tmstorage/TmStoreBackendIF.h @@ -33,50 +33,47 @@ class TmStoreBackendIF : public HasParametersIF { static const ReturnValue_t INVALID_REQUEST = MAKE_RETURN_CODE(15); static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::MEMORY; - static const Event STORE_SEND_WRITE_FAILED = - MAKE_EVENT(0, severity::LOW); //!< Initiating sending data to store failed. Low, par1: - //!< returnCode, par2: integer (debug info) - static const Event STORE_WRITE_FAILED = MAKE_EVENT( - 1, severity::LOW); //!< Data was sent, but writing failed. Low, par1: returnCode, par2: 0 - static const Event STORE_SEND_READ_FAILED = - MAKE_EVENT(2, severity::LOW); //!< Initiating reading data from store failed. Low, par1: - //!< returnCode, par2: 0 - static const Event STORE_READ_FAILED = MAKE_EVENT( - 3, severity::LOW); //!< Data was requested, but access failed. Low, par1: returnCode, par2: 0 - static const Event UNEXPECTED_MSG = - MAKE_EVENT(4, severity::LOW); //!< An unexpected TM packet or data message occurred. Low, - //!< par1: 0, par2: integer (debug info) - static const Event STORING_FAILED = MAKE_EVENT( - 5, severity::LOW); //!< Storing data failed. May simply be a full store. Low, par1: - //!< returnCode, par2: integer (sequence count of failed packet). - static const Event TM_DUMP_FAILED = - MAKE_EVENT(6, severity::LOW); //!< Dumping retrieved data failed. Low, par1: returnCode, - //!< par2: integer (sequence count of failed packet). - static const Event STORE_INIT_FAILED = - MAKE_EVENT(7, severity::LOW); //!< Corrupted init data or read error. Low, par1: returnCode, - //!< par2: integer (debug info) - static const Event STORE_INIT_EMPTY = MAKE_EVENT( - 8, severity::INFO); //!< Store was not initialized. Starts empty. Info, parameters both zero. - static const Event STORE_CONTENT_CORRUPTED = - MAKE_EVENT(9, severity::LOW); //!< Data was read out, but it is inconsistent. Low par1: - //!< Memory address of corruption, par2: integer (debug info) - static const Event STORE_INITIALIZE = - MAKE_EVENT(10, severity::INFO); //!< Info event indicating the store will be initialized, - //!< either at boot or after IOB switch. Info. pars: 0 - static const Event INIT_DONE = MAKE_EVENT( - 11, severity::INFO); //!< Info event indicating the store was successfully initialized, - //!< either at boot or after IOB switch. Info. pars: 0 - static const Event DUMP_FINISHED = MAKE_EVENT( - 12, severity::INFO); //!< Info event indicating that dumping finished successfully. par1: - //!< Number of dumped packets. par2: APID/SSC (16bits each) - static const Event DELETION_FINISHED = MAKE_EVENT( - 13, severity::INFO); //!< Info event indicating that deletion finished successfully. par1: - //!< Number of deleted packets. par2: APID/SSC (16bits each) - static const Event DELETION_FAILED = MAKE_EVENT( - 14, - severity::LOW); //!< Info event indicating that something went wrong during deletion. pars: 0 - static const Event AUTO_CATALOGS_SENDING_FAILED = - MAKE_EVENT(15, severity::INFO); //!< Info that the a auto catalog report failed + //! Initiating sending data to store failed. Low, par1: + //! returnCode, par2: integer (debug info) + static const Event STORE_SEND_WRITE_FAILED = MAKE_EVENT(0, severity::LOW); + //! Data was sent, but writing failed. Low, par1: returnCode, par2: 0 + static const Event STORE_WRITE_FAILED = MAKE_EVENT(1, severity::LOW); + //! Initiating reading data from store failed. Low, par1: returnCode, par2: 0 + static const Event STORE_SEND_READ_FAILED = MAKE_EVENT(2, severity::LOW); + //! Data was requested, but access failed. Low, par1: returnCode, par2: 0 + static const Event STORE_READ_FAILED = MAKE_EVENT(3, severity::LOW); + //! An unexpected TM packet or data message occurred. Low, par1: 0, par2: integer (debug info) + static const Event UNEXPECTED_MSG = MAKE_EVENT(4, severity::LOW); + //! Storing data failed. May simply be a full store. Low, par1: returnCode, + //! par2: integer (sequence count of failed packet). + static const Event STORING_FAILED = MAKE_EVENT(5, severity::LOW); + //! Dumping retrieved data failed. Low, par1: returnCode, + //! par2: integer (sequence count of failed packet). + static const Event TM_DUMP_FAILED = MAKE_EVENT(6, severity::LOW); + //! Corrupted init data or read error. Low, par1: returnCode, par2: integer (debug info) + //! Store was not initialized. Starts empty. Info, parameters both zero. + static const Event STORE_INIT_FAILED = MAKE_EVENT(7, severity::LOW); + //! Data was read out, but it is inconsistent. Low par1: + //! Memory address of corruption, par2: integer (debug info) + static const Event STORE_INIT_EMPTY = MAKE_EVENT(8, severity::INFO); + + static const Event STORE_CONTENT_CORRUPTED = MAKE_EVENT(9, severity::LOW); + //! Info event indicating the store will be initialized, either at boot or after IOB switch. + //! Info. pars: 0 + static const Event STORE_INITIALIZE = MAKE_EVENT(10, severity::INFO); + //! Info event indicating the store was successfully initialized, either at boot or after + //! IOB switch. Info. pars: 0 + static const Event INIT_DONE = MAKE_EVENT(11, severity::INFO); + //! Info event indicating that dumping finished successfully. + //! par1: Number of dumped packets. par2: APID/SSC (16bits each) + static const Event DUMP_FINISHED = MAKE_EVENT(12, severity::INFO); + //! Info event indicating that deletion finished successfully. + //! par1:Number of deleted packets. par2: APID/SSC (16bits each) + static const Event DELETION_FINISHED = MAKE_EVENT(13, severity::INFO); + //! Info event indicating that something went wrong during deletion. pars: 0 + static const Event DELETION_FAILED = MAKE_EVENT(14, severity::LOW); + //! Info that the a auto catalog report failed + static const Event AUTO_CATALOGS_SENDING_FAILED = MAKE_EVENT(15, severity::INFO); virtual ~TmStoreBackendIF() {} From 973996e1027500c7a6028949d8333b0cc23ba49f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 3 Feb 2022 17:08:30 +0100 Subject: [PATCH 15/81] more fixes --- src/fsfw/fdir/FailureIsolationBase.h | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/fsfw/fdir/FailureIsolationBase.h b/src/fsfw/fdir/FailureIsolationBase.h index 85d18add..0b637080 100644 --- a/src/fsfw/fdir/FailureIsolationBase.h +++ b/src/fsfw/fdir/FailureIsolationBase.h @@ -14,13 +14,15 @@ class FailureIsolationBase : public HasReturnvaluesIF, public HasParametersIF { public: static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::FDIR_1; + //! FDIR has an internal state, which changed from par2 (oldState) to par1 (newState). static const Event FDIR_CHANGED_STATE = - MAKE_EVENT(1, severity::INFO); //!< FDIR has an internal state, which changed from par2 - //!< (oldState) to par1 (newState). + MAKE_EVENT(1, severity::INFO); + //! FDIR tries to restart device. Par1: event that caused recovery. static const Event FDIR_STARTS_RECOVERY = MAKE_EVENT( - 2, severity::MEDIUM); //!< FDIR tries to restart device. Par1: event that caused recovery. + 2, severity::MEDIUM); + //! FDIR turns off device. Par1: event that caused recovery. static const Event FDIR_TURNS_OFF_DEVICE = MAKE_EVENT( - 3, severity::MEDIUM); //!< FDIR turns off device. Par1: event that caused recovery. + 3, severity::MEDIUM); FailureIsolationBase(object_id_t owner, object_id_t parent = objects::NO_OBJECT, uint8_t messageDepth = 10, uint8_t parameterDomainBase = 0xF0); From a910a055416962a97d575bf37c39acd6f8ac59ea Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 3 Feb 2022 17:09:58 +0100 Subject: [PATCH 16/81] parser is not perfect.. --- src/fsfw/fdir/FailureIsolationBase.h | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/fsfw/fdir/FailureIsolationBase.h b/src/fsfw/fdir/FailureIsolationBase.h index 0b637080..7d128083 100644 --- a/src/fsfw/fdir/FailureIsolationBase.h +++ b/src/fsfw/fdir/FailureIsolationBase.h @@ -15,14 +15,11 @@ class FailureIsolationBase : public HasReturnvaluesIF, public: static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::FDIR_1; //! FDIR has an internal state, which changed from par2 (oldState) to par1 (newState). - static const Event FDIR_CHANGED_STATE = - MAKE_EVENT(1, severity::INFO); + static const Event FDIR_CHANGED_STATE = MAKE_EVENT(1, severity::INFO); //! FDIR tries to restart device. Par1: event that caused recovery. - static const Event FDIR_STARTS_RECOVERY = MAKE_EVENT( - 2, severity::MEDIUM); + static const Event FDIR_STARTS_RECOVERY = MAKE_EVENT(2, severity::MEDIUM); //! FDIR turns off device. Par1: event that caused recovery. - static const Event FDIR_TURNS_OFF_DEVICE = MAKE_EVENT( - 3, severity::MEDIUM); + static const Event FDIR_TURNS_OFF_DEVICE = MAKE_EVENT(3, severity::MEDIUM); FailureIsolationBase(object_id_t owner, object_id_t parent = objects::NO_OBJECT, uint8_t messageDepth = 10, uint8_t parameterDomainBase = 0xF0); From b7f6a6961b6f0815890c2ae98d4fd1a11c72b8c0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 3 Feb 2022 17:14:11 +0100 Subject: [PATCH 17/81] delete switch helper, some other fixes --- src/fsfw/datapool/HkSwitchHelper.cpp | 67 --------------------------- src/fsfw/datapool/HkSwitchHelper.h | 44 ------------------ src/fsfw/pus/Service9TimeManagement.h | 8 ++-- 3 files changed, 4 insertions(+), 115 deletions(-) delete mode 100644 src/fsfw/datapool/HkSwitchHelper.cpp delete mode 100644 src/fsfw/datapool/HkSwitchHelper.h diff --git a/src/fsfw/datapool/HkSwitchHelper.cpp b/src/fsfw/datapool/HkSwitchHelper.cpp deleted file mode 100644 index 7f6ffd17..00000000 --- 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 a0becd81..00000000 --- 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/pus/Service9TimeManagement.h b/src/fsfw/pus/Service9TimeManagement.h index 9369e207..1bea2f51 100644 --- a/src/fsfw/pus/Service9TimeManagement.h +++ b/src/fsfw/pus/Service9TimeManagement.h @@ -6,10 +6,10 @@ class Service9TimeManagement : public PusServiceBase { public: static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_9; - static constexpr Event CLOCK_SET = - MAKE_EVENT(0, severity::INFO); //!< Clock has been set. P1: New Uptime. P2: Old Uptime - static constexpr Event CLOCK_SET_FAILURE = - MAKE_EVENT(1, severity::LOW); //!< Clock could not be set. P1: Returncode. + //!< Clock has been set. P1: New Uptime. P2: Old Uptime + static constexpr Event CLOCK_SET = MAKE_EVENT(0, severity::INFO); + //!< Clock could not be set. P1: Returncode. + static constexpr Event CLOCK_SET_FAILURE = MAKE_EVENT(1, severity::LOW); static constexpr uint8_t CLASS_ID = CLASS_ID::PUS_SERVICE_9; From 0d4bd856bd6753db60e2465a754ee728ac44cabc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 3 Feb 2022 17:33:46 +0100 Subject: [PATCH 18/81] removed HK switch helper from cmake file --- src/fsfw/datapool/CMakeLists.txt | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/fsfw/datapool/CMakeLists.txt b/src/fsfw/datapool/CMakeLists.txt index 0d53e1ba..535be2b0 100644 --- a/src/fsfw/datapool/CMakeLists.txt +++ b/src/fsfw/datapool/CMakeLists.txt @@ -1,6 +1,4 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - HkSwitchHelper.cpp - PoolDataSetBase.cpp - PoolEntry.cpp +target_sources(${LIB_FSFW_NAME} PRIVATE + PoolDataSetBase.cpp + PoolEntry.cpp ) \ No newline at end of file From 940c53eba62fa2e41bdd590d109b54a704e4f617 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 3 Feb 2022 17:33:46 +0100 Subject: [PATCH 19/81] removed HK switch helper from cmake file --- src/fsfw/datapool/CMakeLists.txt | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/fsfw/datapool/CMakeLists.txt b/src/fsfw/datapool/CMakeLists.txt index 0d53e1ba..535be2b0 100644 --- a/src/fsfw/datapool/CMakeLists.txt +++ b/src/fsfw/datapool/CMakeLists.txt @@ -1,6 +1,4 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - HkSwitchHelper.cpp - PoolDataSetBase.cpp - PoolEntry.cpp +target_sources(${LIB_FSFW_NAME} PRIVATE + PoolDataSetBase.cpp + PoolEntry.cpp ) \ No newline at end of file From 32a9e0c7044665f0265c10108c8d62d45c047769 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 3 Feb 2022 17:37:11 +0100 Subject: [PATCH 20/81] another include removed --- src/fsfw/controller/ControllerBase.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/fsfw/controller/ControllerBase.h b/src/fsfw/controller/ControllerBase.h index db75982c..7032f817 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" From 581ae4c990ac9b356f5952066228d2a1e9939ee0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 3 Feb 2022 17:37:11 +0100 Subject: [PATCH 21/81] another include removed --- src/fsfw/controller/ControllerBase.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/fsfw/controller/ControllerBase.h b/src/fsfw/controller/ControllerBase.h index db75982c..7032f817 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" From 235fd79dfbd7d0ca1c099f816a1d6167874d4d08 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Sat, 5 Feb 2022 16:08:28 +0100 Subject: [PATCH 22/81] added missing baudrates --- hal/src/fsfw_hal/linux/uart/UartComIF.cpp | 90 ++++++++++++----------- 1 file changed, 47 insertions(+), 43 deletions(-) diff --git a/hal/src/fsfw_hal/linux/uart/UartComIF.cpp b/hal/src/fsfw_hal/linux/uart/UartComIF.cpp index a648df3a..12ed969c 100644 --- a/hal/src/fsfw_hal/linux/uart/UartComIF.cpp +++ b/hal/src/fsfw_hal/linux/uart/UartComIF.cpp @@ -7,8 +7,8 @@ #include -#include "fsfw/FSFW.h" -#include "fsfw/serviceinterface.h" +#include "OBSWConfig.h" +#include "fsfw/serviceinterface/ServiceInterface.h" #include "fsfw_hal/linux/utility.h" UartComIF::UartComIF(object_id_t objectId) : SystemObject(objectId) {} @@ -25,9 +25,7 @@ ReturnValue_t UartComIF::initializeInterface(CookieIF* cookie) { UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "UartComIF::initializeInterface: Invalid UART Cookie!" << std::endl; -#endif return NULLPOINTER; } @@ -43,17 +41,13 @@ ReturnValue_t UartComIF::initializeInterface(CookieIF* cookie) { UartElements uartElements = {fileDescriptor, std::vector(maxReplyLen), 0}; auto status = uartDeviceMap.emplace(deviceFile, uartElements); if (status.second == false) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::initializeInterface: Failed to insert device " << deviceFile << "to UART device map" << std::endl; -#endif return RETURN_FAILED; } } else { -#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::initializeInterface: UART device " << deviceFile << " already in use" << std::endl; -#endif return RETURN_FAILED; } @@ -73,19 +67,15 @@ int UartComIF::configureUartPort(UartCookie* uartCookie) { int fd = open(deviceFile.c_str(), flags); if (fd < 0) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::configureUartPort: Failed to open uart " << deviceFile << "with error code " << errno << strerror(errno) << std::endl; -#endif return fd; } /* Read in existing settings */ if (tcgetattr(fd, &options) != 0) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::configureUartPort: Error " << errno << "from tcgetattr: " << strerror(errno) << std::endl; -#endif return fd; } @@ -106,10 +96,8 @@ int UartComIF::configureUartPort(UartCookie* uartCookie) { /* Save option settings */ if (tcsetattr(fd, TCSANOW, &options) != 0) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::configureUartPort: Failed to set options with error " << errno << ": " << strerror(errno); -#endif return fd; } return fd; @@ -161,9 +149,7 @@ void UartComIF::setDatasizeOptions(struct termios* options, UartCookie* uartCook options->c_cflag |= CS8; break; default: -#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::setDatasizeOptions: Invalid size specified" << std::endl; -#endif break; } } @@ -269,10 +255,52 @@ void UartComIF::configureBaudrate(struct termios* options, UartCookie* uartCooki cfsetispeed(options, B460800); cfsetospeed(options, B460800); break; + case 500000: + cfsetispeed(options, B500000); + cfsetospeed(options, B500000); + break; + case 576000: + cfsetispeed(options, B576000); + cfsetospeed(options, B576000); + break; + case 921600: + cfsetispeed(options, B921600); + cfsetospeed(options, B921600); + break; + case 1000000: + cfsetispeed(options, B1000000); + cfsetospeed(options, B1000000); + break; + case 1152000: + cfsetispeed(options, B1152000); + cfsetospeed(options, B1152000); + break; + case 1500000: + cfsetispeed(options, B1500000); + cfsetospeed(options, B1500000); + break; + case 2000000: + cfsetispeed(options, B2000000); + cfsetospeed(options, B2000000); + break; + case 2500000: + cfsetispeed(options, B2500000); + cfsetospeed(options, B2500000); + break; + case 3000000: + cfsetispeed(options, B3000000); + cfsetospeed(options, B3000000); + break; + case 3500000: + cfsetispeed(options, B3500000); + cfsetospeed(options, B3500000); + break; + case 4000000: + cfsetispeed(options, B4000000); + cfsetospeed(options, B4000000); + break; default: -#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::configureBaudrate: Baudrate not supported" << std::endl; -#endif break; } } @@ -287,37 +315,29 @@ ReturnValue_t UartComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, } if (sendData == nullptr) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::sendMessage: Send data is nullptr" << std::endl; -#endif return RETURN_FAILED; } UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::sendMessasge: Invalid UART Cookie!" << std::endl; -#endif return NULLPOINTER; } deviceFile = uartCookie->getDeviceFile(); uartDeviceMapIter = uartDeviceMap.find(deviceFile); if (uartDeviceMapIter == uartDeviceMap.end()) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::debug << "UartComIF::sendMessage: Device file " << deviceFile << "not in UART map" << std::endl; -#endif return RETURN_FAILED; } fd = uartDeviceMapIter->second.fileDescriptor; - if (write(fd, sendData, sendLen) != static_cast(sendLen)) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 + if (write(fd, sendData, sendLen) != (int)sendLen) { sif::error << "UartComIF::sendMessage: Failed to send data with error code " << errno << ": Error description: " << strerror(errno) << std::endl; -#endif return RETURN_FAILED; } @@ -332,9 +352,7 @@ ReturnValue_t UartComIF::requestReceiveMessage(CookieIF* cookie, size_t requestL UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::debug << "UartComIF::requestReceiveMessage: Invalid Uart Cookie!" << std::endl; -#endif return NULLPOINTER; } @@ -347,10 +365,8 @@ ReturnValue_t UartComIF::requestReceiveMessage(CookieIF* cookie, size_t requestL } if (uartDeviceMapIter == uartDeviceMap.end()) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::debug << "UartComIF::requestReceiveMessage: Device file " << deviceFile << " not in uart map" << std::endl; -#endif return RETURN_FAILED; } @@ -444,10 +460,8 @@ ReturnValue_t UartComIF::handleNoncanonicalRead(UartCookie& uartCookie, UartDevi return RETURN_FAILED; } else if (bytesRead != static_cast(requestLen)) { if (uartCookie.isReplySizeFixed()) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::requestReceiveMessage: Only read " << bytesRead << " of " << requestLen << " bytes" << std::endl; -#endif return RETURN_FAILED; } } @@ -461,19 +475,15 @@ ReturnValue_t UartComIF::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::debug << "UartComIF::readReceivedMessage: Invalid uart cookie!" << std::endl; -#endif return NULLPOINTER; } deviceFile = uartCookie->getDeviceFile(); uartDeviceMapIter = uartDeviceMap.find(deviceFile); if (uartDeviceMapIter == uartDeviceMap.end()) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::debug << "UartComIF::readReceivedMessage: Device file " << deviceFile << " not in uart map" << std::endl; -#endif return RETURN_FAILED; } @@ -491,9 +501,7 @@ ReturnValue_t UartComIF::flushUartRxBuffer(CookieIF* cookie) { UartDeviceMapIter uartDeviceMapIter; UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::flushUartRxBuffer: Invalid uart cookie!" << std::endl; -#endif return NULLPOINTER; } deviceFile = uartCookie->getDeviceFile(); @@ -511,9 +519,7 @@ ReturnValue_t UartComIF::flushUartTxBuffer(CookieIF* cookie) { UartDeviceMapIter uartDeviceMapIter; UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::flushUartTxBuffer: Invalid uart cookie!" << std::endl; -#endif return NULLPOINTER; } deviceFile = uartCookie->getDeviceFile(); @@ -531,9 +537,7 @@ ReturnValue_t UartComIF::flushUartTxAndRxBuf(CookieIF* cookie) { UartDeviceMapIter uartDeviceMapIter; UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::flushUartTxAndRxBuf: Invalid uart cookie!" << std::endl; -#endif return NULLPOINTER; } deviceFile = uartCookie->getDeviceFile(); From 9579e94a7111865c4a9cf2c5d07fbfa32a6ed010 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Sat, 5 Feb 2022 16:09:23 +0100 Subject: [PATCH 23/81] option to exclude libgpiod from build --- hal/src/fsfw_hal/linux/CMakeLists.txt | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/hal/src/fsfw_hal/linux/CMakeLists.txt b/hal/src/fsfw_hal/linux/CMakeLists.txt index 0fb2d385..9fcb3b24 100644 --- a/hal/src/fsfw_hal/linux/CMakeLists.txt +++ b/hal/src/fsfw_hal/linux/CMakeLists.txt @@ -8,11 +8,11 @@ target_sources(${LIB_FSFW_NAME} PRIVATE utility.cpp ) -if(FSFW_HAL_LINUX_ADD_PERIPHERAL_DRIVERS) +if(FSFW_HAL_ADD_LIBGPIOD) add_subdirectory(gpio) - add_subdirectory(spi) - add_subdirectory(i2c) - add_subdirectory(uart) endif() -add_subdirectory(uio) +add_subdirectory(spi) +add_subdirectory(i2c) +add_subdirectory(uart) +add_subdirectory(uio) \ No newline at end of file From c0648a789bfc80162a6a80bc85393695042356bc Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Sat, 5 Feb 2022 17:07:06 +0100 Subject: [PATCH 24/81] merged develop --- hal/CMakeLists.txt | 1 + hal/src/fsfw_hal/linux/CMakeLists.txt | 12 +++++++----- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/hal/CMakeLists.txt b/hal/CMakeLists.txt index 424a012d..7a97ae0f 100644 --- a/hal/CMakeLists.txt +++ b/hal/CMakeLists.txt @@ -9,6 +9,7 @@ option(FSFW_HAL_ADD_LINUX "Add the Linux HAL to the sources. Requires gpiod libr # 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_LINUX_ADD_LIBGPIOD "Target implements libgpiod" 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) diff --git a/hal/src/fsfw_hal/linux/CMakeLists.txt b/hal/src/fsfw_hal/linux/CMakeLists.txt index 9fcb3b24..56d4bf48 100644 --- a/hal/src/fsfw_hal/linux/CMakeLists.txt +++ b/hal/src/fsfw_hal/linux/CMakeLists.txt @@ -8,11 +8,13 @@ target_sources(${LIB_FSFW_NAME} PRIVATE utility.cpp ) -if(FSFW_HAL_ADD_LIBGPIOD) +if(FSFW_HAL_LINUX_ADD_PERIPHERAL_DRIVERS) +if(FSFW_HAL_LINUX_ADD_LIBGPIOD) add_subdirectory(gpio) endif() + add_subdirectory(spi) + add_subdirectory(i2c) + add_subdirectory(uart) +endif() -add_subdirectory(spi) -add_subdirectory(i2c) -add_subdirectory(uart) -add_subdirectory(uio) \ No newline at end of file +add_subdirectory(uio) From 018d814f29972929577a020a00de33c34fa2034b Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Sat, 5 Feb 2022 17:12:42 +0100 Subject: [PATCH 25/81] adapt to develop --- hal/src/fsfw_hal/linux/uart/UartComIF.cpp | 46 +++++++++++++++++++++-- 1 file changed, 43 insertions(+), 3 deletions(-) diff --git a/hal/src/fsfw_hal/linux/uart/UartComIF.cpp b/hal/src/fsfw_hal/linux/uart/UartComIF.cpp index 12ed969c..8a4ef0c3 100644 --- a/hal/src/fsfw_hal/linux/uart/UartComIF.cpp +++ b/hal/src/fsfw_hal/linux/uart/UartComIF.cpp @@ -7,8 +7,8 @@ #include -#include "OBSWConfig.h" -#include "fsfw/serviceinterface/ServiceInterface.h" +#include "fsfw/FSFW.h" +#include "fsfw/serviceinterface.h" #include "fsfw_hal/linux/utility.h" UartComIF::UartComIF(object_id_t objectId) : SystemObject(objectId) {} @@ -25,7 +25,9 @@ ReturnValue_t UartComIF::initializeInterface(CookieIF* cookie) { UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "UartComIF::initializeInterface: Invalid UART Cookie!" << std::endl; +#endif return NULLPOINTER; } @@ -41,13 +43,17 @@ ReturnValue_t UartComIF::initializeInterface(CookieIF* cookie) { UartElements uartElements = {fileDescriptor, std::vector(maxReplyLen), 0}; auto status = uartDeviceMap.emplace(deviceFile, uartElements); if (status.second == false) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::initializeInterface: Failed to insert device " << deviceFile << "to UART device map" << std::endl; +#endif return RETURN_FAILED; } } else { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::initializeInterface: UART device " << deviceFile << " already in use" << std::endl; +#endif return RETURN_FAILED; } @@ -67,15 +73,19 @@ int UartComIF::configureUartPort(UartCookie* uartCookie) { int fd = open(deviceFile.c_str(), flags); if (fd < 0) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::configureUartPort: Failed to open uart " << deviceFile << "with error code " << errno << strerror(errno) << std::endl; +#endif return fd; } /* Read in existing settings */ if (tcgetattr(fd, &options) != 0) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::configureUartPort: Error " << errno << "from tcgetattr: " << strerror(errno) << std::endl; +#endif return fd; } @@ -96,8 +106,10 @@ int UartComIF::configureUartPort(UartCookie* uartCookie) { /* Save option settings */ if (tcsetattr(fd, TCSANOW, &options) != 0) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::configureUartPort: Failed to set options with error " << errno << ": " << strerror(errno); +#endif return fd; } return fd; @@ -149,7 +161,9 @@ void UartComIF::setDatasizeOptions(struct termios* options, UartCookie* uartCook options->c_cflag |= CS8; break; default: +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::setDatasizeOptions: Invalid size specified" << std::endl; +#endif break; } } @@ -300,7 +314,9 @@ void UartComIF::configureBaudrate(struct termios* options, UartCookie* uartCooki cfsetospeed(options, B4000000); break; default: +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::configureBaudrate: Baudrate not supported" << std::endl; +#endif break; } } @@ -315,29 +331,37 @@ ReturnValue_t UartComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, } if (sendData == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::sendMessage: Send data is nullptr" << std::endl; +#endif return RETURN_FAILED; } UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::sendMessasge: Invalid UART Cookie!" << std::endl; +#endif return NULLPOINTER; } deviceFile = uartCookie->getDeviceFile(); uartDeviceMapIter = uartDeviceMap.find(deviceFile); if (uartDeviceMapIter == uartDeviceMap.end()) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::debug << "UartComIF::sendMessage: Device file " << deviceFile << "not in UART map" << std::endl; +#endif return RETURN_FAILED; } fd = uartDeviceMapIter->second.fileDescriptor; - if (write(fd, sendData, sendLen) != (int)sendLen) { + if (write(fd, sendData, sendLen) != static_cast(sendLen)) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "UartComIF::sendMessage: Failed to send data with error code " << errno << ": Error description: " << strerror(errno) << std::endl; +#endif return RETURN_FAILED; } @@ -352,7 +376,9 @@ ReturnValue_t UartComIF::requestReceiveMessage(CookieIF* cookie, size_t requestL UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::debug << "UartComIF::requestReceiveMessage: Invalid Uart Cookie!" << std::endl; +#endif return NULLPOINTER; } @@ -365,8 +391,10 @@ ReturnValue_t UartComIF::requestReceiveMessage(CookieIF* cookie, size_t requestL } if (uartDeviceMapIter == uartDeviceMap.end()) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::debug << "UartComIF::requestReceiveMessage: Device file " << deviceFile << " not in uart map" << std::endl; +#endif return RETURN_FAILED; } @@ -460,8 +488,10 @@ ReturnValue_t UartComIF::handleNoncanonicalRead(UartCookie& uartCookie, UartDevi return RETURN_FAILED; } else if (bytesRead != static_cast(requestLen)) { if (uartCookie.isReplySizeFixed()) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::requestReceiveMessage: Only read " << bytesRead << " of " << requestLen << " bytes" << std::endl; +#endif return RETURN_FAILED; } } @@ -475,15 +505,19 @@ ReturnValue_t UartComIF::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::debug << "UartComIF::readReceivedMessage: Invalid uart cookie!" << std::endl; +#endif return NULLPOINTER; } deviceFile = uartCookie->getDeviceFile(); uartDeviceMapIter = uartDeviceMap.find(deviceFile); if (uartDeviceMapIter == uartDeviceMap.end()) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::debug << "UartComIF::readReceivedMessage: Device file " << deviceFile << " not in uart map" << std::endl; +#endif return RETURN_FAILED; } @@ -501,7 +535,9 @@ ReturnValue_t UartComIF::flushUartRxBuffer(CookieIF* cookie) { UartDeviceMapIter uartDeviceMapIter; UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::flushUartRxBuffer: Invalid uart cookie!" << std::endl; +#endif return NULLPOINTER; } deviceFile = uartCookie->getDeviceFile(); @@ -519,7 +555,9 @@ ReturnValue_t UartComIF::flushUartTxBuffer(CookieIF* cookie) { UartDeviceMapIter uartDeviceMapIter; UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::flushUartTxBuffer: Invalid uart cookie!" << std::endl; +#endif return NULLPOINTER; } deviceFile = uartCookie->getDeviceFile(); @@ -537,7 +575,9 @@ ReturnValue_t UartComIF::flushUartTxAndRxBuf(CookieIF* cookie) { UartDeviceMapIter uartDeviceMapIter; UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::flushUartTxAndRxBuf: Invalid uart cookie!" << std::endl; +#endif return NULLPOINTER; } deviceFile = uartCookie->getDeviceFile(); From 3c06d2dbbb0d79608e003a8a3c89ae90cc71f409 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Sat, 5 Feb 2022 18:11:23 +0100 Subject: [PATCH 26/81] run clang format script --- src/fsfw/rmap/RMAPChannelIF.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fsfw/rmap/RMAPChannelIF.h b/src/fsfw/rmap/RMAPChannelIF.h index 0c937dc8..56ede1e2 100644 --- a/src/fsfw/rmap/RMAPChannelIF.h +++ b/src/fsfw/rmap/RMAPChannelIF.h @@ -77,9 +77,9 @@ class RMAPChannelIF { * 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_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... From 062e93fd8805b019e6e0c1d7f1323f214439158b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 10 Feb 2022 13:53:01 +0100 Subject: [PATCH 27/81] started DHB docs --- docs/devicehandlers.rst | 102 ++++++++++++++++++++++++++++++++++++++++ 1 file changed, 102 insertions(+) diff --git a/docs/devicehandlers.rst b/docs/devicehandlers.rst index 58c2df78..d1a5fb67 100644 --- a/docs/devicehandlers.rst +++ b/docs/devicehandlers.rst @@ -1,3 +1,105 @@ Device Handlers ================== +Device handler components rerpresent, control and monitor equipment, for example sensors or actuators of a spacecraft or the payload. + +Most device handlers have the same common functionality or +requirements, which are fulfilled by implementing an certain interface: + +- The handler/device needs to be commandable: :cpp:class:`HasActionsIF` +- The handler needs to communicate with the physical device via a dedicated + communication bus, for example SpaceWire, UART or SPI: :cpp:class:`DeviceCommunicationIF` +- The handler has housekeeping data which has to be exposed to the operator and/or other software + components: :cpp:class:`HasLocalDataPoolIF` +- The handler has configurable parameters +- The handler has health states, for example to indicate a broken device: + :cpp:class:`HasHealthIF` +- The handler has modes. For example there are the core modes `MODE_ON`, `MODE_OFF` + and `MODE_NORMAL` provided by the FSFW. `MODE_ON` means that a device is physically powered + but that it is not periodically polling data from the + physical device, `MODE_NORMAL` means that it is able to do that: :cpp:class`HasModesIF` + +The device handler base therefore provides abstractions for a lot of common +functionality, which can potentially avoid high amounts or logic and code duplication. + +Template Device Handler Base File +---------------------------------- + +This is an example template device handler header file with all necessary +functions implemented: + +.. code-block:: cpp + + #ifndef __TESTDEVICEHANDLER_H_ + #define __TESTDEVICEHANDLER_H_ + + #include + + class TestDeviceHandler: DeviceHandlerBase { + public: + TestDeviceHandler(object_id_t objectId, object_id_t comIF, CookieIF* cookie); + private: + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t* id) override; + ReturnValue_t buildTransitionDeviceCommand(DeviceCommandId_t* id) override; + void fillCommandAndReplyMap() override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, const uint8_t* commandData, + size_t commandDataLen) override; + ReturnValue_t scanForReply(const uint8_t* start, size_t remainingSize, DeviceCommandId_t* foundId, + size_t* foundLen) override; + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t* packet) override; + uint32_t getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool& localDataPoolMap, + LocalDataPoolManager& poolManager) override; + + }; + + #endif /* __TESTDEVICEHANDLER_H_ */ + +and the respective source file with sensible default return values: + +.. code-block:: cpp + #include "TestDeviceHandler.h" + + TestDeviceHandler::TestDeviceHandler(object_id_t objectId, object_id_t comIF, CookieIF* cookie) + : DeviceHandlerBase(objectId, comIF, cookie) {} + + void TestDeviceHandler::doStartUp() {} + + void TestDeviceHandler::doShutDown() {} + + ReturnValue_t TestDeviceHandler::buildNormalDeviceCommand(DeviceCommandId_t* id) { + return HasReturnvaluesIF::RETURN_OK; + } + + ReturnValue_t TestDeviceHandler::buildTransitionDeviceCommand(DeviceCommandId_t* id) { + return HasReturnvaluesIF::RETURN_OK; + } + + void TestDeviceHandler::fillCommandAndReplyMap() {} + + ReturnValue_t TestDeviceHandler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t* commandData, + size_t commandDataLen) { + return HasReturnvaluesIF::RETURN_OK; + } + + ReturnValue_t TestDeviceHandler::scanForReply(const uint8_t* start, size_t remainingSize, + DeviceCommandId_t* foundId, size_t* foundLen) { + return HasReturnvaluesIF::RETURN_OK; + } + + ReturnValue_t TestDeviceHandler::interpretDeviceReply(DeviceCommandId_t id, + const uint8_t* packet) { + return HasReturnvaluesIF::RETURN_OK; + } + + uint32_t TestDeviceHandler::getTransitionDelayMs(Mode_t modeFrom, Mode_t modeTo) { + return 10000; + } + + ReturnValue_t TestDeviceHandler::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, + LocalDataPoolManager& poolManager) { + return HasReturnvaluesIF::RETURN_OK; + } From 9e92afbf076b57df843a1ee526bd63c3303995a5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 16 Feb 2022 18:54:55 +0100 Subject: [PATCH 28/81] bugfix in test task --- tests/src/fsfw_tests/integration/task/TestTask.cpp | 1 - tests/src/fsfw_tests/integration/task/TestTask.h | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/tests/src/fsfw_tests/integration/task/TestTask.cpp b/tests/src/fsfw_tests/integration/task/TestTask.cpp index 65e444e3..765f780e 100644 --- a/tests/src/fsfw_tests/integration/task/TestTask.cpp +++ b/tests/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) { diff --git a/tests/src/fsfw_tests/integration/task/TestTask.h b/tests/src/fsfw_tests/integration/task/TestTask.h index 557b50b2..cd630ee3 100644 --- a/tests/src/fsfw_tests/integration/task/TestTask.h +++ b/tests/src/fsfw_tests/integration/task/TestTask.h @@ -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; }; From b3482eba245520ffc248c0b18a28b7bfe45d11b1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Feb 2022 20:41:47 +0100 Subject: [PATCH 29/81] error check in event manager --- src/fsfw/events/EventManager.cpp | 13 ++++++++++--- src/fsfw/globalfunctions/CRC.cpp | 2 +- src/fsfw/osal/linux/MessageQueue.cpp | 2 +- src/fsfw/osal/linux/PeriodicPosixTask.h | 7 ++++--- src/fsfw/osal/rtems/PeriodicTask.h | 4 ++-- src/fsfw/rmap/RMAP.h | 8 ++++---- src/fsfw/rmap/RMAPChannelIF.h | 12 ++++++------ 7 files changed, 28 insertions(+), 20 deletions(-) diff --git a/src/fsfw/events/EventManager.cpp b/src/fsfw/events/EventManager.cpp index aaa7d6c5..ca45ef04 100644 --- a/src/fsfw/events/EventManager.cpp +++ b/src/fsfw/events/EventManager.cpp @@ -46,9 +46,16 @@ ReturnValue_t EventManager::performOperation(uint8_t opCode) { void EventManager::notifyListeners(EventMessage* message) { lockMutex(); - for (auto iter = listenerList.begin(); iter != listenerList.end(); ++iter) { - if (iter->second.match(message)) { - MessageQueueSenderIF::sendMessage(iter->first, message, message->getSender()); + for (auto& listener : listenerList) { + if (listener.second.match(message)) { + ReturnValue_t result = + MessageQueueSenderIF::sendMessage(listener.first, message, message->getSender()); + if (result != HasReturnvaluesIF::RETURN_OK) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "Sending message to listener failed with result " << std::hex << std::setw(4) + << result << std::endl; +#endif + } } } unlockMutex(); diff --git a/src/fsfw/globalfunctions/CRC.cpp b/src/fsfw/globalfunctions/CRC.cpp index 6b8140c5..033920d0 100644 --- a/src/fsfw/globalfunctions/CRC.cpp +++ b/src/fsfw/globalfunctions/CRC.cpp @@ -117,7 +117,7 @@ uint16_t CRC::crc16ccitt(uint8_t const input[], uint32_t length, uint16_t starti // { // if (xor_out[i] == true) // crc_value = crc_value + pow(2,(15 -i)); // reverse CrC result before - //Final XOR + // Final XOR // } // // crc_value = 0;// for debug mode diff --git a/src/fsfw/osal/linux/MessageQueue.cpp b/src/fsfw/osal/linux/MessageQueue.cpp index f876ec6e..c806783b 100644 --- a/src/fsfw/osal/linux/MessageQueue.cpp +++ b/src/fsfw/osal/linux/MessageQueue.cpp @@ -256,7 +256,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(); } } diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.h b/src/fsfw/osal/linux/PeriodicPosixTask.h index 3c9a3a0d..1c3a52c7 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.h +++ b/src/fsfw/osal/linux/PeriodicPosixTask.h @@ -65,9 +65,10 @@ class PeriodicPosixTask : public PosixThread, public PeriodicTaskIF { /** * @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); /** diff --git a/src/fsfw/osal/rtems/PeriodicTask.h b/src/fsfw/osal/rtems/PeriodicTask.h index ff8617fc..119329f2 100644 --- a/src/fsfw/osal/rtems/PeriodicTask.h +++ b/src/fsfw/osal/rtems/PeriodicTask.h @@ -13,8 +13,8 @@ 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 */ diff --git a/src/fsfw/rmap/RMAP.h b/src/fsfw/rmap/RMAP.h index 42ee1ac5..7c654262 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 20dfd5f8..7dab07c1 100644 --- a/src/fsfw/rmap/RMAPChannelIF.h +++ b/src/fsfw/rmap/RMAPChannelIF.h @@ -75,10 +75,10 @@ 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 + * - @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 @@ -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) From bd05afbddd7e2da43b19b8ceafb6272e73d5464d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 18 Feb 2022 13:09:18 +0100 Subject: [PATCH 30/81] printout improvements --- src/fsfw/events/EventManager.cpp | 2 ++ src/fsfw/internalerror/InternalErrorReporter.cpp | 15 +++++++-------- src/fsfw/rmap/RMAP.h | 8 ++++---- src/fsfw/rmap/RMAPChannelIF.h | 12 ++++++------ 4 files changed, 19 insertions(+), 18 deletions(-) diff --git a/src/fsfw/events/EventManager.cpp b/src/fsfw/events/EventManager.cpp index ca45ef04..35dc9d22 100644 --- a/src/fsfw/events/EventManager.cpp +++ b/src/fsfw/events/EventManager.cpp @@ -54,6 +54,8 @@ void EventManager::notifyListeners(EventMessage* message) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "Sending message to listener failed with result " << std::hex << std::setw(4) << result << std::endl; +#else + sif::printError("Sending message to listener failed with result %04x\n", result); #endif } } diff --git a/src/fsfw/internalerror/InternalErrorReporter.cpp b/src/fsfw/internalerror/InternalErrorReporter.cpp index e7088e2c..a085cc5b 100644 --- a/src/fsfw/internalerror/InternalErrorReporter.cpp +++ b/src/fsfw/internalerror/InternalErrorReporter.cpp @@ -36,15 +36,14 @@ ReturnValue_t InternalErrorReporter::performOperation(uint8_t opCode) { if ((newQueueHits > 0) or (newTmHits > 0) or (newStoreHits > 0)) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::debug << "InternalErrorReporter::performOperation: Errors " - << "occured!" << std::endl; - sif::debug << "Queue errors: " << newQueueHits << std::endl; - sif::debug << "TM errors: " << newTmHits << std::endl; - sif::debug << "Store errors: " << newStoreHits << std::endl; + << "occured: Queue | TM | Store : " << newQueueHits << " | " << newTmHits << " | " + << newStoreHits << std::endl; #else - sif::printDebug("InternalErrorReporter::performOperation: Errors occured!\n"); - sif::printDebug("Queue errors: %lu\n", static_cast(newQueueHits)); - sif::printDebug("TM errors: %lu\n", static_cast(newTmHits)); - sif::printDebug("Store errors: %lu\n", static_cast(newStoreHits)); + sif::printDebug( + "InternalErrorReporter::performOperation: Errors occured: Queue | TM | Store: %lu | %lu " + "| %lu\n", + static_cast(newQueueHits), static_cast(newTmHits), + static_cast(newStoreHits)); #endif } } diff --git a/src/fsfw/rmap/RMAP.h b/src/fsfw/rmap/RMAP.h index 7c654262..d274fb15 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 7dab07c1..0c937dc8 100644 --- a/src/fsfw/rmap/RMAPChannelIF.h +++ b/src/fsfw/rmap/RMAPChannelIF.h @@ -75,10 +75,10 @@ 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 + * - @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 @@ -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) From a12e98d9481e224a30a1150e9dfd0a48501f410f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 18 Feb 2022 13:39:42 +0100 Subject: [PATCH 31/81] update event mngr printout --- src/fsfw/events/EventManager.cpp | 23 ++++++++++++++++++++--- src/fsfw/events/EventManager.h | 1 + src/fsfw/osal/linux/MessageQueue.cpp | 4 ++-- src/fsfw/pus/Service5EventReporting.cpp | 1 + src/fsfw/rmap/RMAPChannelIF.h | 6 +++--- 5 files changed, 27 insertions(+), 8 deletions(-) diff --git a/src/fsfw/events/EventManager.cpp b/src/fsfw/events/EventManager.cpp index 35dc9d22..c027fce5 100644 --- a/src/fsfw/events/EventManager.cpp +++ b/src/fsfw/events/EventManager.cpp @@ -52,10 +52,12 @@ void EventManager::notifyListeners(EventMessage* message) { MessageQueueSenderIF::sendMessage(listener.first, message, message->getSender()); if (result != HasReturnvaluesIF::RETURN_OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "Sending message to listener failed with result " << std::hex << std::setw(4) - << result << std::endl; + sif::error << std::hex << "EventManager::notifyListeners: MSG to " << std::setw(8) + << std::setfill('0') << "0x" << listener.first << " failed with result " + << std::setw(4) << "0x" << result << std::setfill(' ') << std::endl; #else - sif::printError("Sending message to listener failed with result %04x\n", result); + sif::printError("Sending message to listener 0x%08x failed with result %04x\n", + listener.first, result); #endif } } @@ -198,4 +200,19 @@ void EventManager::printUtility(sif::OutputTypes printType, EventMessage* messag } } +void EventManager::printListeners() { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "Event manager listener MQ IDs:" << std::endl; + for (auto& listener : listenerList) { + sif::info << std::hex << std::setw(8) << std::setfill('0') << "0x" << listener.first << std::dec + << std::setfill(' ') << std::endl; + } +#else + sif::printInfo("Event manager listener MQ IDs:\n"); + for (auto& listener : listenerList) { + sif::printInfo("0x%08x\n", listener.first); + } +#endif +} + #endif /* FSFW_OBJ_EVENT_TRANSLATION == 1 */ diff --git a/src/fsfw/events/EventManager.h b/src/fsfw/events/EventManager.h index f2d642ff..e2a8dd95 100644 --- a/src/fsfw/events/EventManager.h +++ b/src/fsfw/events/EventManager.h @@ -42,6 +42,7 @@ class EventManager : public EventManagerIF, public ExecutableObjectIF, public Sy object_id_t reporterFrom = 0, object_id_t reporterTo = 0, bool reporterInverted = false); ReturnValue_t performOperation(uint8_t opCode); + void printListeners(); protected: MessageQueueIF* eventReportQueue = nullptr; diff --git a/src/fsfw/osal/linux/MessageQueue.cpp b/src/fsfw/osal/linux/MessageQueue.cpp index c806783b..bdba912e 100644 --- a/src/fsfw/osal/linux/MessageQueue.cpp +++ b/src/fsfw/osal/linux/MessageQueue.cpp @@ -240,9 +240,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; } diff --git a/src/fsfw/pus/Service5EventReporting.cpp b/src/fsfw/pus/Service5EventReporting.cpp index 4517bc26..dfce9ba2 100644 --- a/src/fsfw/pus/Service5EventReporting.cpp +++ b/src/fsfw/pus/Service5EventReporting.cpp @@ -13,6 +13,7 @@ Service5EventReporting::Service5EventReporting(object_id_t objectId, uint16_t ap : PusServiceBase(objectId, apid, serviceId), maxNumberReportsPerCycle(maxNumberReportsPerCycle) { eventQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth); + sif::info << eventQueue->getId() << std::endl; } Service5EventReporting::~Service5EventReporting() { diff --git a/src/fsfw/rmap/RMAPChannelIF.h b/src/fsfw/rmap/RMAPChannelIF.h index 0c937dc8..9e666dfb 100644 --- a/src/fsfw/rmap/RMAPChannelIF.h +++ b/src/fsfw/rmap/RMAPChannelIF.h @@ -77,9 +77,9 @@ class RMAPChannelIF { * 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_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... From a5871ed0b123f809145393f868b95e8d31cb60fa Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 18 Feb 2022 14:00:06 +0100 Subject: [PATCH 32/81] added void* args to queue factory and mq ctor --- src/fsfw/events/EventManager.cpp | 12 ++++++------ src/fsfw/ipc/QueueFactory.h | 3 ++- src/fsfw/osal/freertos/MessageQueue.cpp | 2 +- src/fsfw/osal/freertos/MessageQueue.h | 3 ++- src/fsfw/osal/freertos/QueueFactory.cpp | 4 ++-- src/fsfw/osal/host/MessageQueue.cpp | 2 +- src/fsfw/osal/host/MessageQueue.h | 3 ++- src/fsfw/osal/host/QueueFactory.cpp | 4 ++-- src/fsfw/osal/linux/MessageQueue.cpp | 2 +- src/fsfw/osal/linux/MessageQueue.h | 3 ++- src/fsfw/osal/linux/QueueFactory.cpp | 4 ++-- src/fsfw/osal/rtems/MessageQueue.cpp | 2 +- src/fsfw/osal/rtems/MessageQueue.h | 3 ++- src/fsfw/osal/rtems/QueueFactory.cpp | 4 ++-- 14 files changed, 28 insertions(+), 23 deletions(-) diff --git a/src/fsfw/events/EventManager.cpp b/src/fsfw/events/EventManager.cpp index c027fce5..d24e893a 100644 --- a/src/fsfw/events/EventManager.cpp +++ b/src/fsfw/events/EventManager.cpp @@ -52,9 +52,9 @@ void EventManager::notifyListeners(EventMessage* message) { MessageQueueSenderIF::sendMessage(listener.first, message, message->getSender()); if (result != HasReturnvaluesIF::RETURN_OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << std::hex << "EventManager::notifyListeners: MSG to " << std::setw(8) - << std::setfill('0') << "0x" << listener.first << " failed with result " - << std::setw(4) << "0x" << result << std::setfill(' ') << std::endl; + sif::error << std::hex << "EventManager::notifyListeners: MSG to 0x" << std::setfill('0') + << std::setw(8) << listener.first << " failed with result 0x" + << std::setw(4) << result << std::setfill(' ') << std::endl; #else sif::printError("Sending message to listener 0x%08x failed with result %04x\n", listener.first, result); @@ -202,11 +202,11 @@ void EventManager::printUtility(sif::OutputTypes printType, EventMessage* messag void EventManager::printListeners() { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::info << "Event manager listener MQ IDs:" << std::endl; + sif::info << "Event manager listener MQ IDs:" << std::setfill('0') << std::hex << std::endl; for (auto& listener : listenerList) { - sif::info << std::hex << std::setw(8) << std::setfill('0') << "0x" << listener.first << std::dec - << std::setfill(' ') << std::endl; + sif::info << "0x" << std::setw(8) << listener.first << std::endl; } + sif::info << std::dec << std::setfill(' '); #else sif::printInfo("Event manager listener MQ IDs:\n"); for (auto& listener : listenerList) { diff --git a/src/fsfw/ipc/QueueFactory.h b/src/fsfw/ipc/QueueFactory.h index 864c456d..07107bec 100644 --- a/src/fsfw/ipc/QueueFactory.h +++ b/src/fsfw/ipc/QueueFactory.h @@ -22,7 +22,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, + void* args = nullptr); void deleteMessageQueue(MessageQueueIF* queue); diff --git a/src/fsfw/osal/freertos/MessageQueue.cpp b/src/fsfw/osal/freertos/MessageQueue.cpp index a8333fe5..6e9bdd44 100644 --- a/src/fsfw/osal/freertos/MessageQueue.cpp +++ b/src/fsfw/osal/freertos/MessageQueue.cpp @@ -4,7 +4,7 @@ #include "fsfw/osal/freertos/QueueMapManager.h" #include "fsfw/serviceinterface/ServiceInterface.h" -MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize) +MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize, void* args) : maxMessageSize(maxMessageSize) { handle = xQueueCreate(messageDepth, maxMessageSize); if (handle == nullptr) { diff --git a/src/fsfw/osal/freertos/MessageQueue.h b/src/fsfw/osal/freertos/MessageQueue.h index 1cb343d1..24054bda 100644 --- a/src/fsfw/osal/freertos/MessageQueue.h +++ b/src/fsfw/osal/freertos/MessageQueue.h @@ -53,7 +53,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, + void* args = nullptr); /** Copying message queues forbidden */ MessageQueue(const MessageQueue&) = delete; diff --git a/src/fsfw/osal/freertos/QueueFactory.cpp b/src/fsfw/osal/freertos/QueueFactory.cpp index f4941481..d2914929 100644 --- a/src/fsfw/osal/freertos/QueueFactory.cpp +++ b/src/fsfw/osal/freertos/QueueFactory.cpp @@ -22,8 +22,8 @@ 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, void* args) { + return new MessageQueue(messageDepth, maxMessageSize, args); } void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) { delete queue; } diff --git a/src/fsfw/osal/host/MessageQueue.cpp b/src/fsfw/osal/host/MessageQueue.cpp index db66b671..b822578d 100644 --- a/src/fsfw/osal/host/MessageQueue.cpp +++ b/src/fsfw/osal/host/MessageQueue.cpp @@ -8,7 +8,7 @@ #include "fsfw/osal/host/QueueMapManager.h" #include "fsfw/serviceinterface/ServiceInterface.h" -MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize) +MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize, void* args) : messageSize(maxMessageSize), messageDepth(messageDepth) { queueLock = MutexFactory::instance()->createMutex(); auto result = QueueMapManager::instance()->addMessageQueue(this, &mqId); diff --git a/src/fsfw/osal/host/MessageQueue.h b/src/fsfw/osal/host/MessageQueue.h index 49375bb5..20a2a7e1 100644 --- a/src/fsfw/osal/host/MessageQueue.h +++ b/src/fsfw/osal/host/MessageQueue.h @@ -54,7 +54,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, + void* args); /** Copying message queues forbidden */ MessageQueue(const MessageQueue&) = delete; diff --git a/src/fsfw/osal/host/QueueFactory.cpp b/src/fsfw/osal/host/QueueFactory.cpp index 3c63e6c9..0ecc5207 100644 --- a/src/fsfw/osal/host/QueueFactory.cpp +++ b/src/fsfw/osal/host/QueueFactory.cpp @@ -27,12 +27,12 @@ QueueFactory::QueueFactory() {} QueueFactory::~QueueFactory() {} -MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize) { +MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize, void* 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/linux/MessageQueue.cpp b/src/fsfw/osal/linux/MessageQueue.cpp index bdba912e..28e84ece 100644 --- a/src/fsfw/osal/linux/MessageQueue.cpp +++ b/src/fsfw/osal/linux/MessageQueue.cpp @@ -11,7 +11,7 @@ #include "fsfw/osal/linux/unixUtility.h" #include "fsfw/serviceinterface/ServiceInterface.h" -MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize) +MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize, void* args) : id(MessageQueueIF::NO_QUEUE), lastPartner(MessageQueueIF::NO_QUEUE), defaultDestination(MessageQueueIF::NO_QUEUE), diff --git a/src/fsfw/osal/linux/MessageQueue.h b/src/fsfw/osal/linux/MessageQueue.h index dbf6555e..8fd6f11c 100644 --- a/src/fsfw/osal/linux/MessageQueue.h +++ b/src/fsfw/osal/linux/MessageQueue.h @@ -42,7 +42,8 @@ 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, + void* args = nullptr); /** * @brief The destructor deletes the formerly created message queue. * @details This is accomplished by using the delete call provided by the operating system. diff --git a/src/fsfw/osal/linux/QueueFactory.cpp b/src/fsfw/osal/linux/QueueFactory.cpp index d1b1cfdb..7ef4bc0f 100644 --- a/src/fsfw/osal/linux/QueueFactory.cpp +++ b/src/fsfw/osal/linux/QueueFactory.cpp @@ -28,8 +28,8 @@ 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, void* args) { + return new MessageQueue(messageDepth, maxMessageSize, args); } void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) { delete queue; } diff --git a/src/fsfw/osal/rtems/MessageQueue.cpp b/src/fsfw/osal/rtems/MessageQueue.cpp index e45679d5..d47b4df2 100644 --- a/src/fsfw/osal/rtems/MessageQueue.cpp +++ b/src/fsfw/osal/rtems/MessageQueue.cpp @@ -6,7 +6,7 @@ #include "fsfw/osal/rtems/RtemsBasic.h" #include "fsfw/serviceinterface/ServiceInterface.h" -MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size) +MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size, void* args) : id(0), lastPartner(0), defaultDestination(NO_QUEUE), internalErrorReporter(nullptr) { rtems_name name = ('Q' << 24) + (queueCounter++ << 8); rtems_status_code status = diff --git a/src/fsfw/osal/rtems/MessageQueue.h b/src/fsfw/osal/rtems/MessageQueue.h index 89aae2ad..f0efa9f5 100644 --- a/src/fsfw/osal/rtems/MessageQueue.h +++ b/src/fsfw/osal/rtems/MessageQueue.h @@ -34,7 +34,8 @@ class MessageQueue : public MessageQueueIF { * This should be left default. */ MessageQueue(size_t message_depth = 3, - size_t max_message_size = MessageQueueMessage::MAX_MESSAGE_SIZE); + size_t max_message_size = MessageQueueMessage::MAX_MESSAGE_SIZE, + void* args); /** * @brief The destructor deletes the formerly created message queue. * @details This is accomplished by using the delete call provided by the operating system. diff --git a/src/fsfw/osal/rtems/QueueFactory.cpp b/src/fsfw/osal/rtems/QueueFactory.cpp index 3d517f60..63d0de8a 100644 --- a/src/fsfw/osal/rtems/QueueFactory.cpp +++ b/src/fsfw/osal/rtems/QueueFactory.cpp @@ -49,8 +49,8 @@ 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, void* args) { + return new MessageQueue(messageDepth, maxMessageSize, args); } void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) { delete queue; } From 0d66569687eac9fb5a1ae7eaad8a4c9fbe187b14 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 18 Feb 2022 14:07:41 +0100 Subject: [PATCH 33/81] this is a bit cleaner --- src/fsfw/ipc/QueueFactory.h | 3 ++- src/fsfw/ipc/definitions.h | 10 ++++++++++ src/fsfw/osal/freertos/MessageQueue.cpp | 2 +- src/fsfw/osal/freertos/MessageQueue.h | 3 ++- src/fsfw/osal/freertos/QueueFactory.cpp | 2 +- src/fsfw/osal/host/MessageQueue.cpp | 2 +- src/fsfw/osal/host/MessageQueue.h | 3 ++- src/fsfw/osal/host/QueueFactory.cpp | 2 +- src/fsfw/osal/linux/MessageQueue.cpp | 2 +- src/fsfw/osal/linux/MessageQueue.h | 3 ++- src/fsfw/osal/linux/QueueFactory.cpp | 2 +- src/fsfw/osal/rtems/MessageQueue.cpp | 2 +- src/fsfw/osal/rtems/MessageQueue.h | 3 ++- src/fsfw/osal/rtems/QueueFactory.cpp | 2 +- 14 files changed, 28 insertions(+), 13 deletions(-) create mode 100644 src/fsfw/ipc/definitions.h diff --git a/src/fsfw/ipc/QueueFactory.h b/src/fsfw/ipc/QueueFactory.h index 07107bec..e9e5adf1 100644 --- a/src/fsfw/ipc/QueueFactory.h +++ b/src/fsfw/ipc/QueueFactory.h @@ -4,6 +4,7 @@ #include #include "MessageQueueIF.h" +#include "definitions.h" #include "MessageQueueMessage.h" /** @@ -23,7 +24,7 @@ class QueueFactory { MessageQueueIF* createMessageQueue(uint32_t messageDepth = 3, size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE, - void* args = nullptr); + 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 00000000..5273f5ca --- /dev/null +++ b/src/fsfw/ipc/definitions.h @@ -0,0 +1,10 @@ +#ifndef FSFW_SRC_FSFW_IPC_DEFINITIONS_H_ +#define FSFW_SRC_FSFW_IPC_DEFINITIONS_H_ +#include + +struct MqArgs { + object_id_t objectId = 0; + void* args = nullptr; +}; + +#endif /* FSFW_SRC_FSFW_IPC_DEFINITIONS_H_ */ diff --git a/src/fsfw/osal/freertos/MessageQueue.cpp b/src/fsfw/osal/freertos/MessageQueue.cpp index 6e9bdd44..927d7820 100644 --- a/src/fsfw/osal/freertos/MessageQueue.cpp +++ b/src/fsfw/osal/freertos/MessageQueue.cpp @@ -4,7 +4,7 @@ #include "fsfw/osal/freertos/QueueMapManager.h" #include "fsfw/serviceinterface/ServiceInterface.h" -MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize, void* args) +MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize, MqArgs* args) : maxMessageSize(maxMessageSize) { handle = xQueueCreate(messageDepth, maxMessageSize); if (handle == nullptr) { diff --git a/src/fsfw/osal/freertos/MessageQueue.h b/src/fsfw/osal/freertos/MessageQueue.h index 24054bda..52708990 100644 --- a/src/fsfw/osal/freertos/MessageQueue.h +++ b/src/fsfw/osal/freertos/MessageQueue.h @@ -7,6 +7,7 @@ #include "fsfw/ipc/MessageQueueIF.h" #include "fsfw/ipc/MessageQueueMessage.h" #include "fsfw/ipc/MessageQueueMessageIF.h" +#include "fsfw/ipc/definitions.h" #include "queue.h" /** @@ -54,7 +55,7 @@ class MessageQueue : public MessageQueueIF { */ MessageQueue(size_t messageDepth = 3, size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE, - void* args = nullptr); + MqArgs* args = nullptr); /** Copying message queues forbidden */ MessageQueue(const MessageQueue&) = delete; diff --git a/src/fsfw/osal/freertos/QueueFactory.cpp b/src/fsfw/osal/freertos/QueueFactory.cpp index d2914929..588653cf 100644 --- a/src/fsfw/osal/freertos/QueueFactory.cpp +++ b/src/fsfw/osal/freertos/QueueFactory.cpp @@ -22,7 +22,7 @@ QueueFactory::QueueFactory() {} QueueFactory::~QueueFactory() {} -MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize, void* args) { +MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize, MqArgs* args) { return new MessageQueue(messageDepth, maxMessageSize, args); } diff --git a/src/fsfw/osal/host/MessageQueue.cpp b/src/fsfw/osal/host/MessageQueue.cpp index b822578d..eed29555 100644 --- a/src/fsfw/osal/host/MessageQueue.cpp +++ b/src/fsfw/osal/host/MessageQueue.cpp @@ -8,7 +8,7 @@ #include "fsfw/osal/host/QueueMapManager.h" #include "fsfw/serviceinterface/ServiceInterface.h" -MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize, void* args) +MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize, MqArgs* args) : messageSize(maxMessageSize), messageDepth(messageDepth) { queueLock = MutexFactory::instance()->createMutex(); auto result = QueueMapManager::instance()->addMessageQueue(this, &mqId); diff --git a/src/fsfw/osal/host/MessageQueue.h b/src/fsfw/osal/host/MessageQueue.h index 20a2a7e1..aa662dae 100644 --- a/src/fsfw/osal/host/MessageQueue.h +++ b/src/fsfw/osal/host/MessageQueue.h @@ -7,6 +7,7 @@ #include "fsfw/internalerror/InternalErrorReporterIF.h" #include "fsfw/ipc/MessageQueueIF.h" #include "fsfw/ipc/MessageQueueMessage.h" +#include "fsfw/ipc/definitions.h" #include "fsfw/ipc/MutexIF.h" #include "fsfw/timemanager/Clock.h" @@ -55,7 +56,7 @@ class MessageQueue : public MessageQueueIF { */ MessageQueue(size_t messageDepth = 3, size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE, - void* args); + MqArgs* args = nullptr); /** Copying message queues forbidden */ MessageQueue(const MessageQueue&) = delete; diff --git a/src/fsfw/osal/host/QueueFactory.cpp b/src/fsfw/osal/host/QueueFactory.cpp index 0ecc5207..caea8af7 100644 --- a/src/fsfw/osal/host/QueueFactory.cpp +++ b/src/fsfw/osal/host/QueueFactory.cpp @@ -27,7 +27,7 @@ QueueFactory::QueueFactory() {} QueueFactory::~QueueFactory() {} -MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize, void* args) { +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 diff --git a/src/fsfw/osal/linux/MessageQueue.cpp b/src/fsfw/osal/linux/MessageQueue.cpp index 28e84ece..c1de8f13 100644 --- a/src/fsfw/osal/linux/MessageQueue.cpp +++ b/src/fsfw/osal/linux/MessageQueue.cpp @@ -11,7 +11,7 @@ #include "fsfw/osal/linux/unixUtility.h" #include "fsfw/serviceinterface/ServiceInterface.h" -MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize, void* args) +MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize, MqArgs* args) : id(MessageQueueIF::NO_QUEUE), lastPartner(MessageQueueIF::NO_QUEUE), defaultDestination(MessageQueueIF::NO_QUEUE), diff --git a/src/fsfw/osal/linux/MessageQueue.h b/src/fsfw/osal/linux/MessageQueue.h index 8fd6f11c..84052914 100644 --- a/src/fsfw/osal/linux/MessageQueue.h +++ b/src/fsfw/osal/linux/MessageQueue.h @@ -5,6 +5,7 @@ #include "fsfw/internalerror/InternalErrorReporterIF.h" #include "fsfw/ipc/MessageQueueIF.h" +#include "fsfw/ipc/definitions.h" #include "fsfw/ipc/MessageQueueMessage.h" /** * @brief This class manages sending and receiving of message queue messages. @@ -43,7 +44,7 @@ class MessageQueue : public MessageQueueIF { */ MessageQueue(uint32_t messageDepth = 3, size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE, - void* args = nullptr); + MqArgs* args = nullptr); /** * @brief The destructor deletes the formerly created message queue. * @details This is accomplished by using the delete call provided by the operating system. diff --git a/src/fsfw/osal/linux/QueueFactory.cpp b/src/fsfw/osal/linux/QueueFactory.cpp index 7ef4bc0f..d74eb8c5 100644 --- a/src/fsfw/osal/linux/QueueFactory.cpp +++ b/src/fsfw/osal/linux/QueueFactory.cpp @@ -28,7 +28,7 @@ QueueFactory::QueueFactory() {} QueueFactory::~QueueFactory() {} -MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize, void* args) { +MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize, MqArgs* args) { return new MessageQueue(messageDepth, maxMessageSize, args); } diff --git a/src/fsfw/osal/rtems/MessageQueue.cpp b/src/fsfw/osal/rtems/MessageQueue.cpp index d47b4df2..87073067 100644 --- a/src/fsfw/osal/rtems/MessageQueue.cpp +++ b/src/fsfw/osal/rtems/MessageQueue.cpp @@ -6,7 +6,7 @@ #include "fsfw/osal/rtems/RtemsBasic.h" #include "fsfw/serviceinterface/ServiceInterface.h" -MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size, void* args) +MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size, MqArgs* args) : id(0), lastPartner(0), defaultDestination(NO_QUEUE), internalErrorReporter(nullptr) { rtems_name name = ('Q' << 24) + (queueCounter++ << 8); rtems_status_code status = diff --git a/src/fsfw/osal/rtems/MessageQueue.h b/src/fsfw/osal/rtems/MessageQueue.h index f0efa9f5..b18b2eaf 100644 --- a/src/fsfw/osal/rtems/MessageQueue.h +++ b/src/fsfw/osal/rtems/MessageQueue.h @@ -4,6 +4,7 @@ #include "RtemsBasic.h" #include "fsfw/internalerror/InternalErrorReporterIF.h" #include "fsfw/ipc/MessageQueueIF.h" +#include "fsfw/ipc/definitions.h" #include "fsfw/ipc/MessageQueueMessage.h" /** @@ -35,7 +36,7 @@ class MessageQueue : public MessageQueueIF { */ MessageQueue(size_t message_depth = 3, size_t max_message_size = MessageQueueMessage::MAX_MESSAGE_SIZE, - void* args); + MqArgs* args = nullptr); /** * @brief The destructor deletes the formerly created message queue. * @details This is accomplished by using the delete call provided by the operating system. diff --git a/src/fsfw/osal/rtems/QueueFactory.cpp b/src/fsfw/osal/rtems/QueueFactory.cpp index 63d0de8a..8b625da8 100644 --- a/src/fsfw/osal/rtems/QueueFactory.cpp +++ b/src/fsfw/osal/rtems/QueueFactory.cpp @@ -49,7 +49,7 @@ QueueFactory::QueueFactory() {} QueueFactory::~QueueFactory() {} -MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize, void* args) { +MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize, MqArgs* args) { return new MessageQueue(messageDepth, maxMessageSize, args); } From 508979d32d4e5910259bfb52b8fe16d1bb4f1cdb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 18 Feb 2022 14:52:25 +0100 Subject: [PATCH 34/81] cache mq args --- src/fsfw/events/EventManager.cpp | 8 ++++---- src/fsfw/ipc/QueueFactory.h | 4 ++-- src/fsfw/ipc/definitions.h | 6 ++++-- src/fsfw/osal/freertos/MessageQueue.h | 2 +- src/fsfw/osal/freertos/QueueFactory.cpp | 3 ++- src/fsfw/osal/host/MessageQueue.h | 4 ++-- src/fsfw/osal/host/QueueFactory.cpp | 3 ++- src/fsfw/osal/linux/MessageQueue.cpp | 3 +++ src/fsfw/osal/linux/MessageQueue.h | 6 ++++-- src/fsfw/osal/linux/QueueFactory.cpp | 3 ++- src/fsfw/osal/rtems/MessageQueue.h | 4 ++-- src/fsfw/osal/rtems/QueueFactory.cpp | 3 ++- 12 files changed, 30 insertions(+), 19 deletions(-) diff --git a/src/fsfw/events/EventManager.cpp b/src/fsfw/events/EventManager.cpp index d24e893a..0fd893b2 100644 --- a/src/fsfw/events/EventManager.cpp +++ b/src/fsfw/events/EventManager.cpp @@ -53,8 +53,8 @@ void EventManager::notifyListeners(EventMessage* message) { if (result != HasReturnvaluesIF::RETURN_OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << std::hex << "EventManager::notifyListeners: MSG to 0x" << std::setfill('0') - << std::setw(8) << listener.first << " failed with result 0x" - << std::setw(4) << result << std::setfill(' ') << std::endl; + << std::setw(8) << listener.first << " failed with result 0x" << std::setw(4) + << result << std::setfill(' ') << std::endl; #else sif::printError("Sending message to listener 0x%08x failed with result %04x\n", listener.first, result); @@ -204,9 +204,9 @@ void EventManager::printListeners() { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::info << "Event manager listener MQ IDs:" << std::setfill('0') << std::hex << std::endl; for (auto& listener : listenerList) { - sif::info << "0x" << std::setw(8) << listener.first << std::endl; + sif::info << "0x" << std::setw(8) << listener.first << std::endl; } - sif::info << std::dec << std::setfill(' '); + sif::info << std::dec << std::setfill(' '); #else sif::printInfo("Event manager listener MQ IDs:\n"); for (auto& listener : listenerList) { diff --git a/src/fsfw/ipc/QueueFactory.h b/src/fsfw/ipc/QueueFactory.h index e9e5adf1..8069d836 100644 --- a/src/fsfw/ipc/QueueFactory.h +++ b/src/fsfw/ipc/QueueFactory.h @@ -4,8 +4,8 @@ #include #include "MessageQueueIF.h" -#include "definitions.h" #include "MessageQueueMessage.h" +#include "definitions.h" /** * Creates message queues. @@ -24,7 +24,7 @@ class QueueFactory { MessageQueueIF* createMessageQueue(uint32_t messageDepth = 3, size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE, - MqArgs* args = nullptr); + MqArgs* args = nullptr); void deleteMessageQueue(MessageQueueIF* queue); diff --git a/src/fsfw/ipc/definitions.h b/src/fsfw/ipc/definitions.h index 5273f5ca..1cc7a3c4 100644 --- a/src/fsfw/ipc/definitions.h +++ b/src/fsfw/ipc/definitions.h @@ -3,8 +3,10 @@ #include struct MqArgs { - object_id_t objectId = 0; - void* args = nullptr; + MqArgs(){}; + MqArgs(object_id_t objectId, void* args = nullptr) : objectId(objectId), args(args) {} + object_id_t objectId = 0; + void* args = nullptr; }; #endif /* FSFW_SRC_FSFW_IPC_DEFINITIONS_H_ */ diff --git a/src/fsfw/osal/freertos/MessageQueue.h b/src/fsfw/osal/freertos/MessageQueue.h index 52708990..fc1d78e5 100644 --- a/src/fsfw/osal/freertos/MessageQueue.h +++ b/src/fsfw/osal/freertos/MessageQueue.h @@ -55,7 +55,7 @@ class MessageQueue : public MessageQueueIF { */ MessageQueue(size_t messageDepth = 3, size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE, - MqArgs* args = nullptr); + MqArgs* args = nullptr); /** Copying message queues forbidden */ MessageQueue(const MessageQueue&) = delete; diff --git a/src/fsfw/osal/freertos/QueueFactory.cpp b/src/fsfw/osal/freertos/QueueFactory.cpp index 588653cf..8424123c 100644 --- a/src/fsfw/osal/freertos/QueueFactory.cpp +++ b/src/fsfw/osal/freertos/QueueFactory.cpp @@ -22,7 +22,8 @@ QueueFactory::QueueFactory() {} QueueFactory::~QueueFactory() {} -MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize, MqArgs* args) { +MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize, + MqArgs* args) { return new MessageQueue(messageDepth, maxMessageSize, args); } diff --git a/src/fsfw/osal/host/MessageQueue.h b/src/fsfw/osal/host/MessageQueue.h index aa662dae..d090d269 100644 --- a/src/fsfw/osal/host/MessageQueue.h +++ b/src/fsfw/osal/host/MessageQueue.h @@ -7,8 +7,8 @@ #include "fsfw/internalerror/InternalErrorReporterIF.h" #include "fsfw/ipc/MessageQueueIF.h" #include "fsfw/ipc/MessageQueueMessage.h" -#include "fsfw/ipc/definitions.h" #include "fsfw/ipc/MutexIF.h" +#include "fsfw/ipc/definitions.h" #include "fsfw/timemanager/Clock.h" /** @@ -56,7 +56,7 @@ class MessageQueue : public MessageQueueIF { */ MessageQueue(size_t messageDepth = 3, size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE, - MqArgs* args = nullptr); + MqArgs* args = nullptr); /** Copying message queues forbidden */ MessageQueue(const MessageQueue&) = delete; diff --git a/src/fsfw/osal/host/QueueFactory.cpp b/src/fsfw/osal/host/QueueFactory.cpp index caea8af7..732892ca 100644 --- a/src/fsfw/osal/host/QueueFactory.cpp +++ b/src/fsfw/osal/host/QueueFactory.cpp @@ -27,7 +27,8 @@ QueueFactory::QueueFactory() {} QueueFactory::~QueueFactory() {} -MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize, MqArgs* args) { +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 diff --git a/src/fsfw/osal/linux/MessageQueue.cpp b/src/fsfw/osal/linux/MessageQueue.cpp index c1de8f13..bfc1d0f7 100644 --- a/src/fsfw/osal/linux/MessageQueue.cpp +++ b/src/fsfw/osal/linux/MessageQueue.cpp @@ -37,6 +37,9 @@ MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize, MqArgs* // Successful mq_open call this->id = tempId; } + if (args != nullptr) { + this->mqArgs = *args; + } } MessageQueue::~MessageQueue() { diff --git a/src/fsfw/osal/linux/MessageQueue.h b/src/fsfw/osal/linux/MessageQueue.h index 84052914..58afccd6 100644 --- a/src/fsfw/osal/linux/MessageQueue.h +++ b/src/fsfw/osal/linux/MessageQueue.h @@ -5,8 +5,8 @@ #include "fsfw/internalerror/InternalErrorReporterIF.h" #include "fsfw/ipc/MessageQueueIF.h" -#include "fsfw/ipc/definitions.h" #include "fsfw/ipc/MessageQueueMessage.h" +#include "fsfw/ipc/definitions.h" /** * @brief This class manages sending and receiving of message queue messages. * @@ -44,7 +44,7 @@ class MessageQueue : public MessageQueueIF { */ MessageQueue(uint32_t messageDepth = 3, size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE, - MqArgs* args = nullptr); + MqArgs* args = nullptr); /** * @brief The destructor deletes the formerly created message queue. * @details This is accomplished by using the delete call provided by the operating system. @@ -186,6 +186,8 @@ class MessageQueue : public MessageQueueIF { */ char name[16]; + MqArgs mqArgs = {}; + static uint16_t queueCounter; const size_t maxMessageSize; diff --git a/src/fsfw/osal/linux/QueueFactory.cpp b/src/fsfw/osal/linux/QueueFactory.cpp index d74eb8c5..24ace1ae 100644 --- a/src/fsfw/osal/linux/QueueFactory.cpp +++ b/src/fsfw/osal/linux/QueueFactory.cpp @@ -28,7 +28,8 @@ QueueFactory::QueueFactory() {} QueueFactory::~QueueFactory() {} -MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize, MqArgs* args) { +MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize, + MqArgs* args) { return new MessageQueue(messageDepth, maxMessageSize, args); } diff --git a/src/fsfw/osal/rtems/MessageQueue.h b/src/fsfw/osal/rtems/MessageQueue.h index b18b2eaf..0f1ee6ee 100644 --- a/src/fsfw/osal/rtems/MessageQueue.h +++ b/src/fsfw/osal/rtems/MessageQueue.h @@ -4,8 +4,8 @@ #include "RtemsBasic.h" #include "fsfw/internalerror/InternalErrorReporterIF.h" #include "fsfw/ipc/MessageQueueIF.h" -#include "fsfw/ipc/definitions.h" #include "fsfw/ipc/MessageQueueMessage.h" +#include "fsfw/ipc/definitions.h" /** * @brief This class manages sending and receiving of message queue messages. @@ -36,7 +36,7 @@ class MessageQueue : public MessageQueueIF { */ MessageQueue(size_t message_depth = 3, size_t max_message_size = MessageQueueMessage::MAX_MESSAGE_SIZE, - MqArgs* args = nullptr); + MqArgs* args = nullptr); /** * @brief The destructor deletes the formerly created message queue. * @details This is accomplished by using the delete call provided by the operating system. diff --git a/src/fsfw/osal/rtems/QueueFactory.cpp b/src/fsfw/osal/rtems/QueueFactory.cpp index 8b625da8..2519f444 100644 --- a/src/fsfw/osal/rtems/QueueFactory.cpp +++ b/src/fsfw/osal/rtems/QueueFactory.cpp @@ -49,7 +49,8 @@ QueueFactory::QueueFactory() {} QueueFactory::~QueueFactory() {} -MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize, MqArgs* args) { +MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, size_t maxMessageSize, + MqArgs* args) { return new MessageQueue(messageDepth, maxMessageSize, args); } From cf69af4e7e523bfcd73318f64e9d720f4779ea39 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 19 Feb 2022 16:14:02 +0100 Subject: [PATCH 35/81] passing mqArgs to all mq ctor calls --- src/fsfw/cfdp/CFDPHandler.cpp | 4 +++- src/fsfw/controller/ControllerBase.cpp | 4 +++- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 3 ++- src/fsfw/devicehandlers/HealthDevice.cpp | 3 ++- src/fsfw/events/EventManager.cpp | 4 +++- src/fsfw/fdir/FailureIsolationBase.cpp | 3 ++- src/fsfw/internalerror/InternalErrorReporter.cpp | 3 ++- src/fsfw/power/PowerSensor.cpp | 3 ++- src/fsfw/pus/Service1TelecommandVerification.cpp | 3 ++- src/fsfw/pus/Service5EventReporting.cpp | 3 ++- src/fsfw/subsystem/SubsystemBase.cpp | 8 +++++--- src/fsfw/tcdistribution/TcDistributor.cpp | 3 ++- src/fsfw/thermal/AbstractTemperatureSensor.cpp | 6 +++--- src/fsfw/thermal/AbstractTemperatureSensor.h | 2 +- src/fsfw/thermal/Heater.cpp | 3 ++- src/fsfw/tmtcservices/CommandingServiceBase.cpp | 6 ++++-- src/fsfw/tmtcservices/PusServiceBase.cpp | 3 ++- src/fsfw/tmtcservices/TmTcBridge.cpp | 3 ++- 18 files changed, 44 insertions(+), 23 deletions(-) diff --git a/src/fsfw/cfdp/CFDPHandler.cpp b/src/fsfw/cfdp/CFDPHandler.cpp index 96baa98c..1d41a80d 100644 --- a/src/fsfw/cfdp/CFDPHandler.cpp +++ b/src/fsfw/cfdp/CFDPHandler.cpp @@ -12,7 +12,9 @@ object_id_t CFDPHandler::packetDestination = 0; CFDPHandler::CFDPHandler(object_id_t setObjectId, CFDPDistributor* dist) : SystemObject(setObjectId) { - requestQueue = QueueFactory::instance()->createMessageQueue(CFDP_HANDLER_MAX_RECEPTION); + auto mqArgs = MqArgs(setObjectId, static_cast(this)); + requestQueue = QueueFactory::instance()->createMessageQueue(CFDP_HANDLER_MAX_RECEPTION, + MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); distributor = dist; } diff --git a/src/fsfw/controller/ControllerBase.cpp b/src/fsfw/controller/ControllerBase.cpp index 953dacb4..6af4dd7d 100644 --- a/src/fsfw/controller/ControllerBase.cpp +++ b/src/fsfw/controller/ControllerBase.cpp @@ -13,7 +13,9 @@ ControllerBase::ControllerBase(object_id_t setObjectId, object_id_t parentId, submode(SUBMODE_NONE), modeHelper(this), healthHelper(this, setObjectId) { - commandQueue = QueueFactory::instance()->createMessageQueue(commandQueueDepth); + auto mqArgs = MqArgs(setObjectId, static_cast(this)); + commandQueue = QueueFactory::instance()->createMessageQueue(commandQueueDepth, + MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } ControllerBase::~ControllerBase() { QueueFactory::instance()->deleteMessageQueue(commandQueue); } diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index ea1fcdf1..3f90a72c 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -39,8 +39,9 @@ DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId, object_id_t device childTransitionDelay(5000), transitionSourceMode(_MODE_POWER_DOWN), transitionSourceSubMode(SUBMODE_NONE) { + auto mqArgs = MqArgs(setObjectId, static_cast(this)); commandQueue = QueueFactory::instance()->createMessageQueue( - cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE); + cmdQueueSize, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); insertInCommandMap(RAW_COMMAND_ID); cookieInfo.state = COOKIE_UNUSED; cookieInfo.pendingCommand = deviceCommandMap.end(); diff --git a/src/fsfw/devicehandlers/HealthDevice.cpp b/src/fsfw/devicehandlers/HealthDevice.cpp index a626fa6c..c666ceb8 100644 --- a/src/fsfw/devicehandlers/HealthDevice.cpp +++ b/src/fsfw/devicehandlers/HealthDevice.cpp @@ -8,7 +8,8 @@ HealthDevice::HealthDevice(object_id_t setObjectId, MessageQueueId_t parentQueue parentQueue(parentQueue), commandQueue(), healthHelper(this, setObjectId) { - commandQueue = QueueFactory::instance()->createMessageQueue(3); + auto mqArgs = MqArgs(setObjectId, static_cast(this)); + commandQueue = QueueFactory::instance()->createMessageQueue(3, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } HealthDevice::~HealthDevice() { QueueFactory::instance()->deleteMessageQueue(commandQueue); } diff --git a/src/fsfw/events/EventManager.cpp b/src/fsfw/events/EventManager.cpp index 0fd893b2..7583bafd 100644 --- a/src/fsfw/events/EventManager.cpp +++ b/src/fsfw/events/EventManager.cpp @@ -18,8 +18,10 @@ const LocalPool::LocalPoolConfig EventManager::poolConfig = { EventManager::EventManager(object_id_t setObjectId) : SystemObject(setObjectId), factoryBackend(0, poolConfig, false, true) { mutex = MutexFactory::instance()->createMutex(); + auto mqArgs = MqArgs(setObjectId, static_cast(this)); eventReportQueue = QueueFactory::instance()->createMessageQueue(MAX_EVENTS_PER_CYCLE, - EventMessage::EVENT_MESSAGE_SIZE); + EventMessage::EVENT_MESSAGE_SIZE, + &mqArgs); } EventManager::~EventManager() { diff --git a/src/fsfw/fdir/FailureIsolationBase.cpp b/src/fsfw/fdir/FailureIsolationBase.cpp index fedef869..ca7077a9 100644 --- a/src/fsfw/fdir/FailureIsolationBase.cpp +++ b/src/fsfw/fdir/FailureIsolationBase.cpp @@ -9,8 +9,9 @@ FailureIsolationBase::FailureIsolationBase(object_id_t owner, object_id_t parent, uint8_t messageDepth, uint8_t parameterDomainBase) : ownerId(owner), faultTreeParent(parent), parameterDomainBase(parameterDomainBase) { + auto mqArgs = MqArgs(owner, static_cast(this)); eventQueue = - QueueFactory::instance()->createMessageQueue(messageDepth, EventMessage::EVENT_MESSAGE_SIZE); + QueueFactory::instance()->createMessageQueue(messageDepth, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs); } FailureIsolationBase::~FailureIsolationBase() { diff --git a/src/fsfw/internalerror/InternalErrorReporter.cpp b/src/fsfw/internalerror/InternalErrorReporter.cpp index a085cc5b..0ad60492 100644 --- a/src/fsfw/internalerror/InternalErrorReporter.cpp +++ b/src/fsfw/internalerror/InternalErrorReporter.cpp @@ -7,11 +7,12 @@ InternalErrorReporter::InternalErrorReporter(object_id_t setObjectId, uint32_t messageQueueDepth) : SystemObject(setObjectId), - commandQueue(QueueFactory::instance()->createMessageQueue(messageQueueDepth)), poolManager(this, commandQueue), internalErrorSid(setObjectId, InternalErrorDataset::ERROR_SET_ID), internalErrorDataset(this) { mutex = MutexFactory::instance()->createMutex(); + auto mqArgs = MqArgs(setObjectId, static_cast(this)); + commandQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } InternalErrorReporter::~InternalErrorReporter() { MutexFactory::instance()->deleteMutex(mutex); } diff --git a/src/fsfw/power/PowerSensor.cpp b/src/fsfw/power/PowerSensor.cpp index 08ff4724..74cf60f7 100644 --- a/src/fsfw/power/PowerSensor.cpp +++ b/src/fsfw/power/PowerSensor.cpp @@ -15,7 +15,8 @@ PowerSensor::PowerSensor(object_id_t objectId, sid_t setId, VariableIds ids, Def limits.currentMin, limits.currentMax, events.currentLow, events.currentHigh), voltageLimit(objectId, MODULE_ID_VOLTAGE, ids.pidVoltage, confirmationCount, limits.voltageMin, limits.voltageMax, events.voltageLow, events.voltageHigh) { - commandQueue = QueueFactory::instance()->createMessageQueue(); + auto mqArgs = MqArgs(objectId, static_cast(this)); + commandQueue = QueueFactory::instance()->createMessageQueue(3, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } PowerSensor::~PowerSensor() { QueueFactory::instance()->deleteMessageQueue(commandQueue); } diff --git a/src/fsfw/pus/Service1TelecommandVerification.cpp b/src/fsfw/pus/Service1TelecommandVerification.cpp index 13d6a1c4..9418ceb2 100644 --- a/src/fsfw/pus/Service1TelecommandVerification.cpp +++ b/src/fsfw/pus/Service1TelecommandVerification.cpp @@ -16,7 +16,8 @@ Service1TelecommandVerification::Service1TelecommandVerification(object_id_t obj apid(apid), serviceId(serviceId), targetDestination(targetDestination) { - tmQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth); + auto mqArgs = MqArgs(objectId, static_cast(this)); + tmQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } Service1TelecommandVerification::~Service1TelecommandVerification() { diff --git a/src/fsfw/pus/Service5EventReporting.cpp b/src/fsfw/pus/Service5EventReporting.cpp index dfce9ba2..918abff0 100644 --- a/src/fsfw/pus/Service5EventReporting.cpp +++ b/src/fsfw/pus/Service5EventReporting.cpp @@ -12,7 +12,8 @@ Service5EventReporting::Service5EventReporting(object_id_t objectId, uint16_t ap uint32_t messageQueueDepth) : PusServiceBase(objectId, apid, serviceId), maxNumberReportsPerCycle(maxNumberReportsPerCycle) { - eventQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth); + auto mqArgs = MqArgs(objectId, static_cast(this)); + eventQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); sif::info << eventQueue->getId() << std::endl; } diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp index 104db3c3..974459d3 100644 --- a/src/fsfw/subsystem/SubsystemBase.cpp +++ b/src/fsfw/subsystem/SubsystemBase.cpp @@ -8,11 +8,13 @@ SubsystemBase::SubsystemBase(object_id_t setObjectId, object_id_t parent, Mode_t uint16_t commandQueueDepth) : SystemObject(setObjectId), mode(initialMode), - commandQueue(QueueFactory::instance()->createMessageQueue(commandQueueDepth, - CommandMessage::MAX_MESSAGE_SIZE)), healthHelper(this, setObjectId), modeHelper(this), - parentId(parent) {} + parentId(parent) { + auto mqArgs = MqArgs(setObjectId, static_cast(this)); + commandQueue = QueueFactory::instance()->createMessageQueue(commandQueueDepth, + CommandMessage::MAX_MESSAGE_SIZE, &mqArgs); +} SubsystemBase::~SubsystemBase() { QueueFactory::instance()->deleteMessageQueue(commandQueue); } diff --git a/src/fsfw/tcdistribution/TcDistributor.cpp b/src/fsfw/tcdistribution/TcDistributor.cpp index a650546c..5ce41826 100644 --- a/src/fsfw/tcdistribution/TcDistributor.cpp +++ b/src/fsfw/tcdistribution/TcDistributor.cpp @@ -5,7 +5,8 @@ #include "fsfw/tmtcservices/TmTcMessage.h" TcDistributor::TcDistributor(object_id_t objectId) : SystemObject(objectId) { - tcQueue = QueueFactory::instance()->createMessageQueue(DISTRIBUTER_MAX_PACKETS); + auto mqArgs = MqArgs(objectId); + tcQueue = QueueFactory::instance()->createMessageQueue(DISTRIBUTER_MAX_PACKETS, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } TcDistributor::~TcDistributor() { QueueFactory::instance()->deleteMessageQueue(tcQueue); } diff --git a/src/fsfw/thermal/AbstractTemperatureSensor.cpp b/src/fsfw/thermal/AbstractTemperatureSensor.cpp index 68cd3aca..003f857e 100644 --- a/src/fsfw/thermal/AbstractTemperatureSensor.cpp +++ b/src/fsfw/thermal/AbstractTemperatureSensor.cpp @@ -5,13 +5,13 @@ AbstractTemperatureSensor::AbstractTemperatureSensor(object_id_t setObjectid, ThermalModuleIF *thermalModule) : SystemObject(setObjectid), - commandQueue(NULL), healthHelper(this, setObjectid), parameterHelper(this) { - if (thermalModule != NULL) { + if (thermalModule != nullptr) { thermalModule->registerSensor(this); } - commandQueue = QueueFactory::instance()->createMessageQueue(); + auto mqArgs = MqArgs(setObjectid, static_cast(this)); + commandQueue = QueueFactory::instance()->createMessageQueue(3, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } AbstractTemperatureSensor::~AbstractTemperatureSensor() { diff --git a/src/fsfw/thermal/AbstractTemperatureSensor.h b/src/fsfw/thermal/AbstractTemperatureSensor.h index 0c6493fe..b790b0ca 100644 --- a/src/fsfw/thermal/AbstractTemperatureSensor.h +++ b/src/fsfw/thermal/AbstractTemperatureSensor.h @@ -51,7 +51,7 @@ class AbstractTemperatureSensor : public HasHealthIF, HasHealthIF::HealthState getHealth(); protected: - MessageQueueIF* commandQueue; + MessageQueueIF* commandQueue = nullptr; HealthHelper healthHelper; ParameterHelper parameterHelper; diff --git a/src/fsfw/thermal/Heater.cpp b/src/fsfw/thermal/Heater.cpp index 4f0f8060..2755b855 100644 --- a/src/fsfw/thermal/Heater.cpp +++ b/src/fsfw/thermal/Heater.cpp @@ -12,7 +12,8 @@ Heater::Heater(uint32_t objectId, uint8_t switch0, uint8_t switch1) switch1(switch1), heaterOnCountdown(10800000) /*about two orbits*/, parameterHelper(this) { - eventQueue = QueueFactory::instance()->createMessageQueue(); + auto mqArgs = MqArgs(objectId, static_cast(this)); + eventQueue = QueueFactory::instance()->createMessageQueue(3, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } Heater::~Heater() { QueueFactory::instance()->deleteMessageQueue(eventQueue); } diff --git a/src/fsfw/tmtcservices/CommandingServiceBase.cpp b/src/fsfw/tmtcservices/CommandingServiceBase.cpp index bbdf8d2a..1bbd7fd1 100644 --- a/src/fsfw/tmtcservices/CommandingServiceBase.cpp +++ b/src/fsfw/tmtcservices/CommandingServiceBase.cpp @@ -20,8 +20,10 @@ CommandingServiceBase::CommandingServiceBase(object_id_t setObjectId, uint16_t a service(service), timeoutSeconds(commandTimeoutSeconds), commandMap(numberOfParallelCommands) { - commandQueue = QueueFactory::instance()->createMessageQueue(queueDepth); - requestQueue = QueueFactory::instance()->createMessageQueue(queueDepth); + auto mqArgs = MqArgs(setObjectId, static_cast(this)); + size_t mqSz = MessageQueueMessage::MAX_MESSAGE_SIZE; + commandQueue = QueueFactory::instance()->createMessageQueue(queueDepth, mqSz, &mqArgs); + requestQueue = QueueFactory::instance()->createMessageQueue(queueDepth, mqSz, &mqArgs); } void CommandingServiceBase::setPacketSource(object_id_t packetSource) { diff --git a/src/fsfw/tmtcservices/PusServiceBase.cpp b/src/fsfw/tmtcservices/PusServiceBase.cpp index 3af2b82c..cedcb0a2 100644 --- a/src/fsfw/tmtcservices/PusServiceBase.cpp +++ b/src/fsfw/tmtcservices/PusServiceBase.cpp @@ -13,7 +13,8 @@ object_id_t PusServiceBase::packetDestination = 0; PusServiceBase::PusServiceBase(object_id_t setObjectId, uint16_t setApid, uint8_t setServiceId) : SystemObject(setObjectId), apid(setApid), serviceId(setServiceId) { - requestQueue = QueueFactory::instance()->createMessageQueue(PUS_SERVICE_MAX_RECEPTION); + auto mqArgs = MqArgs(setObjectId, static_cast(this)); + requestQueue = QueueFactory::instance()->createMessageQueue(PUS_SERVICE_MAX_RECEPTION, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } PusServiceBase::~PusServiceBase() { QueueFactory::instance()->deleteMessageQueue(requestQueue); } diff --git a/src/fsfw/tmtcservices/TmTcBridge.cpp b/src/fsfw/tmtcservices/TmTcBridge.cpp index bacadd6d..6b96856b 100644 --- a/src/fsfw/tmtcservices/TmTcBridge.cpp +++ b/src/fsfw/tmtcservices/TmTcBridge.cpp @@ -15,7 +15,8 @@ TmTcBridge::TmTcBridge(object_id_t objectId, object_id_t tcDestination, object_i tcDestination(tcDestination) { - tmTcReceptionQueue = QueueFactory::instance()->createMessageQueue(TMTC_RECEPTION_QUEUE_DEPTH); + auto mqArgs = MqArgs(objectId, static_cast(this)); + tmTcReceptionQueue = QueueFactory::instance()->createMessageQueue(TMTC_RECEPTION_QUEUE_DEPTH, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } TmTcBridge::~TmTcBridge() { QueueFactory::instance()->deleteMessageQueue(tmTcReceptionQueue); } From d74a373f1d6bc341c11d7ad89f369da5ff957928 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 19 Feb 2022 16:41:30 +0100 Subject: [PATCH 36/81] make periodic printout run time configurable --- .../devicehandlers/GyroL3GD20Handler.cpp | 39 ++++++------ .../devicehandlers/GyroL3GD20Handler.h | 13 ++-- .../devicehandlers/MgmLIS3MDLHandler.cpp | 60 ++++++++++--------- .../devicehandlers/MgmLIS3MDLHandler.h | 9 +-- .../devicehandlers/MgmRM3100Handler.cpp | 41 +++++++------ .../devicehandlers/MgmRM3100Handler.h | 11 ++-- src/fsfw/FSFW.h.in | 12 ---- 7 files changed, 92 insertions(+), 93 deletions(-) diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp index f92f5f1f..1bd0665d 100644 --- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp @@ -9,9 +9,6 @@ GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceC : DeviceHandlerBase(objectId, deviceCommunication, comCookie), transitionDelayMs(transitionDelayMs), dataset(this) { -#if FSFW_HAL_L3GD20_GYRO_DEBUG == 1 - debugDivider = new PeriodicOperationDivider(10); -#endif } GyroHandlerL3GD20H::~GyroHandlerL3GD20H() {} @@ -193,22 +190,23 @@ 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 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; -#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); -#endif + 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; + #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); + #endif + } } -#endif + PoolReadGuard readSet(&dataset); if (readSet.getReadResult() == HasReturnvaluesIF::RETURN_OK) { @@ -272,3 +270,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/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h index 784dcf4c..806fb5c0 100644 --- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h +++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h @@ -1,12 +1,11 @@ #ifndef MISSION_DEVICES_GYROL3GD20HANDLER_H_ #define MISSION_DEVICES_GYROL3GD20HANDLER_H_ +#include "devicedefinitions/GyroL3GD20Definitions.h" + #include #include -#include "devicedefinitions/GyroL3GD20Definitions.h" -#include "fsfw/FSFW.h" - /** * @brief Device Handler for the L3GD20H gyroscope sensor * (https://www.st.com/en/mems-and-sensors/l3gd20h.html) @@ -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 @@ -58,6 +59,7 @@ class GyroHandlerL3GD20H : public DeviceHandlerBase { uint32_t transitionDelayMs = 0; GyroPrimaryDataset dataset; + float absLimitX = L3GD20H::RANGE_DPS_00; float absLimitY = L3GD20H::RANGE_DPS_00; float absLimitZ = L3GD20H::RANGE_DPS_00; @@ -80,9 +82,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/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp index cf1d934a..a20a3a0f 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp @@ -12,9 +12,6 @@ MgmLIS3MDLHandler::MgmLIS3MDLHandler(object_id_t objectId, object_id_t deviceCom : DeviceHandlerBase(objectId, deviceCommunication, comCookie), dataset(this), transitionDelay(transitionDelay) { -#if FSFW_HAL_LIS3MDL_MGM_DEBUG == 1 - debugDivider = new PeriodicOperationDivider(10); -#endif // Set to default values right away registers[0] = MGMLIS3MDL::CTRL_REG1_DEFAULT; registers[1] = MGMLIS3MDL::CTRL_REG2_DEFAULT; @@ -272,23 +269,25 @@ 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 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; -#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); -#endif /* FSFW_CPP_OSTREAM_ENABLED == 0 */ + 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; + #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); + #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 +317,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 FSFW_CPP_OSTREAM_ENABLED == 1 - sif::info << "MGMHandlerLIS3: Temperature: " << tempValue << " C" << std::endl; -#else - sif::printInfo("MGMHandlerLIS3: Temperature: %f C\n"); -#endif + if(periodicPrintout) { + if (debugDivider.check()) { + #if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "MGMHandlerLIS3: Temperature: " << tempValue << " C" << std::endl; + #else + sif::printInfo("MGMHandlerLIS3: Temperature: %f C\n"); + #endif + } } -#endif + ReturnValue_t result = dataset.read(); if (result == HasReturnvaluesIF::RETURN_OK) { dataset.temperature = tempValue; @@ -484,3 +484,9 @@ 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/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h index b7cfc378..6605f763 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h +++ b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h @@ -1,9 +1,10 @@ #ifndef MISSION_DEVICES_MGMLIS3MDLHANDLER_H_ #define MISSION_DEVICES_MGMLIS3MDLHANDLER_H_ + #include "devicedefinitions/MgmLIS3HandlerDefs.h" #include "events/subsystemIdRanges.h" -#include "fsfw/FSFW.h" +#include "fsfw/globalfunctions/PeriodicOperationDivider.h" #include "fsfw/devicehandlers/DeviceHandlerBase.h" class PeriodicOperationDivider; @@ -30,6 +31,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 +169,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/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp index a2e2e85f..b0ba8e88 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp @@ -11,9 +11,6 @@ MgmRM3100Handler::MgmRM3100Handler(object_id_t objectId, object_id_t deviceCommu : DeviceHandlerBase(objectId, deviceCommunication, comCookie), primaryDataset(this), transitionDelay(transitionDelay) { -#if FSFW_HAL_RM3100_MGM_DEBUG == 1 - debugDivider = new PeriodicOperationDivider(10); -#endif } MgmRM3100Handler::~MgmRM3100Handler() {} @@ -337,23 +334,24 @@ 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 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; -#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); -#endif + 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; + #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); + #endif + } } -#endif + // TODO: Sanity check on values? PoolReadGuard readGuard(&primaryDataset); @@ -365,3 +363,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/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h index 67362f59..d1048cb6 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h +++ b/hal/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/src/fsfw/FSFW.h.in b/src/fsfw/FSFW.h.in index 7e8bcd79..9cfeab61 100644 --- a/src/fsfw/FSFW.h.in +++ b/src/fsfw/FSFW.h.in @@ -62,16 +62,4 @@ #define FSFW_HAL_I2C_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 */ - #endif /* FSFW_FSFW_H_ */ From 701135e2a620ff814ae9e256c3e29d742bfa8f53 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Feb 2022 10:17:56 +0100 Subject: [PATCH 37/81] applied clang format --- .../devicehandlers/GyroL3GD20Handler.cpp | 12 +++++------- .../devicehandlers/GyroL3GD20Handler.h | 5 ++--- .../devicehandlers/MgmLIS3MDLHandler.cpp | 18 ++++++++---------- .../devicehandlers/MgmLIS3MDLHandler.h | 3 +-- .../devicehandlers/MgmRM3100Handler.cpp | 12 +++++------- src/fsfw/cfdp/CFDPHandler.cpp | 4 ++-- src/fsfw/controller/ControllerBase.cpp | 4 ++-- src/fsfw/devicehandlers/HealthDevice.cpp | 3 ++- src/fsfw/events/EventManager.cpp | 5 ++--- src/fsfw/fdir/FailureIsolationBase.cpp | 4 ++-- .../internalerror/InternalErrorReporter.cpp | 5 +++-- src/fsfw/objectmanager/ObjectManager.cpp | 13 ++++++++----- src/fsfw/power/PowerSensor.cpp | 3 ++- .../pus/Service1TelecommandVerification.cpp | 3 ++- src/fsfw/pus/Service5EventReporting.cpp | 4 ++-- src/fsfw/rmap/RMAPChannelIF.h | 6 +++--- src/fsfw/subsystem/SubsystemBase.cpp | 4 ++-- src/fsfw/tcdistribution/TcDistributor.cpp | 3 ++- src/fsfw/thermal/AbstractTemperatureSensor.cpp | 9 ++++----- src/fsfw/thermal/Heater.cpp | 3 ++- src/fsfw/tmtcservices/PusServiceBase.cpp | 3 ++- src/fsfw/tmtcservices/TmTcBridge.cpp | 3 ++- 22 files changed, 65 insertions(+), 64 deletions(-) diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp index 1bd0665d..94e1331c 100644 --- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp @@ -8,8 +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) { -} + dataset(this) {} GyroHandlerL3GD20H::~GyroHandlerL3GD20H() {} @@ -190,24 +189,23 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, int8_t temperaturOffset = (-1) * packet[L3GD20H::TEMPERATURE_IDX]; float temperature = 25.0 + temperaturOffset; - if(periodicPrintout) { + if (periodicPrintout) { if (debugDivider.checkAndIncrement()) { /* Set terminal to utf-8 if there is an issue with micro printout. */ - #if FSFW_CPP_OSTREAM_ENABLED == 1 +#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; - #else +#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); - #endif +#endif } } - PoolReadGuard readSet(&dataset); if (readSet.getReadResult() == HasReturnvaluesIF::RETURN_OK) { if (std::abs(angVelocX) < this->absLimitX) { diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h index 806fb5c0..7c1ebdac 100644 --- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h +++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h @@ -1,11 +1,11 @@ #ifndef MISSION_DEVICES_GYROL3GD20HANDLER_H_ #define MISSION_DEVICES_GYROL3GD20HANDLER_H_ -#include "devicedefinitions/GyroL3GD20Definitions.h" - #include #include +#include "devicedefinitions/GyroL3GD20Definitions.h" + /** * @brief Device Handler for the L3GD20H gyroscope sensor * (https://www.st.com/en/mems-and-sensors/l3gd20h.html) @@ -59,7 +59,6 @@ class GyroHandlerL3GD20H : public DeviceHandlerBase { uint32_t transitionDelayMs = 0; GyroPrimaryDataset dataset; - float absLimitX = L3GD20H::RANGE_DPS_00; float absLimitY = L3GD20H::RANGE_DPS_00; float absLimitZ = L3GD20H::RANGE_DPS_00; diff --git a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp index a20a3a0f..69f6311e 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp @@ -269,25 +269,24 @@ ReturnValue_t MgmLIS3MDLHandler::interpretDeviceReply(DeviceCommandId_t id, cons float mgmZ = static_cast(mgmMeasurementRawZ) * sensitivityFactor * MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR; - if(periodicPrintout) { + if (periodicPrintout) { if (debugDivider.checkAndIncrement()) { - #if FSFW_CPP_OSTREAM_ENABLED == 1 +#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; - #else +#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); - #endif /* FSFW_CPP_OSTREAM_ENABLED == 0 */ +#endif /* FSFW_CPP_OSTREAM_ENABLED == 0 */ } } - PoolReadGuard readHelper(&dataset); if (readHelper.getReadResult() == HasReturnvaluesIF::RETURN_OK) { if (std::abs(mgmX) < absLimitX) { @@ -317,13 +316,13 @@ 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(periodicPrintout) { + if (periodicPrintout) { if (debugDivider.check()) { - #if FSFW_CPP_OSTREAM_ENABLED == 1 +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::info << "MGMHandlerLIS3: Temperature: " << tempValue << " C" << std::endl; - #else +#else sif::printInfo("MGMHandlerLIS3: Temperature: %f C\n"); - #endif +#endif } } @@ -485,7 +484,6 @@ void MgmLIS3MDLHandler::setAbsoluteLimits(float xLimit, float yLimit, float zLim 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/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h index 6605f763..42bd5d4c 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h +++ b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h @@ -1,11 +1,10 @@ #ifndef MISSION_DEVICES_MGMLIS3MDLHANDLER_H_ #define MISSION_DEVICES_MGMLIS3MDLHANDLER_H_ - #include "devicedefinitions/MgmLIS3HandlerDefs.h" #include "events/subsystemIdRanges.h" -#include "fsfw/globalfunctions/PeriodicOperationDivider.h" #include "fsfw/devicehandlers/DeviceHandlerBase.h" +#include "fsfw/globalfunctions/PeriodicOperationDivider.h" class PeriodicOperationDivider; diff --git a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp b/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp index b0ba8e88..f9929d63 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp @@ -10,8 +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) { -} + transitionDelay(transitionDelay) {} MgmRM3100Handler::~MgmRM3100Handler() {} @@ -334,25 +333,24 @@ ReturnValue_t MgmRM3100Handler::handleDataReadout(const uint8_t *packet) { float fieldStrengthY = fieldStrengthRawY * scaleFactorX; float fieldStrengthZ = fieldStrengthRawZ * scaleFactorX; - if(periodicPrintout) { + if (periodicPrintout) { if (debugDivider.checkAndIncrement()) { - #if FSFW_CPP_OSTREAM_ENABLED == 1 +#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; - #else +#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); - #endif +#endif } } - // TODO: Sanity check on values? PoolReadGuard readGuard(&primaryDataset); if (readGuard.getReadResult() == HasReturnvaluesIF::RETURN_OK) { diff --git a/src/fsfw/cfdp/CFDPHandler.cpp b/src/fsfw/cfdp/CFDPHandler.cpp index 1d41a80d..09a24186 100644 --- a/src/fsfw/cfdp/CFDPHandler.cpp +++ b/src/fsfw/cfdp/CFDPHandler.cpp @@ -13,8 +13,8 @@ object_id_t CFDPHandler::packetDestination = 0; CFDPHandler::CFDPHandler(object_id_t setObjectId, CFDPDistributor* dist) : SystemObject(setObjectId) { auto mqArgs = MqArgs(setObjectId, static_cast(this)); - requestQueue = QueueFactory::instance()->createMessageQueue(CFDP_HANDLER_MAX_RECEPTION, - MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); + requestQueue = QueueFactory::instance()->createMessageQueue( + CFDP_HANDLER_MAX_RECEPTION, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); distributor = dist; } diff --git a/src/fsfw/controller/ControllerBase.cpp b/src/fsfw/controller/ControllerBase.cpp index 6af4dd7d..0e4ff970 100644 --- a/src/fsfw/controller/ControllerBase.cpp +++ b/src/fsfw/controller/ControllerBase.cpp @@ -14,8 +14,8 @@ ControllerBase::ControllerBase(object_id_t setObjectId, object_id_t parentId, modeHelper(this), healthHelper(this, setObjectId) { auto mqArgs = MqArgs(setObjectId, static_cast(this)); - commandQueue = QueueFactory::instance()->createMessageQueue(commandQueueDepth, - MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); + commandQueue = QueueFactory::instance()->createMessageQueue( + commandQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } ControllerBase::~ControllerBase() { QueueFactory::instance()->deleteMessageQueue(commandQueue); } diff --git a/src/fsfw/devicehandlers/HealthDevice.cpp b/src/fsfw/devicehandlers/HealthDevice.cpp index c666ceb8..5514b65f 100644 --- a/src/fsfw/devicehandlers/HealthDevice.cpp +++ b/src/fsfw/devicehandlers/HealthDevice.cpp @@ -9,7 +9,8 @@ HealthDevice::HealthDevice(object_id_t setObjectId, MessageQueueId_t parentQueue commandQueue(), healthHelper(this, setObjectId) { auto mqArgs = MqArgs(setObjectId, static_cast(this)); - commandQueue = QueueFactory::instance()->createMessageQueue(3, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); + commandQueue = QueueFactory::instance()->createMessageQueue( + 3, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } HealthDevice::~HealthDevice() { QueueFactory::instance()->deleteMessageQueue(commandQueue); } diff --git a/src/fsfw/events/EventManager.cpp b/src/fsfw/events/EventManager.cpp index 7583bafd..824682da 100644 --- a/src/fsfw/events/EventManager.cpp +++ b/src/fsfw/events/EventManager.cpp @@ -19,9 +19,8 @@ EventManager::EventManager(object_id_t setObjectId) : SystemObject(setObjectId), factoryBackend(0, poolConfig, false, true) { mutex = MutexFactory::instance()->createMutex(); auto mqArgs = MqArgs(setObjectId, static_cast(this)); - eventReportQueue = QueueFactory::instance()->createMessageQueue(MAX_EVENTS_PER_CYCLE, - EventMessage::EVENT_MESSAGE_SIZE, - &mqArgs); + eventReportQueue = QueueFactory::instance()->createMessageQueue( + MAX_EVENTS_PER_CYCLE, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs); } EventManager::~EventManager() { diff --git a/src/fsfw/fdir/FailureIsolationBase.cpp b/src/fsfw/fdir/FailureIsolationBase.cpp index ca7077a9..5d04588f 100644 --- a/src/fsfw/fdir/FailureIsolationBase.cpp +++ b/src/fsfw/fdir/FailureIsolationBase.cpp @@ -10,8 +10,8 @@ FailureIsolationBase::FailureIsolationBase(object_id_t owner, object_id_t parent uint8_t messageDepth, uint8_t parameterDomainBase) : ownerId(owner), faultTreeParent(parent), parameterDomainBase(parameterDomainBase) { auto mqArgs = MqArgs(owner, static_cast(this)); - eventQueue = - QueueFactory::instance()->createMessageQueue(messageDepth, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs); + eventQueue = QueueFactory::instance()->createMessageQueue( + messageDepth, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs); } FailureIsolationBase::~FailureIsolationBase() { diff --git a/src/fsfw/internalerror/InternalErrorReporter.cpp b/src/fsfw/internalerror/InternalErrorReporter.cpp index 0ad60492..3faadc1e 100644 --- a/src/fsfw/internalerror/InternalErrorReporter.cpp +++ b/src/fsfw/internalerror/InternalErrorReporter.cpp @@ -11,8 +11,9 @@ InternalErrorReporter::InternalErrorReporter(object_id_t setObjectId, uint32_t m internalErrorSid(setObjectId, InternalErrorDataset::ERROR_SET_ID), internalErrorDataset(this) { mutex = MutexFactory::instance()->createMutex(); - auto mqArgs = MqArgs(setObjectId, static_cast(this)); - commandQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); + auto mqArgs = MqArgs(setObjectId, static_cast(this)); + commandQueue = QueueFactory::instance()->createMessageQueue( + messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } InternalErrorReporter::~InternalErrorReporter() { MutexFactory::instance()->deleteMutex(mutex); } diff --git a/src/fsfw/objectmanager/ObjectManager.cpp b/src/fsfw/objectmanager/ObjectManager.cpp index 2017938a..39fef5b5 100644 --- a/src/fsfw/objectmanager/ObjectManager.cpp +++ b/src/fsfw/objectmanager/ObjectManager.cpp @@ -95,13 +95,16 @@ void ObjectManager::initialize() { for (auto const& it : objectList) { result = it.second->initialize(); if (result != RETURN_OK) { +#if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 - object_id_t var = it.first; sif::error << "ObjectManager::initialize: Object 0x" << std::hex << std::setw(8) - << std::setfill('0') << var - << " failed to " - "initialize with code 0x" - << result << std::dec << std::setfill(' ') << std::endl; + << std::setfill('0') << it.first << " failed to initialize with code 0x" << result + << std::dec << std::setfill(' ') << std::endl; +#else + sif::printError( + "ObjectManager::initialize: Object 0x%08x failed to initialize with code 0x%04x\n", var, + it.first); +#endif #endif errorCount++; } diff --git a/src/fsfw/power/PowerSensor.cpp b/src/fsfw/power/PowerSensor.cpp index 74cf60f7..1936e8ee 100644 --- a/src/fsfw/power/PowerSensor.cpp +++ b/src/fsfw/power/PowerSensor.cpp @@ -16,7 +16,8 @@ PowerSensor::PowerSensor(object_id_t objectId, sid_t setId, VariableIds ids, Def voltageLimit(objectId, MODULE_ID_VOLTAGE, ids.pidVoltage, confirmationCount, limits.voltageMin, limits.voltageMax, events.voltageLow, events.voltageHigh) { auto mqArgs = MqArgs(objectId, static_cast(this)); - commandQueue = QueueFactory::instance()->createMessageQueue(3, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); + commandQueue = QueueFactory::instance()->createMessageQueue( + 3, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } PowerSensor::~PowerSensor() { QueueFactory::instance()->deleteMessageQueue(commandQueue); } diff --git a/src/fsfw/pus/Service1TelecommandVerification.cpp b/src/fsfw/pus/Service1TelecommandVerification.cpp index 9418ceb2..772137dd 100644 --- a/src/fsfw/pus/Service1TelecommandVerification.cpp +++ b/src/fsfw/pus/Service1TelecommandVerification.cpp @@ -17,7 +17,8 @@ Service1TelecommandVerification::Service1TelecommandVerification(object_id_t obj serviceId(serviceId), targetDestination(targetDestination) { auto mqArgs = MqArgs(objectId, static_cast(this)); - tmQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); + tmQueue = QueueFactory::instance()->createMessageQueue( + messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } Service1TelecommandVerification::~Service1TelecommandVerification() { diff --git a/src/fsfw/pus/Service5EventReporting.cpp b/src/fsfw/pus/Service5EventReporting.cpp index 918abff0..3c30b8a5 100644 --- a/src/fsfw/pus/Service5EventReporting.cpp +++ b/src/fsfw/pus/Service5EventReporting.cpp @@ -13,8 +13,8 @@ Service5EventReporting::Service5EventReporting(object_id_t objectId, uint16_t ap : PusServiceBase(objectId, apid, serviceId), maxNumberReportsPerCycle(maxNumberReportsPerCycle) { auto mqArgs = MqArgs(objectId, static_cast(this)); - eventQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); - sif::info << eventQueue->getId() << std::endl; + eventQueue = QueueFactory::instance()->createMessageQueue( + messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } Service5EventReporting::~Service5EventReporting() { diff --git a/src/fsfw/rmap/RMAPChannelIF.h b/src/fsfw/rmap/RMAPChannelIF.h index 56ede1e2..9e666dfb 100644 --- a/src/fsfw/rmap/RMAPChannelIF.h +++ b/src/fsfw/rmap/RMAPChannelIF.h @@ -77,9 +77,9 @@ class RMAPChannelIF { * 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_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... diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp index 974459d3..f177ecd4 100644 --- a/src/fsfw/subsystem/SubsystemBase.cpp +++ b/src/fsfw/subsystem/SubsystemBase.cpp @@ -12,8 +12,8 @@ SubsystemBase::SubsystemBase(object_id_t setObjectId, object_id_t parent, Mode_t modeHelper(this), parentId(parent) { auto mqArgs = MqArgs(setObjectId, static_cast(this)); - commandQueue = QueueFactory::instance()->createMessageQueue(commandQueueDepth, - CommandMessage::MAX_MESSAGE_SIZE, &mqArgs); + commandQueue = QueueFactory::instance()->createMessageQueue( + commandQueueDepth, CommandMessage::MAX_MESSAGE_SIZE, &mqArgs); } SubsystemBase::~SubsystemBase() { QueueFactory::instance()->deleteMessageQueue(commandQueue); } diff --git a/src/fsfw/tcdistribution/TcDistributor.cpp b/src/fsfw/tcdistribution/TcDistributor.cpp index 5ce41826..408e736e 100644 --- a/src/fsfw/tcdistribution/TcDistributor.cpp +++ b/src/fsfw/tcdistribution/TcDistributor.cpp @@ -6,7 +6,8 @@ TcDistributor::TcDistributor(object_id_t objectId) : SystemObject(objectId) { auto mqArgs = MqArgs(objectId); - tcQueue = QueueFactory::instance()->createMessageQueue(DISTRIBUTER_MAX_PACKETS, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); + tcQueue = QueueFactory::instance()->createMessageQueue( + DISTRIBUTER_MAX_PACKETS, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } TcDistributor::~TcDistributor() { QueueFactory::instance()->deleteMessageQueue(tcQueue); } diff --git a/src/fsfw/thermal/AbstractTemperatureSensor.cpp b/src/fsfw/thermal/AbstractTemperatureSensor.cpp index 003f857e..f7b31386 100644 --- a/src/fsfw/thermal/AbstractTemperatureSensor.cpp +++ b/src/fsfw/thermal/AbstractTemperatureSensor.cpp @@ -4,14 +4,13 @@ AbstractTemperatureSensor::AbstractTemperatureSensor(object_id_t setObjectid, ThermalModuleIF *thermalModule) - : SystemObject(setObjectid), - healthHelper(this, setObjectid), - parameterHelper(this) { + : SystemObject(setObjectid), healthHelper(this, setObjectid), parameterHelper(this) { if (thermalModule != nullptr) { thermalModule->registerSensor(this); } - auto mqArgs = MqArgs(setObjectid, static_cast(this)); - commandQueue = QueueFactory::instance()->createMessageQueue(3, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); + auto mqArgs = MqArgs(setObjectid, static_cast(this)); + commandQueue = QueueFactory::instance()->createMessageQueue( + 3, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } AbstractTemperatureSensor::~AbstractTemperatureSensor() { diff --git a/src/fsfw/thermal/Heater.cpp b/src/fsfw/thermal/Heater.cpp index 2755b855..46f5c822 100644 --- a/src/fsfw/thermal/Heater.cpp +++ b/src/fsfw/thermal/Heater.cpp @@ -13,7 +13,8 @@ Heater::Heater(uint32_t objectId, uint8_t switch0, uint8_t switch1) heaterOnCountdown(10800000) /*about two orbits*/, parameterHelper(this) { auto mqArgs = MqArgs(objectId, static_cast(this)); - eventQueue = QueueFactory::instance()->createMessageQueue(3, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); + eventQueue = QueueFactory::instance()->createMessageQueue( + 3, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } Heater::~Heater() { QueueFactory::instance()->deleteMessageQueue(eventQueue); } diff --git a/src/fsfw/tmtcservices/PusServiceBase.cpp b/src/fsfw/tmtcservices/PusServiceBase.cpp index cedcb0a2..565ac5cd 100644 --- a/src/fsfw/tmtcservices/PusServiceBase.cpp +++ b/src/fsfw/tmtcservices/PusServiceBase.cpp @@ -14,7 +14,8 @@ object_id_t PusServiceBase::packetDestination = 0; PusServiceBase::PusServiceBase(object_id_t setObjectId, uint16_t setApid, uint8_t setServiceId) : SystemObject(setObjectId), apid(setApid), serviceId(setServiceId) { auto mqArgs = MqArgs(setObjectId, static_cast(this)); - requestQueue = QueueFactory::instance()->createMessageQueue(PUS_SERVICE_MAX_RECEPTION, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); + requestQueue = QueueFactory::instance()->createMessageQueue( + PUS_SERVICE_MAX_RECEPTION, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } PusServiceBase::~PusServiceBase() { QueueFactory::instance()->deleteMessageQueue(requestQueue); } diff --git a/src/fsfw/tmtcservices/TmTcBridge.cpp b/src/fsfw/tmtcservices/TmTcBridge.cpp index 6b96856b..cc6ec599 100644 --- a/src/fsfw/tmtcservices/TmTcBridge.cpp +++ b/src/fsfw/tmtcservices/TmTcBridge.cpp @@ -16,7 +16,8 @@ TmTcBridge::TmTcBridge(object_id_t objectId, object_id_t tcDestination, object_i { auto mqArgs = MqArgs(objectId, static_cast(this)); - tmTcReceptionQueue = QueueFactory::instance()->createMessageQueue(TMTC_RECEPTION_QUEUE_DEPTH, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); + tmTcReceptionQueue = QueueFactory::instance()->createMessageQueue( + TMTC_RECEPTION_QUEUE_DEPTH, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } TmTcBridge::~TmTcBridge() { QueueFactory::instance()->deleteMessageQueue(tmTcReceptionQueue); } From 1e982ec00bcee6e43effd98c5854bc03193e3907 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Feb 2022 11:00:39 +0100 Subject: [PATCH 38/81] updated for windows compatibility --- .../fsfw_hal/common/gpio/gpioDefinitions.h | 20 +++++++++---------- .../fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp | 14 ++++++------- hal/src/fsfw_hal/linux/rpi/GpioRPi.cpp | 2 +- hal/src/fsfw_hal/linux/rpi/GpioRPi.h | 3 ++- 4 files changed, 20 insertions(+), 19 deletions(-) diff --git a/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h b/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h index b429449b..0ef5b2ea 100644 --- a/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h +++ b/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h @@ -11,7 +11,7 @@ namespace gpio { enum Levels : uint8_t { LOW = 0, HIGH = 1, NONE = 99 }; -enum Direction : uint8_t { IN = 0, OUT = 1 }; +enum Direction : uint8_t { DIR_IN = 0, DIR_OUT = 1 }; enum GpioOperation { READ, WRITE }; @@ -20,7 +20,7 @@ enum class GpioTypes { GPIO_REGULAR_BY_CHIP, GPIO_REGULAR_BY_LABEL, GPIO_REGULAR_BY_LINE_NAME, - CALLBACK + TYPE_CALLBACK }; static constexpr gpioId_t NO_GPIO = -1; @@ -57,7 +57,7 @@ class GpioBase { // Can be used to cast GpioBase to a concrete child implementation gpio::GpioTypes gpioType = gpio::GpioTypes::NONE; std::string consumer; - gpio::Direction direction = gpio::Direction::IN; + gpio::Direction direction = gpio::Direction::DIR_IN; gpio::Levels initValue = gpio::Levels::NONE; }; @@ -79,8 +79,8 @@ class GpiodRegularBase : public GpioBase { class GpiodRegularByChip : public GpiodRegularBase { public: GpiodRegularByChip() - : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP, std::string(), gpio::Direction::IN, - gpio::LOW, 0) {} + : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP, std::string(), + gpio::Direction::DIR_IN, gpio::LOW, 0) {} GpiodRegularByChip(std::string chipname_, int lineNum_, std::string consumer_, gpio::Direction direction_, gpio::Levels initValue_) @@ -89,7 +89,7 @@ class GpiodRegularByChip : public GpiodRegularBase { chipname(chipname_) {} GpiodRegularByChip(std::string chipname_, int lineNum_, std::string consumer_) - : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP, consumer_, gpio::Direction::IN, + : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP, consumer_, gpio::Direction::DIR_IN, gpio::LOW, lineNum_), chipname(chipname_) {} @@ -105,7 +105,7 @@ class GpiodRegularByLabel : public GpiodRegularBase { label(label_) {} GpiodRegularByLabel(std::string label_, int lineNum_, std::string consumer_) - : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LABEL, consumer_, gpio::Direction::IN, + : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LABEL, consumer_, gpio::Direction::DIR_IN, gpio::LOW, lineNum_), label(label_) {} @@ -126,8 +126,8 @@ class GpiodRegularByLineName : public GpiodRegularBase { lineName(lineName_) {} GpiodRegularByLineName(std::string lineName_, std::string consumer_) - : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME, consumer_, gpio::Direction::IN, - gpio::LOW), + : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME, consumer_, + gpio::Direction::DIR_IN, gpio::LOW), lineName(lineName_) {} std::string lineName; @@ -137,7 +137,7 @@ class GpioCallback : public GpioBase { public: GpioCallback(std::string consumer, gpio::Direction direction_, gpio::Levels initValue_, gpio::gpio_cb_t callback, void* callbackArgs) - : GpioBase(gpio::GpioTypes::CALLBACK, consumer, direction_, initValue_), + : GpioBase(gpio::GpioTypes::TYPE_CALLBACK, consumer, direction_, initValue_), callback(callback), callbackArgs(callbackArgs) {} diff --git a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp index a33305b6..fa9eeb5a 100644 --- a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp +++ b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp @@ -74,7 +74,7 @@ ReturnValue_t LinuxLibgpioIF::configureGpios(GpioMap& mapToAdd) { configureGpioByLineName(gpioConfig.first, *regularGpio); break; } - case (gpio::GpioTypes::CALLBACK): { + case (gpio::GpioTypes::TYPE_CALLBACK): { auto gpioCallback = dynamic_cast(gpioConfig.second); if (gpioCallback->callback == nullptr) { return GPIO_INVALID_INSTANCE; @@ -161,11 +161,11 @@ ReturnValue_t LinuxLibgpioIF::configureRegularGpio(gpioId_t gpioId, struct gpiod consumer = regularGpio.consumer; /* Configure direction and add a description to the GPIO */ switch (direction) { - case (gpio::OUT): { + case (gpio::DIR_OUT): { result = gpiod_line_request_output(lineHandle, consumer.c_str(), regularGpio.initValue); break; } - case (gpio::IN): { + case (gpio::DIR_IN): { result = gpiod_line_request_input(lineHandle, consumer.c_str()); break; } @@ -326,7 +326,7 @@ ReturnValue_t LinuxLibgpioIF::checkForConflicts(GpioMap& mapToAdd) { } break; } - case (gpio::GpioTypes::CALLBACK): { + case (gpio::GpioTypes::TYPE_CALLBACK): { auto callbackGpio = dynamic_cast(gpioConfig.second); if (callbackGpio == nullptr) { return GPIO_TYPE_FAILURE; @@ -366,13 +366,13 @@ ReturnValue_t LinuxLibgpioIF::checkForConflictsById(gpioId_t gpioIdToCheck, case (gpio::GpioTypes::GPIO_REGULAR_BY_CHIP): case (gpio::GpioTypes::GPIO_REGULAR_BY_LABEL): case (gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME): { - if (gpioType == gpio::GpioTypes::NONE or gpioType == gpio::GpioTypes::CALLBACK) { + if (gpioType == gpio::GpioTypes::NONE or gpioType == gpio::GpioTypes::TYPE_CALLBACK) { eraseDuplicateDifferentType = true; } break; } - case (gpio::GpioTypes::CALLBACK): { - if (gpioType != gpio::GpioTypes::CALLBACK) { + case (gpio::GpioTypes::TYPE_CALLBACK): { + if (gpioType != gpio::GpioTypes::TYPE_CALLBACK) { eraseDuplicateDifferentType = true; } } diff --git a/hal/src/fsfw_hal/linux/rpi/GpioRPi.cpp b/hal/src/fsfw_hal/linux/rpi/GpioRPi.cpp index c005e24f..d3c0a577 100644 --- a/hal/src/fsfw_hal/linux/rpi/GpioRPi.cpp +++ b/hal/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/hal/src/fsfw_hal/linux/rpi/GpioRPi.h index 499f984b..8ca7065a 100644 --- a/hal/src/fsfw_hal/linux/rpi/GpioRPi.h +++ b/hal/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_ */ From 3a5881a0cb8d01689f8eaba1d873d02c2d0aade7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Feb 2022 11:10:02 +0100 Subject: [PATCH 39/81] more time --- tests/src/fsfw_tests/unit/hal/testCommandExecutor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/src/fsfw_tests/unit/hal/testCommandExecutor.cpp b/tests/src/fsfw_tests/unit/hal/testCommandExecutor.cpp index 3ad26876..fa4e0908 100644 --- a/tests/src/fsfw_tests/unit/hal/testCommandExecutor.cpp +++ b/tests/src/fsfw_tests/unit/hal/testCommandExecutor.cpp @@ -73,7 +73,7 @@ TEST_CASE("Command Executor", "[cmd-exec]") { REQUIRE(result != CommandExecutor::COMMAND_ERROR); // This ensures that the tests do not block indefinitely usleep(500); - REQUIRE(limitIdx < 500); + REQUIRE(limitIdx < 50000); } limitIdx = 0; CHECK(bytesHaveBeenRead == true); From 3966b656e9b581a2dd0e8faf98ba94fec06910e7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Feb 2022 11:15:01 +0100 Subject: [PATCH 40/81] apply .clang format --- src/fsfw/globalfunctions/CRC.cpp | 2 +- src/fsfw/osal/linux/PeriodicPosixTask.h | 7 ++++--- src/fsfw/osal/rtems/PeriodicTask.h | 4 ++-- src/fsfw/rmap/RMAP.h | 8 ++++---- src/fsfw/rmap/RMAPChannelIF.h | 12 ++++++------ 5 files changed, 17 insertions(+), 16 deletions(-) diff --git a/src/fsfw/globalfunctions/CRC.cpp b/src/fsfw/globalfunctions/CRC.cpp index 6b8140c5..033920d0 100644 --- a/src/fsfw/globalfunctions/CRC.cpp +++ b/src/fsfw/globalfunctions/CRC.cpp @@ -117,7 +117,7 @@ uint16_t CRC::crc16ccitt(uint8_t const input[], uint32_t length, uint16_t starti // { // if (xor_out[i] == true) // crc_value = crc_value + pow(2,(15 -i)); // reverse CrC result before - //Final XOR + // Final XOR // } // // crc_value = 0;// for debug mode diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.h b/src/fsfw/osal/linux/PeriodicPosixTask.h index 3c9a3a0d..1c3a52c7 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.h +++ b/src/fsfw/osal/linux/PeriodicPosixTask.h @@ -65,9 +65,10 @@ class PeriodicPosixTask : public PosixThread, public PeriodicTaskIF { /** * @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); /** diff --git a/src/fsfw/osal/rtems/PeriodicTask.h b/src/fsfw/osal/rtems/PeriodicTask.h index ff8617fc..119329f2 100644 --- a/src/fsfw/osal/rtems/PeriodicTask.h +++ b/src/fsfw/osal/rtems/PeriodicTask.h @@ -13,8 +13,8 @@ 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 */ diff --git a/src/fsfw/rmap/RMAP.h b/src/fsfw/rmap/RMAP.h index 42ee1ac5..7c654262 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 20dfd5f8..7dab07c1 100644 --- a/src/fsfw/rmap/RMAPChannelIF.h +++ b/src/fsfw/rmap/RMAPChannelIF.h @@ -75,10 +75,10 @@ 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 + * - @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 @@ -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) From b440c302237a22898e3fbe72e95a8f79cda795a4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Feb 2022 11:26:11 +0100 Subject: [PATCH 41/81] update changelog --- CHANGELOG.md | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 335c0f7b..909d6fb8 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,14 @@ and this project adheres to [Semantic Versioning](http://semver.org/). # [unreleased] +# [v5.0.0] + +## Changes + +- GPIO HAL: Renamed entries of enumerations to avoid nameclashes with Windows defines. + `IN` and `OUT` in `Direction` changed to `DIR_IN` and `DIR_OUT`. + `CALLBACK` in `GpioTypes` changed to `TYPE_CALLBACK` + # [v4.0.0] ## Additions From 389641f8fdf8648f9db8e3f0c4d15843692eafd4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Feb 2022 14:00:34 +0100 Subject: [PATCH 42/81] display run commands --- scripts/helper.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/scripts/helper.py b/scripts/helper.py index 68693c40..09599d3d 100755 --- a/scripts/helper.py +++ b/scripts/helper.py @@ -97,11 +97,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. " @@ -147,21 +147,21 @@ def handle_tests_type(args, build_dir_list: list): # 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(): os.mkdir(UNITTEST_FOLDER_NAME) os.chdir(UNITTEST_FOLDER_NAME) - os.system("cmake -DFSFW_OSAL=host -DFSFW_BUILD_UNITTESTS=ON ..") + cmd_runner("cmake -DFSFW_OSAL=host -DFSFW_BUILD_UNITTESTS=ON ..") 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 +184,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 +206,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() From d88d7c938f4d48da88ec632a0183bf570a6c6c5e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Feb 2022 18:58:20 +0100 Subject: [PATCH 43/81] update spi clock polarity - Perform an empty SPI transfer after setting speed and mode --- hal/src/fsfw_hal/linux/spi/SpiComIF.cpp | 8 ++++++++ hal/src/fsfw_hal/linux/spi/SpiComIF.h | 1 + 2 files changed, 9 insertions(+) diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp index d95232c1..4e7f6181 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -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. + 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/hal/src/fsfw_hal/linux/spi/SpiComIF.h index 1f825d52..357afa2f 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.h +++ b/hal/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; From 2e230daa145c7fda567aab96e7d5506d4ffd4060 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Feb 2022 18:59:50 +0100 Subject: [PATCH 44/81] additional comment --- hal/src/fsfw_hal/linux/spi/SpiComIF.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp index 4e7f6181..e1c631a2 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -402,7 +402,8 @@ void SpiComIF::setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed) 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. + // 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) { From 4747e54c5d71e930125803f875532ee1f79f5054 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Feb 2022 20:08:19 +0100 Subject: [PATCH 45/81] no default values for srv5 params --- src/fsfw/pus/Service5EventReporting.cpp | 2 +- src/fsfw/pus/Service5EventReporting.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/fsfw/pus/Service5EventReporting.cpp b/src/fsfw/pus/Service5EventReporting.cpp index 3c30b8a5..e285875a 100644 --- a/src/fsfw/pus/Service5EventReporting.cpp +++ b/src/fsfw/pus/Service5EventReporting.cpp @@ -89,7 +89,7 @@ ReturnValue_t Service5EventReporting::handleRequest(uint8_t subservice) { // 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) { + if (manager == nullptr) { return RETURN_FAILED; } // register Service 5 as listener for events diff --git a/src/fsfw/pus/Service5EventReporting.h b/src/fsfw/pus/Service5EventReporting.h index 74264130..c1032d18 100644 --- a/src/fsfw/pus/Service5EventReporting.h +++ b/src/fsfw/pus/Service5EventReporting.h @@ -41,7 +41,7 @@ class Service5EventReporting : public PusServiceBase { public: Service5EventReporting(object_id_t objectId, uint16_t apid, uint8_t serviceId, - size_t maxNumberReportsPerCycle = 10, uint32_t messageQueueDepth = 10); + size_t maxNumberReportsPerCycle, uint32_t messageQueueDepth); virtual ~Service5EventReporting(); /*** From 123f2ff360e71228e1c0c786d80a4c93d7144a2e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 25 Feb 2022 11:10:48 +0100 Subject: [PATCH 46/81] removed unnecessary warning --- src/fsfw/pus/Service5EventReporting.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/fsfw/pus/Service5EventReporting.cpp b/src/fsfw/pus/Service5EventReporting.cpp index e285875a..a2407945 100644 --- a/src/fsfw/pus/Service5EventReporting.cpp +++ b/src/fsfw/pus/Service5EventReporting.cpp @@ -38,9 +38,6 @@ ReturnValue_t Service5EventReporting::performService() { } } } -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "Service5EventReporting::generateEventReport: Too many events" << std::endl; -#endif return HasReturnvaluesIF::RETURN_OK; } From eacb4ac4079ff2bf5e0648626332e59046a409da Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Fri, 25 Feb 2022 14:41:43 +0100 Subject: [PATCH 47/81] initial submode --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 4 +++- src/fsfw/devicehandlers/DeviceHandlerBase.h | 6 ++++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 3f90a72c..1e87e8a7 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1386,6 +1386,8 @@ void DeviceHandlerBase::setTaskIF(PeriodicTaskIF* task) { executingTask = task; void DeviceHandlerBase::debugInterface(uint8_t positionTracker, object_id_t objectId, uint32_t parameter) {} +Submode_t DeviceHandlerBase::getInitialSubmode() { return SUBMODE_NONE; } + void DeviceHandlerBase::performOperationHook() {} ReturnValue_t DeviceHandlerBase::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, @@ -1408,7 +1410,7 @@ ReturnValue_t DeviceHandlerBase::initializeAfterTaskCreation() { this->poolManager.initializeAfterTaskCreation(); if (setStartupImmediately) { - startTransition(MODE_ON, SUBMODE_NONE); + startTransition(MODE_ON, getInitialSubmode()); } return HasReturnvaluesIF::RETURN_OK; } diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index 037f4bd7..e8893615 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -647,6 +647,12 @@ class DeviceHandlerBase : public DeviceHandlerIF, virtual void debugInterface(uint8_t positionTracker = 0, object_id_t objectId = 0, uint32_t parameter = 0); + /** + * @brief Can be overwritten by a child to specify the initial submode when device has been set + * to startup immediately. + */ + virtual Submode_t getInitialSubmode(); + protected: static const uint8_t INTERFACE_ID = CLASS_ID::DEVICE_HANDLER_BASE; From afd3a942e2a947e749116422ffbffb7db3caebe0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Feb 2022 14:46:12 +0100 Subject: [PATCH 48/81] use enum class --- hal/src/fsfw_hal/common/gpio/gpioDefinitions.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h b/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h index 0ef5b2ea..ae092fe9 100644 --- a/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h +++ b/hal/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 : uint8_t { LOW = 0, HIGH = 1, NONE = 99 }; -enum Direction : uint8_t { DIR_IN = 0, DIR_OUT = 1 }; +enum class Direction : uint8_t { DIR_IN = 0, DIR_OUT = 1 }; -enum GpioOperation { READ, WRITE }; +enum class GpioOperation { READ, WRITE }; enum class GpioTypes { NONE, From 331aa9442d9b682624784ec8bf932c0a60e63385 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Feb 2022 14:56:37 +0100 Subject: [PATCH 49/81] some updates --- .../fsfw_hal/common/gpio/gpioDefinitions.h | 22 +++++++++---------- .../fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp | 13 ++++++----- 2 files changed, 18 insertions(+), 17 deletions(-) diff --git a/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h b/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h index ae092fe9..7c81a1db 100644 --- a/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h +++ b/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h @@ -9,9 +9,9 @@ using gpioId_t = uint16_t; namespace gpio { -enum class Levels : uint8_t { LOW = 0, HIGH = 1, NONE = 99 }; +enum class Levels : int { LOW = 0, HIGH = 1, NONE = 99 }; -enum class Direction : uint8_t { DIR_IN = 0, DIR_OUT = 1 }; +enum class Direction : int { IN = 0, OUT = 1 }; enum class GpioOperation { READ, WRITE }; @@ -20,7 +20,7 @@ enum class GpioTypes { GPIO_REGULAR_BY_CHIP, GPIO_REGULAR_BY_LABEL, GPIO_REGULAR_BY_LINE_NAME, - TYPE_CALLBACK + CALLBACK }; static constexpr gpioId_t NO_GPIO = -1; @@ -57,7 +57,7 @@ class GpioBase { // Can be used to cast GpioBase to a concrete child implementation gpio::GpioTypes gpioType = gpio::GpioTypes::NONE; std::string consumer; - gpio::Direction direction = gpio::Direction::DIR_IN; + gpio::Direction direction = gpio::Direction::IN; gpio::Levels initValue = gpio::Levels::NONE; }; @@ -80,7 +80,7 @@ class GpiodRegularByChip : public GpiodRegularBase { public: GpiodRegularByChip() : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP, std::string(), - gpio::Direction::DIR_IN, gpio::LOW, 0) {} + gpio::Direction::IN, gpio::Levels::LOW, 0) {} GpiodRegularByChip(std::string chipname_, int lineNum_, std::string consumer_, gpio::Direction direction_, gpio::Levels initValue_) @@ -89,8 +89,8 @@ class GpiodRegularByChip : public GpiodRegularBase { chipname(chipname_) {} GpiodRegularByChip(std::string chipname_, int lineNum_, std::string consumer_) - : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP, consumer_, gpio::Direction::DIR_IN, - gpio::LOW, lineNum_), + : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP, consumer_, gpio::Direction::IN, + gpio::Levels::LOW, lineNum_), chipname(chipname_) {} std::string chipname; @@ -105,8 +105,8 @@ class GpiodRegularByLabel : public GpiodRegularBase { label(label_) {} GpiodRegularByLabel(std::string label_, int lineNum_, std::string consumer_) - : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LABEL, consumer_, gpio::Direction::DIR_IN, - gpio::LOW, lineNum_), + : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LABEL, consumer_, gpio::Direction::IN, + 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::DIR_IN, gpio::LOW), + gpio::Direction::IN, gpio::Levels::LOW), lineName(lineName_) {} std::string lineName; @@ -137,7 +137,7 @@ class GpioCallback : public GpioBase { public: GpioCallback(std::string consumer, gpio::Direction direction_, gpio::Levels initValue_, gpio::gpio_cb_t callback, void* callbackArgs) - : GpioBase(gpio::GpioTypes::TYPE_CALLBACK, consumer, direction_, initValue_), + : GpioBase(gpio::GpioTypes::CALLBACK, consumer, direction_, initValue_), callback(callback), callbackArgs(callbackArgs) {} diff --git a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp index fa9eeb5a..5a8b3c10 100644 --- a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp +++ b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp @@ -74,7 +74,7 @@ ReturnValue_t LinuxLibgpioIF::configureGpios(GpioMap& mapToAdd) { configureGpioByLineName(gpioConfig.first, *regularGpio); break; } - case (gpio::GpioTypes::TYPE_CALLBACK): { + case (gpio::GpioTypes::CALLBACK): { auto gpioCallback = dynamic_cast(gpioConfig.second); if (gpioCallback->callback == nullptr) { return GPIO_INVALID_INSTANCE; @@ -161,11 +161,12 @@ ReturnValue_t LinuxLibgpioIF::configureRegularGpio(gpioId_t gpioId, struct gpiod consumer = regularGpio.consumer; /* Configure direction and add a description to the GPIO */ switch (direction) { - case (gpio::DIR_OUT): { - result = gpiod_line_request_output(lineHandle, consumer.c_str(), regularGpio.initValue); + case (gpio::Direction::OUT): { + result = gpiod_line_request_output(lineHandle, consumer.c_str(), + static_cast(regularGpio.initValue)); break; } - case (gpio::DIR_IN): { + case (gpio::Direction::IN): { result = gpiod_line_request_input(lineHandle, consumer.c_str()); break; } @@ -243,7 +244,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,7 +259,7 @@ 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 From 6d825a1aa645f9003477f2f2e118c042cf531ced Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Feb 2022 15:16:24 +0100 Subject: [PATCH 50/81] some fixes --- hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp index 5a8b3c10..9a795123 100644 --- a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp +++ b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp @@ -212,7 +212,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) { @@ -263,7 +263,7 @@ ReturnValue_t LinuxLibgpioIF::driveGpio(gpioId_t gpioId, GpiodRegularBase& regul 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 " @@ -327,7 +327,7 @@ ReturnValue_t LinuxLibgpioIF::checkForConflicts(GpioMap& mapToAdd) { } break; } - case (gpio::GpioTypes::TYPE_CALLBACK): { + case (gpio::GpioTypes::CALLBACK): { auto callbackGpio = dynamic_cast(gpioConfig.second); if (callbackGpio == nullptr) { return GPIO_TYPE_FAILURE; @@ -367,13 +367,13 @@ ReturnValue_t LinuxLibgpioIF::checkForConflictsById(gpioId_t gpioIdToCheck, case (gpio::GpioTypes::GPIO_REGULAR_BY_CHIP): case (gpio::GpioTypes::GPIO_REGULAR_BY_LABEL): case (gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME): { - if (gpioType == gpio::GpioTypes::NONE or gpioType == gpio::GpioTypes::TYPE_CALLBACK) { + if (gpioType == gpio::GpioTypes::NONE or gpioType == gpio::GpioTypes::CALLBACK) { eraseDuplicateDifferentType = true; } break; } - case (gpio::GpioTypes::TYPE_CALLBACK): { - if (gpioType != gpio::GpioTypes::TYPE_CALLBACK) { + case (gpio::GpioTypes::CALLBACK): { + if (gpioType != gpio::GpioTypes::CALLBACK) { eraseDuplicateDifferentType = true; } } From 68225586d2dbd12b05f5bff0689201fc1f860f03 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Feb 2022 15:16:24 +0100 Subject: [PATCH 51/81] some fixes --- hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp index 5a8b3c10..9a795123 100644 --- a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp +++ b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp @@ -212,7 +212,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) { @@ -263,7 +263,7 @@ ReturnValue_t LinuxLibgpioIF::driveGpio(gpioId_t gpioId, GpiodRegularBase& regul 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 " @@ -327,7 +327,7 @@ ReturnValue_t LinuxLibgpioIF::checkForConflicts(GpioMap& mapToAdd) { } break; } - case (gpio::GpioTypes::TYPE_CALLBACK): { + case (gpio::GpioTypes::CALLBACK): { auto callbackGpio = dynamic_cast(gpioConfig.second); if (callbackGpio == nullptr) { return GPIO_TYPE_FAILURE; @@ -367,13 +367,13 @@ ReturnValue_t LinuxLibgpioIF::checkForConflictsById(gpioId_t gpioIdToCheck, case (gpio::GpioTypes::GPIO_REGULAR_BY_CHIP): case (gpio::GpioTypes::GPIO_REGULAR_BY_LABEL): case (gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME): { - if (gpioType == gpio::GpioTypes::NONE or gpioType == gpio::GpioTypes::TYPE_CALLBACK) { + if (gpioType == gpio::GpioTypes::NONE or gpioType == gpio::GpioTypes::CALLBACK) { eraseDuplicateDifferentType = true; } break; } - case (gpio::GpioTypes::TYPE_CALLBACK): { - if (gpioType != gpio::GpioTypes::TYPE_CALLBACK) { + case (gpio::GpioTypes::CALLBACK): { + if (gpioType != gpio::GpioTypes::CALLBACK) { eraseDuplicateDifferentType = true; } } From e05e203c83825b4069bb1391005d46f041b08455 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Feb 2022 15:50:27 +0100 Subject: [PATCH 52/81] fix merge conflict --- src/fsfw/rmap/RMAPChannelIF.h | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/src/fsfw/rmap/RMAPChannelIF.h b/src/fsfw/rmap/RMAPChannelIF.h index 752400e5..9e666dfb 100644 --- a/src/fsfw/rmap/RMAPChannelIF.h +++ b/src/fsfw/rmap/RMAPChannelIF.h @@ -75,21 +75,11 @@ class RMAPChannelIF { * - @c RETURN_OK * - @c COMMAND_NO_DESCRIPTORS_AVAILABLE no descriptors available for sending * command; command was not sent -<<<<<<< HEAD - * - @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 ->>>>>>> origin/eive/develop * - @c COMMAND_CHANNEL_DEACTIVATED the channel has no port set * - @c NOT_SUPPORTED if you dont feel like * implementing something... @@ -107,14 +97,8 @@ the * - @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 -<<<<<<< HEAD - * - @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) ->>>>>>> origin/eive/develop * - @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) From 0ccaf27fcb0e7a0f2dd1ca7175a7051a0267c8ec Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 1 Mar 2022 19:43:21 +0100 Subject: [PATCH 53/81] better printout for parameter code --- .../fsfw_hal/common/gpio/gpioDefinitions.h | 8 ++++---- .../fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp | 2 +- hal/src/fsfw_hal/linux/spi/SpiComIF.cpp | 1 - src/fsfw/parameters/HasParametersIF.h | 2 +- src/fsfw/parameters/ParameterWrapper.cpp | 20 +++++++++++-------- src/fsfw/rmap/RMAP.h | 8 ++++---- src/fsfw/rmap/RMAPChannelIF.h | 12 +++++------ 7 files changed, 28 insertions(+), 25 deletions(-) diff --git a/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h b/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h index 7c81a1db..eb90629e 100644 --- a/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h +++ b/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h @@ -79,8 +79,8 @@ class GpiodRegularBase : public GpioBase { class GpiodRegularByChip : public GpiodRegularBase { public: GpiodRegularByChip() - : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP, std::string(), - gpio::Direction::IN, gpio::Levels::LOW, 0) {} + : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP, std::string(), gpio::Direction::IN, + gpio::Levels::LOW, 0) {} GpiodRegularByChip(std::string chipname_, int lineNum_, std::string consumer_, gpio::Direction direction_, gpio::Levels initValue_) @@ -126,8 +126,8 @@ class GpiodRegularByLineName : public GpiodRegularBase { lineName(lineName_) {} GpiodRegularByLineName(std::string lineName_, std::string consumer_) - : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME, consumer_, - gpio::Direction::IN, gpio::Levels::LOW), + : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME, consumer_, gpio::Direction::IN, + gpio::Levels::LOW), lineName(lineName_) {} std::string lineName; diff --git a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp index 9a795123..3b9a21f7 100644 --- a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp +++ b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp @@ -163,7 +163,7 @@ ReturnValue_t LinuxLibgpioIF::configureRegularGpio(gpioId_t gpioId, struct gpiod switch (direction) { case (gpio::Direction::OUT): { result = gpiod_line_request_output(lineHandle, consumer.c_str(), - static_cast(regularGpio.initValue)); + static_cast(regularGpio.initValue)); break; } case (gpio::Direction::IN): { diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp index e1c631a2..dcf92b5d 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -409,5 +409,4 @@ void SpiComIF::setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed) if (retval != 0) { utility::handleIoctlError("SpiComIF::setSpiSpeedAndMode: Updating SPI default clock failed"); } - } diff --git a/src/fsfw/parameters/HasParametersIF.h b/src/fsfw/parameters/HasParametersIF.h index 48557b4a..e95b69ab 100644 --- a/src/fsfw/parameters/HasParametersIF.h +++ b/src/fsfw/parameters/HasParametersIF.h @@ -66,7 +66,7 @@ class HasParametersIF { * @param newValues * @param startAtIndex Linear index, runs left to right, top to bottom for * matrix indexes. - * @return + * @return RETURN_OK if parameter is valid and a set function of the parameter wrapper was called. */ virtual ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueIdentifier, ParameterWrapper *parameterWrapper, diff --git a/src/fsfw/parameters/ParameterWrapper.cpp b/src/fsfw/parameters/ParameterWrapper.cpp index 27552290..e772f1b6 100644 --- a/src/fsfw/parameters/ParameterWrapper.cpp +++ b/src/fsfw/parameters/ParameterWrapper.cpp @@ -211,9 +211,13 @@ ReturnValue_t ParameterWrapper::copyFrom(const ParameterWrapper *from, if (data == nullptr) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "ParameterWrapper::copyFrom: Called on read-only variable!" << std::endl; + sif::warning << "ParameterWrapper::copyFrom: Called on read-only variable or " + "data pointer not set" + << std::endl; #else - sif::printWarning("ParameterWrapper::copyFrom: Called on read-only variable!\n"); + sif::printWarning( + "ParameterWrapper::copyFrom: Called on read-only variable " + "or data pointer not set\n"); #endif #endif /* FSFW_VERBOSE_LEVEL >= 1 */ return READONLY; @@ -222,9 +226,9 @@ ReturnValue_t ParameterWrapper::copyFrom(const ParameterWrapper *from, if (from->readonlyData == nullptr) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "ParameterWrapper::copyFrom: Source not set!" << std::endl; + sif::warning << "ParameterWrapper::copyFrom: Source not set" << std::endl; #else - sif::printWarning("ParameterWrapper::copyFrom: Source not set!\n"); + sif::printWarning("ParameterWrapper::copyFrom: Source not set\n"); #endif #endif /* FSFW_VERBOSE_LEVEL >= 1 */ return SOURCE_NOT_SET; @@ -233,9 +237,9 @@ ReturnValue_t ParameterWrapper::copyFrom(const ParameterWrapper *from, if (type != from->type) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "ParameterWrapper::copyFrom: Datatype missmatch!" << std::endl; + sif::warning << "ParameterWrapper::copyFrom: Datatype missmatch" << std::endl; #else - sif::printWarning("ParameterWrapper::copyFrom: Datatype missmatch!\n"); + sif::printWarning("ParameterWrapper::copyFrom: Datatype missmatch\n"); #endif #endif /* FSFW_VERBOSE_LEVEL >= 1 */ return DATATYPE_MISSMATCH; @@ -245,9 +249,9 @@ ReturnValue_t ParameterWrapper::copyFrom(const ParameterWrapper *from, if (rows == 0 or columns == 0) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "ParameterWrapper::copyFrom: Columns or rows zero!" << std::endl; + sif::warning << "ParameterWrapper::copyFrom: Columns or rows zero" << std::endl; #else - sif::printWarning("ParameterWrapper::copyFrom: Columns or rows zero!\n"); + sif::printWarning("ParameterWrapper::copyFrom: Columns or rows zero\n"); #endif #endif /* FSFW_VERBOSE_LEVEL >= 1 */ return COLUMN_OR_ROWS_ZERO; diff --git a/src/fsfw/rmap/RMAP.h b/src/fsfw/rmap/RMAP.h index 7c654262..d274fb15 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 7dab07c1..0c937dc8 100644 --- a/src/fsfw/rmap/RMAPChannelIF.h +++ b/src/fsfw/rmap/RMAPChannelIF.h @@ -75,10 +75,10 @@ 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 + * - @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 @@ -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) From 4e6c1cb72ad623208ea8e08349f6ab39bfa45ed0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Mar 2022 00:55:41 +0100 Subject: [PATCH 54/81] docs --- src/fsfw/devicehandlers/AssemblyBase.h | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/fsfw/devicehandlers/AssemblyBase.h b/src/fsfw/devicehandlers/AssemblyBase.h index 3e235928..58b280f1 100644 --- a/src/fsfw/devicehandlers/AssemblyBase.h +++ b/src/fsfw/devicehandlers/AssemblyBase.h @@ -120,8 +120,19 @@ class AssemblyBase : public SubsystemBase { virtual ReturnValue_t handleHealthReply(CommandMessage *message); - virtual void performChildOperation(); + /** + * @brief Default periodic handler + * @details + * This is the default periodic handler which will be called by the SubsystemBase + * performOperation. It performs the child transitions or reacts to changed health/mode states + * of children objects + */ + virtual void performChildOperation() override; + /** + * This function handles changed mode or health states of children + * @return + */ bool handleChildrenChanged(); /** From 45f0d7fd453eafddbc8a364e6c61a90b5f577c85 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Mar 2022 18:06:57 +0100 Subject: [PATCH 55/81] docs --- src/fsfw/devicehandlers/AssemblyBase.h | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/src/fsfw/devicehandlers/AssemblyBase.h b/src/fsfw/devicehandlers/AssemblyBase.h index 58b280f1..f6192533 100644 --- a/src/fsfw/devicehandlers/AssemblyBase.h +++ b/src/fsfw/devicehandlers/AssemblyBase.h @@ -53,7 +53,7 @@ class AssemblyBase : public SubsystemBase { * @param mode * @param submode * @return - * - @c RETURN_OK if ok + * - @c RETURN_OK if OK * - @c NEED_SECOND_STEP if children need to be commanded again */ virtual ReturnValue_t commandChildren(Mode_t mode, Submode_t submode) = 0; @@ -145,12 +145,31 @@ class AssemblyBase : public SubsystemBase { bool handleChildrenChangedHealth(); + /** + * Core transition handler. The default implementation will only do something if + * #commandsOutstanding is smaller or equal to zero, which means that all mode commands + * from the #doPerformTransition call were executed successfully. + * + * Unless a second step was requested, the function will then use #checkChildrenState to + * determine whether the target mode was reached. + * + * There is some special handling for certain (internal) modes: + * - A second step is necessary. #commandChildren will be performed again + * - The device health was overwritten. #commandChildren will be called + * - A recovery is ongoing. #checkAndHandleRecovery will be called. + */ virtual void handleChildrenTransition(); ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, uint32_t *msToReachTheMode); virtual void startTransition(Mode_t mode, Submode_t submode); + /** + * This function starts the transition by setting the internal #targetSubmode and #targetMode + * variables and then calling the #commandChildren function. + * @param mode + * @param submode + */ virtual void doStartTransition(Mode_t mode, Submode_t submode); virtual bool isInTransition(); From 3c53e2c259c43d2ebcc8fc3642fbb6bff84093c6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 5 Mar 2022 03:01:43 +0100 Subject: [PATCH 56/81] renamed some ModeIF definitions --- src/fsfw/devicehandlers/AssemblyBase.cpp | 2 +- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 2 +- src/fsfw/modes/HasModesIF.h | 10 +++++++--- src/fsfw/subsystem/SubsystemBase.cpp | 3 ++- .../fsfw_tests/integration/assemblies/TestAssembly.cpp | 2 +- 5 files changed, 12 insertions(+), 7 deletions(-) diff --git a/src/fsfw/devicehandlers/AssemblyBase.cpp b/src/fsfw/devicehandlers/AssemblyBase.cpp index c29022e5..45f62ba0 100644 --- a/src/fsfw/devicehandlers/AssemblyBase.cpp +++ b/src/fsfw/devicehandlers/AssemblyBase.cpp @@ -196,7 +196,7 @@ ReturnValue_t AssemblyBase::checkModeCommand(Mode_t mode, Submode_t submode, } if ((mode != MODE_ON) && (mode != DeviceHandlerIF::MODE_NORMAL)) { - return INVALID_MODE; + return INVALID_MODE_RETVAL; } if (internalState != STATE_NONE) { diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 1e87e8a7..65c5a946 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -403,7 +403,7 @@ ReturnValue_t DeviceHandlerBase::isModeCombinationValid(Mode_t mode, Submode_t s return INVALID_SUBMODE; } default: - return HasModesIF::INVALID_MODE; + return HasModesIF::INVALID_MODE_RETVAL; } } diff --git a/src/fsfw/modes/HasModesIF.h b/src/fsfw/modes/HasModesIF.h index 850d4349..d59e0294 100644 --- a/src/fsfw/modes/HasModesIF.h +++ b/src/fsfw/modes/HasModesIF.h @@ -13,7 +13,7 @@ class HasModesIF { public: static const uint8_t INTERFACE_ID = CLASS_ID::HAS_MODES_IF; - static const ReturnValue_t INVALID_MODE = MAKE_RETURN_CODE(0x01); + static const ReturnValue_t INVALID_MODE_RETVAL = MAKE_RETURN_CODE(0x01); static const ReturnValue_t TRANS_NOT_ALLOWED = MAKE_RETURN_CODE(0x02); static const ReturnValue_t IN_TRANSITION = MAKE_RETURN_CODE(0x03); static const ReturnValue_t INVALID_SUBMODE = MAKE_RETURN_CODE(0x04); @@ -37,9 +37,13 @@ class HasModesIF { //! The device is powered and ready to perform operations. In this mode, no commands are //! sent by the device handler itself, but direct commands van be commanded and will be //! interpreted - static const Mode_t MODE_ON = 1; + static constexpr Mode_t MODE_ON = 1; //! The device is powered off. The only command accepted in this mode is a mode change to on. - static const Mode_t MODE_OFF = 0; + static constexpr Mode_t MODE_OFF = 0; + + static constexpr Mode_t INVALID_MODE = -1; + static constexpr Mode_t UNDEFINED_MODE = -2; + //! To avoid checks against magic number "0". static const Submode_t SUBMODE_NONE = 0; diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp index f177ecd4..71c47c5f 100644 --- a/src/fsfw/subsystem/SubsystemBase.cpp +++ b/src/fsfw/subsystem/SubsystemBase.cpp @@ -33,8 +33,9 @@ ReturnValue_t SubsystemBase::registerChild(object_id_t objectId) { info.mode = MODE_OFF; } } else { + // intentional to force an initial command during system startup info.commandQueue = child->getCommandQueue(); - info.mode = -1; // intentional to force an initial command during system startup + info.mode = HasModesIF::UNDEFINED_MODE; } info.submode = SUBMODE_NONE; diff --git a/tests/src/fsfw_tests/integration/assemblies/TestAssembly.cpp b/tests/src/fsfw_tests/integration/assemblies/TestAssembly.cpp index a0313f96..8e79bbc6 100644 --- a/tests/src/fsfw_tests/integration/assemblies/TestAssembly.cpp +++ b/tests/src/fsfw_tests/integration/assemblies/TestAssembly.cpp @@ -125,7 +125,7 @@ ReturnValue_t TestAssembly::isModeCombinationValid(Mode_t mode, Submode_t submod return INVALID_SUBMODE; } } - return INVALID_MODE; + return INVALID_MODE_RETVAL; } ReturnValue_t TestAssembly::initialize() { From a7cb2d435456235c28a5041ae783eeee6426ee6d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 7 Mar 2022 15:54:56 +0100 Subject: [PATCH 57/81] small test device handler fixes --- .../fsfw_tests/integration/devices/TestDeviceHandler.cpp | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/tests/src/fsfw_tests/integration/devices/TestDeviceHandler.cpp b/tests/src/fsfw_tests/integration/devices/TestDeviceHandler.cpp index 41098723..cd15d6e0 100644 --- a/tests/src/fsfw_tests/integration/devices/TestDeviceHandler.cpp +++ b/tests/src/fsfw_tests/integration/devices/TestDeviceHandler.cpp @@ -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; From 8b1c277c58b608db34bf221a1a79469a809ecbda Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Mar 2022 09:34:22 +0100 Subject: [PATCH 58/81] better name of invalid mode retval --- src/fsfw/devicehandlers/AssemblyBase.cpp | 2 +- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 2 +- src/fsfw/modes/HasModesIF.h | 2 +- tests/src/fsfw_tests/integration/assemblies/TestAssembly.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/fsfw/devicehandlers/AssemblyBase.cpp b/src/fsfw/devicehandlers/AssemblyBase.cpp index 45f62ba0..837f5bcb 100644 --- a/src/fsfw/devicehandlers/AssemblyBase.cpp +++ b/src/fsfw/devicehandlers/AssemblyBase.cpp @@ -196,7 +196,7 @@ ReturnValue_t AssemblyBase::checkModeCommand(Mode_t mode, Submode_t submode, } if ((mode != MODE_ON) && (mode != DeviceHandlerIF::MODE_NORMAL)) { - return INVALID_MODE_RETVAL; + return MOVE_IS_INVALID; } if (internalState != STATE_NONE) { diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 6213ea86..7cfd38eb 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -403,7 +403,7 @@ ReturnValue_t DeviceHandlerBase::isModeCombinationValid(Mode_t mode, Submode_t s return INVALID_SUBMODE; } default: - return HasModesIF::INVALID_MODE_RETVAL; + return HasModesIF::MOVE_IS_INVALID; } } diff --git a/src/fsfw/modes/HasModesIF.h b/src/fsfw/modes/HasModesIF.h index d59e0294..9295d2b9 100644 --- a/src/fsfw/modes/HasModesIF.h +++ b/src/fsfw/modes/HasModesIF.h @@ -13,7 +13,7 @@ class HasModesIF { public: static const uint8_t INTERFACE_ID = CLASS_ID::HAS_MODES_IF; - static const ReturnValue_t INVALID_MODE_RETVAL = MAKE_RETURN_CODE(0x01); + static const ReturnValue_t MOVE_IS_INVALID = MAKE_RETURN_CODE(0x01); static const ReturnValue_t TRANS_NOT_ALLOWED = MAKE_RETURN_CODE(0x02); static const ReturnValue_t IN_TRANSITION = MAKE_RETURN_CODE(0x03); static const ReturnValue_t INVALID_SUBMODE = MAKE_RETURN_CODE(0x04); diff --git a/tests/src/fsfw_tests/integration/assemblies/TestAssembly.cpp b/tests/src/fsfw_tests/integration/assemblies/TestAssembly.cpp index 8e79bbc6..b410ab36 100644 --- a/tests/src/fsfw_tests/integration/assemblies/TestAssembly.cpp +++ b/tests/src/fsfw_tests/integration/assemblies/TestAssembly.cpp @@ -125,7 +125,7 @@ ReturnValue_t TestAssembly::isModeCombinationValid(Mode_t mode, Submode_t submod return INVALID_SUBMODE; } } - return INVALID_MODE_RETVAL; + return MOVE_IS_INVALID; } ReturnValue_t TestAssembly::initialize() { From 84f95e8d7641be7a83faf1cbee718c6cc5de152d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Mar 2022 09:45:58 +0100 Subject: [PATCH 59/81] this is better --- src/fsfw/devicehandlers/AssemblyBase.cpp | 2 +- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 2 +- src/fsfw/modes/HasModesIF.h | 6 +++--- src/fsfw/subsystem/SubsystemBase.cpp | 2 +- .../src/fsfw_tests/integration/assemblies/TestAssembly.cpp | 2 +- 5 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/fsfw/devicehandlers/AssemblyBase.cpp b/src/fsfw/devicehandlers/AssemblyBase.cpp index 837f5bcb..c29022e5 100644 --- a/src/fsfw/devicehandlers/AssemblyBase.cpp +++ b/src/fsfw/devicehandlers/AssemblyBase.cpp @@ -196,7 +196,7 @@ ReturnValue_t AssemblyBase::checkModeCommand(Mode_t mode, Submode_t submode, } if ((mode != MODE_ON) && (mode != DeviceHandlerIF::MODE_NORMAL)) { - return MOVE_IS_INVALID; + return INVALID_MODE; } if (internalState != STATE_NONE) { diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 7cfd38eb..03f8da54 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -403,7 +403,7 @@ ReturnValue_t DeviceHandlerBase::isModeCombinationValid(Mode_t mode, Submode_t s return INVALID_SUBMODE; } default: - return HasModesIF::MOVE_IS_INVALID; + return HasModesIF::INVALID_MODE; } } diff --git a/src/fsfw/modes/HasModesIF.h b/src/fsfw/modes/HasModesIF.h index 9295d2b9..57c4fb66 100644 --- a/src/fsfw/modes/HasModesIF.h +++ b/src/fsfw/modes/HasModesIF.h @@ -13,7 +13,7 @@ class HasModesIF { public: static const uint8_t INTERFACE_ID = CLASS_ID::HAS_MODES_IF; - static const ReturnValue_t MOVE_IS_INVALID = MAKE_RETURN_CODE(0x01); + static const ReturnValue_t INVALID_MODE = MAKE_RETURN_CODE(0x01); static const ReturnValue_t TRANS_NOT_ALLOWED = MAKE_RETURN_CODE(0x02); static const ReturnValue_t IN_TRANSITION = MAKE_RETURN_CODE(0x03); static const ReturnValue_t INVALID_SUBMODE = MAKE_RETURN_CODE(0x04); @@ -41,8 +41,8 @@ class HasModesIF { //! The device is powered off. The only command accepted in this mode is a mode change to on. static constexpr Mode_t MODE_OFF = 0; - static constexpr Mode_t INVALID_MODE = -1; - static constexpr Mode_t UNDEFINED_MODE = -2; + static constexpr Mode_t MODE_INVALID = -1; + static constexpr Mode_t MODE_UNDEFINED = -2; //! To avoid checks against magic number "0". static const Submode_t SUBMODE_NONE = 0; diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp index 71c47c5f..aec96434 100644 --- a/src/fsfw/subsystem/SubsystemBase.cpp +++ b/src/fsfw/subsystem/SubsystemBase.cpp @@ -35,7 +35,7 @@ ReturnValue_t SubsystemBase::registerChild(object_id_t objectId) { } else { // intentional to force an initial command during system startup info.commandQueue = child->getCommandQueue(); - info.mode = HasModesIF::UNDEFINED_MODE; + info.mode = HasModesIF::MODE_UNDEFINED; } info.submode = SUBMODE_NONE; diff --git a/tests/src/fsfw_tests/integration/assemblies/TestAssembly.cpp b/tests/src/fsfw_tests/integration/assemblies/TestAssembly.cpp index b410ab36..a0313f96 100644 --- a/tests/src/fsfw_tests/integration/assemblies/TestAssembly.cpp +++ b/tests/src/fsfw_tests/integration/assemblies/TestAssembly.cpp @@ -125,7 +125,7 @@ ReturnValue_t TestAssembly::isModeCombinationValid(Mode_t mode, Submode_t submod return INVALID_SUBMODE; } } - return MOVE_IS_INVALID; + return INVALID_MODE; } ReturnValue_t TestAssembly::initialize() { From dba3c27b99c4f7473909a27af7c60b1d8a31c10c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Mar 2022 09:56:24 +0100 Subject: [PATCH 60/81] define FSFW_DISABLE_PRINTOUT in any case --- src/fsfw/FSFW.h.in | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/fsfw/FSFW.h.in b/src/fsfw/FSFW.h.in index 9cfeab61..953677bd 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 */ From 155d66e534f2e228378135ff779c8a4e0c39c253 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Mar 2022 09:56:24 +0100 Subject: [PATCH 61/81] define FSFW_DISABLE_PRINTOUT in any case --- src/fsfw/FSFW.h.in | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/fsfw/FSFW.h.in b/src/fsfw/FSFW.h.in index 7e52b646..af75e726 100644 --- a/src/fsfw/FSFW.h.in +++ b/src/fsfw/FSFW.h.in @@ -26,6 +26,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 */ From 7ca6d1a695e72b0a606bfbb393fb53dcdf44b5b2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Mar 2022 10:05:18 +0100 Subject: [PATCH 62/81] function to get fsfw version --- src/fsfw/CMakeLists.txt | 4 ++++ src/fsfw/version.cpp | 8 ++++++++ src/fsfw/version.h | 18 ++++++++++++++++++ 3 files changed, 30 insertions(+) create mode 100644 src/fsfw/version.cpp create mode 100644 src/fsfw/version.h diff --git a/src/fsfw/CMakeLists.txt b/src/fsfw/CMakeLists.txt index 12c673ed..efb9f6c7 100644 --- a/src/fsfw/CMakeLists.txt +++ b/src/fsfw/CMakeLists.txt @@ -1,3 +1,7 @@ +target_sources(${LIB_FSFW_NAME} PRIVATE + version.cpp +) + # Core add_subdirectory(action) diff --git a/src/fsfw/version.cpp b/src/fsfw/version.cpp new file mode 100644 index 00000000..1bdd6cc7 --- /dev/null +++ b/src/fsfw/version.cpp @@ -0,0 +1,8 @@ +#include "version.h" +#include "fsfw/FSFWVersion.h" + +void fsfw::getVersion(Version& v) { + v.major = FSFW_VERSION; + v.minor = FSFW_SUBVERSION; + v.revision = FSFW_REVISION; +} diff --git a/src/fsfw/version.h b/src/fsfw/version.h new file mode 100644 index 00000000..04b7e3ce --- /dev/null +++ b/src/fsfw/version.h @@ -0,0 +1,18 @@ +#ifndef FSFW_SRC_FSFW_VERSION_H_ +#define FSFW_SRC_FSFW_VERSION_H_ + +#include + +namespace fsfw { + +struct Version { + uint32_t major = 0; + uint32_t minor = 0; + uint32_t revision = 0; +}; + +void getVersion(Version& version); + +} + +#endif /* FSFW_SRC_FSFW_VERSION_H_ */ From d63c01b96f2eb031eb4d568879dfee2e2151537a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Mar 2022 11:50:47 +0100 Subject: [PATCH 63/81] set timeout in countdown ctor --- src/fsfw/timemanager/Countdown.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/fsfw/timemanager/Countdown.cpp b/src/fsfw/timemanager/Countdown.cpp index 8ff46f8d..a8ba78cb 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() {} From 47d158156b9efa0edbca9b3a1694f4132b0b30e5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Mar 2022 11:52:33 +0100 Subject: [PATCH 64/81] call setTimeout --- src/fsfw/timemanager/Countdown.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/fsfw/timemanager/Countdown.cpp b/src/fsfw/timemanager/Countdown.cpp index 8ff46f8d..a8ba78cb 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() {} From 45b51f9ac87cd15ad46555198c682fd279daeab0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 9 Mar 2022 19:05:07 +0100 Subject: [PATCH 65/81] improved version.h --- src/fsfw/FSFWVersion.h.in | 6 +++--- src/fsfw/version.cpp | 13 ++++++++----- src/fsfw/version.h | 25 +++++++++++++++++++++++-- 3 files changed, 34 insertions(+), 10 deletions(-) diff --git a/src/fsfw/FSFWVersion.h.in b/src/fsfw/FSFWVersion.h.in index 7935b2f6..19a56214 100644 --- a/src/fsfw/FSFWVersion.h.in +++ b/src/fsfw/FSFWVersion.h.in @@ -2,8 +2,8 @@ #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@ +#define FSFW_VERSION_MAJOR @FSFW_VERSION@ +#define FSFW_VERSION_MINOR @FSFW_SUBVERSION@ +#define FSFW_VERSION_REVISION @FSFW_REVISION@ #endif /* FSFW_VERSION_H_ */ diff --git a/src/fsfw/version.cpp b/src/fsfw/version.cpp index 1bdd6cc7..e1fc32c9 100644 --- a/src/fsfw/version.cpp +++ b/src/fsfw/version.cpp @@ -1,8 +1,11 @@ #include "version.h" + #include "fsfw/FSFWVersion.h" -void fsfw::getVersion(Version& v) { - v.major = FSFW_VERSION; - v.minor = FSFW_SUBVERSION; - v.revision = FSFW_REVISION; -} +const fsfw::Version fsfw::FSFW_VERSION = {FSFW_VERSION_MAJOR, FSFW_VERSION_MINOR, + FSFW_VERSION_REVISION}; + +fsfw::Version::Version(uint32_t major, uint32_t minor, uint32_t revision) + : major(major), minor(minor), revision(revision) {} + +void fsfw::getVersion(Version& version) {} diff --git a/src/fsfw/version.h b/src/fsfw/version.h index 04b7e3ce..8340fc9d 100644 --- a/src/fsfw/version.h +++ b/src/fsfw/version.h @@ -5,14 +5,35 @@ namespace fsfw { -struct Version { +class Version { + public: + Version(uint32_t major, uint32_t minor, uint32_t revision); uint32_t major = 0; uint32_t minor = 0; uint32_t revision = 0; + + 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); } + + 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)); } }; +extern const fsfw::Version FSFW_VERSION; + void getVersion(Version& version); -} +} // namespace fsfw #endif /* FSFW_SRC_FSFW_VERSION_H_ */ From b27f3b84aa37d5c8723f9a0408d2eb40e9302478 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 9 Mar 2022 19:10:05 +0100 Subject: [PATCH 66/81] getter not required anymore --- src/fsfw/version.cpp | 2 -- src/fsfw/version.h | 2 -- 2 files changed, 4 deletions(-) diff --git a/src/fsfw/version.cpp b/src/fsfw/version.cpp index e1fc32c9..133b9674 100644 --- a/src/fsfw/version.cpp +++ b/src/fsfw/version.cpp @@ -7,5 +7,3 @@ const fsfw::Version fsfw::FSFW_VERSION = {FSFW_VERSION_MAJOR, FSFW_VERSION_MINOR fsfw::Version::Version(uint32_t major, uint32_t minor, uint32_t revision) : major(major), minor(minor), revision(revision) {} - -void fsfw::getVersion(Version& version) {} diff --git a/src/fsfw/version.h b/src/fsfw/version.h index 8340fc9d..47860865 100644 --- a/src/fsfw/version.h +++ b/src/fsfw/version.h @@ -32,8 +32,6 @@ class Version { extern const fsfw::Version FSFW_VERSION; -void getVersion(Version& version); - } // namespace fsfw #endif /* FSFW_SRC_FSFW_VERSION_H_ */ From 97bc71a3ff9048e45c5306b02afafd30e783fa48 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 10 Mar 2022 09:34:29 +0100 Subject: [PATCH 67/81] added tests --- src/fsfw/version.h | 28 ++++++++++++ tests/src/fsfw_tests/unit/CMakeLists.txt | 11 +++-- tests/src/fsfw_tests/unit/version.cpp | 55 ++++++++++++++++++++++++ 3 files changed, 88 insertions(+), 6 deletions(-) create mode 100644 tests/src/fsfw_tests/unit/version.cpp diff --git a/src/fsfw/version.h b/src/fsfw/version.h index 47860865..4ed748d1 100644 --- a/src/fsfw/version.h +++ b/src/fsfw/version.h @@ -1,7 +1,13 @@ #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 +#include namespace fsfw { @@ -28,6 +34,28 @@ class Version { 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) { + os << v.major << "." << v.minor << "." << v.revision; + return os; + } +#endif + + /** + * Get version as format "major.minor.revision" + * @param str + * @param maxLen + */ + void getVersion(char* str, size_t maxLen) const { + snprintf(str, maxLen, "%d.%d.%d", major, minor, revision); + } }; extern const fsfw::Version FSFW_VERSION; diff --git a/tests/src/fsfw_tests/unit/CMakeLists.txt b/tests/src/fsfw_tests/unit/CMakeLists.txt index a83e2f7f..a8d31d88 100644 --- a/tests/src/fsfw_tests/unit/CMakeLists.txt +++ b/tests/src/fsfw_tests/unit/CMakeLists.txt @@ -2,14 +2,13 @@ target_sources(${FSFW_TEST_TGT} PRIVATE CatchDefinitions.cpp CatchFactory.cpp printChar.cpp + version.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(action) diff --git a/tests/src/fsfw_tests/unit/version.cpp b/tests/src/fsfw_tests/unit/version.cpp new file mode 100644 index 00000000..6fcff215 --- /dev/null +++ b/tests/src/fsfw_tests/unit/version.cpp @@ -0,0 +1,55 @@ + +#include "fsfw/version.h" + +#include + +#include "fsfw/serviceinterface.h" +#include "fsfw_tests/unit/CatchDefinitions.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); + fsfw::Version v1 = fsfw::Version(1, 1, 1); + fsfw::Version v2 = fsfw::Version(1, 1, 1); + v1.revision -= 1; + REQUIRE(v1 != v2); + REQUIRE(v1 < v2); + REQUIRE(v1 <= v2); + v1.revision += 1; + v1.minor -= 1; + REQUIRE(v1 != v2); + REQUIRE(v1 < v2); + REQUIRE(v1 <= v2); + v1.minor += 1; + v1.major -= 1; + REQUIRE(v1 != v2); + REQUIRE(v1 < v2); + REQUIRE(v1 <= v2); + v1.major += 1; + REQUIRE(v1 == v2); + v1.major += 1; + REQUIRE(v1 != v2); + REQUIRE(v1 > v2); + REQUIRE(v1 >= v2); + v1.major -= 1; + v1.minor += 1; + REQUIRE(v1 != v2); + REQUIRE(v1 > v2); + REQUIRE(v1 >= v2); + v1.minor -= 1; + v1.revision += 1; + REQUIRE(v1 != v2); + REQUIRE(v1 > v2); + REQUIRE(v1 >= v2); + v1.revision -= 1; + REQUIRE(v1 == v2); + sif::info << "v" << fsfw::FSFW_VERSION << std::endl; + char verString[10] = {}; + fsfw::FSFW_VERSION.getVersion(verString, sizeof(verString)); + sif::info << "v" << verString << std::endl; +} From b5d6b9745f3e5fdd10463677ca2acb688cad4e84 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 10 Mar 2022 09:56:23 +0100 Subject: [PATCH 68/81] undef major and minor --- src/fsfw/version.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/fsfw/version.cpp b/src/fsfw/version.cpp index 133b9674..aa777c9e 100644 --- a/src/fsfw/version.cpp +++ b/src/fsfw/version.cpp @@ -2,6 +2,9 @@ #include "fsfw/FSFWVersion.h" +#undef major +#undef minor + const fsfw::Version fsfw::FSFW_VERSION = {FSFW_VERSION_MAJOR, FSFW_VERSION_MINOR, FSFW_VERSION_REVISION}; From fec5f83f4f2459facee25939e0292115f89a6d73 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 11 Mar 2022 14:25:01 +0100 Subject: [PATCH 69/81] minor event changes --- src/fsfw/events/Event.h | 24 ++++++++++++++---------- 1 file changed, 14 insertions(+), 10 deletions(-) diff --git a/src/fsfw/events/Event.h b/src/fsfw/events/Event.h index 20b66679..edc1d838 100644 --- a/src/fsfw/events/Event.h +++ b/src/fsfw/events/Event.h @@ -7,8 +7,19 @@ // 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 +31,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_ */ From d4ade5e885e0edf92158f42760b9a0e58e0310b9 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Mon, 14 Mar 2022 15:01:17 +0100 Subject: [PATCH 70/81] sequence count operator overloading --- src/fsfw/tmtcservices/SourceSequenceCounter.h | 41 ++++++++++++------- 1 file changed, 26 insertions(+), 15 deletions(-) diff --git a/src/fsfw/tmtcservices/SourceSequenceCounter.h b/src/fsfw/tmtcservices/SourceSequenceCounter.h index 2b30a53f..53547e08 100644 --- a/src/fsfw/tmtcservices/SourceSequenceCounter.h +++ b/src/fsfw/tmtcservices/SourceSequenceCounter.h @@ -4,21 +4,32 @@ #include "../tmtcpacket/SpacePacketBase.h" class SourceSequenceCounter { -private: - uint16_t sequenceCount; -public: - SourceSequenceCounter() : sequenceCount(0) {} - void increment() { - sequenceCount = (sequenceCount+1) % (SpacePacketBase::LIMIT_SEQUENCE_COUNT); - } - void decrement() { - sequenceCount = (sequenceCount-1) % (SpacePacketBase::LIMIT_SEQUENCE_COUNT); - } - uint16_t get() { return this->sequenceCount; } - void reset(uint16_t toValue = 0) { - sequenceCount = toValue % (SpacePacketBase::LIMIT_SEQUENCE_COUNT); - } + private: + uint16_t sequenceCount; + + public: + SourceSequenceCounter() : sequenceCount(0) {} + void increment() { + sequenceCount = (sequenceCount + 1) % (SpacePacketBase::LIMIT_SEQUENCE_COUNT); + } + void decrement() { + sequenceCount = (sequenceCount - 1) % (SpacePacketBase::LIMIT_SEQUENCE_COUNT); + } + uint16_t get() { return this->sequenceCount; } + void reset(uint16_t toValue = 0) { + sequenceCount = toValue % (SpacePacketBase::LIMIT_SEQUENCE_COUNT); + } + SourceSequenceCounter& operator++(int) { + this->increment(); + return *this; + } + SourceSequenceCounter& operator=(const uint16_t& newCount) { + sequenceCount = newCount; + return *this; + } + operator uint16_t() { + return this->get(); + } }; - #endif /* FSFW_TMTCSERVICES_SOURCESEQUENCECOUNTER_H_ */ From 543daaa95aa15d85a701c257ecf7c65964ea4d6f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Mar 2022 19:19:02 +0100 Subject: [PATCH 71/81] various tweaks and improvements --- src/fsfw/devicehandlers/AssemblyBase.cpp | 20 +++++++++++++------ src/fsfw/devicehandlers/AssemblyBase.h | 15 ++++++++++---- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 20 ++++++++++++++++--- src/fsfw/devicehandlers/DeviceHandlerBase.h | 4 ++++ src/fsfw/fdir/FailureIsolationBase.cpp | 9 +++++---- 5 files changed, 51 insertions(+), 17 deletions(-) diff --git a/src/fsfw/devicehandlers/AssemblyBase.cpp b/src/fsfw/devicehandlers/AssemblyBase.cpp index c29022e5..414098ad 100644 --- a/src/fsfw/devicehandlers/AssemblyBase.cpp +++ b/src/fsfw/devicehandlers/AssemblyBase.cpp @@ -26,11 +26,7 @@ void AssemblyBase::performChildOperation() { void AssemblyBase::startTransition(Mode_t mode, Submode_t submode) { doStartTransition(mode, submode); - if (modeHelper.isForced()) { - triggerEvent(FORCING_MODE, mode, submode); - } else { - triggerEvent(CHANGING_MODE, mode, submode); - } + triggerModeHelperEvents(mode, submode); } void AssemblyBase::doStartTransition(Mode_t mode, Submode_t submode) { @@ -77,9 +73,10 @@ bool AssemblyBase::handleChildrenChangedHealth() { } HealthState healthState = healthHelper.healthTable->getHealth(iter->first); if (healthState == HasHealthIF::NEEDS_RECOVERY) { - triggerEvent(TRYING_RECOVERY); + triggerEvent(TRYING_RECOVERY, iter->first, 0); recoveryState = RECOVERY_STARTED; recoveringDevice = iter; + // The user needs to take care of commanding the children off in commandChildren doStartTransition(targetMode, targetSubmode); } else { triggerEvent(CHILD_CHANGED_HEALTH); @@ -228,6 +225,9 @@ ReturnValue_t AssemblyBase::handleHealthReply(CommandMessage* message) { bool AssemblyBase::checkAndHandleRecovery() { switch (recoveryState) { case RECOVERY_STARTED: + // The recovery was already start in #handleChildrenChangedHealth and we just need + // to wait for an off time period. + // TODO: make time period configurable recoveryState = RECOVERY_WAIT; recoveryOffTimer.resetTimer(); return true; @@ -266,3 +266,11 @@ void AssemblyBase::overwriteDeviceHealth(object_id_t objectId, HasHealthIF::Heal modeHelper.setForced(true); sendHealthCommand(childrenMap[objectId].commandQueue, EXTERNAL_CONTROL); } + +void AssemblyBase::triggerModeHelperEvents(Mode_t mode, Submode_t submode) { + if (modeHelper.isForced()) { + triggerEvent(FORCING_MODE, mode, submode); + } else { + triggerEvent(CHANGING_MODE, mode, submode); + } +} diff --git a/src/fsfw/devicehandlers/AssemblyBase.h b/src/fsfw/devicehandlers/AssemblyBase.h index f6192533..fd5c6ad6 100644 --- a/src/fsfw/devicehandlers/AssemblyBase.h +++ b/src/fsfw/devicehandlers/AssemblyBase.h @@ -47,9 +47,8 @@ class AssemblyBase : public SubsystemBase { protected: /** - * Command children to reach [mode,submode] combination - * Can be done by setting #commandsOutstanding correctly, - * or using executeTable() + * Command children to reach [mode,submode] combination. Can be done by setting + * #commandsOutstanding correctly, or using #executeTable. * @param mode * @param submode * @return @@ -162,6 +161,12 @@ class AssemblyBase : public SubsystemBase { ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, uint32_t *msToReachTheMode); + /** + * Calls #doStartTransition and triggers an informative event as well that the mode will + * change + * @param mode + * @param submode + */ virtual void startTransition(Mode_t mode, Submode_t submode); /** @@ -190,7 +195,7 @@ class AssemblyBase : public SubsystemBase { * Manages recovery of a device * @return true if recovery is still ongoing, false else. */ - bool checkAndHandleRecovery(); + virtual bool checkAndHandleRecovery(); /** * Helper method to overwrite health state of one of the children. @@ -198,6 +203,8 @@ class AssemblyBase : public SubsystemBase { * @param objectId Must be a registered child. */ void overwriteDeviceHealth(object_id_t objectId, HasHealthIF::HealthState oldHealth); + + void triggerModeHelperEvents(Mode_t mode, Submode_t submode); }; #endif /* FSFW_DEVICEHANDLERS_ASSEMBLYBASE_H_ */ diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 03f8da54..18f8a2f7 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -49,9 +49,6 @@ DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId, object_id_t device printWarningOrError(sif::OutputTypes::OUT_ERROR, "DeviceHandlerBase", HasReturnvaluesIF::RETURN_FAILED, "Invalid cookie"); } - if (this->fdirInstance == nullptr) { - this->fdirInstance = new DeviceHandlerFailureIsolation(setObjectId, defaultFdirParentId); - } } void DeviceHandlerBase::setHkDestination(object_id_t hkDestination) { @@ -127,6 +124,17 @@ ReturnValue_t DeviceHandlerBase::initialize() { if (result != RETURN_OK) { return result; } + if (this->fdirInstance == nullptr) { + this->fdirInstance = new DeviceHandlerFailureIsolation(this->getObjectId(), defaultFdirParentId); + } + + if(this->parent != objects::NO_OBJECT) { + HasModesIF* modeIF = ObjectManager::instance()->get(this->parent); + HasHealthIF* healthIF = ObjectManager::instance()->get(this->parent); + if(modeIF != nullptr and healthIF != nullptr) { + setParentQueue(modeIF->getCommandQueue()); + } + } communicationInterface = ObjectManager::instance()->get(deviceCommunicationId); @@ -1494,3 +1502,9 @@ MessageQueueId_t DeviceHandlerBase::getCommanderQueueId(DeviceCommandId_t replyI } return commandIter->second.sendReplyTo; } + +void DeviceHandlerBase::setCustomFdir(FailureIsolationBase* fdir) { this->fdirInstance = fdir; } + +void DeviceHandlerBase::setParent(object_id_t parent) { this->parent = parent; } + +void DeviceHandlerBase::setPowerSwitcher(PowerSwitchIF* switcher) { this->powerSwitcher = switcher; } diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index bf6900d9..a8de34eb 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -103,6 +103,9 @@ class DeviceHandlerBase : public DeviceHandlerIF, DeviceHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, CookieIF *comCookie, FailureIsolationBase *fdirInstance = nullptr, size_t cmdQueueSize = 20); + void setCustomFdir(FailureIsolationBase* fdir); + void setParent(object_id_t parent); + void setPowerSwitcher(PowerSwitchIF* switcher); void setHkDestination(object_id_t hkDestination); /** @@ -824,6 +827,7 @@ class DeviceHandlerBase : public DeviceHandlerIF, /** Pointer to the used FDIR instance. If not provided by child, * default class is instantiated. */ FailureIsolationBase *fdirInstance; + object_id_t parent = objects::NO_OBJECT; //! To correctly delete the default instance. bool defaultFDIRUsed; diff --git a/src/fsfw/fdir/FailureIsolationBase.cpp b/src/fsfw/fdir/FailureIsolationBase.cpp index 5d04588f..f576ecbd 100644 --- a/src/fsfw/fdir/FailureIsolationBase.cpp +++ b/src/fsfw/fdir/FailureIsolationBase.cpp @@ -53,10 +53,11 @@ ReturnValue_t FailureIsolationBase::initialize() { if (parentIF == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "FailureIsolationBase::intialize: Parent object" - << "invalid." << std::endl; -#endif -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "Make sure it implements ConfirmsFailuresIF." << std::endl; + << "invalid" << std::endl; + sif::error << "Make sure it implements ConfirmsFailuresIF" << std::endl; +#else + sif::printError("FailureIsolationBase::intialize: Parent object invalid\n"); + sif::printError("Make sure it implements ConfirmsFailuresIF\n"); #endif return ObjectManagerIF::CHILD_INIT_FAILED; return RETURN_FAILED; From ddc1cdb1f5d1ee6f532c04b0419e24f8f40566cf Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Mar 2022 19:22:24 +0100 Subject: [PATCH 72/81] additional docs --- src/fsfw/devicehandlers/AssemblyBase.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/fsfw/devicehandlers/AssemblyBase.h b/src/fsfw/devicehandlers/AssemblyBase.h index fd5c6ad6..ab5f2201 100644 --- a/src/fsfw/devicehandlers/AssemblyBase.h +++ b/src/fsfw/devicehandlers/AssemblyBase.h @@ -48,7 +48,9 @@ class AssemblyBase : public SubsystemBase { protected: /** * Command children to reach [mode,submode] combination. Can be done by setting - * #commandsOutstanding correctly, or using #executeTable. + * #commandsOutstanding correctly, or using #executeTable. In case of an FDIR recovery, + * the user needs to ensure that the target devices are healthy. If a device is not healthy, + * a recovery might be on-going and the device needs to be commanded to off first. * @param mode * @param submode * @return From b6ed45a85cae1f1af83f352b4e6f8bb787743cb1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Mar 2022 19:59:16 +0100 Subject: [PATCH 73/81] small form fix --- src/fsfw/fdir/FailureIsolationBase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/fdir/FailureIsolationBase.cpp b/src/fsfw/fdir/FailureIsolationBase.cpp index f576ecbd..c17cf624 100644 --- a/src/fsfw/fdir/FailureIsolationBase.cpp +++ b/src/fsfw/fdir/FailureIsolationBase.cpp @@ -52,7 +52,7 @@ ReturnValue_t FailureIsolationBase::initialize() { ObjectManager::instance()->get(faultTreeParent); if (parentIF == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "FailureIsolationBase::intialize: Parent object" + sif::error << "FailureIsolationBase::intialize: Parent object " << "invalid" << std::endl; sif::error << "Make sure it implements ConfirmsFailuresIF" << std::endl; #else From caf78835b2e4d5e69227e34b453f35bf64f7572c Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Thu, 17 Mar 2022 20:00:17 +0100 Subject: [PATCH 74/81] added -- operator --- src/fsfw/tmtcservices/SourceSequenceCounter.h | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/fsfw/tmtcservices/SourceSequenceCounter.h b/src/fsfw/tmtcservices/SourceSequenceCounter.h index 53547e08..6f2778a4 100644 --- a/src/fsfw/tmtcservices/SourceSequenceCounter.h +++ b/src/fsfw/tmtcservices/SourceSequenceCounter.h @@ -23,13 +23,15 @@ class SourceSequenceCounter { this->increment(); return *this; } + SourceSequenceCounter& operator--(int) { + this->decrement(); + return *this; + } SourceSequenceCounter& operator=(const uint16_t& newCount) { sequenceCount = newCount; return *this; } - operator uint16_t() { - return this->get(); - } + operator uint16_t() { return this->get(); } }; #endif /* FSFW_TMTCSERVICES_SOURCESEQUENCECOUNTER_H_ */ From 927041209bdc4e2ffc6528d4083dbcc9d0ced098 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 21 Mar 2022 09:12:56 +0100 Subject: [PATCH 75/81] added override specifiers --- src/fsfw/subsystem/SubsystemBase.h | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/fsfw/subsystem/SubsystemBase.h b/src/fsfw/subsystem/SubsystemBase.h index 8cfd5be0..20507ce0 100644 --- a/src/fsfw/subsystem/SubsystemBase.h +++ b/src/fsfw/subsystem/SubsystemBase.h @@ -15,7 +15,9 @@ /** * @defgroup subsystems Subsystem Objects - * Contains all Subsystem and Assemblies + * All Subsystem and Assemblies can derive from this class. It contains helper classes to + * perform mode and health handling, which allows OBSW developers to build a mode tree for + * the whole satellite */ class SubsystemBase : public SystemObject, public HasModesIF, @@ -123,11 +125,11 @@ 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(); From d791fc87b7bba11c1baa68d544cd991028ff3330 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Mar 2022 15:49:04 +0100 Subject: [PATCH 76/81] some improvements for PowerSwitcher --- src/fsfw/power/PowerSwitchIF.h | 5 +++-- src/fsfw/power/PowerSwitcher.cpp | 17 +++++------------ src/fsfw/power/PowerSwitcher.h | 15 +++++++-------- src/fsfw/power/definitions.h | 13 +++++++++++++ 4 files changed, 28 insertions(+), 22 deletions(-) create mode 100644 src/fsfw/power/definitions.h diff --git a/src/fsfw/power/PowerSwitchIF.h b/src/fsfw/power/PowerSwitchIF.h index a31d8971..b2c0cb9b 100644 --- a/src/fsfw/power/PowerSwitchIF.h +++ b/src/fsfw/power/PowerSwitchIF.h @@ -1,6 +1,7 @@ #ifndef FSFW_POWER_POWERSWITCHIF_H_ #define FSFW_POWER_POWERSWITCHIF_H_ +#include "definitions.h" #include "../events/Event.h" #include "../returnvalues/HasReturnvaluesIF.h" /** @@ -37,7 +38,7 @@ 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 void sendSwitchCommand(power::Switch_t switchNr, ReturnValue_t onOff) const = 0; /** * Sends a command to the Power Unit to enable a certain fuse. */ @@ -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 0a3f8521..4f8e6cbe 100644 --- a/src/fsfw/power/PowerSwitcher.cpp +++ b/src/fsfw/power/PowerSwitcher.cpp @@ -1,19 +1,12 @@ +#include "definitions.h" #include "fsfw/power/PowerSwitcher.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(); @@ -63,7 +56,7 @@ void PowerSwitcher::turnOff() { } 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 c72c241d..46e8d0fd 100644 --- a/src/fsfw/power/PowerSwitcher.h +++ b/src/fsfw/power/PowerSwitcher.h @@ -14,28 +14,27 @@ 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, uint8_t setSwitch1, uint8_t setSwitch2 = power::NO_SWITCH, State_t setStartState = SWITCH_IS_OFF); - ReturnValue_t initialize(object_id_t powerSwitchId); void turnOn(); void turnOff(); 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/definitions.h b/src/fsfw/power/definitions.h new file mode 100644 index 00000000..1e4fe296 --- /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; + +} + +#endif /* FSFW_SRC_FSFW_POWER_DEFINITIONS_H_ */ From 6dd6f28db0259ec37c351a81ee8d26696f47118a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Mar 2022 15:55:18 +0100 Subject: [PATCH 77/81] added active function --- src/fsfw/power/PowerSwitcher.cpp | 7 +++++++ src/fsfw/power/PowerSwitcher.h | 1 + 2 files changed, 8 insertions(+) diff --git a/src/fsfw/power/PowerSwitcher.cpp b/src/fsfw/power/PowerSwitcher.cpp index 4f8e6cbe..75273af0 100644 --- a/src/fsfw/power/PowerSwitcher.cpp +++ b/src/fsfw/power/PowerSwitcher.cpp @@ -55,6 +55,13 @@ void PowerSwitcher::turnOff() { 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 == power::NO_SWITCH) { return ONE_SWITCH; diff --git a/src/fsfw/power/PowerSwitcher.h b/src/fsfw/power/PowerSwitcher.h index 46e8d0fd..9c3302c7 100644 --- a/src/fsfw/power/PowerSwitcher.h +++ b/src/fsfw/power/PowerSwitcher.h @@ -22,6 +22,7 @@ class PowerSwitcher : public HasReturnvaluesIF { State_t setStartState = SWITCH_IS_OFF); void turnOn(); void turnOff(); + bool active(); void doStateMachine(); State_t getState(); ReturnValue_t checkSwitchState(); From 2823420c46410fe0ce8bd0c412835405a715e485 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Mar 2022 16:10:20 +0100 Subject: [PATCH 78/81] more docs --- src/fsfw/subsystem/Subsystem.h | 6 +++++- src/fsfw/subsystem/SubsystemBase.h | 8 +++++++- 2 files changed, 12 insertions(+), 2 deletions(-) diff --git a/src/fsfw/subsystem/Subsystem.h b/src/fsfw/subsystem/Subsystem.h index 2c78c8cd..953bb682 100644 --- a/src/fsfw/subsystem/Subsystem.h +++ b/src/fsfw/subsystem/Subsystem.h @@ -12,8 +12,12 @@ #include "modes/ModeDefinitions.h" /** - * @brief TODO: documentation missing + * @brief This class extends the SubsystemBase to perform the management of mode tables + * and mode sequences * @details + * This class is able to use mode tables and sequences to command all its children into the + * right mode. Fallback sequences can be used to handle failed transitions or have a fallback + * in case a component can't keep its current mode. */ class Subsystem : public SubsystemBase, public HasModeSequenceIF { public: diff --git a/src/fsfw/subsystem/SubsystemBase.h b/src/fsfw/subsystem/SubsystemBase.h index 20507ce0..0853ed6e 100644 --- a/src/fsfw/subsystem/SubsystemBase.h +++ b/src/fsfw/subsystem/SubsystemBase.h @@ -17,7 +17,12 @@ * @defgroup subsystems Subsystem Objects * All Subsystem and Assemblies can derive from this class. It contains helper classes to * perform mode and health handling, which allows OBSW developers to build a mode tree for - * the whole satellite + * the whole satellite. + * + * Aside from setting up a mode tree and being able to executing mode tables, this class does not + * provide an implementation on what to do with the features. To build a mode tree, helper classes + * like the #AssemblyBase or the #Subsystem class extend and use the functionality of the base + * class. */ class SubsystemBase : public SystemObject, public HasModesIF, @@ -98,6 +103,7 @@ class SubsystemBase : public SystemObject, Submode_t targetSubmode); /** + * This function takes care of sending all according mode commands specified inside a mode table. * We need to know the target Submode, as children are able to inherit the submode * Still, we have a default for all child implementations which do not use submode inheritance */ From bbe21e7e8957a39e6697c6dd4fb2fe044b560044 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Mar 2022 16:14:23 +0100 Subject: [PATCH 79/81] doc additions --- src/fsfw/devicehandlers/AssemblyBase.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/fsfw/devicehandlers/AssemblyBase.h b/src/fsfw/devicehandlers/AssemblyBase.h index ab5f2201..a6e6e3db 100644 --- a/src/fsfw/devicehandlers/AssemblyBase.h +++ b/src/fsfw/devicehandlers/AssemblyBase.h @@ -12,7 +12,8 @@ * Documentation: Dissertation Baetz p.156, 157. * * This class reduces the complexity of controller components which would - * otherwise be needed for the handling of redundant devices. + * otherwise be needed for the handling of redundant devices. However, it can also be used to + * manage the mode keeping and recovery of non-redundant devices * * The template class monitors mode and health state of its children * and checks availability of devices on every detected change. @@ -26,11 +27,9 @@ * * Important: * - * The implementation must call registerChild(object_id_t child) - * for all commanded children during initialization. + * The implementation must call #registerChild for all commanded children during initialization. * The implementation must call the initialization function of the base class. * (This will call the function in SubsystemBase) - * */ class AssemblyBase : public SubsystemBase { public: From 4b5e3e70f7eb65780b697a81ba95ecc2a74a8b84 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 22 Mar 2022 20:35:04 +0100 Subject: [PATCH 80/81] add option to directly check switch state --- src/fsfw/power/PowerSwitcher.cpp | 16 ++++++++++++++-- src/fsfw/power/PowerSwitcher.h | 4 ++-- 2 files changed, 16 insertions(+), 4 deletions(-) diff --git a/src/fsfw/power/PowerSwitcher.cpp b/src/fsfw/power/PowerSwitcher.cpp index 75273af0..83679772 100644 --- a/src/fsfw/power/PowerSwitcher.cpp +++ b/src/fsfw/power/PowerSwitcher.cpp @@ -45,12 +45,24 @@ 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; } diff --git a/src/fsfw/power/PowerSwitcher.h b/src/fsfw/power/PowerSwitcher.h index 9c3302c7..853bbb40 100644 --- a/src/fsfw/power/PowerSwitcher.h +++ b/src/fsfw/power/PowerSwitcher.h @@ -20,8 +20,8 @@ class PowerSwitcher : public HasReturnvaluesIF { static const ReturnValue_t SWITCH_STATE_MISMATCH = MAKE_RETURN_CODE(2); PowerSwitcher(PowerSwitchIF* switcher, uint8_t setSwitch1, uint8_t setSwitch2 = power::NO_SWITCH, State_t setStartState = SWITCH_IS_OFF); - void turnOn(); - void turnOff(); + void turnOn(bool checkCurrentState = true); + void turnOff(bool checkCurrentState = true); bool active(); void doStateMachine(); State_t getState(); From b52f19254b8072a32203eaab91ea3c65b513ba7e Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Mon, 28 Mar 2022 09:12:29 +0200 Subject: [PATCH 81/81] class to abstract gpio handling --- hal/src/fsfw_hal/linux/gpio/Gpio.h | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) create mode 100644 hal/src/fsfw_hal/linux/gpio/Gpio.h diff --git a/hal/src/fsfw_hal/linux/gpio/Gpio.h b/hal/src/fsfw_hal/linux/gpio/Gpio.h new file mode 100644 index 00000000..2ffe2146 --- /dev/null +++ b/hal/src/fsfw_hal/linux/gpio/Gpio.h @@ -0,0 +1,30 @@ +#ifndef FSFW_HAL_SRC_FSFW_HAL_LINUX_GPIO_GPIO_H_ +#define FSFW_HAL_SRC_FSFW_HAL_LINUX_GPIO_GPIO_H_ + +#include "fsfw_hal/common/gpio/GpioIF.h" +#include "fsfw_hal/common/gpio/gpioDefinitions.h" + +/** + * @brief Additional abstraction layer for handling GPIOs. + * + * @author J. Meier + */ +class Gpio { + public: + Gpio(gpioId_t gpioId, GpioIF* gpioIF) : gpioId(gpioId), gpioIF(gpioIF) { + if (gpioIF == nullptr) { + sif::error << "Gpio::Gpio: Invalid GpioIF" << std::endl; + } + } + ReturnValue_t pullHigh() { + return gpioIF->pullHigh(gpioId); + } + ReturnValue_t pullLow() { + return gpioIF->pullLow(gpioId); + } + private: + gpioId_t gpioId = gpio::NO_GPIO; + GpioIF* gpioIF = nullptr; +}; + +#endif /* FSFW_HAL_SRC_FSFW_HAL_LINUX_GPIO_GPIO_H_ */