From d8c5bd125e7bb8af22f9c369aa01e61b501a8651 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 2 Feb 2022 12:02:58 +0100 Subject: [PATCH 001/404] 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 002/404] 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 003/404] 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 004/404] 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 005/404] 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 006/404] 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 007/404] 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 008/404] 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 009/404] 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 010/404] 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 011/404] 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 012/404] 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 013/404] 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 014/404] 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 015/404] 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 016/404] 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 017/404] 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 018/404] 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 019/404] 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 020/404] 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 021/404] 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 022/404] 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 023/404] 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 024/404] 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 025/404] 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 026/404] 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 027/404] 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 028/404] 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 029/404] 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 030/404] 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 031/404] 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 032/404] 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 033/404] 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 034/404] 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 035/404] 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 036/404] 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 037/404] 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 038/404] 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 039/404] 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 040/404] 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 041/404] 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 042/404] 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 043/404] 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 044/404] 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 045/404] 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 046/404] 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 047/404] 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 048/404] 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 049/404] 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 050/404] 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 051/404] 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 052/404] 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 053/404] 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 054/404] 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 055/404] 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 056/404] 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 057/404] 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 058/404] 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 059/404] 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 060/404] 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 061/404] 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 062/404] 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 063/404] 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 064/404] 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 065/404] 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 066/404] 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 067/404] 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 068/404] 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 069/404] 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 070/404] 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 071/404] 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 072/404] 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 073/404] 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 074/404] 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 075/404] 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 076/404] 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 077/404] 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 078/404] 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 079/404] 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 080/404] 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 081/404] 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_ */ From 6ea1eabb2dd3688f2c7db484cd141f15c9f4fe49 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Mar 2022 12:21:25 +0200 Subject: [PATCH 082/404] small order change in DHB --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 18f8a2f7..742fe6f9 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -361,14 +361,12 @@ void DeviceHandlerBase::doStateMachine() { } } break; case _MODE_WAIT_OFF: { - uint32_t currentUptime; - Clock::getUptime(¤tUptime); - if (powerSwitcher == nullptr) { setMode(MODE_OFF); break; } - + uint32_t currentUptime; + Clock::getUptime(¤tUptime); if (currentUptime - timeoutStart >= powerSwitcher->getSwitchDelayMs()) { triggerEvent(MODE_TRANSITION_FAILED, PowerSwitchIF::SWITCH_TIMEOUT, 0); setMode(MODE_ERROR_ON); From 60972228ef7f8a3efaa192e857e409ea80b77efb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Mar 2022 12:47:09 +0200 Subject: [PATCH 083/404] reworked power switch interface --- src/fsfw/power/CMakeLists.txt | 12 ++++---- src/fsfw/power/DummyPowerSwitcher.cpp | 42 +++++++++++++++++++++++++++ src/fsfw/power/DummyPowerSwitcher.h | 31 ++++++++++++++++++++ src/fsfw/power/PowerSwitchIF.h | 4 +-- 4 files changed, 81 insertions(+), 8 deletions(-) create mode 100644 src/fsfw/power/DummyPowerSwitcher.cpp create mode 100644 src/fsfw/power/DummyPowerSwitcher.h diff --git a/src/fsfw/power/CMakeLists.txt b/src/fsfw/power/CMakeLists.txt index 1c625db1..10e4a44d 100644 --- a/src/fsfw/power/CMakeLists.txt +++ b/src/fsfw/power/CMakeLists.txt @@ -1,7 +1,7 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - Fuse.cpp - PowerComponent.cpp - PowerSensor.cpp - PowerSwitcher.cpp +target_sources(${LIB_FSFW_NAME} PRIVATE + Fuse.cpp + PowerComponent.cpp + PowerSensor.cpp + PowerSwitcher.cpp + DummyPowerSwitcher.cpp ) \ No newline at end of file diff --git a/src/fsfw/power/DummyPowerSwitcher.cpp b/src/fsfw/power/DummyPowerSwitcher.cpp new file mode 100644 index 00000000..fe900e72 --- /dev/null +++ b/src/fsfw/power/DummyPowerSwitcher.cpp @@ -0,0 +1,42 @@ +#include "DummyPowerSwitcher.h" + +DummyPowerSwitcher::DummyPowerSwitcher(size_t numberOfSwitches, size_t numberOfFuses, uint32_t switchDelayMs) +: switcherList(numberOfSwitches), fuseList(numberOfFuses), switchDelayMs(switchDelayMs) { +} + +void DummyPowerSwitcher::setInitialSwitcherList(std::vector switcherList) { + this->switcherList = switcherList; +} + +void DummyPowerSwitcher::setInitialFusesList(std::vector fuseList) { + this->fuseList = fuseList; +} + +ReturnValue_t DummyPowerSwitcher::sendSwitchCommand(power::Switch_t switchNr, ReturnValue_t onOff) { + if (switchNr < switcherList.capacity()) { + switcherList[switchNr] = onOff; + } + return RETURN_FAILED; +} + +ReturnValue_t DummyPowerSwitcher::sendFuseOnCommand(uint8_t fuseNr) { + if(fuseNr < fuseList.capacity()) { + fuseList[fuseNr] = FUSE_ON; + } + return RETURN_FAILED; +} + +ReturnValue_t DummyPowerSwitcher::getSwitchState(power::Switch_t switchNr) const { + if (switchNr < switcherList.capacity()) { + return switcherList[switchNr]; + } + return HasReturnvaluesIF::RETURN_FAILED; +} + +ReturnValue_t DummyPowerSwitcher::getFuseState(uint8_t fuseNr) const { + return RETURN_OK; +} + +uint32_t DummyPowerSwitcher::getSwitchDelayMs(void) const { + return 5000; +} diff --git a/src/fsfw/power/DummyPowerSwitcher.h b/src/fsfw/power/DummyPowerSwitcher.h new file mode 100644 index 00000000..39de608d --- /dev/null +++ b/src/fsfw/power/DummyPowerSwitcher.h @@ -0,0 +1,31 @@ +#ifndef FSFW_SRC_FSFW_POWER_DUMMYPOWERSWITCHER_H_ +#define FSFW_SRC_FSFW_POWER_DUMMYPOWERSWITCHER_H_ + +#include "PowerSwitchIF.h" +#include "definitions.h" + +#include +#include + +class DummyPowerSwitcher: public PowerSwitchIF { +public: + DummyPowerSwitcher(size_t numberOfSwitches, size_t numberOfFuses, uint32_t switchDelayMs = 5000); + + void setInitialSwitcherList(std::vector switcherList); + void setInitialFusesList(std::vector switcherList); + + virtual ReturnValue_t sendSwitchCommand(power::Switch_t switchNr, ReturnValue_t onOff) override; + virtual ReturnValue_t sendFuseOnCommand(uint8_t fuseNr) override; + virtual ReturnValue_t getSwitchState(power::Switch_t switchNr) const override; + virtual ReturnValue_t getFuseState(uint8_t fuseNr) const override; + virtual uint32_t getSwitchDelayMs(void) const override; + +private: + std::vector switcherList; + std::vector fuseList; + uint32_t switchDelayMs = 5000; +}; + + + +#endif /* FSFW_SRC_FSFW_POWER_DUMMYPOWERSWITCHER_H_ */ diff --git a/src/fsfw/power/PowerSwitchIF.h b/src/fsfw/power/PowerSwitchIF.h index b2c0cb9b..71c5523d 100644 --- a/src/fsfw/power/PowerSwitchIF.h +++ b/src/fsfw/power/PowerSwitchIF.h @@ -38,11 +38,11 @@ class PowerSwitchIF : public HasReturnvaluesIF { * @param switchNr * @param onOff on == @c SWITCH_ON; off != @c SWITCH_ON */ - virtual void sendSwitchCommand(power::Switch_t switchNr, ReturnValue_t onOff) const = 0; + virtual ReturnValue_t sendSwitchCommand(power::Switch_t switchNr, ReturnValue_t onOff) = 0; /** * Sends a command to the Power Unit to enable a certain fuse. */ - virtual void sendFuseOnCommand(uint8_t fuseNr) const = 0; + virtual ReturnValue_t sendFuseOnCommand(uint8_t fuseNr) = 0; /** * get the state of the Switches. From 79f3c7324a3db5a6d36b19ac41b5eb158a723c50 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Mar 2022 12:59:32 +0200 Subject: [PATCH 084/404] tweaks for dummy power switcher --- src/fsfw/power/DummyPowerSwitcher.cpp | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/fsfw/power/DummyPowerSwitcher.cpp b/src/fsfw/power/DummyPowerSwitcher.cpp index fe900e72..4f18901d 100644 --- a/src/fsfw/power/DummyPowerSwitcher.cpp +++ b/src/fsfw/power/DummyPowerSwitcher.cpp @@ -1,8 +1,8 @@ #include "DummyPowerSwitcher.h" -DummyPowerSwitcher::DummyPowerSwitcher(size_t numberOfSwitches, size_t numberOfFuses, uint32_t switchDelayMs) -: switcherList(numberOfSwitches), fuseList(numberOfFuses), switchDelayMs(switchDelayMs) { -} +DummyPowerSwitcher::DummyPowerSwitcher(size_t numberOfSwitches, size_t numberOfFuses, + uint32_t switchDelayMs) + : switcherList(numberOfSwitches), fuseList(numberOfFuses), switchDelayMs(switchDelayMs) {} void DummyPowerSwitcher::setInitialSwitcherList(std::vector switcherList) { this->switcherList = switcherList; @@ -20,7 +20,7 @@ ReturnValue_t DummyPowerSwitcher::sendSwitchCommand(power::Switch_t switchNr, Re } ReturnValue_t DummyPowerSwitcher::sendFuseOnCommand(uint8_t fuseNr) { - if(fuseNr < fuseList.capacity()) { + if (fuseNr < fuseList.capacity()) { fuseList[fuseNr] = FUSE_ON; } return RETURN_FAILED; @@ -34,9 +34,10 @@ ReturnValue_t DummyPowerSwitcher::getSwitchState(power::Switch_t switchNr) const } ReturnValue_t DummyPowerSwitcher::getFuseState(uint8_t fuseNr) const { - return RETURN_OK; + if (fuseNr < fuseList.capacity()) { + return fuseList[fuseNr]; + } + return HasReturnvaluesIF::RETURN_FAILED; } -uint32_t DummyPowerSwitcher::getSwitchDelayMs(void) const { - return 5000; -} +uint32_t DummyPowerSwitcher::getSwitchDelayMs(void) const { return switchDelayMs; } From 3ea9f999b746963efad591517e80ccd904cb051f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Mar 2022 12:59:51 +0200 Subject: [PATCH 085/404] apply auto-formatter --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 11 ++-- src/fsfw/devicehandlers/DeviceHandlerBase.h | 4 +- src/fsfw/power/DummyPowerSwitcher.h | 14 +++--- src/fsfw/power/PowerSwitchIF.h | 2 +- src/fsfw/power/PowerSwitcher.cpp | 12 ++--- src/fsfw/power/definitions.h | 2 +- src/fsfw/version.cpp | 3 +- src/fsfw/version.h | 2 +- tests/src/fsfw_tests/unit/version.cpp | 50 +++++++++---------- 9 files changed, 51 insertions(+), 49 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 742fe6f9..453ae8ac 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -125,13 +125,14 @@ ReturnValue_t DeviceHandlerBase::initialize() { return result; } if (this->fdirInstance == nullptr) { - this->fdirInstance = new DeviceHandlerFailureIsolation(this->getObjectId(), defaultFdirParentId); + this->fdirInstance = + new DeviceHandlerFailureIsolation(this->getObjectId(), defaultFdirParentId); } - if(this->parent != objects::NO_OBJECT) { + 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) { + if (modeIF != nullptr and healthIF != nullptr) { setParentQueue(modeIF->getCommandQueue()); } } @@ -1505,4 +1506,6 @@ void DeviceHandlerBase::setCustomFdir(FailureIsolationBase* fdir) { this->fdirIn void DeviceHandlerBase::setParent(object_id_t parent) { this->parent = parent; } -void DeviceHandlerBase::setPowerSwitcher(PowerSwitchIF* switcher) { this->powerSwitcher = switcher; } +void DeviceHandlerBase::setPowerSwitcher(PowerSwitchIF* switcher) { + this->powerSwitcher = switcher; +} diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index a8de34eb..69711bff 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -103,9 +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 setCustomFdir(FailureIsolationBase *fdir); void setParent(object_id_t parent); - void setPowerSwitcher(PowerSwitchIF* switcher); + void setPowerSwitcher(PowerSwitchIF *switcher); void setHkDestination(object_id_t hkDestination); /** diff --git a/src/fsfw/power/DummyPowerSwitcher.h b/src/fsfw/power/DummyPowerSwitcher.h index 39de608d..3582f8d7 100644 --- a/src/fsfw/power/DummyPowerSwitcher.h +++ b/src/fsfw/power/DummyPowerSwitcher.h @@ -1,14 +1,14 @@ #ifndef FSFW_SRC_FSFW_POWER_DUMMYPOWERSWITCHER_H_ #define FSFW_SRC_FSFW_POWER_DUMMYPOWERSWITCHER_H_ +#include +#include + #include "PowerSwitchIF.h" #include "definitions.h" -#include -#include - -class DummyPowerSwitcher: public PowerSwitchIF { -public: +class DummyPowerSwitcher : public PowerSwitchIF { + public: DummyPowerSwitcher(size_t numberOfSwitches, size_t numberOfFuses, uint32_t switchDelayMs = 5000); void setInitialSwitcherList(std::vector switcherList); @@ -20,12 +20,10 @@ public: virtual ReturnValue_t getFuseState(uint8_t fuseNr) const override; virtual uint32_t getSwitchDelayMs(void) const override; -private: + private: std::vector switcherList; std::vector fuseList; uint32_t switchDelayMs = 5000; }; - - #endif /* FSFW_SRC_FSFW_POWER_DUMMYPOWERSWITCHER_H_ */ diff --git a/src/fsfw/power/PowerSwitchIF.h b/src/fsfw/power/PowerSwitchIF.h index 71c5523d..bc883fbc 100644 --- a/src/fsfw/power/PowerSwitchIF.h +++ b/src/fsfw/power/PowerSwitchIF.h @@ -1,9 +1,9 @@ #ifndef FSFW_POWER_POWERSWITCHIF_H_ #define FSFW_POWER_POWERSWITCHIF_H_ -#include "definitions.h" #include "../events/Event.h" #include "../returnvalues/HasReturnvaluesIF.h" +#include "definitions.h" /** * * @brief This interface defines a connection to a device that is capable of diff --git a/src/fsfw/power/PowerSwitcher.cpp b/src/fsfw/power/PowerSwitcher.cpp index 83679772..7608c6e7 100644 --- a/src/fsfw/power/PowerSwitcher.cpp +++ b/src/fsfw/power/PowerSwitcher.cpp @@ -1,6 +1,6 @@ -#include "definitions.h" #include "fsfw/power/PowerSwitcher.h" +#include "definitions.h" #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/serviceinterface/ServiceInterface.h" @@ -46,8 +46,8 @@ void PowerSwitcher::commandSwitches(ReturnValue_t onOff) { } void PowerSwitcher::turnOn(bool checkCurrentState) { - if(checkCurrentState) { - if(getStateOfSwitches() == PowerSwitchIF::SWITCH_ON) { + if (checkCurrentState) { + if (getStateOfSwitches() == PowerSwitchIF::SWITCH_ON) { state = SWITCH_IS_ON; return; } @@ -57,8 +57,8 @@ void PowerSwitcher::turnOn(bool checkCurrentState) { } void PowerSwitcher::turnOff(bool checkCurrentState) { - if(checkCurrentState) { - if(getStateOfSwitches() == PowerSwitchIF::SWITCH_OFF) { + if (checkCurrentState) { + if (getStateOfSwitches() == PowerSwitchIF::SWITCH_OFF) { state = SWITCH_IS_OFF; return; } @@ -68,7 +68,7 @@ void PowerSwitcher::turnOff(bool checkCurrentState) { } bool PowerSwitcher::active() { - if(state == WAIT_OFF or state == WAIT_ON) { + if (state == WAIT_OFF or state == WAIT_ON) { return true; } return false; diff --git a/src/fsfw/power/definitions.h b/src/fsfw/power/definitions.h index 1e4fe296..ed2a0037 100644 --- a/src/fsfw/power/definitions.h +++ b/src/fsfw/power/definitions.h @@ -8,6 +8,6 @@ namespace power { using Switch_t = uint8_t; static constexpr Switch_t NO_SWITCH = 0xFF; -} +} // namespace power #endif /* FSFW_SRC_FSFW_POWER_DEFINITIONS_H_ */ diff --git a/src/fsfw/version.cpp b/src/fsfw/version.cpp index 926e465f..e4a62002 100644 --- a/src/fsfw/version.cpp +++ b/src/fsfw/version.cpp @@ -1,8 +1,9 @@ #include "version.h" -#include "fsfw/FSFWVersion.h" #include +#include "fsfw/FSFWVersion.h" + #ifdef major #undef major #endif diff --git a/src/fsfw/version.h b/src/fsfw/version.h index 7cddf193..bb4d0399 100644 --- a/src/fsfw/version.h +++ b/src/fsfw/version.h @@ -29,7 +29,7 @@ class Version { } friend bool operator>(const Version& v1, const Version& v2) { - return not (v1 < v2) and not (v1 == v2); + return not(v1 < v2) and not(v1 == v2); } friend bool operator<=(const Version& v1, const Version& v2) { return ((v1 == v2) or (v1 < v2)); } diff --git a/tests/src/fsfw_tests/unit/version.cpp b/tests/src/fsfw_tests/unit/version.cpp index 92a930dc..2967dfa5 100644 --- a/tests/src/fsfw_tests/unit/version.cpp +++ b/tests/src/fsfw_tests/unit/version.cpp @@ -17,15 +17,15 @@ TEST_CASE("Version API Tests", "[TestVersionAPI]") { fsfw::Version v1 = fsfw::Version(1, 1, 1); fsfw::Version v2 = fsfw::Version(1, 1, 1); REQUIRE(v1 == v2); - REQUIRE(not (v1 < v2)); - REQUIRE(not (v1 > v2)); + REQUIRE(not(v1 < v2)); + REQUIRE(not(v1 > v2)); REQUIRE(v1 <= v2); REQUIRE(v1 >= v2); v1.revision -= 1; REQUIRE(v1 != v2); - REQUIRE(not (v1 == v2)); - REQUIRE(not (v1 > v2)); - REQUIRE(not (v1 >= v2)); + REQUIRE(not(v1 == v2)); + REQUIRE(not(v1 > v2)); + REQUIRE(not(v1 >= v2)); REQUIRE(v1 < v2); REQUIRE(v1 <= v2); v1.revision += 1; @@ -33,60 +33,60 @@ TEST_CASE("Version API Tests", "[TestVersionAPI]") { REQUIRE(v1 != v2); REQUIRE(v1 < v2); REQUIRE(v1 <= v2); - REQUIRE(not (v1 == v2)); - REQUIRE(not (v1 > v2)); - REQUIRE(not (v1 >= v2)); + REQUIRE(not(v1 == v2)); + REQUIRE(not(v1 > v2)); + REQUIRE(not(v1 >= v2)); v1.minor += 1; v1.major -= 1; REQUIRE(v1 != v2); REQUIRE(v1 < v2); REQUIRE(v1 <= v2); - REQUIRE(not (v1 == v2)); - REQUIRE(not (v1 > v2)); - REQUIRE(not (v1 >= v2)); + REQUIRE(not(v1 == v2)); + REQUIRE(not(v1 > v2)); + REQUIRE(not(v1 >= v2)); v1.major += 1; REQUIRE(v1 == v2); REQUIRE(v1 <= v2); REQUIRE(v1 >= v2); - REQUIRE(not (v1 != v2)); - REQUIRE(not (v1 > v2)); - REQUIRE(not (v1 < v2)); + REQUIRE(not(v1 != v2)); + REQUIRE(not(v1 > v2)); + REQUIRE(not(v1 < v2)); v1.major += 1; v1.minor -= 1; REQUIRE(v1 != v2); REQUIRE(v1 > v2); REQUIRE(v1 >= v2); - REQUIRE(not (v1 == v2)); - REQUIRE(not (v1 < v2)); - REQUIRE(not (v1 <= v2)); + REQUIRE(not(v1 == v2)); + REQUIRE(not(v1 < v2)); + REQUIRE(not(v1 <= v2)); v1.major -= 1; v1.minor += 2; v1.revision -= 1; REQUIRE(v1 != v2); REQUIRE(v1 > v2); REQUIRE(v1 >= v2); - REQUIRE(not (v1 == v2)); - REQUIRE(not (v1 < v2)); - REQUIRE(not (v1 <= v2)); + REQUIRE(not(v1 == v2)); + REQUIRE(not(v1 < v2)); + REQUIRE(not(v1 <= v2)); v1.minor -= 1; v1.revision += 2; REQUIRE(v1 != v2); REQUIRE(v1 > v2); REQUIRE(v1 >= v2); - REQUIRE(not (v1 == v2)); - REQUIRE(not (v1 < v2)); - REQUIRE(not (v1 <= v2)); + REQUIRE(not(v1 == v2)); + REQUIRE(not(v1 < v2)); + REQUIRE(not(v1 <= v2)); v1.revision -= 1; REQUIRE(v1 == v2); REQUIRE(v1 <= v2); REQUIRE(v1 >= v2); - REQUIRE(not (v1 != v2)); + REQUIRE(not(v1 != v2)); #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::info << "v" << fsfw::FSFW_VERSION << std::endl; #endif char verString[10] = {}; fsfw::FSFW_VERSION.getVersion(verString, sizeof(verString)); #if FSFW_DISABLE_PRINTOUT == 0 - printf("v%s\n",verString); + printf("v%s\n", verString); #endif } From a230dc43134fa99378c2d919728bbbdc3bd68e9e Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Wed, 30 Mar 2022 09:20:46 +0200 Subject: [PATCH 086/404] command action default arguments --- src/fsfw/action/CommandActionHelper.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/fsfw/action/CommandActionHelper.h b/src/fsfw/action/CommandActionHelper.h index dd8ad7f1..f0eac6d9 100644 --- a/src/fsfw/action/CommandActionHelper.h +++ b/src/fsfw/action/CommandActionHelper.h @@ -16,9 +16,10 @@ class CommandActionHelper { public: CommandActionHelper(CommandsActionsIF* owner); virtual ~CommandActionHelper(); - ReturnValue_t commandAction(object_id_t commandTo, ActionId_t actionId, const uint8_t* data, - uint32_t size); - ReturnValue_t commandAction(object_id_t commandTo, ActionId_t actionId, SerializeIF* data); + ReturnValue_t commandAction(object_id_t commandTo, ActionId_t actionId, + const uint8_t* data = nullptr, uint32_t size = 0); + ReturnValue_t commandAction(object_id_t commandTo, ActionId_t actionId, + SerializeIF* data); ReturnValue_t initialize(); ReturnValue_t handleReply(CommandMessage* reply); uint8_t getCommandCount() const; From 532607bf8f0e5b62788e5fb8a33ef5ae3a29aeb5 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Wed, 30 Mar 2022 09:21:03 +0200 Subject: [PATCH 087/404] extended command info --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 12 +++++++++--- src/fsfw/devicehandlers/DeviceHandlerBase.h | 6 +++++- 2 files changed, 14 insertions(+), 4 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 18f8a2f7..91ece6b4 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -419,7 +419,7 @@ ReturnValue_t DeviceHandlerBase::insertInCommandAndReplyMap( DeviceCommandId_t deviceCommand, uint16_t maxDelayCycles, LocalPoolDataSetBase* replyDataSet, size_t replyLen, bool periodic, bool hasDifferentReplyId, DeviceCommandId_t replyId) { // No need to check, as we may try to insert multiple times. - insertInCommandMap(deviceCommand); + insertInCommandMap(deviceCommand, hasDifferentReplyId, replyId); if (hasDifferentReplyId) { return insertInReplyMap(replyId, maxDelayCycles, replyDataSet, replyLen, periodic); } else { @@ -446,11 +446,15 @@ ReturnValue_t DeviceHandlerBase::insertInReplyMap(DeviceCommandId_t replyId, } } -ReturnValue_t DeviceHandlerBase::insertInCommandMap(DeviceCommandId_t deviceCommand) { +ReturnValue_t DeviceHandlerBase::insertInCommandMap(DeviceCommandId_t deviceCommand, + bool useAlternativeReply, + DeviceCommandId_t alternativeReplyId) { DeviceCommandInfo info; info.expectedReplies = 0; info.isExecuting = false; info.sendReplyTo = NO_COMMANDER; + info.useAlternativeReplyId = alternativeReplyId; + info.alternativeReplyId = alternativeReplyId; auto resultPair = deviceCommandMap.emplace(deviceCommand, info); if (resultPair.second) { return RETURN_OK; @@ -660,7 +664,9 @@ void DeviceHandlerBase::doGetWrite() { // We need to distinguish here, because a raw command never expects a reply. //(Could be done in eRIRM, but then child implementations need to be careful. - result = enableReplyInReplyMap(cookieInfo.pendingCommand); + DeviceCommandMap::iterator command = cookieInfo.pendingCommand; + result = enableReplyInReplyMap(command, 1, command->second.useAlternativeReplyId, + command->second.alternativeReplyId); } else { // always generate a failure event, so that FDIR knows what's up triggerEvent(DEVICE_SENDING_COMMAND_FAILED, result, cookieInfo.pendingCommand->first); diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index a8de34eb..c9bb1aac 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -481,7 +481,9 @@ class DeviceHandlerBase : public DeviceHandlerIF, * @return - @c RETURN_OK when the command was successfully inserted, * - @c RETURN_FAILED else. */ - ReturnValue_t insertInCommandMap(DeviceCommandId_t deviceCommand); + ReturnValue_t insertInCommandMap(DeviceCommandId_t deviceCommand, + bool useAlternativeReply = false, + DeviceCommandId_t alternativeReplyId = 0); /** * Enables a periodic reply for a given command. It sets to delay cycles to the specified @@ -760,6 +762,8 @@ class DeviceHandlerBase : public DeviceHandlerIF, //! if this is != NO_COMMANDER, DHB was commanded externally and shall //! report everything to commander. MessageQueueId_t sendReplyTo; + bool useAlternativeReplyId; + DeviceCommandId_t alternativeReplyId; }; using DeviceCommandMap = std::map; /** From 29cf8c9009d1bbdc67512c35b2d161fe13659c34 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Thu, 31 Mar 2022 11:42:39 +0200 Subject: [PATCH 088/404] fix in getReplyLength --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 91ece6b4..6ae53f8c 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -464,12 +464,21 @@ ReturnValue_t DeviceHandlerBase::insertInCommandMap(DeviceCommandId_t deviceComm } size_t DeviceHandlerBase::getNextReplyLength(DeviceCommandId_t commandId) { - DeviceReplyIter iter = deviceReplyMap.find(commandId); - if (iter != deviceReplyMap.end()) { - return iter->second.replyLen; - } else { - return 0; + DeviceCommandId_t replyId = NO_COMMAND_ID; + DeviceCommandMap::iterator command = cookieInfo.pendingCommand; + if (command->second.useAlternativeReplyId) { + replyId = command->second.alternativeReplyId; } + else { + replyId = commandId; + } + DeviceReplyIter iter = deviceReplyMap.find(replyId); + if (iter != deviceReplyMap.end()) { + if (iter->second.delayCycles != 0) { + return iter->second.replyLen; + } + } + return 0; } ReturnValue_t DeviceHandlerBase::updateReplyMapEntry(DeviceCommandId_t deviceReply, From 1bc7a918694030d1f49f1d2998a289d77b132849 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 1 Apr 2022 14:08:29 +0200 Subject: [PATCH 089/404] apply auto-formatter --- hal/src/fsfw_hal/linux/gpio/Gpio.h | 15 ++++++--------- src/fsfw/action/CommandActionHelper.h | 3 +-- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 13 ++++++------- 3 files changed, 13 insertions(+), 18 deletions(-) diff --git a/hal/src/fsfw_hal/linux/gpio/Gpio.h b/hal/src/fsfw_hal/linux/gpio/Gpio.h index 2ffe2146..40cc1df5 100644 --- a/hal/src/fsfw_hal/linux/gpio/Gpio.h +++ b/hal/src/fsfw_hal/linux/gpio/Gpio.h @@ -12,16 +12,13 @@ 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); + 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; diff --git a/src/fsfw/action/CommandActionHelper.h b/src/fsfw/action/CommandActionHelper.h index f0eac6d9..3b8acb04 100644 --- a/src/fsfw/action/CommandActionHelper.h +++ b/src/fsfw/action/CommandActionHelper.h @@ -18,8 +18,7 @@ class CommandActionHelper { virtual ~CommandActionHelper(); ReturnValue_t commandAction(object_id_t commandTo, ActionId_t actionId, const uint8_t* data = nullptr, uint32_t size = 0); - ReturnValue_t commandAction(object_id_t commandTo, ActionId_t actionId, - SerializeIF* data); + ReturnValue_t commandAction(object_id_t commandTo, ActionId_t actionId, SerializeIF* data); ReturnValue_t initialize(); ReturnValue_t handleReply(CommandMessage* reply); uint8_t getCommandCount() const; diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 12473590..b0a5d1f0 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -466,16 +466,15 @@ size_t DeviceHandlerBase::getNextReplyLength(DeviceCommandId_t commandId) { DeviceCommandId_t replyId = NO_COMMAND_ID; DeviceCommandMap::iterator command = cookieInfo.pendingCommand; if (command->second.useAlternativeReplyId) { - replyId = command->second.alternativeReplyId; - } - else { - replyId = commandId; + replyId = command->second.alternativeReplyId; + } else { + replyId = commandId; } DeviceReplyIter iter = deviceReplyMap.find(replyId); if (iter != deviceReplyMap.end()) { - if (iter->second.delayCycles != 0) { - return iter->second.replyLen; - } + if (iter->second.delayCycles != 0) { + return iter->second.replyLen; + } } return 0; } From 518666f8221cc65d34407062b3037e7d5322b24a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 1 Apr 2022 17:01:56 +0200 Subject: [PATCH 090/404] add power switcher component --- src/fsfw/power/CMakeLists.txt | 1 + src/fsfw/power/PowerSwitcherComponent.cpp | 108 ++++++++++++++++++++++ src/fsfw/power/PowerSwitcherComponent.h | 64 +++++++++++++ 3 files changed, 173 insertions(+) create mode 100644 src/fsfw/power/PowerSwitcherComponent.cpp create mode 100644 src/fsfw/power/PowerSwitcherComponent.h diff --git a/src/fsfw/power/CMakeLists.txt b/src/fsfw/power/CMakeLists.txt index 10e4a44d..e195b1c0 100644 --- a/src/fsfw/power/CMakeLists.txt +++ b/src/fsfw/power/CMakeLists.txt @@ -4,4 +4,5 @@ target_sources(${LIB_FSFW_NAME} PRIVATE PowerSensor.cpp PowerSwitcher.cpp DummyPowerSwitcher.cpp + PowerSwitcherComponent.cpp ) \ No newline at end of file diff --git a/src/fsfw/power/PowerSwitcherComponent.cpp b/src/fsfw/power/PowerSwitcherComponent.cpp new file mode 100644 index 00000000..5dda02c3 --- /dev/null +++ b/src/fsfw/power/PowerSwitcherComponent.cpp @@ -0,0 +1,108 @@ +#include "PowerSwitcherComponent.h" + +#include +#include + +PowerSwitcherComponent::PowerSwitcherComponent(object_id_t objectId, PowerSwitchIF* pwrSwitcher, power::Switch_t pwrSwitch) + : SystemObject(objectId), switcher(pwrSwitcher, pwrSwitch), modeHelper(this), + healthHelper(this, objectId) { + queue = QueueFactory::instance()->createMessageQueue(); +} + +ReturnValue_t PowerSwitcherComponent::performOperation(uint8_t opCode) { + ReturnValue_t result; + CommandMessage command; + + for (result = queue->receiveMessage(&command); result == RETURN_OK; + result = queue->receiveMessage(&command)) { + result = healthHelper.handleHealthCommand(&command); + if (result == RETURN_OK) { + continue; + } + + result = modeHelper.handleModeCommand(&command); + if (result == RETURN_OK) { + continue; + } + } + if(switcher.active()) { + switcher.doStateMachine(); + auto currState = switcher.getState(); + if (currState == PowerSwitcher::SWITCH_IS_OFF) { + setMode(MODE_OFF, 0); + } else if(currState == PowerSwitcher::SWITCH_IS_ON) { + setMode(MODE_ON, 0); + } + } + return RETURN_OK; +} + +ReturnValue_t PowerSwitcherComponent::initialize() { + ReturnValue_t result = modeHelper.initialize(); + if(result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + result = healthHelper.initialize(); + if(result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return SystemObject::initialize(); +} + +MessageQueueId_t PowerSwitcherComponent::getCommandQueue() const { + return queue->getId(); +} + +void PowerSwitcherComponent::getMode(Mode_t *mode, Submode_t *submode) { + *mode = this->mode; + *submode = this->submode; +} + +ReturnValue_t PowerSwitcherComponent::setHealth(HealthState health) { + healthHelper.setHealth(health); + return RETURN_OK; +} + +ReturnValue_t PowerSwitcherComponent::checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t *msToReachTheMode) { + *msToReachTheMode = 5000; + if(mode != MODE_ON and mode != MODE_OFF) { + return TRANS_NOT_ALLOWED; + } + return RETURN_OK; +} + +void PowerSwitcherComponent::startTransition(Mode_t mode, Submode_t submode) { + if(mode == MODE_OFF) { + switcher.turnOff(true); + switcher.doStateMachine(); + if(switcher.getState() == PowerSwitcher::SWITCH_IS_OFF) { + setMode(MODE_OFF, 0); + } + } else if (mode == MODE_ON) { + switcher.turnOn(true); + switcher.doStateMachine(); + if(switcher.getState() == PowerSwitcher::SWITCH_IS_ON) { + setMode(MODE_ON, 0); + } + } +} + +void PowerSwitcherComponent::setToExternalControl() { + healthHelper.setHealth(HasHealthIF::EXTERNAL_CONTROL); +} + +void PowerSwitcherComponent::announceMode(bool recursive) { + triggerEvent(MODE_INFO, mode, submode); +} + +void PowerSwitcherComponent::setMode(Mode_t newMode, Submode_t newSubmode) { + this->mode = newMode; + this->submode = newSubmode; + modeHelper.modeChanged(mode, submode); + announceMode(false); +} + +HasHealthIF::HealthState PowerSwitcherComponent::getHealth() { + return healthHelper.getHealth(); +} diff --git a/src/fsfw/power/PowerSwitcherComponent.h b/src/fsfw/power/PowerSwitcherComponent.h new file mode 100644 index 00000000..3a075c12 --- /dev/null +++ b/src/fsfw/power/PowerSwitcherComponent.h @@ -0,0 +1,64 @@ +#ifndef _FSFW_POWER_POWERSWITCHERCOMPONENT_H_ +#define _FSFW_POWER_POWERSWITCHERCOMPONENT_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +class PowerSwitchIF; + +/** + * @brief Allows to create an power switch object with its own mode and health + * @details + * This basic component allows to create an object which is solely responsible for managing a + * switch. It also has a mode and a health by implementing the respective interface components + * which allows integrating this component into a system mode tree. + * + * Commanding this component to MODE_OFF will cause the switcher to turn the switch off while + * commanding in to MODE_ON will cause the switcher to turn the switch on. + */ +class PowerSwitcherComponent: + public SystemObject, + public HasReturnvaluesIF, + public ExecutableObjectIF, + public HasModesIF, + public HasHealthIF { +public: + PowerSwitcherComponent(object_id_t objectId, PowerSwitchIF* pwrSwitcher, + power::Switch_t pwrSwitch); + +private: + + MessageQueueIF* queue = nullptr; + PowerSwitcher switcher; + + Mode_t mode = MODE_OFF; + Submode_t submode = 0; + + ModeHelper modeHelper; + HealthHelper healthHelper; + + void setMode(Mode_t newMode, Submode_t newSubmode); + + virtual ReturnValue_t performOperation(uint8_t opCode) override; + + ReturnValue_t initialize() override; + + MessageQueueId_t getCommandQueue() const override; + void getMode(Mode_t *mode, Submode_t *submode) override; + ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, + uint32_t *msToReachTheMode) override; + void startTransition(Mode_t mode, Submode_t submode) override; + void setToExternalControl() override; + void announceMode(bool recursive) override; + + ReturnValue_t setHealth(HealthState health) override; + HasHealthIF::HealthState getHealth() override; +}; + +#endif /* _FSFW_POWER_POWERSWITCHERCOMPONENT_H_ */ From 761a0c9bac4ba94e88eb285eecf88ca9665b5eab Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 4 Apr 2022 15:39:02 +0200 Subject: [PATCH 091/404] new pool ctor which only takes len --- src/fsfw/datapool/PoolEntry.cpp | 14 ++++++++++---- src/fsfw/datapool/PoolEntry.h | 5 ++++- 2 files changed, 14 insertions(+), 5 deletions(-) diff --git a/src/fsfw/datapool/PoolEntry.cpp b/src/fsfw/datapool/PoolEntry.cpp index fd110e6c..d8e1d634 100644 --- a/src/fsfw/datapool/PoolEntry.cpp +++ b/src/fsfw/datapool/PoolEntry.cpp @@ -7,13 +7,19 @@ #include "fsfw/serviceinterface/ServiceInterface.h" template -PoolEntry::PoolEntry(std::initializer_list initValue, bool setValid) - : length(static_cast(initValue.size())), valid(setValid) { +PoolEntry::PoolEntry(uint8_t len, bool setValid): length(len), valid(setValid) { this->address = new T[this->length]; - if (initValue.size() == 0) { + std::memset(this->address, 0, this->getByteSize()); +} + +template +PoolEntry::PoolEntry(std::initializer_list initValues, bool setValid) + : length(static_cast(initValues.size())), valid(setValid) { + this->address = new T[this->length]; + if (initValues.size() == 0) { std::memset(this->address, 0, this->getByteSize()); } else { - std::copy(initValue.begin(), initValue.end(), this->address); + std::copy(initValues.begin(), initValues.end(), this->address); } } diff --git a/src/fsfw/datapool/PoolEntry.h b/src/fsfw/datapool/PoolEntry.h index d3d80f09..8bf9a41c 100644 --- a/src/fsfw/datapool/PoolEntry.h +++ b/src/fsfw/datapool/PoolEntry.h @@ -33,6 +33,9 @@ class PoolEntry : public PoolEntryIF { "instead! The ECSS standard defines a boolean as a one bit " "field. Therefore it is preferred to store a boolean as an " "uint8_t"); + + PoolEntry(uint8_t len = 1, bool setValid = false); + /** * @brief In the classe's constructor, space is allocated on the heap and * potential initialization values are copied to that space. @@ -49,7 +52,7 @@ class PoolEntry : public PoolEntryIF { * @param setValid * Sets the initialization flag. It is invalid by default. */ - PoolEntry(std::initializer_list initValue = {0}, bool setValid = false); + PoolEntry(std::initializer_list initValue, bool setValid = false); /** * @brief In the classe's constructor, space is allocated on the heap and From e4c6a69f776cd8056985ce56ba6528bd842a1ea8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 4 Apr 2022 15:44:03 +0200 Subject: [PATCH 092/404] this should also zero-init the pool entries --- src/fsfw/datapool/PoolEntry.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/fsfw/datapool/PoolEntry.cpp b/src/fsfw/datapool/PoolEntry.cpp index d8e1d634..7e922bcb 100644 --- a/src/fsfw/datapool/PoolEntry.cpp +++ b/src/fsfw/datapool/PoolEntry.cpp @@ -8,17 +8,15 @@ template PoolEntry::PoolEntry(uint8_t len, bool setValid): length(len), valid(setValid) { - this->address = new T[this->length]; + this->address = new T[this->length](); std::memset(this->address, 0, this->getByteSize()); } template PoolEntry::PoolEntry(std::initializer_list initValues, bool setValid) : length(static_cast(initValues.size())), valid(setValid) { - this->address = new T[this->length]; - if (initValues.size() == 0) { - std::memset(this->address, 0, this->getByteSize()); - } else { + this->address = new T[this->length](); + if (initValues.size() > 0) { std::copy(initValues.begin(), initValues.end(), this->address); } } @@ -26,11 +24,9 @@ PoolEntry::PoolEntry(std::initializer_list initValues, bool setValid) template PoolEntry::PoolEntry(T* initValue, uint8_t setLength, bool setValid) : length(setLength), valid(setValid) { - this->address = new T[this->length]; + this->address = new T[this->length](); if (initValue != nullptr) { std::memcpy(this->address, initValue, this->getByteSize()); - } else { - std::memset(this->address, 0, this->getByteSize()); } } From aded4fae1e4f8a8a325c49b8c0d4bb4d7408d676 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 4 Apr 2022 20:34:31 +0200 Subject: [PATCH 093/404] printout improvement --- src/fsfw/pus/Service3Housekeeping.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/fsfw/pus/Service3Housekeeping.cpp b/src/fsfw/pus/Service3Housekeeping.cpp index 07574783..cce8fc91 100644 --- a/src/fsfw/pus/Service3Housekeeping.cpp +++ b/src/fsfw/pus/Service3Housekeeping.cpp @@ -214,11 +214,11 @@ ReturnValue_t Service3Housekeeping::handleReply(const CommandMessage* reply, default: #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "Service3Housekeeping::handleReply: Invalid reply with " - << "reply command " << command << "!" << std::endl; + << "reply command " << command << std::endl; #else sif::printWarning( "Service3Housekeeping::handleReply: Invalid reply with " - "reply command %hu!\n", + "reply command %hu\n", command); #endif return CommandingServiceBase::INVALID_REPLY; From 0677de39aa4c20e8ee1c066909818d2dfffcbc2a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 7 Apr 2022 00:15:42 +0200 Subject: [PATCH 094/404] make reporting setter public --- src/fsfw/datapoollocal/LocalPoolDataSetBase.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/datapoollocal/LocalPoolDataSetBase.h b/src/fsfw/datapoollocal/LocalPoolDataSetBase.h index 17cf8be2..9166cf34 100644 --- a/src/fsfw/datapoollocal/LocalPoolDataSetBase.h +++ b/src/fsfw/datapoollocal/LocalPoolDataSetBase.h @@ -162,6 +162,7 @@ class LocalPoolDataSetBase : public PoolDataSetBase, public MarkChangedIF { object_id_t getCreatorObjectId(); bool getReportingEnabled() const; + void setReportingEnabled(bool enabled); /** * Returns the current periodic HK generation interval this set @@ -189,7 +190,6 @@ class LocalPoolDataSetBase : public PoolDataSetBase, public MarkChangedIF { * Used for periodic generation. */ bool reportingEnabled = false; - void setReportingEnabled(bool enabled); void initializePeriodicHelper(float collectionInterval, dur_millis_t minimumPeriodicInterval, uint8_t nonDiagIntervalFactor = 5); From e3ffcae3e09a9ef674383b9a93a101676b0f575e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 7 Apr 2022 11:01:39 +0200 Subject: [PATCH 095/404] emit warning in linux clock --- src/fsfw/osal/linux/Clock.cpp | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/src/fsfw/osal/linux/Clock.cpp b/src/fsfw/osal/linux/Clock.cpp index 61eb970f..9b8fcb65 100644 --- a/src/fsfw/osal/linux/Clock.cpp +++ b/src/fsfw/osal/linux/Clock.cpp @@ -1,4 +1,5 @@ #include "fsfw/timemanager/Clock.h" +#include "fsfw/serviceinterface/ServiceInterface.h" #include #include @@ -7,12 +8,13 @@ #include #include - -#include "fsfw/serviceinterface/ServiceInterface.h" +#include uint16_t Clock::leapSeconds = 0; MutexIF* Clock::timeMutex = NULL; +void handleClockError(); + uint32_t Clock::getTicksPerSecond(void) { uint32_t ticks = sysconf(_SC_CLK_TCK); return ticks; @@ -27,7 +29,7 @@ ReturnValue_t Clock::setClock(const TimeOfDay_t* time) { int status = clock_settime(CLOCK_REALTIME, &timeUnix); if (status != 0) { - // TODO errno + handleClockError(); return HasReturnvaluesIF::RETURN_FAILED; } return HasReturnvaluesIF::RETURN_OK; @@ -39,12 +41,23 @@ ReturnValue_t Clock::setClock(const timeval* time) { timeUnix.tv_nsec = (__syscall_slong_t)time->tv_usec * 1000; int status = clock_settime(CLOCK_REALTIME, &timeUnix); if (status != 0) { - // TODO errno + handleClockError(); return HasReturnvaluesIF::RETURN_FAILED; } return HasReturnvaluesIF::RETURN_OK; } +void handleClockError() { +#if FSFW_VERBOSE_LEVEL >= 1 +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "Clock::setClock: Failed with code " << errno << ": " << strerror(errno) + << std::endl; +#else + sif::printWarning("Clock::setClock: Failed with code %d: %s\n", errno, strerror(errno)); +#endif +#endif +} + ReturnValue_t Clock::getClock_timeval(timeval* time) { timespec timeUnix; int status = clock_gettime(CLOCK_REALTIME, &timeUnix); From 5bda877d97b41e82bf36d939c9f367cef611446a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 7 Apr 2022 17:23:06 +0200 Subject: [PATCH 096/404] improve clock error handler --- src/fsfw/osal/linux/Clock.cpp | 30 ++++++++++++++++-------------- 1 file changed, 16 insertions(+), 14 deletions(-) diff --git a/src/fsfw/osal/linux/Clock.cpp b/src/fsfw/osal/linux/Clock.cpp index 9b8fcb65..dd11ee34 100644 --- a/src/fsfw/osal/linux/Clock.cpp +++ b/src/fsfw/osal/linux/Clock.cpp @@ -13,7 +13,7 @@ uint16_t Clock::leapSeconds = 0; MutexIF* Clock::timeMutex = NULL; -void handleClockError(); +void handleClockError(const char* func); uint32_t Clock::getTicksPerSecond(void) { uint32_t ticks = sysconf(_SC_CLK_TCK); @@ -29,7 +29,7 @@ ReturnValue_t Clock::setClock(const TimeOfDay_t* time) { int status = clock_settime(CLOCK_REALTIME, &timeUnix); if (status != 0) { - handleClockError(); + handleClockError("setClock"); return HasReturnvaluesIF::RETURN_FAILED; } return HasReturnvaluesIF::RETURN_OK; @@ -41,27 +41,17 @@ ReturnValue_t Clock::setClock(const timeval* time) { timeUnix.tv_nsec = (__syscall_slong_t)time->tv_usec * 1000; int status = clock_settime(CLOCK_REALTIME, &timeUnix); if (status != 0) { - handleClockError(); + handleClockError("setClock"); return HasReturnvaluesIF::RETURN_FAILED; } return HasReturnvaluesIF::RETURN_OK; } -void handleClockError() { -#if FSFW_VERBOSE_LEVEL >= 1 -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "Clock::setClock: Failed with code " << errno << ": " << strerror(errno) - << std::endl; -#else - sif::printWarning("Clock::setClock: Failed with code %d: %s\n", errno, strerror(errno)); -#endif -#endif -} - ReturnValue_t Clock::getClock_timeval(timeval* time) { timespec timeUnix; int status = clock_gettime(CLOCK_REALTIME, &timeUnix); if (status != 0) { + handleClockError("getClock_timeval"); return HasReturnvaluesIF::RETURN_FAILED; } time->tv_sec = timeUnix.tv_sec; @@ -164,3 +154,15 @@ ReturnValue_t Clock::convertTimevalToJD2000(timeval time, double* JD2000) { *JD2000 = (time.tv_sec - 946728000. + time.tv_usec / 1000000.) / 24. / 3600.; return HasReturnvaluesIF::RETURN_OK; } + + +void handleClockError(const char* func) { +#if FSFW_VERBOSE_LEVEL >= 1 +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "Clock::" << func << ": Failed with code " << errno << ": " << strerror(errno) + << std::endl; +#else + sif::printWarning("Clock::%s: Failed with code %d: %s\n", func, errno, strerror(errno)); +#endif +#endif +} From 47ced1efacd6aaed01cac9ec97e5b81b66974217 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 7 Apr 2022 19:47:37 +0200 Subject: [PATCH 097/404] pool entry takes const T* now --- src/fsfw/datapool/PoolEntry.cpp | 2 +- src/fsfw/datapool/PoolEntry.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/fsfw/datapool/PoolEntry.cpp b/src/fsfw/datapool/PoolEntry.cpp index 7e922bcb..a74e6306 100644 --- a/src/fsfw/datapool/PoolEntry.cpp +++ b/src/fsfw/datapool/PoolEntry.cpp @@ -22,7 +22,7 @@ PoolEntry::PoolEntry(std::initializer_list initValues, bool setValid) } template -PoolEntry::PoolEntry(T* initValue, uint8_t setLength, bool setValid) +PoolEntry::PoolEntry(const T* initValue, uint8_t setLength, bool setValid) : length(setLength), valid(setValid) { this->address = new T[this->length](); if (initValue != nullptr) { diff --git a/src/fsfw/datapool/PoolEntry.h b/src/fsfw/datapool/PoolEntry.h index 8bf9a41c..4010f78d 100644 --- a/src/fsfw/datapool/PoolEntry.h +++ b/src/fsfw/datapool/PoolEntry.h @@ -65,7 +65,7 @@ class PoolEntry : public PoolEntryIF { * @param setValid * Sets the initialization flag. It is invalid by default. */ - PoolEntry(T* initValue, uint8_t setLength = 1, bool setValid = false); + PoolEntry(const T* initValue, uint8_t setLength = 1, bool setValid = false); //! Explicitely deleted copy ctor, copying is not allowed. PoolEntry(const PoolEntry&) = delete; From 478b305fbe3bfec363dd2c3ac5f9cbdfb167d33f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 11 Apr 2022 16:01:26 +0200 Subject: [PATCH 098/404] fix compiler warnings --- src/fsfw/container/HybridIterator.h | 14 ++++++++++--- src/fsfw/globalfunctions/matching/MatchTree.h | 3 +++ src/fsfw/subsystem/Subsystem.cpp | 20 +++++++++++++------ src/fsfw/subsystem/Subsystem.h | 2 +- 4 files changed, 29 insertions(+), 10 deletions(-) diff --git a/src/fsfw/container/HybridIterator.h b/src/fsfw/container/HybridIterator.h index e8b24a3d..50a37988 100644 --- a/src/fsfw/container/HybridIterator.h +++ b/src/fsfw/container/HybridIterator.h @@ -10,16 +10,24 @@ class HybridIterator : public LinkedElement::Iterator, public ArrayList::Iterator *iter) - : LinkedElement::Iterator(*iter), value(iter->value), linked(true) {} + : LinkedElement::Iterator(*iter), value(iter->value), linked(true) { + if(iter != nullptr) { + value = iter->value; + } + } HybridIterator(LinkedElement *start) - : LinkedElement::Iterator(start), value(start->value), linked(true) {} + : LinkedElement::Iterator(start), linked(true) { + if(start != nullptr) { + value = start->value; + } + } HybridIterator(typename ArrayList::Iterator start, typename ArrayList::Iterator end) : ArrayList::Iterator(start), value(start.value), linked(false), end(end.value) { if (value == this->end) { - value = NULL; + value = nullptr; } } diff --git a/src/fsfw/globalfunctions/matching/MatchTree.h b/src/fsfw/globalfunctions/matching/MatchTree.h index f7775d45..47e400da 100644 --- a/src/fsfw/globalfunctions/matching/MatchTree.h +++ b/src/fsfw/globalfunctions/matching/MatchTree.h @@ -179,6 +179,9 @@ class MatchTree : public SerializeableMatcherIF, public BinaryTreematch(number); if (isMatch) { if (iter.left() == this->end()) { diff --git a/src/fsfw/subsystem/Subsystem.cpp b/src/fsfw/subsystem/Subsystem.cpp index a837bf83..767cfe39 100644 --- a/src/fsfw/subsystem/Subsystem.cpp +++ b/src/fsfw/subsystem/Subsystem.cpp @@ -30,11 +30,11 @@ ReturnValue_t Subsystem::checkSequence(HybridIterator iter, return FALLBACK_SEQUENCE_DOES_NOT_EXIST; } - if (iter.value == NULL) { + if (iter.value ==nullptr) { return NO_TARGET_TABLE; } - for (; iter.value != NULL; ++iter) { + for (; iter.value != nullptr; ++iter) { if (!existsModeTable(iter->getTableId())) { return TABLE_DOES_NOT_EXIST; } else { @@ -66,13 +66,18 @@ HybridIterator Subsystem::getCurrentTable() { void Subsystem::performChildOperation() { if (isInTransition) { if (commandsOutstanding <= 0) { // all children of the current table were commanded and replied - if (currentSequenceIterator.value == NULL) { // we're through with this sequence + if (currentSequenceIterator.value == nullptr) { // we're through with this sequence if (checkStateAgainstTable(currentTargetTable, targetSubmode) == RETURN_OK) { setMode(targetMode, targetSubmode); isInTransition = false; return; } else { - transitionFailed(TARGET_TABLE_NOT_REACHED, getSequence(targetMode)->getTableId()); + Mode_t tableId = 0; + auto seq = getSequence(targetMode); + if(seq.value != nullptr) { + tableId = seq->getTableId(); + } + transitionFailed(TARGET_TABLE_NOT_REACHED, tableId); return; } } @@ -248,10 +253,13 @@ ReturnValue_t Subsystem::handleCommandMessage(CommandMessage *message) { case ModeSequenceMessage::READ_TABLE: { ReturnValue_t result; Mode_t table = ModeSequenceMessage::getSequenceId(message); - EntryPointer *entry = NULL; + EntryPointer *entry = nullptr; result = modeTables.find(table, &entry); - if (result != RETURN_OK) { + if (result != RETURN_OK or entry == nullptr) { replyToCommand(result, 0); + if(entry == nullptr) { + return result; + } } SerializeIF *elements[2]; diff --git a/src/fsfw/subsystem/Subsystem.h b/src/fsfw/subsystem/Subsystem.h index 2c78c8cd..e0fafb51 100644 --- a/src/fsfw/subsystem/Subsystem.h +++ b/src/fsfw/subsystem/Subsystem.h @@ -1,7 +1,7 @@ #ifndef FSFW_SUBSYSTEM_SUBSYSTEM_H_ #define FSFW_SUBSYSTEM_SUBSYSTEM_H_ -#include +#include "fsfw/FSFW.h" #include "../container/FixedArrayList.h" #include "../container/FixedMap.h" From 5ff88129b8158a3a66366968231a68d01564f8fe Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 13 Apr 2022 14:45:36 +0200 Subject: [PATCH 099/404] added DLE parser --- src/fsfw/globalfunctions/CMakeLists.txt | 20 +- src/fsfw/globalfunctions/DleParser.cpp | 231 ++++++++++++++++++++++++ src/fsfw/globalfunctions/DleParser.h | 127 +++++++++++++ 3 files changed, 368 insertions(+), 10 deletions(-) create mode 100644 src/fsfw/globalfunctions/DleParser.cpp create mode 100644 src/fsfw/globalfunctions/DleParser.h diff --git a/src/fsfw/globalfunctions/CMakeLists.txt b/src/fsfw/globalfunctions/CMakeLists.txt index 5ccd3c4c..acd1edbe 100644 --- a/src/fsfw/globalfunctions/CMakeLists.txt +++ b/src/fsfw/globalfunctions/CMakeLists.txt @@ -1,13 +1,13 @@ -target_sources(${LIB_FSFW_NAME} - PRIVATE - arrayprinter.cpp - AsciiConverter.cpp - CRC.cpp - DleEncoder.cpp - PeriodicOperationDivider.cpp - timevalOperations.cpp - Type.cpp - bitutility.cpp +target_sources(${LIB_FSFW_NAME} PRIVATE + arrayprinter.cpp + AsciiConverter.cpp + CRC.cpp + DleEncoder.cpp + DleParser.cpp + PeriodicOperationDivider.cpp + timevalOperations.cpp + Type.cpp + bitutility.cpp ) add_subdirectory(math) diff --git a/src/fsfw/globalfunctions/DleParser.cpp b/src/fsfw/globalfunctions/DleParser.cpp new file mode 100644 index 00000000..b68dbde1 --- /dev/null +++ b/src/fsfw/globalfunctions/DleParser.cpp @@ -0,0 +1,231 @@ +#include "DleParser.h" + +#include +#include + +#include + +DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf, + BufPair decodedBuf, UserHandler handler, void* args) + : decodeRingBuf(decodeRingBuf), + decoder(decoder), + encodedBuf(encodedBuf), + decodedBuf(decodedBuf), + handler(handler), + ctx(args) { + if (handler == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "DleParser::DleParser: Invalid user handler" << std::endl; +#else + sif::printError("DleParser::DleParser: Invalid user handler\n"); +#endif + } +} + +ReturnValue_t DleParser::passData(uint8_t* data, size_t len) { + if (data == nullptr or len == 0 or handler == nullptr) { + return RETURN_FAILED; + } + size_t copyIntoRingBufFromHere = 0; + size_t copyAmount = len; + size_t startIdx = 0; + ReturnValue_t result = RETURN_OK; + bool startFoundInThisPacket = false; + for (size_t idx = 0; idx < len; idx++) { + if (data[idx] == DleEncoder::STX_CHAR) { + if (not startFound and not startFoundInThisPacket) { + startIdx = idx; + copyIntoRingBufFromHere = idx; + copyAmount = len - idx; + } else { + // Maybe print warning, should not happen + decodeRingBuf.clear(); + ErrorInfo info; + info.len = idx; + prepareErrorContext(ErrorTypes::CONSECUTIVE_STX_CHARS, info); + handler(ctx); + copyIntoRingBufFromHere = idx; + copyAmount = len - idx; + } + startFound = true; + startFoundInThisPacket = true; + } else if (data[idx] == DleEncoder::ETX_CHAR) { + if (startFoundInThisPacket) { + size_t readLen = 0; + size_t decodedLen = 0; + result = decoder.decode(data + startIdx, idx + 1 - startIdx, &readLen, decodedBuf.first, + decodedBuf.second, &decodedLen); + if (result == HasReturnvaluesIF::RETURN_OK) { + ctx.setType(ContextType::PACKET_FOUND); + ctx.decodedPacket.first = decodedBuf.first; + ctx.decodedPacket.second = decodedLen; + this->handler(ctx); + } else if (result == DleEncoder::STREAM_TOO_SHORT) { + ErrorInfo info; + info.res = result; + prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); + handler(ctx); + } else { + ErrorInfo info; + info.res = result; + prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); + handler(ctx); + } + decodeRingBuf.clear(); + if ((idx + 1) < len) { + copyIntoRingBufFromHere = idx + 1; + copyAmount = len - idx - 1; + } else { + copyAmount = 0; + } + } else if (startFound) { + // ETX found but STX was found in another mini packet. Reconstruct the full packet + // to decode it + result = decodeRingBuf.writeData(data, idx + 1); + if (result != HasReturnvaluesIF::RETURN_OK) { + ErrorInfo info; + info.res = result; + prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info); + handler(ctx); + } + size_t fullEncodedLen = decodeRingBuf.getAvailableReadData(); + if (fullEncodedLen > encodedBuf.second) { + ErrorInfo info; + info.len = fullEncodedLen; + prepareErrorContext(ErrorTypes::ENCODED_BUF_TOO_SMALL, info); + handler(ctx); + decodeRingBuf.clear(); + } else { + size_t decodedLen = 0; + size_t readLen = 0; + decodeRingBuf.readData(encodedBuf.first, fullEncodedLen, true); + result = decoder.decode(encodedBuf.first, fullEncodedLen, &readLen, decodedBuf.first, + decodedBuf.second, &decodedLen); + if (result == HasReturnvaluesIF::RETURN_OK) { + if (this->handler != nullptr) { + ctx.setType(ContextType::PACKET_FOUND); + ctx.decodedPacket.first = decodedBuf.first; + ctx.decodedPacket.second = decodedLen; + this->handler(ctx); + } + } else if (result == DleEncoder::STREAM_TOO_SHORT) { + ErrorInfo info; + info.res = result; + prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); + handler(ctx); + } else { + ErrorInfo info; + info.res = result; + prepareErrorContext(ErrorTypes::DECODE_ERROR, info); + handler(ctx); + } + decodeRingBuf.clear(); + startFound = false; + startFoundInThisPacket = false; + if ((idx + 1) < len) { + copyIntoRingBufFromHere = idx + 1; + copyAmount = len - idx - 1; + } else { + copyAmount = 0; + } + } + } else { + // End data without preceeding STX + ErrorInfo info; + info.len = idx + 1; + prepareErrorContext(ErrorTypes::CONSECUTIVE_ETX_CHARS, info); + handler(ctx); + decodeRingBuf.clear(); + if ((idx + 1) < len) { + copyIntoRingBufFromHere = idx + 1; + copyAmount = len - idx - 1; + } else { + copyAmount = 0; + } + } + startFoundInThisPacket = false; + startFound = false; + } + } + if (copyAmount > 0) { + result = decodeRingBuf.writeData(data + copyIntoRingBufFromHere, copyAmount); + if (result != HasReturnvaluesIF::RETURN_OK) { + ErrorInfo info; + info.res = result; + prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info); + handler(ctx); + } + } + return RETURN_OK; +} + +void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* args) { +#if FSFW_VERBOSE_LEVEL >= 1 +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "DleParserBase::handleFoundPacket: Detected DLE packet with " << len << " bytes" + << std::endl; +#else + sif::printInfo("DleParserBase::handleFoundPacket: Detected DLE packet with %d bytes\n", len); +#endif +#endif +} + +void DleParser::defaultErrorHandler(ErrorTypes err, ErrorInfo ctx) { + switch (err) { + case (ErrorTypes::NONE): { + errorPrinter("No error"); + break; + } + case (ErrorTypes::DECODE_ERROR): { + errorPrinter("Decode Error"); + break; + } + case (ErrorTypes::RING_BUF_ERROR): { + errorPrinter("Ring Buffer Error"); + break; + } + case (ErrorTypes::ENCODED_BUF_TOO_SMALL): + case (ErrorTypes::DECODING_BUF_TOO_SMALL): { + char opt[64]; + snprintf(opt, sizeof(opt), ": Too small for packet with length %d", ctx.len); + if (err == ErrorTypes::ENCODED_BUF_TOO_SMALL) { + errorPrinter("Encoded buf too small", opt); + } else { + errorPrinter("Decoding buf too small", opt); + } + break; + } + case (ErrorTypes::CONSECUTIVE_STX_CHARS): { + errorPrinter("Consecutive STX chars detected"); + break; + } + case (ErrorTypes::CONSECUTIVE_ETX_CHARS): { + errorPrinter("Consecutive ETX chars detected"); + break; + } + } +} + +void DleParser::errorPrinter(const char* str, const char* opt) { + if (opt == nullptr) { + opt = ""; + } +#if FSFW_VERBOSE_LEVEL >= 1 +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::info << "DleParserBase::handleParseError: " << str << opt << std::endl; +#else + sif::printInfo("DleParserBase::handleParseError: %s%s\n", str, opt); +#endif +#endif +} + +void DleParser::prepareErrorContext(ErrorTypes err, ErrorInfo info) { + ctx.setType(ContextType::ERROR); + ctx.error.first = err; + ctx.error.second = info; +} + +void DleParser::reset() { + startFound = false; + decodeRingBuf.clear(); +} diff --git a/src/fsfw/globalfunctions/DleParser.h b/src/fsfw/globalfunctions/DleParser.h new file mode 100644 index 00000000..32fe38cb --- /dev/null +++ b/src/fsfw/globalfunctions/DleParser.h @@ -0,0 +1,127 @@ +#ifndef MISSION_DEVICES_DLEPARSER_H_ +#define MISSION_DEVICES_DLEPARSER_H_ + +#include +#include +#include + +#include +#include + +/** + * @brief This base helper class can be used to extract DLE encoded packets from a data stream + * @details + * The core API of the parser takes received packets which can contains DLE packets. The parser + * can deal with DLE packets split across multiple packets. It does so by using a dedicated + * decoding ring buffer. The user can process received packets and detect errors by + * overriding two provided virtual methods. This also allows detecting multiple DLE packets + * inside one passed packet. + */ +class DleParser : public HasReturnvaluesIF { + public: + using BufPair = std::pair; + + enum class ContextType { PACKET_FOUND, ERROR }; + + enum class ErrorTypes { + NONE, + ENCODED_BUF_TOO_SMALL, + DECODING_BUF_TOO_SMALL, + DECODE_ERROR, + RING_BUF_ERROR, + CONSECUTIVE_STX_CHARS, + CONSECUTIVE_ETX_CHARS + }; + + union ErrorInfo { + size_t len; + ReturnValue_t res; + }; + + using ErrorPair = std::pair; + + struct Context { + public: + Context(void* args) : userArgs(args) { setType(ContextType::PACKET_FOUND); } + + void setType(ContextType type) { + if (type == ContextType::PACKET_FOUND) { + error.first = ErrorTypes::NONE; + error.second.len = 0; + } else { + decodedPacket.first = nullptr; + decodedPacket.second = 0; + } + } + + ContextType getType() const { return type; } + + BufPair decodedPacket = {}; + ErrorPair error; + void* userArgs; + + private: + ContextType type; + }; + + using UserHandler = void (*)(const Context& ctx); + + /** + * Base class constructor + * @param decodeRingBuf Ring buffer used to store multiple packets to allow detecting DLE packets + * split across multiple packets + * @param decoder Decoder instance + * @param encodedBuf Buffer used to store encoded packets. It has to be large enough to hold + * the largest expected encoded DLE packet size + * @param decodedBuf Buffer used to store decoded packets. It has to be large enough to hold the + * largest expected decoded DLE packet size + * @param handler Function which will be called on a found packet + * @param args Arbitrary user argument + */ + DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf, + BufPair decodedBuf, UserHandler handler, void* args); + + /** + * This function allows to pass new data into the parser. It then scans for DLE packets + * automatically and inserts (part of) the packet into a ring buffer if necessary. + * @param data + * @param len + * @return + */ + ReturnValue_t passData(uint8_t* data, size_t len); + + /** + * Example found packet handler + * function call + * @param packet Decoded packet + * @param len Length of detected packet + */ + void defaultFoundPacketHandler(uint8_t* packet, size_t len, void* args); + /** + * Will be called if an error occured in the #passData call + * @param err + * @param ctx Context information depending on the error type + * - For buffer length errors, will be set to the detected packet length which is too large + * - For decode or ring buffer errors, will be set to the result returned from the failed call + */ + static void defaultErrorHandler(ErrorTypes err, ErrorInfo ctx); + + static void errorPrinter(const char* str, const char* opt = nullptr); + + void prepareErrorContext(ErrorTypes err, ErrorInfo ctx); + /** + * Resets the parser by resetting the internal states and clearing the decoding ring buffer + */ + void reset(); + + private: + SimpleRingBuffer& decodeRingBuf; + DleEncoder& decoder; + BufPair encodedBuf; + BufPair decodedBuf; + UserHandler handler = nullptr; + Context ctx; + bool startFound = false; +}; + +#endif /* MISSION_DEVICES_DLEPARSER_H_ */ From 935a8e13a582dbdaf3b3ee9f034267d52fa59794 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 13 Apr 2022 14:57:43 +0200 Subject: [PATCH 100/404] uart cookie API change --- hal/src/fsfw_hal/linux/uart/UartCookie.cpp | 8 +++----- hal/src/fsfw_hal/linux/uart/UartCookie.h | 4 ++-- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/hal/src/fsfw_hal/linux/uart/UartCookie.cpp b/hal/src/fsfw_hal/linux/uart/UartCookie.cpp index aa2dd214..31dbc903 100644 --- a/hal/src/fsfw_hal/linux/uart/UartCookie.cpp +++ b/hal/src/fsfw_hal/linux/uart/UartCookie.cpp @@ -2,8 +2,8 @@ #include -UartCookie::UartCookie(object_id_t handlerId, std::string deviceFile, UartModes uartMode, - UartBaudRate baudrate, size_t maxReplyLen) +UartCookie::UartCookie(object_id_t handlerId, std::string deviceFile, UartBaudRate baudrate, + size_t maxReplyLen, UartModes uartMode) : handlerId(handlerId), deviceFile(deviceFile), uartMode(uartMode), @@ -24,9 +24,7 @@ void UartCookie::setParityEven() { parity = Parity::EVEN; } Parity UartCookie::getParity() const { return parity; } -void UartCookie::setBitsPerWord(BitsPerWord bitsPerWord_) { - bitsPerWord = bitsPerWord_; -} +void UartCookie::setBitsPerWord(BitsPerWord bitsPerWord_) { bitsPerWord = bitsPerWord_; } BitsPerWord UartCookie::getBitsPerWord() const { return bitsPerWord; } diff --git a/hal/src/fsfw_hal/linux/uart/UartCookie.h b/hal/src/fsfw_hal/linux/uart/UartCookie.h index 6840b352..cae33d58 100644 --- a/hal/src/fsfw_hal/linux/uart/UartCookie.h +++ b/hal/src/fsfw_hal/linux/uart/UartCookie.h @@ -69,8 +69,8 @@ class UartCookie : public CookieIF { * 8 databits (number of bits transfered with one uart frame) * One stop bit */ - UartCookie(object_id_t handlerId, std::string deviceFile, UartModes uartMode, - UartBaudRate baudrate, size_t maxReplyLen); + UartCookie(object_id_t handlerId, std::string deviceFile, UartBaudRate baudrate, + size_t maxReplyLen, UartModes uartMode = UartModes::NON_CANONICAL); virtual ~UartCookie(); From 613dbe9592c30d9acf4cdb95d81d9f216f07374b Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Thu, 21 Apr 2022 09:33:06 +0200 Subject: [PATCH 101/404] default argument --- src/fsfw/tmtcpacket/SpacePacket.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/tmtcpacket/SpacePacket.h b/src/fsfw/tmtcpacket/SpacePacket.h index 10140db1..4fadd236 100644 --- a/src/fsfw/tmtcpacket/SpacePacket.h +++ b/src/fsfw/tmtcpacket/SpacePacket.h @@ -25,7 +25,7 @@ class SpacePacket : public SpacePacketBase { * @param apid Sets the packet's APID field. The default value describes an idle packet. * @param sequenceCount ets the packet's Source Sequence Count field. */ - SpacePacket(uint16_t packetDataLength, bool isTelecommand = false, + SpacePacket(uint16_t packetDataLength = PACKET_MAX_SIZE, bool isTelecommand = false, uint16_t apid = APID_IDLE_PACKET, uint16_t sequenceCount = 0); /** * The class's default destructor. From 085213c60f0f86dd7e28769767a0d235ed333336 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 21 Apr 2022 10:30:46 +0200 Subject: [PATCH 102/404] add nullptr check --- hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp index 52b6dc07..278f6f4b 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp @@ -375,13 +375,16 @@ float MgmLIS3MDLHandler::getSensitivityFactor(MGMLIS3MDL::Sensitivies sens) { ReturnValue_t MgmLIS3MDLHandler::enableTemperatureSensor(const uint8_t *commandData, size_t commandDataLen) { + if(commandData == nullptr) { + return INVALID_COMMAND_PARAMETER; + } triggerEvent(CHANGE_OF_SETUP_PARAMETER); uint32_t size = 2; commandBuffer[0] = writeCommand(MGMLIS3MDL::CTRL_REG1); if (commandDataLen > 1) { return INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS; } - switch (*commandData) { + switch (commandData[0]) { case (MGMLIS3MDL::ON): { commandBuffer[1] = registers[0] | (1 << 7); break; From 9f7b9be800402215e97759eb35d4c40a484d1f68 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Thu, 21 Apr 2022 14:24:20 +0200 Subject: [PATCH 103/404] space packet default length 0 --- src/fsfw/tmtcpacket/SpacePacket.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/tmtcpacket/SpacePacket.h b/src/fsfw/tmtcpacket/SpacePacket.h index 4fadd236..dc45576e 100644 --- a/src/fsfw/tmtcpacket/SpacePacket.h +++ b/src/fsfw/tmtcpacket/SpacePacket.h @@ -25,7 +25,7 @@ class SpacePacket : public SpacePacketBase { * @param apid Sets the packet's APID field. The default value describes an idle packet. * @param sequenceCount ets the packet's Source Sequence Count field. */ - SpacePacket(uint16_t packetDataLength = PACKET_MAX_SIZE, bool isTelecommand = false, + SpacePacket(uint16_t packetDataLength = 0, bool isTelecommand = false, uint16_t apid = APID_IDLE_PACKET, uint16_t sequenceCount = 0); /** * The class's default destructor. From 900ef5b9124063b5eec88de19d851006b2e1e601 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Tue, 26 Apr 2022 09:07:03 +0200 Subject: [PATCH 104/404] option to use coutdwon object to time out replies --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 73 +++++++++++++++---- src/fsfw/devicehandlers/DeviceHandlerBase.h | 27 ++++++- src/fsfw/timemanager/Countdown.cpp | 9 ++- src/fsfw/timemanager/Countdown.h | 3 +- 4 files changed, 93 insertions(+), 19 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index b0a5d1f0..52178683 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -243,17 +243,28 @@ ReturnValue_t DeviceHandlerBase::initialize() { } void DeviceHandlerBase::decrementDeviceReplyMap() { + bool timedOut = false; for (std::pair& replyPair : deviceReplyMap) { - if (replyPair.second.delayCycles != 0) { + if (replyPair.second.countdown != nullptr && replyPair.second.active) { + if (replyPair.second.countdown->hasTimedOut()) { + timedOut = true; + } + } + if (replyPair.second.delayCycles != 0 && replyPair.second.countdown == nullptr) { replyPair.second.delayCycles--; if (replyPair.second.delayCycles == 0) { if (replyPair.second.periodic) { replyPair.second.delayCycles = replyPair.second.maxDelayCycles; } - replyToReply(replyPair.first, replyPair.second, TIMEOUT); - missedReply(replyPair.first); + timedOut = true; } } + if (timedOut) { + replyToReply(replyPair.first, replyPair.second, TIMEOUT); + missedReply(replyPair.first); + timedOut = false; + replyPair.second.active = false; + } } } @@ -416,20 +427,22 @@ ReturnValue_t DeviceHandlerBase::isModeCombinationValid(Mode_t mode, Submode_t s ReturnValue_t DeviceHandlerBase::insertInCommandAndReplyMap( DeviceCommandId_t deviceCommand, uint16_t maxDelayCycles, LocalPoolDataSetBase* replyDataSet, - size_t replyLen, bool periodic, bool hasDifferentReplyId, DeviceCommandId_t replyId) { + size_t replyLen, bool periodic, bool hasDifferentReplyId, DeviceCommandId_t replyId, + Countdown* countdown) { // No need to check, as we may try to insert multiple times. insertInCommandMap(deviceCommand, hasDifferentReplyId, replyId); if (hasDifferentReplyId) { - return insertInReplyMap(replyId, maxDelayCycles, replyDataSet, replyLen, periodic); + return insertInReplyMap(replyId, maxDelayCycles, replyDataSet, replyLen, periodic, countdown); } else { - return insertInReplyMap(deviceCommand, maxDelayCycles, replyDataSet, replyLen, periodic); + return insertInReplyMap(deviceCommand, maxDelayCycles, replyDataSet, replyLen, periodic, + countdown); } } ReturnValue_t DeviceHandlerBase::insertInReplyMap(DeviceCommandId_t replyId, uint16_t maxDelayCycles, LocalPoolDataSetBase* dataSet, size_t replyLen, - bool periodic) { + bool periodic, Countdown* countdown) { DeviceReplyInfo info; info.maxDelayCycles = maxDelayCycles; info.periodic = periodic; @@ -437,6 +450,10 @@ ReturnValue_t DeviceHandlerBase::insertInReplyMap(DeviceCommandId_t replyId, info.replyLen = replyLen; info.dataSet = dataSet; info.command = deviceCommandMap.end(); + info.countdown = countdown; + if (info.periodic) { + info.active = true; + } auto resultPair = deviceReplyMap.emplace(replyId, info); if (resultPair.second) { return RETURN_OK; @@ -472,7 +489,8 @@ size_t DeviceHandlerBase::getNextReplyLength(DeviceCommandId_t commandId) { } DeviceReplyIter iter = deviceReplyMap.find(replyId); if (iter != deviceReplyMap.end()) { - if (iter->second.delayCycles != 0) { + if ((iter->second.delayCycles != 0 && iter->second.countdown == nullptr) || + (iter->second.active && iter->second.countdown != nullptr)) { return iter->second.replyLen; } } @@ -816,17 +834,18 @@ void DeviceHandlerBase::handleReply(const uint8_t* receivedData, DeviceCommandId DeviceReplyInfo* info = &(iter->second); - if (info->delayCycles != 0) { + if ((info->delayCycles != 0 && info->countdown == nullptr) || + (info->active && info->countdown != nullptr)) { result = interpretDeviceReply(foundId, receivedData); if (result == IGNORE_REPLY_DATA) { return; } - if (info->periodic) { - info->delayCycles = info->maxDelayCycles; - } else { - info->delayCycles = 0; + if (info->active && info->countdown != nullptr) { + disableTimeoutControlledReply(info); + } else if (info->delayCycles != 0) { + disableDelayCyclesControlledReply(info); } if (result != RETURN_OK) { @@ -845,6 +864,24 @@ void DeviceHandlerBase::handleReply(const uint8_t* receivedData, DeviceCommandId } } +void DeviceHandlerBase::disableTimeoutControlledReply(DeviceReplyInfo* info) { + if (info->periodic) { + info->countdown->resetTimer(); + } else { + info->active = false; + info->countdown->timeOut(); + } +} + +void DeviceHandlerBase::disableDelayCyclesControlledReply(DeviceReplyInfo* info) { + if (info->periodic) { + info->delayCycles = info->maxDelayCycles; + } else { + info->delayCycles = 0; + info->active = false; + } +} + ReturnValue_t DeviceHandlerBase::getStorageData(store_address_t storageAddress, uint8_t** data, size_t* len) { size_t lenTmp; @@ -970,6 +1007,10 @@ ReturnValue_t DeviceHandlerBase::enableReplyInReplyMap(DeviceCommandMap::iterato info->delayCycles = info->maxDelayCycles; info->command = command; command->second.expectedReplies = expectedReplies; + if (info->countdown != nullptr) { + info->countdown->resetTimer(); + } + info->active = true; return RETURN_OK; } else { return NO_REPLY_EXPECTED; @@ -1204,7 +1245,8 @@ void DeviceHandlerBase::setParentQueue(MessageQueueId_t parentQueueId) { bool DeviceHandlerBase::isAwaitingReply() { std::map::iterator iter; for (iter = deviceReplyMap.begin(); iter != deviceReplyMap.end(); ++iter) { - if (iter->second.delayCycles != 0) { + if ((iter->second.delayCycles != 0 && iter->second.countdown == nullptr) || + (iter->second.active && iter->second.countdown != nullptr)) { return true; } } @@ -1360,6 +1402,9 @@ uint8_t DeviceHandlerBase::getReplyDelayCycles(DeviceCommandId_t deviceCommand) if (iter == deviceReplyMap.end()) { return 0; } + else if (iter->second.countdown != nullptr) { + return 0; + } return iter->second.delayCycles; } diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index d8c3363e..4e457c33 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -451,6 +451,9 @@ class DeviceHandlerBase : public DeviceHandlerIF, * by the device repeatedly without request) or not. Default is aperiodic (0). * Please note that periodic replies are disabled by default. You can enable them with * #updatePeriodicReply + * @param countdown Instead of using maxDelayCycles to timeout a device reply it is also possible + * to provide a pointer to a Countdown object which will signal the timeout + * when expired * @return - @c RETURN_OK when the command was successfully inserted, * - @c RETURN_FAILED else. */ @@ -458,7 +461,8 @@ class DeviceHandlerBase : public DeviceHandlerIF, LocalPoolDataSetBase *replyDataSet = nullptr, size_t replyLen = 0, bool periodic = false, bool hasDifferentReplyId = false, - DeviceCommandId_t replyId = 0); + DeviceCommandId_t replyId = 0, + Countdown *countdown = nullptr); /** * @brief This is a helper method to insert replies in the reply map. * @param deviceCommand Identifier of the reply to add. @@ -468,12 +472,15 @@ class DeviceHandlerBase : public DeviceHandlerIF, * by the device repeatedly without request) or not. Default is aperiodic (0). * Please note that periodic replies are disabled by default. You can enable them with * #updatePeriodicReply + * @param countdown Instead of using maxDelayCycles to timeout a device reply it is also possible + * to provide a pointer to a Countdown object which will signal the timeout + * when expired * @return - @c RETURN_OK when the command was successfully inserted, * - @c RETURN_FAILED else. */ ReturnValue_t insertInReplyMap(DeviceCommandId_t deviceCommand, uint16_t maxDelayCycles, LocalPoolDataSetBase *dataSet = nullptr, size_t replyLen = 0, - bool periodic = false); + bool periodic = false, Countdown *countdown = nullptr); /** * @brief A simple command to add a command to the commandList. @@ -792,6 +799,11 @@ class DeviceHandlerBase : public DeviceHandlerIF, LocalPoolDataSetBase *dataSet = nullptr; //! The command that expects this reply. DeviceCommandMap::iterator command; + //! Instead of using delayCycles to specify the maximum time to wait for the device reply, it + //! is also possible specify a countdown + Countdown* countdown = nullptr; + //! will be set to true when reply is enabled + bool active = false; }; using DeviceReplyMap = std::map; @@ -1253,6 +1265,17 @@ class DeviceHandlerBase : public DeviceHandlerIF, */ void doGetRead(void); + /** + * @brief Handles disabling of replies which use a timeout to detect missed replies. + */ + void disableTimeoutControlledReply(DeviceReplyInfo* info); + + /** + * @brief Handles disabling of replies which use a number of maximum delay cycles to detect + * missed replies. + */ + void disableDelayCyclesControlledReply(DeviceReplyInfo* info); + /** * Retrive data from the #IPCStore. * diff --git a/src/fsfw/timemanager/Countdown.cpp b/src/fsfw/timemanager/Countdown.cpp index a8ba78cb..327995f8 100644 --- a/src/fsfw/timemanager/Countdown.cpp +++ b/src/fsfw/timemanager/Countdown.cpp @@ -1,7 +1,12 @@ #include "fsfw/timemanager/Countdown.h" -Countdown::Countdown(uint32_t initialTimeout) : timeout(initialTimeout) { - setTimeout(initialTimeout); +Countdown::Countdown(uint32_t initialTimeout, bool startImmediately) : timeout(initialTimeout) { + if (startImmediately) { + setTimeout(initialTimeout); + } + else { + timeout = initialTimeout; + } } Countdown::~Countdown() {} diff --git a/src/fsfw/timemanager/Countdown.h b/src/fsfw/timemanager/Countdown.h index 44be2b1a..26534789 100644 --- a/src/fsfw/timemanager/Countdown.h +++ b/src/fsfw/timemanager/Countdown.h @@ -26,8 +26,9 @@ class Countdown { * Otherwise a call to hasTimedOut might return True. * * @param initialTimeout Countdown duration in milliseconds + * @param startImmediately Set to false if countdown should not be started immediately */ - Countdown(uint32_t initialTimeout = 0); + Countdown(uint32_t initialTimeout = 0, bool startImmediately = true); ~Countdown(); /** * Call to set a new countdown duration. From 1739edd9b07933ddbdfcbfd19cf98146815ebe3f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 26 Apr 2022 10:32:37 +0200 Subject: [PATCH 105/404] warning fix for modern compilers --- src/fsfw/globalfunctions/DleParser.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/globalfunctions/DleParser.cpp b/src/fsfw/globalfunctions/DleParser.cpp index b68dbde1..71da7e6a 100644 --- a/src/fsfw/globalfunctions/DleParser.cpp +++ b/src/fsfw/globalfunctions/DleParser.cpp @@ -187,7 +187,7 @@ void DleParser::defaultErrorHandler(ErrorTypes err, ErrorInfo ctx) { case (ErrorTypes::ENCODED_BUF_TOO_SMALL): case (ErrorTypes::DECODING_BUF_TOO_SMALL): { char opt[64]; - snprintf(opt, sizeof(opt), ": Too small for packet with length %d", ctx.len); + snprintf(opt, sizeof(opt), ": Too small for packet with length %zu", ctx.len); if (err == ErrorTypes::ENCODED_BUF_TOO_SMALL) { errorPrinter("Encoded buf too small", opt); } else { From 3d047f9629c3a6fe25514f92dd008bd34d0784dd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 26 Apr 2022 11:15:24 +0200 Subject: [PATCH 106/404] trying to export ETL lib --- CMakeLists.txt | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3cc1671c..003b9723 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,7 +12,7 @@ set(MSG_PREFIX "fsfw |") set(FSFW_ETL_LIB_MAJOR_VERSION 20 CACHE STRING "ETL library major version requirement" ) -set(FSFW_ETL_LIB_VERSION ${FSFW_ETL_LIB_MAJOR_VERSION}.27.2 CACHE STRING +set(FSFW_ETL_LIB_VERSION ${FSFW_ETL_LIB_MAJOR_VERSION}.27.3 CACHE STRING "ETL library exact version requirement" ) @@ -132,7 +132,7 @@ if(FSFW_BUILD_UNITTESTS) endif() endif() -message(STATUS "Finding and/or providing ETL library") +message(STATUS "Finding and/or providing ETL library with version ${FSFW_ETL_LIB_MAJOR_VERSION}") # Check whether the user has already installed ETL first find_package(${FSFW_ETL_LIB_NAME} ${FSFW_ETL_LIB_MAJOR_VERSION} QUIET) @@ -153,6 +153,11 @@ if(NOT ${FSFW_ETL_LIB_NAME}_FOUND) FetchContent_MakeAvailable(etl) endif() +export(EXPORT etl + FILE "${CMAKE_CURRENT_BINARY_DIR}/cmake/etl.cmake" + NAMESPACE fsfw:: +) + set(FSFW_CORE_INC_PATH "inc") set_property(CACHE FSFW_OSAL PROPERTY STRINGS host linux rtems freertos) From 6a8da303fbfb9dd9c0178b0b68d1d08ab1880d49 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 26 Apr 2022 14:06:30 +0200 Subject: [PATCH 107/404] exporting etl target --- CMakeLists.txt | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 003b9723..e5b945f4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -153,10 +153,18 @@ if(NOT ${FSFW_ETL_LIB_NAME}_FOUND) FetchContent_MakeAvailable(etl) endif() -export(EXPORT etl - FILE "${CMAKE_CURRENT_BINARY_DIR}/cmake/etl.cmake" - NAMESPACE fsfw:: +# Export the ETL library and make it available in the fsfw namespace. This allows user code +# to use the same ETL dependency the framework is using +# You can do this by using the two following directives +# include(fsfw-etl) +# target_link_libraries(${TARGET_NAME} PRIVATE fsfw::etl) +export( + TARGETS ${FSFW_ETL_LIB_NAME} + FILE "${CMAKE_CURRENT_BINARY_DIR}/cmake/fsfw-etl.cmake" + NAMESPACE fsfw:: ) +# Append the export directory to the module path +list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_BINARY_DIR}/cmake") set(FSFW_CORE_INC_PATH "inc") From c5a7b98a7dc7c53b5dffbbcf2869c05be29a1208 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 26 Apr 2022 16:24:14 +0200 Subject: [PATCH 108/404] name correction --- CMakeLists.txt | 17 ++--------------- 1 file changed, 2 insertions(+), 15 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e5b945f4..25d9bb93 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -132,14 +132,14 @@ if(FSFW_BUILD_UNITTESTS) endif() endif() -message(STATUS "Finding and/or providing ETL library with version ${FSFW_ETL_LIB_MAJOR_VERSION}") +message(STATUS "Finding and/or providing etl library with version ${FSFW_ETL_LIB_MAJOR_VERSION}") # Check whether the user has already installed ETL first find_package(${FSFW_ETL_LIB_NAME} ${FSFW_ETL_LIB_MAJOR_VERSION} QUIET) # Not installed, so use FetchContent to download and provide etl if(NOT ${FSFW_ETL_LIB_NAME}_FOUND) message(STATUS - "No ETL installation was found with find_package. Installing and providing " + "No etl installation was found with find_package. Installing and providing " "etl with FindPackage" ) include(FetchContent) @@ -153,19 +153,6 @@ if(NOT ${FSFW_ETL_LIB_NAME}_FOUND) FetchContent_MakeAvailable(etl) endif() -# Export the ETL library and make it available in the fsfw namespace. This allows user code -# to use the same ETL dependency the framework is using -# You can do this by using the two following directives -# include(fsfw-etl) -# target_link_libraries(${TARGET_NAME} PRIVATE fsfw::etl) -export( - TARGETS ${FSFW_ETL_LIB_NAME} - FILE "${CMAKE_CURRENT_BINARY_DIR}/cmake/fsfw-etl.cmake" - NAMESPACE fsfw:: -) -# Append the export directory to the module path -list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_BINARY_DIR}/cmake") - set(FSFW_CORE_INC_PATH "inc") set_property(CACHE FSFW_OSAL PROPERTY STRINGS host linux rtems freertos) From ec2e274f22e3e254469abc9819bc906f03fe590a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 26 Apr 2022 16:31:45 +0200 Subject: [PATCH 109/404] find_package call for Catch2 quiet --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 25d9bb93..2354f504 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -98,7 +98,7 @@ endif() if(FSFW_BUILD_UNITTESTS) message(STATUS "${MSG_PREFIX} Building the FSFW unittests in addition to the static library") # Check whether the user has already installed Catch2 first - find_package(Catch2 ${FSFW_CATCH2_LIB_MAJOR_VERSION}) + find_package(Catch2 ${FSFW_CATCH2_LIB_MAJOR_VERSION} QUIET) # Not installed, so use FetchContent to download and provide Catch2 if(NOT Catch2_FOUND) message(STATUS "${MSG_PREFIX} Catch2 installation not found. Downloading Catch2 library with FetchContent") From c1be1fe2320eed167e808eecf3e582c87b688cde Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 26 Apr 2022 20:06:26 +0200 Subject: [PATCH 110/404] update CMakeLists.txt etl handling --- CMakeLists.txt | 34 +++++++++---------- .../devicehandlers/MgmLIS3MDLHandler.cpp | 2 +- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2354f504..337a56f4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,5 @@ cmake_minimum_required(VERSION 3.13) +project(${LIB_FSFW_NAME}) set(FSFW_VERSION_IF_GIT_FAILS 4) set(FSFW_SUBVERSION_IF_GIT_FAILS 0) @@ -60,7 +61,6 @@ set(LIB_FSFW_NAME fsfw) set(FSFW_TEST_TGT fsfw-tests) set(FSFW_DUMMY_TGT fsfw-dummy) -project(${LIB_FSFW_NAME}) add_library(${LIB_FSFW_NAME}) set(FSFW_GIT_VER_HANDLING_OK FALSE) @@ -98,7 +98,7 @@ endif() if(FSFW_BUILD_UNITTESTS) message(STATUS "${MSG_PREFIX} Building the FSFW unittests in addition to the static library") # Check whether the user has already installed Catch2 first - find_package(Catch2 ${FSFW_CATCH2_LIB_MAJOR_VERSION} QUIET) + find_package(Catch2 ${FSFW_CATCH2_LIB_MAJOR_VERSION} CONFIG QUIET) # Not installed, so use FetchContent to download and provide Catch2 if(NOT Catch2_FOUND) message(STATUS "${MSG_PREFIX} Catch2 installation not found. Downloading Catch2 library with FetchContent") @@ -135,23 +135,23 @@ endif() message(STATUS "Finding and/or providing etl library with version ${FSFW_ETL_LIB_MAJOR_VERSION}") # Check whether the user has already installed ETL first -find_package(${FSFW_ETL_LIB_NAME} ${FSFW_ETL_LIB_MAJOR_VERSION} QUIET) +# find_package(${FSFW_ETL_LIB_NAME} ${FSFW_ETL_LIB_MAJOR_VERSION} CONFIG QUIET) # Not installed, so use FetchContent to download and provide etl -if(NOT ${FSFW_ETL_LIB_NAME}_FOUND) - message(STATUS - "No etl installation was found with find_package. Installing and providing " - "etl with FindPackage" - ) - include(FetchContent) +# if(NOT ${FSFW_ETL_LIB_NAME}_FOUND) +message(STATUS + "No etl installation was found with find_package. Installing and providing " + "etl with FetchContent" +) +include(FetchContent) - FetchContent_Declare( - ${FSFW_ETL_LIB_NAME} - GIT_REPOSITORY https://github.com/ETLCPP/etl - GIT_TAG ${FSFW_ETL_LIB_VERSION} - ) +FetchContent_Declare( + ${FSFW_ETL_LIB_NAME} + GIT_REPOSITORY https://github.com/ETLCPP/etl + GIT_TAG ${FSFW_ETL_LIB_VERSION} +) - FetchContent_MakeAvailable(etl) -endif() +FetchContent_MakeAvailable(${FSFW_ETL_LIB_NAME}) +# endif() set(FSFW_CORE_INC_PATH "inc") @@ -414,7 +414,7 @@ target_compile_options(${LIB_FSFW_NAME} PRIVATE ) target_link_libraries(${LIB_FSFW_NAME} PRIVATE - ${FSFW_ETL_LIB_NAME} + etl ${FSFW_ADDITIONAL_LINK_LIBS} ) diff --git a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp index a887934d..a13ae791 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp @@ -375,7 +375,7 @@ float MgmLIS3MDLHandler::getSensitivityFactor(MGMLIS3MDL::Sensitivies sens) { ReturnValue_t MgmLIS3MDLHandler::enableTemperatureSensor(const uint8_t *commandData, size_t commandDataLen) { - if(commandData == nullptr) { + if (commandData == nullptr) { return INVALID_COMMAND_PARAMETER; } triggerEvent(CHANGE_OF_SETUP_PARAMETER); From 3225a8e350efbc3ff2e8d9dd9757de52be90d4fd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 28 Apr 2022 16:48:14 +0200 Subject: [PATCH 111/404] added option to change initial submode --- src/fsfw/subsystem/Subsystem.cpp | 6 +++++- src/fsfw/subsystem/Subsystem.h | 3 ++- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/fsfw/subsystem/Subsystem.cpp b/src/fsfw/subsystem/Subsystem.cpp index 1e860efc..27e6ae8e 100644 --- a/src/fsfw/subsystem/Subsystem.cpp +++ b/src/fsfw/subsystem/Subsystem.cpp @@ -467,6 +467,7 @@ ReturnValue_t Subsystem::initialize() { } mode = initialMode; + submode = initSubmode; return RETURN_OK; } @@ -604,7 +605,10 @@ ReturnValue_t Subsystem::checkObjectConnections() { return RETURN_OK; } -void Subsystem::setInitialMode(Mode_t mode) { initialMode = mode; } +void Subsystem::setInitialMode(Mode_t mode, Submode_t submode) { + this->initialMode = mode; + this->initSubmode = submode; +} void Subsystem::cantKeepMode() { ReturnValue_t result; diff --git a/src/fsfw/subsystem/Subsystem.h b/src/fsfw/subsystem/Subsystem.h index a3ee5744..1055def3 100644 --- a/src/fsfw/subsystem/Subsystem.h +++ b/src/fsfw/subsystem/Subsystem.h @@ -78,7 +78,7 @@ class Subsystem : public SubsystemBase, public HasModeSequenceIF { ReturnValue_t addTable(ArrayList *table, Mode_t id, bool inStore = true, bool preInit = true); - void setInitialMode(Mode_t mode); + void setInitialMode(Mode_t mode, Submode_t submode = SUBMODE_NONE); virtual ReturnValue_t initialize() override; @@ -117,6 +117,7 @@ class Subsystem : public SubsystemBase, public HasModeSequenceIF { Submode_t targetSubmode; Mode_t initialMode = 0; + Submode_t initSubmode = SUBMODE_NONE; HybridIterator currentSequenceIterator; From 43aad11859d5ca772d4323b24626d7a94646f3fd Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Fri, 29 Apr 2022 07:43:52 +0200 Subject: [PATCH 112/404] space packet bugfix --- src/fsfw/tmtcpacket/SpacePacket.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/fsfw/tmtcpacket/SpacePacket.cpp b/src/fsfw/tmtcpacket/SpacePacket.cpp index c8ebbd97..16d968fb 100644 --- a/src/fsfw/tmtcpacket/SpacePacket.cpp +++ b/src/fsfw/tmtcpacket/SpacePacket.cpp @@ -19,8 +19,8 @@ SpacePacket::SpacePacket(uint16_t packetDataLength, bool isTelecommand, uint16_t SpacePacket::~SpacePacket(void) {} bool SpacePacket::addWholeData(const uint8_t* p_Data, uint32_t packet_size) { - if (packet_size <= sizeof(this->data)) { - memcpy(&this->localData.byteStream, p_Data, packet_size); + if (packet_size <= sizeof(this->localData)) { + memcpy(this->localData.byteStream, p_Data, packet_size); return true; } else { return false; From 2220120d5487a06036ec6009f2be923d0b4a6331 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 3 May 2022 16:43:15 +0200 Subject: [PATCH 113/404] improved i2c error printout --- hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp | 22 +++++++++++---------- hal/src/fsfw_hal/linux/uart/UartComIF.cpp | 2 +- src/fsfw/devicehandlers/DeviceHandlerBase.h | 3 ++- src/fsfw/objectmanager/SystemObject.h | 3 ++- src/fsfw/osal/common/UdpTcPollingTask.cpp | 2 +- src/fsfw/osal/common/UdpTmTcBridge.cpp | 4 ++-- src/fsfw/osal/common/UdpTmTcBridge.h | 4 ++-- 7 files changed, 22 insertions(+), 18 deletions(-) diff --git a/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp index 4f53dc1f..1740b022 100644 --- a/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp +++ b/hal/src/fsfw_hal/linux/i2c/I2cComIF.cpp @@ -170,18 +170,20 @@ ReturnValue_t I2cComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLe int readLen = read(fd, replyBuffer, requestLen); if (readLen != static_cast(requestLen)) { -#if FSFW_VERBOSE_LEVEL >= 1 and FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "I2cComIF::requestReceiveMessage: Reading from I2C " - << "device failed with error code " << errno << ". Description" - << " of error: " << strerror(errno) << std::endl; - sif::error << "I2cComIF::requestReceiveMessage: Read only " << readLen << " from " << requestLen - << " bytes" << std::endl; +#if FSFW_VERBOSE_LEVEL >= 1 +#if FSFW_CPP_OSTREAM_ENABLED == 1 + if (readLen < 0) { + sif::warning << "I2cComIF::requestReceiveMessage: Reading from I2C " + << "device failed with error code " << errno << " | " << strerror(errno) + << std::endl; + } else { + sif::warning << "I2cComIF::requestReceiveMessage: Read only " << readLen << " from " + << requestLen << " bytes" << std::endl; + } +#else +#endif #endif i2cDeviceMapIter->second.replyLen = 0; -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::debug << "I2cComIF::requestReceiveMessage: Read " << readLen << " of " << requestLen - << " bytes" << std::endl; -#endif return HasReturnvaluesIF::RETURN_FAILED; } diff --git a/hal/src/fsfw_hal/linux/uart/UartComIF.cpp b/hal/src/fsfw_hal/linux/uart/UartComIF.cpp index 72c79df6..f77bdeae 100644 --- a/hal/src/fsfw_hal/linux/uart/UartComIF.cpp +++ b/hal/src/fsfw_hal/linux/uart/UartComIF.cpp @@ -314,7 +314,7 @@ void UartComIF::configureBaudrate(struct termios* options, UartCookie* uartCooki cfsetispeed(options, B4000000); cfsetospeed(options, B4000000); break; -#endif // ! __APPLE__ +#endif // ! __APPLE__ default: #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "UartComIF::configureBaudrate: Baudrate not supported" << std::endl; diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index 4a08e60f..e833e129 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -1072,7 +1072,8 @@ class DeviceHandlerBase : public DeviceHandlerIF, /** * Same as triggerEvent, but for forwarding if object is used as proxy. */ - virtual void forwardEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0) const override; + virtual void forwardEvent(Event event, uint32_t parameter1 = 0, + uint32_t parameter2 = 0) const override; /** * Checks if current mode is transitional mode. diff --git a/src/fsfw/objectmanager/SystemObject.h b/src/fsfw/objectmanager/SystemObject.h index eeb68b8f..c541ac5e 100644 --- a/src/fsfw/objectmanager/SystemObject.h +++ b/src/fsfw/objectmanager/SystemObject.h @@ -50,7 +50,8 @@ class SystemObject : public SystemObjectIF { virtual ReturnValue_t initialize() override; virtual ReturnValue_t checkObjectConnections() override; - virtual void forwardEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0) const override; + virtual void forwardEvent(Event event, uint32_t parameter1 = 0, + uint32_t parameter2 = 0) const override; }; #endif /* FSFW_OBJECTMANAGER_SYSTEMOBJECT_H_ */ diff --git a/src/fsfw/osal/common/UdpTcPollingTask.cpp b/src/fsfw/osal/common/UdpTcPollingTask.cpp index 38fb1921..bcc8e9e3 100644 --- a/src/fsfw/osal/common/UdpTcPollingTask.cpp +++ b/src/fsfw/osal/common/UdpTcPollingTask.cpp @@ -154,7 +154,7 @@ void UdpTcPollingTask::setTimeout(double timeoutSeconds) { #endif } #elif defined(PLATFORM_UNIX) - timeval tval {}; + timeval tval{}; tval = timevalOperations::toTimeval(timeoutSeconds); int result = setsockopt(serverSocket, SOL_SOCKET, SO_RCVTIMEO, &tval, sizeof(receptionTimeout)); if (result == -1) { diff --git a/src/fsfw/osal/common/UdpTmTcBridge.cpp b/src/fsfw/osal/common/UdpTmTcBridge.cpp index 6089f266..e3cad58f 100644 --- a/src/fsfw/osal/common/UdpTmTcBridge.cpp +++ b/src/fsfw/osal/common/UdpTmTcBridge.cpp @@ -20,7 +20,7 @@ const std::string UdpTmTcBridge::DEFAULT_SERVER_PORT = tcpip::DEFAULT_SERVER_PORT; UdpTmTcBridge::UdpTmTcBridge(object_id_t objectId, object_id_t tcDestination, - const std::string& udpServerPort_, object_id_t tmStoreId, + const std::string &udpServerPort_, object_id_t tmStoreId, object_id_t tcStoreId) : TmTcBridge(objectId, tcDestination, tmStoreId, tcStoreId) { if (udpServerPort_.empty()) { @@ -118,7 +118,7 @@ ReturnValue_t UdpTmTcBridge::sendTm(const uint8_t *data, size_t dataLen) { #endif ssize_t bytesSent = sendto(serverSocket, reinterpret_cast(data), dataLen, flags, - &clientAddress, clientAddressLen); + &clientAddress, clientAddressLen); if (bytesSent == SOCKET_ERROR) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "TmTcUdpBridge::sendTm: Send operation failed." << std::endl; diff --git a/src/fsfw/osal/common/UdpTmTcBridge.h b/src/fsfw/osal/common/UdpTmTcBridge.h index b0a67430..92829c46 100644 --- a/src/fsfw/osal/common/UdpTmTcBridge.h +++ b/src/fsfw/osal/common/UdpTmTcBridge.h @@ -29,8 +29,8 @@ class UdpTmTcBridge : public TmTcBridge, public TcpIpBase { /* The ports chosen here should not be used by any other process. */ static const std::string DEFAULT_SERVER_PORT; - UdpTmTcBridge(object_id_t objectId, object_id_t tcDestination, const std::string& udpServerPort = "", - object_id_t tmStoreId = objects::TM_STORE, + UdpTmTcBridge(object_id_t objectId, object_id_t tcDestination, + const std::string& udpServerPort = "", object_id_t tmStoreId = objects::TM_STORE, object_id_t tcStoreId = objects::TC_STORE); ~UdpTmTcBridge() override; From a9041b84a3adc360106c1b4530d4df9d5dc9c080 Mon Sep 17 00:00:00 2001 From: Cleanroom Laptop L15 Date: Wed, 4 May 2022 10:27:20 +0200 Subject: [PATCH 114/404] update read gpio API --- hal/src/fsfw_hal/common/gpio/GpioIF.h | 2 +- hal/src/fsfw_hal/common/gpio/gpioDefinitions.h | 2 +- hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp | 4 ++-- hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/hal/src/fsfw_hal/common/gpio/GpioIF.h b/hal/src/fsfw_hal/common/gpio/GpioIF.h index 5cca1481..2c284094 100644 --- a/hal/src/fsfw_hal/common/gpio/GpioIF.h +++ b/hal/src/fsfw_hal/common/gpio/GpioIF.h @@ -48,7 +48,7 @@ class GpioIF : public HasReturnvaluesIF { * @param gpioId A unique number which specifies the GPIO to read. * @param gpioState State of GPIO will be written to this pointer. */ - virtual ReturnValue_t readGpio(gpioId_t gpioId, int* gpioState) = 0; + virtual ReturnValue_t readGpio(gpioId_t gpioId, gpio::Levels& gpioState) = 0; }; #endif /* COMMON_GPIO_GPIOIF_H_ */ diff --git a/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h b/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h index eb90629e..a15ffbc0 100644 --- a/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h +++ b/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h @@ -9,7 +9,7 @@ using gpioId_t = uint16_t; namespace gpio { -enum class Levels : int { LOW = 0, HIGH = 1, NONE = 99 }; +enum class Levels : int { LOW = 0, HIGH = 1, FAILED = -1, NONE = 99 }; enum class Direction : int { IN = 0, OUT = 1 }; diff --git a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp index f46ad386..e9da5b03 100644 --- a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp +++ b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp @@ -280,7 +280,7 @@ ReturnValue_t LinuxLibgpioIF::driveGpio(gpioId_t gpioId, GpiodRegularBase& regul return RETURN_OK; } -ReturnValue_t LinuxLibgpioIF::readGpio(gpioId_t gpioId, int* gpioState) { +ReturnValue_t LinuxLibgpioIF::readGpio(gpioId_t gpioId, gpio::Levels& gpioState) { gpioMapIter = gpioMap.find(gpioId); if (gpioMapIter == gpioMap.end()) { #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -299,7 +299,7 @@ ReturnValue_t LinuxLibgpioIF::readGpio(gpioId_t gpioId, int* gpioState) { if (regularGpio == nullptr) { return GPIO_TYPE_FAILURE; } - *gpioState = gpiod_line_get_value(regularGpio->lineHandle); + gpioState = static_cast(gpiod_line_get_value(regularGpio->lineHandle)); } else { auto gpioCallback = dynamic_cast(gpioMapIter->second); if (gpioCallback->callback == nullptr) { diff --git a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h index fcc9c775..4ac2e7d9 100644 --- a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +++ b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h @@ -38,7 +38,7 @@ class LinuxLibgpioIF : public GpioIF, public SystemObject { ReturnValue_t addGpios(GpioCookie* gpioCookie) override; ReturnValue_t pullHigh(gpioId_t gpioId) override; ReturnValue_t pullLow(gpioId_t gpioId) override; - ReturnValue_t readGpio(gpioId_t gpioId, int* gpioState) override; + ReturnValue_t readGpio(gpioId_t gpioId, gpio::Levels& gpioState) override; private: static const size_t MAX_CHIPNAME_LENGTH = 11; From 3556eca8e875aaa4aab1c96e60f0b1eaefc7e982 Mon Sep 17 00:00:00 2001 From: Cleanroom Laptop L15 Date: Wed, 4 May 2022 10:33:55 +0200 Subject: [PATCH 115/404] error check on line getter --- hal/src/fsfw_hal/common/gpio/GpioIF.h | 2 +- hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/hal/src/fsfw_hal/common/gpio/GpioIF.h b/hal/src/fsfw_hal/common/gpio/GpioIF.h index 2c284094..f8ef9d9c 100644 --- a/hal/src/fsfw_hal/common/gpio/GpioIF.h +++ b/hal/src/fsfw_hal/common/gpio/GpioIF.h @@ -46,7 +46,7 @@ class GpioIF : public HasReturnvaluesIF { * an ouput or input gpio. * * @param gpioId A unique number which specifies the GPIO to read. - * @param gpioState State of GPIO will be written to this pointer. + * @param gpioState State of GPIO will be written to this reference */ virtual ReturnValue_t readGpio(gpioId_t gpioId, gpio::Levels& gpioState) = 0; }; diff --git a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp index e9da5b03..03bc1830 100644 --- a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp +++ b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp @@ -300,6 +300,10 @@ ReturnValue_t LinuxLibgpioIF::readGpio(gpioId_t gpioId, gpio::Levels& gpioState) return GPIO_TYPE_FAILURE; } gpioState = static_cast(gpiod_line_get_value(regularGpio->lineHandle)); + if (gpioState == gpio::Levels::FAILED) { + // TODO: Print error + return RETURN_FAILED; + } } else { auto gpioCallback = dynamic_cast(gpioMapIter->second); if (gpioCallback->callback == nullptr) { From 8ee26f81f9449e28566a8d34fbb3454cfd1da01c Mon Sep 17 00:00:00 2001 From: Cleanroom Laptop L15 Date: Wed, 4 May 2022 10:36:32 +0200 Subject: [PATCH 116/404] dedicated returnvalue for line get failure --- hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp | 3 +-- hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h | 4 +++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp index 03bc1830..136dfe11 100644 --- a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp +++ b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp @@ -301,8 +301,7 @@ ReturnValue_t LinuxLibgpioIF::readGpio(gpioId_t gpioId, gpio::Levels& gpioState) } gpioState = static_cast(gpiod_line_get_value(regularGpio->lineHandle)); if (gpioState == gpio::Levels::FAILED) { - // TODO: Print error - return RETURN_FAILED; + return GPIO_GET_VALUE_FAILED; } } else { auto gpioCallback = dynamic_cast(gpioMapIter->second); diff --git a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h index 4ac2e7d9..a50e480d 100644 --- a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +++ b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h @@ -31,7 +31,9 @@ class LinuxLibgpioIF : public GpioIF, public SystemObject { HasReturnvaluesIF::makeReturnCode(gpioRetvalId, 5); static constexpr ReturnValue_t GPIO_INIT_FAILED = HasReturnvaluesIF::makeReturnCode(gpioRetvalId, 6); - + // Will be returned if getting the line value failed. Error type will be set to errno in this case + static constexpr ReturnValue_t GPIO_GET_VALUE_FAILED = + HasReturnvaluesIF::makeReturnCode(gpioRetvalId, 7); LinuxLibgpioIF(object_id_t objectId); virtual ~LinuxLibgpioIF(); From f59b05c86cd54390d2a259eda770ee7935587112 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 5 May 2022 02:00:41 +0200 Subject: [PATCH 117/404] use warning instead of error --- src/fsfw/osal/freertos/FixedTimeslotTask.cpp | 2 +- src/fsfw/osal/linux/FixedTimeslotTask.cpp | 2 +- src/fsfw/osal/rtems/FixedTimeslotTask.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fsfw/osal/freertos/FixedTimeslotTask.cpp b/src/fsfw/osal/freertos/FixedTimeslotTask.cpp index e0e3779e..0f3bfe18 100644 --- a/src/fsfw/osal/freertos/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/freertos/FixedTimeslotTask.cpp @@ -46,7 +46,7 @@ void FixedTimeslotTask::missedDeadlineCounter() { FixedTimeslotTask::deadlineMissedCount++; if (FixedTimeslotTask::deadlineMissedCount % 10 == 0) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PST missed " << FixedTimeslotTask::deadlineMissedCount << " deadlines." + sif::warning << "PST missed " << FixedTimeslotTask::deadlineMissedCount << " deadlines." << std::endl; #endif } diff --git a/src/fsfw/osal/linux/FixedTimeslotTask.cpp b/src/fsfw/osal/linux/FixedTimeslotTask.cpp index 1f4d6e23..d64c020d 100644 --- a/src/fsfw/osal/linux/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/linux/FixedTimeslotTask.cpp @@ -87,7 +87,7 @@ void FixedTimeslotTask::missedDeadlineCounter() { FixedTimeslotTask::deadlineMissedCount++; if (FixedTimeslotTask::deadlineMissedCount % 10 == 0) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PST missed " << FixedTimeslotTask::deadlineMissedCount << " deadlines." + sif::warning << "PST missed " << FixedTimeslotTask::deadlineMissedCount << " deadlines." << std::endl; #endif } diff --git a/src/fsfw/osal/rtems/FixedTimeslotTask.cpp b/src/fsfw/osal/rtems/FixedTimeslotTask.cpp index 3347739a..12434e1b 100644 --- a/src/fsfw/osal/rtems/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/rtems/FixedTimeslotTask.cpp @@ -50,7 +50,7 @@ void FixedTimeslotTask::missedDeadlineCounter() { FixedTimeslotTask::deadlineMissedCount++; if (FixedTimeslotTask::deadlineMissedCount % 10 == 0) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PST missed " << FixedTimeslotTask::deadlineMissedCount << " deadlines." + sif::warning << "PST missed " << FixedTimeslotTask::deadlineMissedCount << " deadlines." << std::endl; #endif } From 71f704c980ec8847bdd0bb1bf354c8de30d3524b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 5 May 2022 12:29:46 +0200 Subject: [PATCH 118/404] remove the dot --- src/fsfw/osal/freertos/FixedTimeslotTask.cpp | 2 +- src/fsfw/osal/linux/FixedTimeslotTask.cpp | 2 +- src/fsfw/osal/rtems/FixedTimeslotTask.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fsfw/osal/freertos/FixedTimeslotTask.cpp b/src/fsfw/osal/freertos/FixedTimeslotTask.cpp index 0f3bfe18..e8823f8a 100644 --- a/src/fsfw/osal/freertos/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/freertos/FixedTimeslotTask.cpp @@ -46,7 +46,7 @@ void FixedTimeslotTask::missedDeadlineCounter() { FixedTimeslotTask::deadlineMissedCount++; if (FixedTimeslotTask::deadlineMissedCount % 10 == 0) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "PST missed " << FixedTimeslotTask::deadlineMissedCount << " deadlines." + sif::warning << "PST missed " << FixedTimeslotTask::deadlineMissedCount << " deadlines" << std::endl; #endif } diff --git a/src/fsfw/osal/linux/FixedTimeslotTask.cpp b/src/fsfw/osal/linux/FixedTimeslotTask.cpp index d64c020d..37b958de 100644 --- a/src/fsfw/osal/linux/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/linux/FixedTimeslotTask.cpp @@ -87,7 +87,7 @@ void FixedTimeslotTask::missedDeadlineCounter() { FixedTimeslotTask::deadlineMissedCount++; if (FixedTimeslotTask::deadlineMissedCount % 10 == 0) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "PST missed " << FixedTimeslotTask::deadlineMissedCount << " deadlines." + sif::warning << "PST missed " << FixedTimeslotTask::deadlineMissedCount << " deadlines" << std::endl; #endif } diff --git a/src/fsfw/osal/rtems/FixedTimeslotTask.cpp b/src/fsfw/osal/rtems/FixedTimeslotTask.cpp index 12434e1b..745b8d70 100644 --- a/src/fsfw/osal/rtems/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/rtems/FixedTimeslotTask.cpp @@ -50,7 +50,7 @@ void FixedTimeslotTask::missedDeadlineCounter() { FixedTimeslotTask::deadlineMissedCount++; if (FixedTimeslotTask::deadlineMissedCount % 10 == 0) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "PST missed " << FixedTimeslotTask::deadlineMissedCount << " deadlines." + sif::warning << "PST missed " << FixedTimeslotTask::deadlineMissedCount << " deadlines" << std::endl; #endif } From d72b212fa629029924d9862e3e862d0388911f8e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 6 May 2022 10:36:01 +0200 Subject: [PATCH 119/404] cmakelists.txt hotfix --- CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2f10ab8e..5c898369 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,5 @@ cmake_minimum_required(VERSION 3.13) +set(LIB_FSFW_NAME fsfw) project(${LIB_FSFW_NAME}) set(FSFW_VERSION_IF_GIT_FAILS 4) @@ -58,7 +59,7 @@ option(FSFW_ADD_TMSTORAGE "Compile with tm storage components" OFF) # Contrib sources option(FSFW_ADD_SGP4_PROPAGATOR "Add SGP4 propagator code" OFF) -set(LIB_FSFW_NAME fsfw) + set(FSFW_TEST_TGT fsfw-tests) set(FSFW_DUMMY_TGT fsfw-dummy) From 29c3a4376058bb392edd9ef25623723f000058a4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 11 May 2022 10:54:13 +0200 Subject: [PATCH 120/404] getter functions for speed and mode --- hal/src/fsfw_hal/linux/spi/SpiComIF.cpp | 14 ++++++++++++++ hal/src/fsfw_hal/linux/spi/SpiComIF.h | 1 + 2 files changed, 15 insertions(+) diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp index dcf92b5d..db694613 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -410,3 +410,17 @@ void SpiComIF::setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed) utility::handleIoctlError("SpiComIF::setSpiSpeedAndMode: Updating SPI default clock failed"); } } + +void SpiComIF::getSpiSpeedAndMode(int spiFd, spi::SpiModes &mode, uint32_t &speed) const { + uint8_t tmpMode = 0; + int retval = ioctl(spiFd, SPI_IOC_RD_MODE, &tmpMode); + if (retval != 0) { + utility::handleIoctlError("SpiComIF::getSpiSpeedAndMode: Reading SPI mode failed"); + } + mode = static_cast(tmpMode); + + retval = ioctl(spiFd, SPI_IOC_RD_MAX_SPEED_HZ, &speed); + if (retval != 0) { + utility::handleIoctlError("SpiComIF::getSpiSpeedAndMode: Getting SPI speed failed"); + } +} diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.h b/hal/src/fsfw_hal/linux/spi/SpiComIF.h index 357afa2f..ded47e04 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.h +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.h @@ -59,6 +59,7 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { GpioIF* getGpioInterface(); void setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed); + void getSpiSpeedAndMode(int spiFd, spi::SpiModes& mode, uint32_t& speed) const; void performSpiWiretapping(SpiCookie* spiCookie); ReturnValue_t getReadBuffer(address_t spiAddress, uint8_t** buffer); From 0e880de0d08d05b14125a5af785930aeff636d1b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 11 May 2022 10:54:39 +0200 Subject: [PATCH 121/404] update changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 84fb2ca1..856580cf 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -46,6 +46,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/). ## Additions +- Linux HAL: Getter functions for SPI speed and mode. - Linux HAL: Add wiretapping option for I2C. Enabled with `FSFW_HAL_I2C_WIRETAPPING` defined to 1 - Dedicated Version class and constant `fsfw::FSFW_VERSION` containing version information inside `fsfw/version.h` From e06c457743991b2a0c2d84de9e169508ac428aa8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 11 May 2022 11:11:39 +0200 Subject: [PATCH 122/404] Cache SPI device name in ComIF - Architecturally, this makes a lot more sense because each ComIF should be responsible for one SPI bus --- hal/src/fsfw_hal/linux/UnixFileGuard.cpp | 2 +- hal/src/fsfw_hal/linux/UnixFileGuard.h | 2 +- hal/src/fsfw_hal/linux/spi/SpiComIF.cpp | 17 +++++------ hal/src/fsfw_hal/linux/spi/SpiComIF.h | 6 ++-- hal/src/fsfw_hal/linux/spi/SpiCookie.cpp | 29 +++++++++---------- hal/src/fsfw_hal/linux/spi/SpiCookie.h | 17 +++++------ src/fsfw/container/FixedArrayList.h | 1 + src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 3 +- src/fsfw/devicehandlers/DeviceHandlerBase.h | 6 ++-- src/fsfw/osal/common/TcpTmTcServer.cpp | 6 ++-- src/fsfw/osal/freertos/FixedTimeslotTask.cpp | 2 +- src/fsfw/osal/linux/FixedTimeslotTask.cpp | 2 +- src/fsfw/osal/rtems/FixedTimeslotTask.cpp | 2 +- src/fsfw/pus/CService201HealthCommanding.cpp | 3 +- src/fsfw/pus/CService201HealthCommanding.h | 2 +- src/fsfw/timemanager/Countdown.cpp | 3 +- 16 files changed, 49 insertions(+), 54 deletions(-) diff --git a/hal/src/fsfw_hal/linux/UnixFileGuard.cpp b/hal/src/fsfw_hal/linux/UnixFileGuard.cpp index 41293815..3e916ba2 100644 --- a/hal/src/fsfw_hal/linux/UnixFileGuard.cpp +++ b/hal/src/fsfw_hal/linux/UnixFileGuard.cpp @@ -6,7 +6,7 @@ #include "fsfw/FSFW.h" #include "fsfw/serviceinterface.h" -UnixFileGuard::UnixFileGuard(std::string device, int* fileDescriptor, int flags, +UnixFileGuard::UnixFileGuard(const std::string& device, int* fileDescriptor, int flags, std::string diagnosticPrefix) : fileDescriptor(fileDescriptor) { if (fileDescriptor == nullptr) { diff --git a/hal/src/fsfw_hal/linux/UnixFileGuard.h b/hal/src/fsfw_hal/linux/UnixFileGuard.h index d94234b6..04f379d6 100644 --- a/hal/src/fsfw_hal/linux/UnixFileGuard.h +++ b/hal/src/fsfw_hal/linux/UnixFileGuard.h @@ -15,7 +15,7 @@ class UnixFileGuard { static constexpr ReturnValue_t OPEN_FILE_FAILED = 1; - UnixFileGuard(std::string device, int* fileDescriptor, int flags, + UnixFileGuard(const std::string& device, int* fileDescriptor, int flags, std::string diagnosticPrefix = ""); virtual ~UnixFileGuard(); diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp index db694613..da95bfcb 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -15,8 +15,8 @@ #include "fsfw_hal/linux/spi/SpiCookie.h" #include "fsfw_hal/linux/utility.h" -SpiComIF::SpiComIF(object_id_t objectId, GpioIF* gpioComIF) - : SystemObject(objectId), gpioComIF(gpioComIF) { +SpiComIF::SpiComIF(object_id_t objectId, std::string devname, GpioIF* gpioComIF) + : SystemObject(objectId), gpioComIF(gpioComIF), dev(std::move(devname)) { if (gpioComIF == nullptr) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -85,8 +85,7 @@ ReturnValue_t SpiComIF::initializeInterface(CookieIF* cookie) { spiCookie->getSpiParameters(spiMode, spiSpeed, ¶ms); int fileDescriptor = 0; - UnixFileGuard fileHelper(spiCookie->getSpiDevice(), &fileDescriptor, O_RDWR, - "SpiComIF::initializeInterface"); + UnixFileGuard fileHelper(dev, &fileDescriptor, O_RDWR, "SpiComIF::initializeInterface"); if (fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) { return fileHelper.getOpenResult(); } @@ -182,8 +181,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const int retval = 0; /* Prepare transfer */ int fileDescriptor = 0; - std::string device = spiCookie->getSpiDevice(); - UnixFileGuard fileHelper(device, &fileDescriptor, O_RDWR, "SpiComIF::sendMessage"); + UnixFileGuard fileHelper(dev, &fileDescriptor, O_RDWR, "SpiComIF::sendMessage"); if (fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) { return OPENING_FILE_FAILED; } @@ -278,9 +276,8 @@ ReturnValue_t SpiComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLe ReturnValue_t SpiComIF::performHalfDuplexReception(SpiCookie* spiCookie) { ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; - std::string device = spiCookie->getSpiDevice(); int fileDescriptor = 0; - UnixFileGuard fileHelper(device, &fileDescriptor, O_RDWR, "SpiComIF::requestReceiveMessage"); + UnixFileGuard fileHelper(dev, &fileDescriptor, O_RDWR, "SpiComIF::requestReceiveMessage"); if (fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) { return OPENING_FILE_FAILED; } @@ -411,7 +408,7 @@ void SpiComIF::setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed) } } -void SpiComIF::getSpiSpeedAndMode(int spiFd, spi::SpiModes &mode, uint32_t &speed) const { +void SpiComIF::getSpiSpeedAndMode(int spiFd, spi::SpiModes& mode, uint32_t& speed) const { uint8_t tmpMode = 0; int retval = ioctl(spiFd, SPI_IOC_RD_MODE, &tmpMode); if (retval != 0) { @@ -424,3 +421,5 @@ void SpiComIF::getSpiSpeedAndMode(int spiFd, spi::SpiModes &mode, uint32_t &spee utility::handleIoctlError("SpiComIF::getSpiSpeedAndMode: Getting SPI speed failed"); } } + +const std::string& SpiComIF::getSpiDev() const { return dev; } diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.h b/hal/src/fsfw_hal/linux/spi/SpiComIF.h index ded47e04..881af8c4 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.h +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.h @@ -32,7 +32,7 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { static constexpr ReturnValue_t HALF_DUPLEX_TRANSFER_FAILED = HasReturnvaluesIF::makeReturnCode(spiRetvalId, 2); - SpiComIF(object_id_t objectId, GpioIF* gpioComIF); + SpiComIF(object_id_t objectId, std::string devname, GpioIF* gpioComIF); ReturnValue_t initializeInterface(CookieIF* cookie) override; ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override; @@ -60,6 +60,8 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { GpioIF* getGpioInterface(); void setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed); void getSpiSpeedAndMode(int spiFd, spi::SpiModes& mode, uint32_t& speed) const; + + const std::string& getSpiDev() const; void performSpiWiretapping(SpiCookie* spiCookie); ReturnValue_t getReadBuffer(address_t spiAddress, uint8_t** buffer); @@ -71,7 +73,7 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { }; GpioIF* gpioComIF = nullptr; - + std::string dev = ""; MutexIF* spiMutex = nullptr; MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; uint32_t timeoutMs = 20; diff --git a/hal/src/fsfw_hal/linux/spi/SpiCookie.cpp b/hal/src/fsfw_hal/linux/spi/SpiCookie.cpp index c94fcdf1..85f96f28 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiCookie.cpp +++ b/hal/src/fsfw_hal/linux/spi/SpiCookie.cpp @@ -1,26 +1,25 @@ #include "SpiCookie.h" -SpiCookie::SpiCookie(address_t spiAddress, gpioId_t chipSelect, std::string spiDev, - const size_t maxSize, spi::SpiModes spiMode, uint32_t spiSpeed) - : SpiCookie(spi::SpiComIfModes::REGULAR, spiAddress, chipSelect, spiDev, maxSize, spiMode, - spiSpeed, nullptr, nullptr) {} - -SpiCookie::SpiCookie(address_t spiAddress, std::string spiDev, const size_t maxSize, +SpiCookie::SpiCookie(address_t spiAddress, gpioId_t chipSelect, const size_t maxSize, spi::SpiModes spiMode, uint32_t spiSpeed) - : SpiCookie(spiAddress, gpio::NO_GPIO, spiDev, maxSize, spiMode, spiSpeed) {} + : SpiCookie(spi::SpiComIfModes::REGULAR, spiAddress, chipSelect, maxSize, spiMode, spiSpeed, + nullptr, nullptr) {} -SpiCookie::SpiCookie(address_t spiAddress, gpioId_t chipSelect, std::string spiDev, - const size_t maxSize, spi::SpiModes spiMode, uint32_t spiSpeed, +SpiCookie::SpiCookie(address_t spiAddress, const size_t maxSize, spi::SpiModes spiMode, + uint32_t spiSpeed) + : SpiCookie(spiAddress, gpio::NO_GPIO, maxSize, spiMode, spiSpeed) {} + +SpiCookie::SpiCookie(address_t spiAddress, gpioId_t chipSelect, const size_t maxSize, + spi::SpiModes spiMode, uint32_t spiSpeed, spi::send_callback_function_t callback, void* args) - : SpiCookie(spi::SpiComIfModes::CALLBACK, spiAddress, chipSelect, spiDev, maxSize, spiMode, - spiSpeed, callback, args) {} + : SpiCookie(spi::SpiComIfModes::CALLBACK, spiAddress, chipSelect, maxSize, spiMode, spiSpeed, + callback, args) {} SpiCookie::SpiCookie(spi::SpiComIfModes comIfMode, address_t spiAddress, gpioId_t chipSelect, - std::string spiDev, const size_t maxSize, spi::SpiModes spiMode, - uint32_t spiSpeed, spi::send_callback_function_t callback, void* args) + const size_t maxSize, spi::SpiModes spiMode, uint32_t spiSpeed, + spi::send_callback_function_t callback, void* args) : spiAddress(spiAddress), chipSelectPin(chipSelect), - spiDevice(spiDev), comIfMode(comIfMode), maxSize(maxSize), spiMode(spiMode), @@ -50,8 +49,6 @@ size_t SpiCookie::getMaxBufferSize() const { return maxSize; } address_t SpiCookie::getSpiAddress() const { return spiAddress; } -std::string SpiCookie::getSpiDevice() const { return spiDevice; } - void SpiCookie::setThreeWireSpi(bool enable) { uncommonParameters.threeWireSpi = enable; } void SpiCookie::setLsbFirst(bool enable) { uncommonParameters.lsbFirst = enable; } diff --git a/hal/src/fsfw_hal/linux/spi/SpiCookie.h b/hal/src/fsfw_hal/linux/spi/SpiCookie.h index 5f4bf2d5..d6d4078f 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiCookie.h +++ b/hal/src/fsfw_hal/linux/spi/SpiCookie.h @@ -29,23 +29,22 @@ class SpiCookie : public CookieIF { * @param spiDev * @param maxSize */ - SpiCookie(address_t spiAddress, gpioId_t chipSelect, std::string spiDev, const size_t maxSize, - spi::SpiModes spiMode, uint32_t spiSpeed); + SpiCookie(address_t spiAddress, gpioId_t chipSelect, const size_t maxSize, spi::SpiModes spiMode, + uint32_t spiSpeed); /** * Like constructor above, but without a dedicated GPIO CS. Can be used for hardware * slave select or if CS logic is performed with decoders. */ - SpiCookie(address_t spiAddress, std::string spiDev, const size_t maxReplySize, - spi::SpiModes spiMode, uint32_t spiSpeed); + SpiCookie(address_t spiAddress, const size_t maxReplySize, spi::SpiModes spiMode, + uint32_t spiSpeed); /** * Use the callback mode of the SPI communication interface. The user can pass the callback * function here or by using the setter function #setCallbackMode */ - SpiCookie(address_t spiAddress, gpioId_t chipSelect, std::string spiDev, const size_t maxSize, - spi::SpiModes spiMode, uint32_t spiSpeed, spi::send_callback_function_t callback, - void* args); + SpiCookie(address_t spiAddress, gpioId_t chipSelect, const size_t maxSize, spi::SpiModes spiMode, + uint32_t spiSpeed, spi::send_callback_function_t callback, void* args); /** * Get the callback function @@ -55,7 +54,6 @@ class SpiCookie : public CookieIF { void getCallback(spi::send_callback_function_t* callback, void** args); address_t getSpiAddress() const; - std::string getSpiDevice() const; gpioId_t getChipSelectPin() const; size_t getMaxBufferSize() const; @@ -154,12 +152,11 @@ class SpiCookie : public CookieIF { * @param args */ SpiCookie(spi::SpiComIfModes comIfMode, address_t spiAddress, gpioId_t chipSelect, - std::string spiDev, const size_t maxSize, spi::SpiModes spiMode, uint32_t spiSpeed, + const size_t maxSize, spi::SpiModes spiMode, uint32_t spiSpeed, spi::send_callback_function_t callback, void* args); address_t spiAddress; gpioId_t chipSelectPin; - std::string spiDevice; spi::SpiComIfModes comIfMode; diff --git a/src/fsfw/container/FixedArrayList.h b/src/fsfw/container/FixedArrayList.h index 11882537..fc8be393 100644 --- a/src/fsfw/container/FixedArrayList.h +++ b/src/fsfw/container/FixedArrayList.h @@ -12,6 +12,7 @@ template class FixedArrayList : public ArrayList { static_assert(MAX_SIZE <= std::numeric_limits::max(), "count_t is not large enough to hold MAX_SIZE"); + private: T data[MAX_SIZE]; diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 52178683..69baf54f 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1401,8 +1401,7 @@ uint8_t DeviceHandlerBase::getReplyDelayCycles(DeviceCommandId_t deviceCommand) DeviceReplyMap::iterator iter = deviceReplyMap.find(deviceCommand); if (iter == deviceReplyMap.end()) { return 0; - } - else if (iter->second.countdown != nullptr) { + } else if (iter->second.countdown != nullptr) { return 0; } return iter->second.delayCycles; diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index c67aed27..0a273675 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -801,7 +801,7 @@ class DeviceHandlerBase : public DeviceHandlerIF, DeviceCommandMap::iterator command; //! Instead of using delayCycles to specify the maximum time to wait for the device reply, it //! is also possible specify a countdown - Countdown* countdown = nullptr; + Countdown *countdown = nullptr; //! will be set to true when reply is enabled bool active = false; }; @@ -1269,13 +1269,13 @@ class DeviceHandlerBase : public DeviceHandlerIF, /** * @brief Handles disabling of replies which use a timeout to detect missed replies. */ - void disableTimeoutControlledReply(DeviceReplyInfo* info); + void disableTimeoutControlledReply(DeviceReplyInfo *info); /** * @brief Handles disabling of replies which use a number of maximum delay cycles to detect * missed replies. */ - void disableDelayCyclesControlledReply(DeviceReplyInfo* info); + void disableDelayCyclesControlledReply(DeviceReplyInfo *info); /** * Retrive data from the #IPCStore. diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index 91cb9574..b9089245 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -161,7 +161,7 @@ void TcpTmTcServer::handleServerOperation(socket_t& connSocket) { while (true) { ssize_t retval = recv(connSocket, reinterpret_cast(receptionBuffer.data()), - receptionBuffer.capacity(), tcpConfig.tcpFlags); + receptionBuffer.capacity(), tcpConfig.tcpFlags); if (retval == 0) { size_t availableReadData = ringBuffer.getAvailableReadData(); if (availableReadData > lastRingBufferSize) { @@ -285,7 +285,7 @@ ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket, bool& tmSent) arrayprinter::print(storeAccessor.data(), storeAccessor.size()); } ssize_t retval = send(connSocket, reinterpret_cast(storeAccessor.data()), - storeAccessor.size(), tcpConfig.tcpTmFlags); + storeAccessor.size(), tcpConfig.tcpTmFlags); if (retval == static_cast(storeAccessor.size())) { // Packet sent, clear FIFO entry tmtcBridge->tmFifo->pop(); @@ -340,7 +340,7 @@ ReturnValue_t TcpTmTcServer::handleTcRingBufferData(size_t availableReadData) { size_t foundSize = 0; size_t readLen = 0; while (readLen < readAmount) { - if(spacePacketParser == nullptr) { + if (spacePacketParser == nullptr) { return HasReturnvaluesIF::RETURN_FAILED; } result = diff --git a/src/fsfw/osal/freertos/FixedTimeslotTask.cpp b/src/fsfw/osal/freertos/FixedTimeslotTask.cpp index e8823f8a..87d262f3 100644 --- a/src/fsfw/osal/freertos/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/freertos/FixedTimeslotTask.cpp @@ -47,7 +47,7 @@ void FixedTimeslotTask::missedDeadlineCounter() { if (FixedTimeslotTask::deadlineMissedCount % 10 == 0) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "PST missed " << FixedTimeslotTask::deadlineMissedCount << " deadlines" - << std::endl; + << std::endl; #endif } } diff --git a/src/fsfw/osal/linux/FixedTimeslotTask.cpp b/src/fsfw/osal/linux/FixedTimeslotTask.cpp index 37b958de..a6337fb0 100644 --- a/src/fsfw/osal/linux/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/linux/FixedTimeslotTask.cpp @@ -88,7 +88,7 @@ void FixedTimeslotTask::missedDeadlineCounter() { if (FixedTimeslotTask::deadlineMissedCount % 10 == 0) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "PST missed " << FixedTimeslotTask::deadlineMissedCount << " deadlines" - << std::endl; + << std::endl; #endif } } diff --git a/src/fsfw/osal/rtems/FixedTimeslotTask.cpp b/src/fsfw/osal/rtems/FixedTimeslotTask.cpp index 745b8d70..d83a4d4a 100644 --- a/src/fsfw/osal/rtems/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/rtems/FixedTimeslotTask.cpp @@ -51,7 +51,7 @@ void FixedTimeslotTask::missedDeadlineCounter() { if (FixedTimeslotTask::deadlineMissedCount % 10 == 0) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "PST missed " << FixedTimeslotTask::deadlineMissedCount << " deadlines" - << std::endl; + << std::endl; #endif } } diff --git a/src/fsfw/pus/CService201HealthCommanding.cpp b/src/fsfw/pus/CService201HealthCommanding.cpp index f6c49cd3..644e0d7c 100644 --- a/src/fsfw/pus/CService201HealthCommanding.cpp +++ b/src/fsfw/pus/CService201HealthCommanding.cpp @@ -97,7 +97,8 @@ ReturnValue_t CService201HealthCommanding::handleReply(const CommandMessage *rep } // Not used for now, health state already reported by event -[[maybe_unused]] ReturnValue_t CService201HealthCommanding::prepareHealthSetReply(const CommandMessage *reply) { +[[maybe_unused]] ReturnValue_t CService201HealthCommanding::prepareHealthSetReply( + const CommandMessage *reply) { auto health = static_cast(HealthMessage::getHealth(reply)); auto oldHealth = static_cast(HealthMessage::getOldHealth(reply)); HealthSetReply healthSetReply(health, oldHealth); diff --git a/src/fsfw/pus/CService201HealthCommanding.h b/src/fsfw/pus/CService201HealthCommanding.h index 7ffa06d2..71b7caa0 100644 --- a/src/fsfw/pus/CService201HealthCommanding.h +++ b/src/fsfw/pus/CService201HealthCommanding.h @@ -39,7 +39,7 @@ class CService201HealthCommanding : public CommandingServiceBase { private: static ReturnValue_t checkInterfaceAndAcquireMessageQueue(MessageQueueId_t *MessageQueueToSet, - const object_id_t *objectId); + const object_id_t *objectId); [[maybe_unused]] ReturnValue_t prepareHealthSetReply(const CommandMessage *reply); diff --git a/src/fsfw/timemanager/Countdown.cpp b/src/fsfw/timemanager/Countdown.cpp index 327995f8..334883ae 100644 --- a/src/fsfw/timemanager/Countdown.cpp +++ b/src/fsfw/timemanager/Countdown.cpp @@ -3,8 +3,7 @@ Countdown::Countdown(uint32_t initialTimeout, bool startImmediately) : timeout(initialTimeout) { if (startImmediately) { setTimeout(initialTimeout); - } - else { + } else { timeout = initialTimeout; } } From 56e4fca06f9644d857493ddfd27d8788b48a47a9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 11 May 2022 11:24:06 +0200 Subject: [PATCH 123/404] Some improvements - Rename mutex to csMutex to better represent its purpose - Move the empty transfer to update the line polarity to separate function --- hal/src/fsfw_hal/linux/spi/SpiComIF.cpp | 35 ++++++++++++++----------- hal/src/fsfw_hal/linux/spi/SpiComIF.h | 17 +++++++++++- 2 files changed, 36 insertions(+), 16 deletions(-) diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp index da95bfcb..12d95f0d 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -27,7 +27,7 @@ SpiComIF::SpiComIF(object_id_t objectId, std::string devname, GpioIF* gpioComIF) #endif /* FSFW_VERBOSE_LEVEL >= 1 */ } - spiMutex = MutexFactory::instance()->createMutex(); + csMutex = MutexFactory::instance()->createMutex(); } ReturnValue_t SpiComIF::initializeInterface(CookieIF* cookie) { @@ -197,7 +197,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const /* Pull SPI CS low. For now, no support for active high given */ if (gpioId != gpio::NO_GPIO) { - result = spiMutex->lockMutex(timeoutType, timeoutMs); + result = csMutex->lockMutex(timeoutType, timeoutMs); if (result != RETURN_OK) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -208,6 +208,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const #endif return result; } + updateLinePolarity(fileDescriptor); ReturnValue_t result = gpioComIF->pullLow(gpioId); if (result != HasReturnvaluesIF::RETURN_OK) { #if FSFW_VERBOSE_LEVEL >= 1 @@ -219,6 +220,8 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const #endif return result; } + } else { + updateLinePolarity(fileDescriptor); } /* Execute transfer */ @@ -248,7 +251,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const if (gpioId != gpio::NO_GPIO) { gpioComIF->pullHigh(gpioId); - result = spiMutex->unlockMutex(); + result = csMutex->unlockMutex(); if (result != RETURN_OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "SpiComIF::sendMessage: Failed to unlock mutex" << std::endl; @@ -291,7 +294,7 @@ ReturnValue_t SpiComIF::performHalfDuplexReception(SpiCookie* spiCookie) { gpioId_t gpioId = spiCookie->getChipSelectPin(); if (gpioId != gpio::NO_GPIO) { - result = spiMutex->lockMutex(timeoutType, timeoutMs); + result = csMutex->lockMutex(timeoutType, timeoutMs); if (result != RETURN_OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "SpiComIF::getSendSuccess: Failed to lock mutex" << std::endl; @@ -314,7 +317,7 @@ ReturnValue_t SpiComIF::performHalfDuplexReception(SpiCookie* spiCookie) { if (gpioId != gpio::NO_GPIO) { gpioComIF->pullHigh(gpioId); - result = spiMutex->unlockMutex(); + result = csMutex->unlockMutex(); if (result != RETURN_OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "SpiComIF::getSendSuccess: Failed to unlock mutex" << std::endl; @@ -350,7 +353,7 @@ MutexIF* SpiComIF::getMutex(MutexIF::TimeoutType* timeoutType, uint32_t* timeout if (timeoutMs != nullptr) { *timeoutMs = this->timeoutMs; } - return spiMutex; + return csMutex; } void SpiComIF::performSpiWiretapping(SpiCookie* spiCookie) { @@ -398,14 +401,6 @@ void SpiComIF::setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed) if (retval != 0) { utility::handleIoctlError("SpiComIF::setSpiSpeedAndMode: Setting SPI speed failed"); } - // This updates the SPI clock default polarity. Only setting the mode does not update - // the line state, which can be an issue on mode switches because the clock line will - // switch the state after the chip select is pulled low - clockUpdateTransfer.len = 0; - retval = ioctl(spiFd, SPI_IOC_MESSAGE(1), &clockUpdateTransfer); - if (retval != 0) { - utility::handleIoctlError("SpiComIF::setSpiSpeedAndMode: Updating SPI default clock failed"); - } } void SpiComIF::getSpiSpeedAndMode(int spiFd, spi::SpiModes& mode, uint32_t& speed) const { @@ -422,4 +417,14 @@ void SpiComIF::getSpiSpeedAndMode(int spiFd, spi::SpiModes& mode, uint32_t& spee } } -const std::string& SpiComIF::getSpiDev() const { return dev; } +const std::string& SpiComIF::getSpiDev() const { + return dev; +} + +void SpiComIF::updateLinePolarity(int spiFd) { + clockUpdateTransfer.len = 0; + int 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 881af8c4..aca94765 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.h +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.h @@ -59,6 +59,17 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { GpioIF* getGpioInterface(); void setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed); + + /** + * This updates the SPI clock default polarity. Only setting the mode does not update + * the line state, which can be an issue on mode switches because the clock line will + * switch the state after the chip select is pulled low. + * + * It is recommended to call this function after #setSpiSpeedAndMode if the SPI bus + * has multiple SPI devices with different speed and SPI modes attached. + * @param spiFd + */ + void updateLinePolarity(int spiFd); void getSpiSpeedAndMode(int spiFd, spi::SpiModes& mode, uint32_t& speed) const; const std::string& getSpiDev() const; @@ -74,7 +85,11 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { GpioIF* gpioComIF = nullptr; std::string dev = ""; - MutexIF* spiMutex = nullptr; + /** + * Protects the chip select operations. Lock when GPIO is pulled low, unlock after it was + * pulled high + */ + MutexIF* csMutex = nullptr; MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; uint32_t timeoutMs = 20; spi_ioc_transfer clockUpdateTransfer = {}; From ab2d7ca98fbfbb862e129ea57b65b712e3dec589 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 11 May 2022 11:29:28 +0200 Subject: [PATCH 124/404] update changelog and docs --- CHANGELOG.md | 13 +++++++++++-- hal/src/fsfw_hal/linux/spi/SpiComIF.h | 4 ++-- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 34f40ed3..2019fdcd 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -60,8 +60,6 @@ and this project adheres to [Semantic Versioning](http://semver.org/). ## Additions -- Linux HAL: Getter functions for SPI speed and mode. -- Linux HAL: Add wiretapping option for I2C. Enabled with `FSFW_HAL_I2C_WIRETAPPING` defined to 1 - Dedicated Version class and constant `fsfw::FSFW_VERSION` containing version information inside `fsfw/version.h` PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/559 @@ -69,6 +67,17 @@ and this project adheres to [Semantic Versioning](http://semver.org/). PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/592 - `Subsystem`: New API to add table and sequence entries +## HAL + +- SPI: Cache the SPI device in the communication interface. Architecturally, this makes a + lot more sense because each ComIF should be responsible for one SPI bus. +- SPI: Move the empty transfer to update the line polarity to separate function. This means + it is not automatically called when calling the setter function for SPI speed and mode. + The user should call this function after locking the CS mutex if multiple SPI devices with + differing speeds and modes are attached to one bus. +- SPI: Getter functions for SPI speed and mode. +- I2C: Add wiretapping option for I2C. Enabled with `FSFW_HAL_I2C_WIRETAPPING` defined to 1. + ## Fixed - TCP TMTC Server: `MutexGuard` was not created properly in diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.h b/hal/src/fsfw_hal/linux/spi/SpiComIF.h index aca94765..3d15009d 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.h +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.h @@ -65,8 +65,8 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { * 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. * - * It is recommended to call this function after #setSpiSpeedAndMode if the SPI bus - * has multiple SPI devices with different speed and SPI modes attached. + * It is recommended to call this function after #setSpiSpeedAndMode and after locking the + * CS mutex if the SPI bus has multiple SPI devices with different speed and SPI modes attached. * @param spiFd */ void updateLinePolarity(int spiFd); From bc994595dae47fbe57d5a4fa70ad23f90e9410f9 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Wed, 11 May 2022 14:31:49 +0200 Subject: [PATCH 125/404] sequence count init value --- src/fsfw/tmtcservices/SourceSequenceCounter.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/fsfw/tmtcservices/SourceSequenceCounter.h b/src/fsfw/tmtcservices/SourceSequenceCounter.h index 6f2778a4..d7ab5d4a 100644 --- a/src/fsfw/tmtcservices/SourceSequenceCounter.h +++ b/src/fsfw/tmtcservices/SourceSequenceCounter.h @@ -8,7 +8,7 @@ class SourceSequenceCounter { uint16_t sequenceCount; public: - SourceSequenceCounter() : sequenceCount(0) {} + SourceSequenceCounter(uint16_t initialSequenceCount = 0) : sequenceCount(initialSequenceCount) {} void increment() { sequenceCount = (sequenceCount + 1) % (SpacePacketBase::LIMIT_SEQUENCE_COUNT); } @@ -31,6 +31,7 @@ class SourceSequenceCounter { sequenceCount = newCount; return *this; } + operator uint16_t() { return this->get(); } }; From 0a97077a0e0f5e1c4859d9c867a74f277405c00c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 11 May 2022 15:42:52 +0200 Subject: [PATCH 126/404] hotfix --- src/fsfw/tmtcservices/SourceSequenceCounter.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/tmtcservices/SourceSequenceCounter.h b/src/fsfw/tmtcservices/SourceSequenceCounter.h index 6f2778a4..00f4021e 100644 --- a/src/fsfw/tmtcservices/SourceSequenceCounter.h +++ b/src/fsfw/tmtcservices/SourceSequenceCounter.h @@ -5,7 +5,7 @@ class SourceSequenceCounter { private: - uint16_t sequenceCount; + uint16_t sequenceCount = 0; public: SourceSequenceCounter() : sequenceCount(0) {} From d1ff32bf968cbf769ca8daa37265af70e050d9c0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 11 May 2022 16:12:24 +0200 Subject: [PATCH 127/404] reset read vec values, add getter function --- hal/src/fsfw_hal/linux/CommandExecutor.cpp | 6 ++++++ hal/src/fsfw_hal/linux/CommandExecutor.h | 2 ++ 2 files changed, 8 insertions(+) diff --git a/hal/src/fsfw_hal/linux/CommandExecutor.cpp b/hal/src/fsfw_hal/linux/CommandExecutor.cpp index 49c44ebf..54464be2 100644 --- a/hal/src/fsfw_hal/linux/CommandExecutor.cpp +++ b/hal/src/fsfw_hal/linux/CommandExecutor.cpp @@ -32,6 +32,8 @@ ReturnValue_t CommandExecutor::execute() { } else if (state == States::PENDING) { return COMMAND_PENDING; } + // Reset data in read vector + std::memset(readVec.data(), 0, readVec.size()); currentCmdFile = popen(currentCmd.c_str(), "r"); if (currentCmdFile == nullptr) { lastError = errno; @@ -205,3 +207,7 @@ ReturnValue_t CommandExecutor::executeBlocking() { } return HasReturnvaluesIF::RETURN_OK; } + +const std::vector& CommandExecutor::getReadVector() const { + return readVec; +} diff --git a/hal/src/fsfw_hal/linux/CommandExecutor.h b/hal/src/fsfw_hal/linux/CommandExecutor.h index 90662c0f..5d403848 100644 --- a/hal/src/fsfw_hal/linux/CommandExecutor.h +++ b/hal/src/fsfw_hal/linux/CommandExecutor.h @@ -109,6 +109,8 @@ class CommandExecutor { */ void reset(); + const std::vector& getReadVector() const; + private: std::string currentCmd; bool blocking = true; From ab45aa1296a8b3fae0b6a99b9d8b822b7be1ae3c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 12 May 2022 20:06:10 +0200 Subject: [PATCH 128/404] HasHealthIF additions --- src/fsfw/health/HasHealthIF.h | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/fsfw/health/HasHealthIF.h b/src/fsfw/health/HasHealthIF.h index 41abeef3..197f08aa 100644 --- a/src/fsfw/health/HasHealthIF.h +++ b/src/fsfw/health/HasHealthIF.h @@ -16,10 +16,15 @@ class HasHealthIF { }; static const uint8_t INTERFACE_ID = CLASS_ID::HAS_HEALTH_IF; - static const ReturnValue_t OBJECT_NOT_HEALTHY = MAKE_RETURN_CODE(1); - static const ReturnValue_t INVALID_HEALTH_STATE = MAKE_RETURN_CODE(2); + static constexpr ReturnValue_t OBJECT_NOT_HEALTHY = + HasReturnvaluesIF::makeReturnCode(INTERFACE_ID, 1); + static constexpr ReturnValue_t INVALID_HEALTH_STATE = + HasReturnvaluesIF::makeReturnCode(INTERFACE_ID, 2); + static constexpr ReturnValue_t IS_EXTERNALLY_CONTROLLED = + HasReturnvaluesIF::makeReturnCode(INTERFACE_ID, 3); static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SYSTEM_MANAGER_1; + //! P1: New Health, P2: Old Health 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); From fc2b709148d273368e59969ee073670cb282ac29 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 12 May 2022 20:48:50 +0200 Subject: [PATCH 129/404] resolve merge conflict --- CHANGELOG.md | 4 ---- 1 file changed, 4 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 773b29c1..2cd9e333 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -85,7 +85,6 @@ https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/593 ## Additions -<<<<<<< HEAD - LTO support: Allow using LTO/IPO by setting `FSFW_ENABLE_LTO=1`. CMake is able to detect whether the user compiler supports IPO/LPO. LTO is on by default now. Most modern compilers support it, can make good use of it and it usually makes the code faster and/or smaller. @@ -100,9 +99,6 @@ https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/593 - https://gitlab.kitware.com/cmake/cmake/-/issues/21696 Easiest solution for now: Keep this option OFF by default. PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/616 -- Linux HAL: Add wiretapping option for I2C. Enabled with `FSFW_HAL_I2C_WIRETAPPING` defined to 1 -======= ->>>>>>> origin/develop - Dedicated Version class and constant `fsfw::FSFW_VERSION` containing version information inside `fsfw/version.h` PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/559 From 4841d5d92d721383e92b521b584c7d0e62c9873d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 13 May 2022 17:24:55 +0200 Subject: [PATCH 130/404] doc update --- src/fsfw/devicehandlers/DeviceHandlerBase.h | 23 ++++++++++++++------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index 0a273675..34171067 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -467,14 +467,14 @@ class DeviceHandlerBase : public DeviceHandlerIF, * @brief This is a helper method to insert replies in the reply map. * @param deviceCommand Identifier of the reply to add. * @param maxDelayCycles The maximum number of delay cycles the reply waits - * until it times out. + * until it times out. * @param periodic Indicates if the command is periodic (i.e. it is sent - * by the device repeatedly without request) or not. Default is aperiodic (0). - * Please note that periodic replies are disabled by default. You can enable them with - * #updatePeriodicReply + * by the device repeatedly without request) or not. Default is aperiodic (0). + * Please note that periodic replies are disabled by default. You can enable them with + * #updatePeriodicReply * @param countdown Instead of using maxDelayCycles to timeout a device reply it is also possible - * to provide a pointer to a Countdown object which will signal the timeout - * when expired + * to provide a pointer to a Countdown object which will signal the timeout + * when expired * @return - @c RETURN_OK when the command was successfully inserted, * - @c RETURN_FAILED else. */ @@ -783,11 +783,18 @@ class DeviceHandlerBase : public DeviceHandlerIF, * This is used to keep track of pending replies. */ struct DeviceReplyInfo { + //! For Command-Reply combinations: //! The maximum number of cycles the handler should wait for a reply //! to this command. + //! + //! Reply Only: + //! For periodic replies, this variable will be the number of delay cycles between the replies. + //! For the non-periodic variant, this variable is not used as there is no meaningful + //! definition for delay uint16_t maxDelayCycles; - //! The currently remaining cycles the handler should wait for a reply, - //! 0 means there is no reply expected + //! This variable will be set to #maxDelayCycles if a reply is expected. + //! For non-periodic replies without a command, this variable is unused. + //! A runtime value of 0 means there is no reply is currently expected. uint16_t delayCycles; size_t replyLen = 0; //!< Expected size of the reply. //! if this is !=0, the delayCycles will not be reset to 0 but to From 7df1922633061419f0a66c03e34ad16d4db7457d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 14 May 2022 09:38:59 +0200 Subject: [PATCH 131/404] refactor task IF --- hal/src/fsfw_hal/linux/CommandExecutor.cpp | 2 + hal/src/fsfw_hal/linux/spi/SpiComIF.cpp | 27 ++++++++++-- hal/src/fsfw_hal/linux/spi/SpiComIF.h | 30 +++++++++++--- src/fsfw/osal/linux/FixedTimeslotTask.cpp | 4 +- src/fsfw/osal/linux/FixedTimeslotTask.h | 13 +++--- src/fsfw/osal/linux/PeriodicPosixTask.cpp | 41 +++++++++++++++---- src/fsfw/osal/linux/PeriodicPosixTask.h | 14 +++++-- src/fsfw/tasks/FixedSlotSequence.cpp | 2 + src/fsfw/tasks/FixedSlotSequence.h | 2 + src/fsfw/tasks/FixedTimeslotTaskIF.h | 2 +- src/fsfw/tasks/PeriodicTaskIF.h | 6 ++- .../unit/power/testPowerSwitcher.cpp | 4 +- 12 files changed, 114 insertions(+), 33 deletions(-) diff --git a/hal/src/fsfw_hal/linux/CommandExecutor.cpp b/hal/src/fsfw_hal/linux/CommandExecutor.cpp index 49c44ebf..dcdd10ee 100644 --- a/hal/src/fsfw_hal/linux/CommandExecutor.cpp +++ b/hal/src/fsfw_hal/linux/CommandExecutor.cpp @@ -205,3 +205,5 @@ ReturnValue_t CommandExecutor::executeBlocking() { } return HasReturnvaluesIF::RETURN_OK; } + +const std::vector& CommandExecutor::getReadVector() const { return readVec; } diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp index dcf92b5d..3c257f1f 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -401,12 +401,33 @@ void SpiComIF::setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed) if (retval != 0) { utility::handleIoctlError("SpiComIF::setSpiSpeedAndMode: Setting SPI speed failed"); } - // This updates the SPI clock default polarity. Only setting the mode does not update - // the line state, which can be an issue on mode switches because the clock line will - // switch the state after the chip select is pulled low +} + +void SpiComIF::getSpiSpeedAndMode(int spiFd, spi::SpiModes& mode, uint32_t& speed) const { + uint8_t tmpMode = 0; + int retval = ioctl(spiFd, SPI_IOC_RD_MODE, &tmpMode); + if (retval != 0) { + utility::handleIoctlError("SpiComIF::getSpiSpeedAndMode: Reading SPI mode failed"); + } + mode = static_cast(tmpMode); + + retval = ioctl(spiFd, SPI_IOC_RD_MAX_SPEED_HZ, &speed); + if (retval != 0) { + utility::handleIoctlError("SpiComIF::getSpiSpeedAndMode: Getting SPI speed failed"); + } +} + +const std::string& SpiComIF::getSpiDev() const { return dev; } + +void SpiComIF::updateLinePolarity(int spiFd) { clockUpdateTransfer.len = 0; retval = ioctl(spiFd, SPI_IOC_MESSAGE(1), &clockUpdateTransfer); if (retval != 0) { utility::handleIoctlError("SpiComIF::setSpiSpeedAndMode: Updating SPI default clock failed"); } } + +void SpiComIF::setMutexParams(MutexIF::TimeoutType timeoutType_, uint32_t timeoutMs_) { + timeoutType = timeoutType_; + timeoutMs = timeoutMs_; +} diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.h b/hal/src/fsfw_hal/linux/spi/SpiComIF.h index 357afa2f..1400dcfc 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.h +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.h @@ -22,15 +22,17 @@ class SpiCookie; */ class SpiComIF : public DeviceCommunicationIF, public SystemObject { public: - static constexpr uint8_t spiRetvalId = CLASS_ID::HAL_SPI; + static constexpr dur_millis_t DEFAULT_MUTEX_TIMEOUT = 20; + + static constexpr uint8_t CLASS_ID = CLASS_ID::HAL_SPI; static constexpr ReturnValue_t OPENING_FILE_FAILED = - HasReturnvaluesIF::makeReturnCode(spiRetvalId, 0); + HasReturnvaluesIF::makeReturnCode(CLASS_ID, 0); /* Full duplex (ioctl) transfer failure */ static constexpr ReturnValue_t FULL_DUPLEX_TRANSFER_FAILED = - HasReturnvaluesIF::makeReturnCode(spiRetvalId, 1); + HasReturnvaluesIF::makeReturnCode(CLASS_ID, 1); /* Half duplex (read/write) transfer failure */ static constexpr ReturnValue_t HALF_DUPLEX_TRANSFER_FAILED = - HasReturnvaluesIF::makeReturnCode(spiRetvalId, 2); + HasReturnvaluesIF::makeReturnCode(CLASS_ID, 2); SpiComIF(object_id_t objectId, GpioIF* gpioComIF); @@ -45,6 +47,7 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { * the chip select must be driven from outside of the com if. */ MutexIF* getMutex(MutexIF::TimeoutType* timeoutType = nullptr, uint32_t* timeoutMs = nullptr); + void setMutexParams(MutexIF::TimeoutType timeoutType, uint32_t timeoutMs); /** * Perform a regular send operation using Linux iotcl. This is public so it can be used @@ -59,6 +62,23 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { GpioIF* getGpioInterface(); void setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed); +<<<<<<< Updated upstream +======= + void getSpiSpeedAndMode(int spiFd, spi::SpiModes& mode, uint32_t& speed) const; + + /** + * This updates the SPI clock default polarity. Only setting the mode does not update + * the line state, which can be an issue on mode switches because the clock line will + * switch the state after the chip select is pulled low. + * + * It is recommended to call this function after #setSpiSpeedAndMode and after locking the + * CS mutex if the SPI bus has multiple SPI devices with different speed and SPI modes attached. + * @param spiFd + */ + void updateLinePolarity(int spiFd); + + const std::string& getSpiDev() const; +>>>>>>> Stashed changes void performSpiWiretapping(SpiCookie* spiCookie); ReturnValue_t getReadBuffer(address_t spiAddress, uint8_t** buffer); @@ -73,7 +93,7 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { MutexIF* spiMutex = nullptr; MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; - uint32_t timeoutMs = 20; + uint32_t timeoutMs = DEFAULT_MUTEX_TIMEOUT; spi_ioc_transfer clockUpdateTransfer = {}; using SpiDeviceMap = std::unordered_map; diff --git a/src/fsfw/osal/linux/FixedTimeslotTask.cpp b/src/fsfw/osal/linux/FixedTimeslotTask.cpp index 1f4d6e23..d1fccdf9 100644 --- a/src/fsfw/osal/linux/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/linux/FixedTimeslotTask.cpp @@ -14,6 +14,8 @@ FixedTimeslotTask::FixedTimeslotTask(const char* name_, int priority_, size_t st FixedTimeslotTask::~FixedTimeslotTask() {} +bool FixedTimeslotTask::isEmpty() const { return pst.isEmpty(); } + void* FixedTimeslotTask::taskEntryPoint(void* arg) { // The argument is re-interpreted as PollingTask. FixedTimeslotTask* originalTask(reinterpret_cast(arg)); @@ -50,7 +52,7 @@ ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId, uint32_t slotT return HasReturnvaluesIF::RETURN_FAILED; } -ReturnValue_t FixedTimeslotTask::checkSequence() const { return pst.checkSequence(); } +ReturnValue_t FixedTimeslotTask::checkSequence() { return pst.checkSequence(); } void FixedTimeslotTask::taskFunctionality() { // Like FreeRTOS pthreads are running as soon as they are created diff --git a/src/fsfw/osal/linux/FixedTimeslotTask.h b/src/fsfw/osal/linux/FixedTimeslotTask.h index 76b92db3..a5dc9032 100644 --- a/src/fsfw/osal/linux/FixedTimeslotTask.h +++ b/src/fsfw/osal/linux/FixedTimeslotTask.h @@ -24,15 +24,18 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF, public PosixThread { FixedTimeslotTask(const char* name_, int priority_, size_t stackSize_, uint32_t periodMs_); virtual ~FixedTimeslotTask(); - virtual ReturnValue_t startTask(); + ReturnValue_t startTask() override; - virtual ReturnValue_t sleepFor(uint32_t ms); + ReturnValue_t sleepFor(uint32_t ms) override; - virtual uint32_t getPeriodMs() const; + uint32_t getPeriodMs() const override; - virtual ReturnValue_t addSlot(object_id_t componentId, uint32_t slotTimeMs, int8_t executionStep); + ReturnValue_t addSlot(object_id_t componentId, uint32_t slotTimeMs, + int8_t executionStep) override; - virtual ReturnValue_t checkSequence() const; + ReturnValue_t checkSequence() override; + + bool isEmpty() const override; /** * This static function can be used as #deadlineMissedFunc. diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.cpp b/src/fsfw/osal/linux/PeriodicPosixTask.cpp index e1937df4..26b6f53e 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.cpp +++ b/src/fsfw/osal/linux/PeriodicPosixTask.cpp @@ -26,12 +26,12 @@ void* PeriodicPosixTask::taskEntryPoint(void* arg) { return NULL; } -ReturnValue_t PeriodicPosixTask::addComponent(object_id_t object) { +ReturnValue_t PeriodicPosixTask::addComponent(object_id_t object, uint8_t opCode) { ExecutableObjectIF* newObject = ObjectManager::instance()->get(object); - return addComponent(newObject); + return addComponent(newObject, opCode); } -ReturnValue_t PeriodicPosixTask::addComponent(ExecutableObjectIF* object) { +ReturnValue_t PeriodicPosixTask::addComponent(ExecutableObjectIF* object, uint8_t opCode) { if (object == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "PeriodicTask::addComponent: Invalid object. Make sure" @@ -43,7 +43,7 @@ ReturnValue_t PeriodicPosixTask::addComponent(ExecutableObjectIF* object) { #endif return HasReturnvaluesIF::RETURN_FAILED; } - objectList.push_back(object); + objectList.emplace(object, opCode); object->setTaskIF(this); return HasReturnvaluesIF::RETURN_OK; @@ -54,6 +54,9 @@ ReturnValue_t PeriodicPosixTask::sleepFor(uint32_t ms) { } ReturnValue_t PeriodicPosixTask::startTask(void) { + if (isEmpty()) { + return HasReturnvaluesIF::RETURN_FAILED; + } started = true; PosixThread::createTask(&taskEntryPoint, this); return HasReturnvaluesIF::RETURN_OK; @@ -64,15 +67,13 @@ void PeriodicPosixTask::taskFunctionality(void) { suspend(); } - for (auto const& object : objectList) { - object->initializeAfterTaskCreation(); - } + initObjsAfterTaskCreation(); uint64_t lastWakeTime = getCurrentMonotonicTimeMs(); // The task's "infinite" inner loop is entered. while (1) { - for (auto const& object : objectList) { - object->performOperation(); + for (auto const& objOpCodePair : objectList) { + objOpCodePair.first->performOperation(objOpCodePair.second); } if (not PosixThread::delayUntil(&lastWakeTime, periodMs)) { @@ -84,3 +85,25 @@ void PeriodicPosixTask::taskFunctionality(void) { } uint32_t PeriodicPosixTask::getPeriodMs() const { return periodMs; } + +bool PeriodicPosixTask::isEmpty() const { return objectList.empty(); } + +ReturnValue_t PeriodicPosixTask::initObjsAfterTaskCreation() { + std::multiset uniqueObjects; + ReturnValue_t status = HasReturnvaluesIF::RETURN_OK; + uint32_t count = 0; + for (const auto& obj : objectList) { + // Ensure that each unique object is initialized once. + if (uniqueObjects.find(obj.first) == uniqueObjects.end()) { + ReturnValue_t result = obj.first->initializeAfterTaskCreation(); + if (result != HasReturnvaluesIF::RETURN_OK) { + count++; + status = result; + } + uniqueObjects.emplace(obj.first); + } + } + if (count > 0) { + } + return status; +} diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.h b/src/fsfw/osal/linux/PeriodicPosixTask.h index 3cd9847a..1142c854 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.h +++ b/src/fsfw/osal/linux/PeriodicPosixTask.h @@ -1,7 +1,7 @@ #ifndef FRAMEWORK_OSAL_LINUX_PERIODICPOSIXTASK_H_ #define FRAMEWORK_OSAL_LINUX_PERIODICPOSIXTASK_H_ -#include +#include #include "../../objectmanager/ObjectManagerIF.h" #include "../../tasks/ExecutableObjectIF.h" @@ -40,7 +40,7 @@ class PeriodicPosixTask : public PosixThread, public PeriodicTaskIF { * @param object Id of the object to add. * @return RETURN_OK on success, RETURN_FAILED if the object could not be added. */ - ReturnValue_t addComponent(object_id_t object) override; + ReturnValue_t addComponent(object_id_t object, uint8_t opCode) override; /** * Adds an object to the list of objects to be executed. @@ -48,14 +48,20 @@ class PeriodicPosixTask : public PosixThread, public PeriodicTaskIF { * @param object pointer to the object to add. * @return RETURN_OK on success, RETURN_FAILED if the object could not be added. */ - ReturnValue_t addComponent(ExecutableObjectIF* object) override; + ReturnValue_t addComponent(ExecutableObjectIF* object, uint8_t opCode) override; uint32_t getPeriodMs() const override; ReturnValue_t sleepFor(uint32_t ms) override; + ReturnValue_t initObjsAfterTaskCreation(); + + bool isEmpty() const override; + private: - typedef std::vector ObjectList; //!< Typedef for the List of objects. + //! Typedef for the List of objects. Will contain the objects to execute and their respective + //! op codes + using ObjectList = std::multiset>; /** * @brief This attribute holds a list of objects to be executed. */ diff --git a/src/fsfw/tasks/FixedSlotSequence.cpp b/src/fsfw/tasks/FixedSlotSequence.cpp index d4c67b4d..62c0e99c 100644 --- a/src/fsfw/tasks/FixedSlotSequence.cpp +++ b/src/fsfw/tasks/FixedSlotSequence.cpp @@ -164,3 +164,5 @@ ReturnValue_t FixedSlotSequence::intializeSequenceAfterTaskCreation() const { void FixedSlotSequence::addCustomCheck(ReturnValue_t (*customCheckFunction)(const SlotList&)) { this->customCheckFunction = customCheckFunction; } + +bool FixedSlotSequence::isEmpty() const { return slotList.empty(); } diff --git a/src/fsfw/tasks/FixedSlotSequence.h b/src/fsfw/tasks/FixedSlotSequence.h index a287c5b2..5ece7126 100644 --- a/src/fsfw/tasks/FixedSlotSequence.h +++ b/src/fsfw/tasks/FixedSlotSequence.h @@ -159,6 +159,8 @@ class FixedSlotSequence { */ ReturnValue_t intializeSequenceAfterTaskCreation() const; + bool isEmpty() const; + protected: /** * @brief This list contains all PollingSlot objects, defining order and diff --git a/src/fsfw/tasks/FixedTimeslotTaskIF.h b/src/fsfw/tasks/FixedTimeslotTaskIF.h index 497db245..9d85ac4a 100644 --- a/src/fsfw/tasks/FixedTimeslotTaskIF.h +++ b/src/fsfw/tasks/FixedTimeslotTaskIF.h @@ -30,7 +30,7 @@ class FixedTimeslotTaskIF : public PeriodicTaskIF { * Check whether the sequence is valid and perform all other required * initialization steps which are needed after task creation */ - virtual ReturnValue_t checkSequence() const = 0; + virtual ReturnValue_t checkSequence() = 0; }; #endif /* FRAMEWORK_TASKS_FIXEDTIMESLOTTASKIF_H_ */ diff --git a/src/fsfw/tasks/PeriodicTaskIF.h b/src/fsfw/tasks/PeriodicTaskIF.h index c78a32de..2ae268fc 100644 --- a/src/fsfw/tasks/PeriodicTaskIF.h +++ b/src/fsfw/tasks/PeriodicTaskIF.h @@ -31,7 +31,7 @@ class PeriodicTaskIF { * Add an object to the task. The object needs to implement ExecutableObjectIF * @return */ - virtual ReturnValue_t addComponent(object_id_t object) { + virtual ReturnValue_t addComponent(object_id_t object, uint8_t opCode = 0) { return HasReturnvaluesIF::RETURN_FAILED; }; @@ -41,13 +41,15 @@ class PeriodicTaskIF { * Add an object to the task. * @return */ - virtual ReturnValue_t addComponent(ExecutableObjectIF* object) { + virtual ReturnValue_t addComponent(ExecutableObjectIF* object, uint8_t opCode = 0) { return HasReturnvaluesIF::RETURN_FAILED; }; virtual ReturnValue_t sleepFor(uint32_t ms) = 0; virtual uint32_t getPeriodMs() const = 0; + + virtual bool isEmpty() const = 0; }; #endif /* PERIODICTASKIF_H_ */ diff --git a/tests/src/fsfw_tests/unit/power/testPowerSwitcher.cpp b/tests/src/fsfw_tests/unit/power/testPowerSwitcher.cpp index d8523558..941055ac 100644 --- a/tests/src/fsfw_tests/unit/power/testPowerSwitcher.cpp +++ b/tests/src/fsfw_tests/unit/power/testPowerSwitcher.cpp @@ -67,7 +67,5 @@ TEST_CASE("Power Switcher", "[power-switcher]") { REQUIRE(not switcherUsingDummy.active()); } - SECTION("More Dummy Tests") { - - } + SECTION("More Dummy Tests") {} } From dba08fed7a00187ba204f46bc40424f7b87f3aea Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 14 May 2022 09:40:31 +0200 Subject: [PATCH 132/404] refactor task IF --- hal/src/fsfw_hal/linux/CommandExecutor.cpp | 4 +- hal/src/fsfw_hal/linux/spi/SpiComIF.cpp | 9 ++-- hal/src/fsfw_hal/linux/spi/SpiComIF.h | 15 ++++--- src/fsfw/osal/linux/FixedTimeslotTask.cpp | 4 +- src/fsfw/osal/linux/FixedTimeslotTask.h | 13 +++--- src/fsfw/osal/linux/PeriodicPosixTask.cpp | 41 +++++++++++++++---- src/fsfw/osal/linux/PeriodicPosixTask.h | 14 +++++-- src/fsfw/tasks/FixedSlotSequence.cpp | 2 + src/fsfw/tasks/FixedSlotSequence.h | 2 + src/fsfw/tasks/FixedTimeslotTaskIF.h | 2 +- src/fsfw/tasks/PeriodicTaskIF.h | 6 ++- .../unit/power/testPowerSwitcher.cpp | 4 +- 12 files changed, 79 insertions(+), 37 deletions(-) diff --git a/hal/src/fsfw_hal/linux/CommandExecutor.cpp b/hal/src/fsfw_hal/linux/CommandExecutor.cpp index 54464be2..3887964d 100644 --- a/hal/src/fsfw_hal/linux/CommandExecutor.cpp +++ b/hal/src/fsfw_hal/linux/CommandExecutor.cpp @@ -208,6 +208,4 @@ ReturnValue_t CommandExecutor::executeBlocking() { return HasReturnvaluesIF::RETURN_OK; } -const std::vector& CommandExecutor::getReadVector() const { - return readVec; -} +const std::vector& CommandExecutor::getReadVector() const { return readVec; } diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp index 12d95f0d..b4ed47f2 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -417,9 +417,7 @@ void SpiComIF::getSpiSpeedAndMode(int spiFd, spi::SpiModes& mode, uint32_t& spee } } -const std::string& SpiComIF::getSpiDev() const { - return dev; -} +const std::string& SpiComIF::getSpiDev() const { return dev; } void SpiComIF::updateLinePolarity(int spiFd) { clockUpdateTransfer.len = 0; @@ -428,3 +426,8 @@ void SpiComIF::updateLinePolarity(int spiFd) { utility::handleIoctlError("SpiComIF::setSpiSpeedAndMode: Updating SPI default clock failed"); } } + +void SpiComIF::setMutexParams(MutexIF::TimeoutType timeoutType_, uint32_t timeoutMs_) { + timeoutType = timeoutType_; + timeoutMs = timeoutMs_; +} diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.h b/hal/src/fsfw_hal/linux/spi/SpiComIF.h index 3d15009d..113b42b0 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.h +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.h @@ -22,15 +22,17 @@ class SpiCookie; */ class SpiComIF : public DeviceCommunicationIF, public SystemObject { public: - static constexpr uint8_t spiRetvalId = CLASS_ID::HAL_SPI; + static constexpr dur_millis_t DEFAULT_MUTEX_TIMEOUT = 20; + + static constexpr uint8_t CLASS_ID = CLASS_ID::HAL_SPI; static constexpr ReturnValue_t OPENING_FILE_FAILED = - HasReturnvaluesIF::makeReturnCode(spiRetvalId, 0); + HasReturnvaluesIF::makeReturnCode(CLASS_ID, 0); /* Full duplex (ioctl) transfer failure */ static constexpr ReturnValue_t FULL_DUPLEX_TRANSFER_FAILED = - HasReturnvaluesIF::makeReturnCode(spiRetvalId, 1); + HasReturnvaluesIF::makeReturnCode(CLASS_ID, 1); /* Half duplex (read/write) transfer failure */ static constexpr ReturnValue_t HALF_DUPLEX_TRANSFER_FAILED = - HasReturnvaluesIF::makeReturnCode(spiRetvalId, 2); + HasReturnvaluesIF::makeReturnCode(CLASS_ID, 2); SpiComIF(object_id_t objectId, std::string devname, GpioIF* gpioComIF); @@ -45,6 +47,7 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { * the chip select must be driven from outside of the com if. */ MutexIF* getMutex(MutexIF::TimeoutType* timeoutType = nullptr, uint32_t* timeoutMs = nullptr); + void setMutexParams(MutexIF::TimeoutType timeoutType, uint32_t timeoutMs); /** * Perform a regular send operation using Linux iotcl. This is public so it can be used @@ -59,6 +62,7 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { GpioIF* getGpioInterface(); void setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed); + void getSpiSpeedAndMode(int spiFd, spi::SpiModes& mode, uint32_t& speed) const; /** * This updates the SPI clock default polarity. Only setting the mode does not update @@ -70,7 +74,6 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { * @param spiFd */ void updateLinePolarity(int spiFd); - void getSpiSpeedAndMode(int spiFd, spi::SpiModes& mode, uint32_t& speed) const; const std::string& getSpiDev() const; void performSpiWiretapping(SpiCookie* spiCookie); @@ -91,7 +94,7 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { */ MutexIF* csMutex = nullptr; MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; - uint32_t timeoutMs = 20; + uint32_t timeoutMs = DEFAULT_MUTEX_TIMEOUT; spi_ioc_transfer clockUpdateTransfer = {}; using SpiDeviceMap = std::unordered_map; diff --git a/src/fsfw/osal/linux/FixedTimeslotTask.cpp b/src/fsfw/osal/linux/FixedTimeslotTask.cpp index a6337fb0..e5fe12e9 100644 --- a/src/fsfw/osal/linux/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/linux/FixedTimeslotTask.cpp @@ -14,6 +14,8 @@ FixedTimeslotTask::FixedTimeslotTask(const char* name_, int priority_, size_t st FixedTimeslotTask::~FixedTimeslotTask() {} +bool FixedTimeslotTask::isEmpty() const { return pst.isEmpty(); } + void* FixedTimeslotTask::taskEntryPoint(void* arg) { // The argument is re-interpreted as PollingTask. FixedTimeslotTask* originalTask(reinterpret_cast(arg)); @@ -50,7 +52,7 @@ ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId, uint32_t slotT return HasReturnvaluesIF::RETURN_FAILED; } -ReturnValue_t FixedTimeslotTask::checkSequence() const { return pst.checkSequence(); } +ReturnValue_t FixedTimeslotTask::checkSequence() { return pst.checkSequence(); } void FixedTimeslotTask::taskFunctionality() { // Like FreeRTOS pthreads are running as soon as they are created diff --git a/src/fsfw/osal/linux/FixedTimeslotTask.h b/src/fsfw/osal/linux/FixedTimeslotTask.h index 76b92db3..a5dc9032 100644 --- a/src/fsfw/osal/linux/FixedTimeslotTask.h +++ b/src/fsfw/osal/linux/FixedTimeslotTask.h @@ -24,15 +24,18 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF, public PosixThread { FixedTimeslotTask(const char* name_, int priority_, size_t stackSize_, uint32_t periodMs_); virtual ~FixedTimeslotTask(); - virtual ReturnValue_t startTask(); + ReturnValue_t startTask() override; - virtual ReturnValue_t sleepFor(uint32_t ms); + ReturnValue_t sleepFor(uint32_t ms) override; - virtual uint32_t getPeriodMs() const; + uint32_t getPeriodMs() const override; - virtual ReturnValue_t addSlot(object_id_t componentId, uint32_t slotTimeMs, int8_t executionStep); + ReturnValue_t addSlot(object_id_t componentId, uint32_t slotTimeMs, + int8_t executionStep) override; - virtual ReturnValue_t checkSequence() const; + ReturnValue_t checkSequence() override; + + bool isEmpty() const override; /** * This static function can be used as #deadlineMissedFunc. diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.cpp b/src/fsfw/osal/linux/PeriodicPosixTask.cpp index e1937df4..26b6f53e 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.cpp +++ b/src/fsfw/osal/linux/PeriodicPosixTask.cpp @@ -26,12 +26,12 @@ void* PeriodicPosixTask::taskEntryPoint(void* arg) { return NULL; } -ReturnValue_t PeriodicPosixTask::addComponent(object_id_t object) { +ReturnValue_t PeriodicPosixTask::addComponent(object_id_t object, uint8_t opCode) { ExecutableObjectIF* newObject = ObjectManager::instance()->get(object); - return addComponent(newObject); + return addComponent(newObject, opCode); } -ReturnValue_t PeriodicPosixTask::addComponent(ExecutableObjectIF* object) { +ReturnValue_t PeriodicPosixTask::addComponent(ExecutableObjectIF* object, uint8_t opCode) { if (object == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "PeriodicTask::addComponent: Invalid object. Make sure" @@ -43,7 +43,7 @@ ReturnValue_t PeriodicPosixTask::addComponent(ExecutableObjectIF* object) { #endif return HasReturnvaluesIF::RETURN_FAILED; } - objectList.push_back(object); + objectList.emplace(object, opCode); object->setTaskIF(this); return HasReturnvaluesIF::RETURN_OK; @@ -54,6 +54,9 @@ ReturnValue_t PeriodicPosixTask::sleepFor(uint32_t ms) { } ReturnValue_t PeriodicPosixTask::startTask(void) { + if (isEmpty()) { + return HasReturnvaluesIF::RETURN_FAILED; + } started = true; PosixThread::createTask(&taskEntryPoint, this); return HasReturnvaluesIF::RETURN_OK; @@ -64,15 +67,13 @@ void PeriodicPosixTask::taskFunctionality(void) { suspend(); } - for (auto const& object : objectList) { - object->initializeAfterTaskCreation(); - } + initObjsAfterTaskCreation(); uint64_t lastWakeTime = getCurrentMonotonicTimeMs(); // The task's "infinite" inner loop is entered. while (1) { - for (auto const& object : objectList) { - object->performOperation(); + for (auto const& objOpCodePair : objectList) { + objOpCodePair.first->performOperation(objOpCodePair.second); } if (not PosixThread::delayUntil(&lastWakeTime, periodMs)) { @@ -84,3 +85,25 @@ void PeriodicPosixTask::taskFunctionality(void) { } uint32_t PeriodicPosixTask::getPeriodMs() const { return periodMs; } + +bool PeriodicPosixTask::isEmpty() const { return objectList.empty(); } + +ReturnValue_t PeriodicPosixTask::initObjsAfterTaskCreation() { + std::multiset uniqueObjects; + ReturnValue_t status = HasReturnvaluesIF::RETURN_OK; + uint32_t count = 0; + for (const auto& obj : objectList) { + // Ensure that each unique object is initialized once. + if (uniqueObjects.find(obj.first) == uniqueObjects.end()) { + ReturnValue_t result = obj.first->initializeAfterTaskCreation(); + if (result != HasReturnvaluesIF::RETURN_OK) { + count++; + status = result; + } + uniqueObjects.emplace(obj.first); + } + } + if (count > 0) { + } + return status; +} diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.h b/src/fsfw/osal/linux/PeriodicPosixTask.h index 3cd9847a..1142c854 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.h +++ b/src/fsfw/osal/linux/PeriodicPosixTask.h @@ -1,7 +1,7 @@ #ifndef FRAMEWORK_OSAL_LINUX_PERIODICPOSIXTASK_H_ #define FRAMEWORK_OSAL_LINUX_PERIODICPOSIXTASK_H_ -#include +#include #include "../../objectmanager/ObjectManagerIF.h" #include "../../tasks/ExecutableObjectIF.h" @@ -40,7 +40,7 @@ class PeriodicPosixTask : public PosixThread, public PeriodicTaskIF { * @param object Id of the object to add. * @return RETURN_OK on success, RETURN_FAILED if the object could not be added. */ - ReturnValue_t addComponent(object_id_t object) override; + ReturnValue_t addComponent(object_id_t object, uint8_t opCode) override; /** * Adds an object to the list of objects to be executed. @@ -48,14 +48,20 @@ class PeriodicPosixTask : public PosixThread, public PeriodicTaskIF { * @param object pointer to the object to add. * @return RETURN_OK on success, RETURN_FAILED if the object could not be added. */ - ReturnValue_t addComponent(ExecutableObjectIF* object) override; + ReturnValue_t addComponent(ExecutableObjectIF* object, uint8_t opCode) override; uint32_t getPeriodMs() const override; ReturnValue_t sleepFor(uint32_t ms) override; + ReturnValue_t initObjsAfterTaskCreation(); + + bool isEmpty() const override; + private: - typedef std::vector ObjectList; //!< Typedef for the List of objects. + //! Typedef for the List of objects. Will contain the objects to execute and their respective + //! op codes + using ObjectList = std::multiset>; /** * @brief This attribute holds a list of objects to be executed. */ diff --git a/src/fsfw/tasks/FixedSlotSequence.cpp b/src/fsfw/tasks/FixedSlotSequence.cpp index d4c67b4d..62c0e99c 100644 --- a/src/fsfw/tasks/FixedSlotSequence.cpp +++ b/src/fsfw/tasks/FixedSlotSequence.cpp @@ -164,3 +164,5 @@ ReturnValue_t FixedSlotSequence::intializeSequenceAfterTaskCreation() const { void FixedSlotSequence::addCustomCheck(ReturnValue_t (*customCheckFunction)(const SlotList&)) { this->customCheckFunction = customCheckFunction; } + +bool FixedSlotSequence::isEmpty() const { return slotList.empty(); } diff --git a/src/fsfw/tasks/FixedSlotSequence.h b/src/fsfw/tasks/FixedSlotSequence.h index a287c5b2..5ece7126 100644 --- a/src/fsfw/tasks/FixedSlotSequence.h +++ b/src/fsfw/tasks/FixedSlotSequence.h @@ -159,6 +159,8 @@ class FixedSlotSequence { */ ReturnValue_t intializeSequenceAfterTaskCreation() const; + bool isEmpty() const; + protected: /** * @brief This list contains all PollingSlot objects, defining order and diff --git a/src/fsfw/tasks/FixedTimeslotTaskIF.h b/src/fsfw/tasks/FixedTimeslotTaskIF.h index 497db245..9d85ac4a 100644 --- a/src/fsfw/tasks/FixedTimeslotTaskIF.h +++ b/src/fsfw/tasks/FixedTimeslotTaskIF.h @@ -30,7 +30,7 @@ class FixedTimeslotTaskIF : public PeriodicTaskIF { * Check whether the sequence is valid and perform all other required * initialization steps which are needed after task creation */ - virtual ReturnValue_t checkSequence() const = 0; + virtual ReturnValue_t checkSequence() = 0; }; #endif /* FRAMEWORK_TASKS_FIXEDTIMESLOTTASKIF_H_ */ diff --git a/src/fsfw/tasks/PeriodicTaskIF.h b/src/fsfw/tasks/PeriodicTaskIF.h index c78a32de..2ae268fc 100644 --- a/src/fsfw/tasks/PeriodicTaskIF.h +++ b/src/fsfw/tasks/PeriodicTaskIF.h @@ -31,7 +31,7 @@ class PeriodicTaskIF { * Add an object to the task. The object needs to implement ExecutableObjectIF * @return */ - virtual ReturnValue_t addComponent(object_id_t object) { + virtual ReturnValue_t addComponent(object_id_t object, uint8_t opCode = 0) { return HasReturnvaluesIF::RETURN_FAILED; }; @@ -41,13 +41,15 @@ class PeriodicTaskIF { * Add an object to the task. * @return */ - virtual ReturnValue_t addComponent(ExecutableObjectIF* object) { + virtual ReturnValue_t addComponent(ExecutableObjectIF* object, uint8_t opCode = 0) { return HasReturnvaluesIF::RETURN_FAILED; }; virtual ReturnValue_t sleepFor(uint32_t ms) = 0; virtual uint32_t getPeriodMs() const = 0; + + virtual bool isEmpty() const = 0; }; #endif /* PERIODICTASKIF_H_ */ diff --git a/tests/src/fsfw_tests/unit/power/testPowerSwitcher.cpp b/tests/src/fsfw_tests/unit/power/testPowerSwitcher.cpp index d8523558..941055ac 100644 --- a/tests/src/fsfw_tests/unit/power/testPowerSwitcher.cpp +++ b/tests/src/fsfw_tests/unit/power/testPowerSwitcher.cpp @@ -67,7 +67,5 @@ TEST_CASE("Power Switcher", "[power-switcher]") { REQUIRE(not switcherUsingDummy.active()); } - SECTION("More Dummy Tests") { - - } + SECTION("More Dummy Tests") {} } From bcd19045cc9f19d9fd0b61f991cb334c6315832a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 14 May 2022 11:33:43 +0200 Subject: [PATCH 133/404] refactored SPI mutex handling --- .../fsfw_hal/linux/spi/ManualCsLockGuard.h | 42 ++++++++++++++ hal/src/fsfw_hal/linux/spi/SpiComIF.cpp | 51 +++++++++-------- hal/src/fsfw_hal/linux/spi/SpiComIF.h | 8 +-- hal/src/fsfw_hal/linux/spi/SpiCookie.cpp | 14 +++++ hal/src/fsfw_hal/linux/spi/SpiCookie.h | 55 +++++++++++++------ 5 files changed, 123 insertions(+), 47 deletions(-) create mode 100644 hal/src/fsfw_hal/linux/spi/ManualCsLockGuard.h diff --git a/hal/src/fsfw_hal/linux/spi/ManualCsLockGuard.h b/hal/src/fsfw_hal/linux/spi/ManualCsLockGuard.h new file mode 100644 index 00000000..99131393 --- /dev/null +++ b/hal/src/fsfw_hal/linux/spi/ManualCsLockGuard.h @@ -0,0 +1,42 @@ +#pragma once + +#include "fsfw/ipc/MutexIF.h" +#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw_hal/common/gpio/GpioIF.h" + +class ManualCsLockWrapper : public HasReturnvaluesIF { + public: + ManualCsLockWrapper(MutexIF* lock, GpioIF* gpioIF, SpiCookie* cookie, + MutexIF::TimeoutType type = MutexIF::TimeoutType::BLOCKING, + uint32_t timeoutMs = 0) + : lock(lock), gpioIF(gpioIF), cookie(cookie), type(type), timeoutMs(timeoutMs) { + if (cookie == nullptr) { + // TODO: Error? Or maybe throw exception.. + return; + } + cookie->setCsLockManual(true); + lockResult = lock->lockMutex(type, timeoutMs); + if (lockResult != RETURN_OK) { + return; + } + gpioResult = gpioIF->pullLow(cookie->getChipSelectPin()); + } + + ~ManualCsLockWrapper() { + if (lockResult == RETURN_OK) { + lock->unlockMutex(); + } + if (gpioResult == RETURN_OK) { + gpioIF->pullHigh(cookie->getChipSelectPin()); + } + } + ReturnValue_t lockResult; + ReturnValue_t gpioResult; + + private: + MutexIF* lock; + GpioIF* gpioIF; + SpiCookie* cookie; + MutexIF::TimeoutType type; + uint32_t timeoutMs = 0; +}; diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp index b4ed47f2..c370ee37 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -194,16 +194,22 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const bool fullDuplex = spiCookie->isFullDuplex(); gpioId_t gpioId = spiCookie->getChipSelectPin(); + bool csLockManual = spiCookie->getCsLockManual(); - /* Pull SPI CS low. For now, no support for active high given */ - if (gpioId != gpio::NO_GPIO) { - result = csMutex->lockMutex(timeoutType, timeoutMs); + MutexIF::TimeoutType csType; + dur_millis_t csTimeout = 0; + // Pull SPI CS low. For now, no support for active high given + if (gpioId != gpio::NO_GPIO and not csLockManual) { + spiCookie->getMutexParams(csType, csTimeout); + result = csMutex->lockMutex(csType, csTimeout); if (result != RETURN_OK) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "SpiComIF::sendMessage: Failed to lock mutex" << std::endl; + sif::error << "SpiComIF::sendMessage: Failed to lock mutex with code " + << "0x" << std::hex << std::setfill('0') << std::setw(4) << result << std::dec + << std::endl; #else - sif::printError("SpiComIF::sendMessage: Failed to lock mutex\n"); + sif::printError("SpiComIF::sendMessage: Failed to lock mutex with code %d\n", result); #endif #endif return result; @@ -249,7 +255,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const } } - if (gpioId != gpio::NO_GPIO) { + if (gpioId != gpio::NO_GPIO and not csLockManual) { gpioComIF->pullHigh(gpioId); result = csMutex->unlockMutex(); if (result != RETURN_OK) { @@ -292,12 +298,22 @@ ReturnValue_t SpiComIF::performHalfDuplexReception(SpiCookie* spiCookie) { return result; } + bool csLockManual = spiCookie->getCsLockManual(); gpioId_t gpioId = spiCookie->getChipSelectPin(); - if (gpioId != gpio::NO_GPIO) { - result = csMutex->lockMutex(timeoutType, timeoutMs); + MutexIF::TimeoutType csType; + dur_millis_t csTimeout = 0; + if (gpioId != gpio::NO_GPIO and not csLockManual) { + spiCookie->getMutexParams(csType, csTimeout); + result = csMutex->lockMutex(csType, csTimeout); if (result != RETURN_OK) { +#if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "SpiComIF::getSendSuccess: Failed to lock mutex" << std::endl; + sif::error << "SpiComIF::sendMessage: Failed to lock mutex with code " + << "0x" << std::hex << std::setfill('0') << std::setw(4) << result << std::dec + << std::endl; +#else + sif::printError("SpiComIF::sendMessage: Failed to lock mutex with code %d\n", result); +#endif #endif return result; } @@ -315,7 +331,7 @@ ReturnValue_t SpiComIF::performHalfDuplexReception(SpiCookie* spiCookie) { result = HALF_DUPLEX_TRANSFER_FAILED; } - if (gpioId != gpio::NO_GPIO) { + if (gpioId != gpio::NO_GPIO and not csLockManual) { gpioComIF->pullHigh(gpioId); result = csMutex->unlockMutex(); if (result != RETURN_OK) { @@ -346,15 +362,7 @@ ReturnValue_t SpiComIF::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, return HasReturnvaluesIF::RETURN_OK; } -MutexIF* SpiComIF::getMutex(MutexIF::TimeoutType* timeoutType, uint32_t* timeoutMs) { - if (timeoutType != nullptr) { - *timeoutType = this->timeoutType; - } - if (timeoutMs != nullptr) { - *timeoutMs = this->timeoutMs; - } - return csMutex; -} +MutexIF* SpiComIF::getCsMutex() { return csMutex; } void SpiComIF::performSpiWiretapping(SpiCookie* spiCookie) { if (spiCookie == nullptr) { @@ -426,8 +434,3 @@ void SpiComIF::updateLinePolarity(int spiFd) { utility::handleIoctlError("SpiComIF::setSpiSpeedAndMode: Updating SPI default clock failed"); } } - -void SpiComIF::setMutexParams(MutexIF::TimeoutType timeoutType_, uint32_t timeoutMs_) { - timeoutType = timeoutType_; - timeoutMs = timeoutMs_; -} diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.h b/hal/src/fsfw_hal/linux/spi/SpiComIF.h index 113b42b0..52673457 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.h +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.h @@ -22,8 +22,6 @@ class SpiCookie; */ class SpiComIF : public DeviceCommunicationIF, public SystemObject { public: - static constexpr dur_millis_t DEFAULT_MUTEX_TIMEOUT = 20; - static constexpr uint8_t CLASS_ID = CLASS_ID::HAL_SPI; static constexpr ReturnValue_t OPENING_FILE_FAILED = HasReturnvaluesIF::makeReturnCode(CLASS_ID, 0); @@ -46,7 +44,7 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { * @brief This function returns the mutex which can be used to protect the spi bus when * the chip select must be driven from outside of the com if. */ - MutexIF* getMutex(MutexIF::TimeoutType* timeoutType = nullptr, uint32_t* timeoutMs = nullptr); + MutexIF* getCsMutex(); void setMutexParams(MutexIF::TimeoutType timeoutType, uint32_t timeoutMs); /** @@ -93,8 +91,8 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { * pulled high */ MutexIF* csMutex = nullptr; - MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; - uint32_t timeoutMs = DEFAULT_MUTEX_TIMEOUT; + // MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; + // uint32_t timeoutMs = DEFAULT_MUTEX_TIMEOUT; spi_ioc_transfer clockUpdateTransfer = {}; using SpiDeviceMap = std::unordered_map; diff --git a/hal/src/fsfw_hal/linux/spi/SpiCookie.cpp b/hal/src/fsfw_hal/linux/spi/SpiCookie.cpp index 85f96f28..e61703e6 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiCookie.cpp +++ b/hal/src/fsfw_hal/linux/spi/SpiCookie.cpp @@ -104,3 +104,17 @@ void SpiCookie::getCallback(spi::send_callback_function_t* callback, void** args *callback = this->sendCallback; *args = this->callbackArgs; } + +void SpiCookie::setCsLockManual(bool enable) { manualCsLock = enable; } + +bool SpiCookie::getCsLockManual() const { return manualCsLock; } + +void SpiCookie::getMutexParams(MutexIF::TimeoutType& csTimeoutType, dur_millis_t& csTimeout) const { + csTimeoutType = this->csTimeoutType; + csTimeout = this->csTimeout; +} + +void SpiCookie::setMutexParams(MutexIF::TimeoutType csTimeoutType, dur_millis_t csTimeout) { + this->csTimeoutType = csTimeoutType; + this->csTimeout = csTimeout; +} diff --git a/hal/src/fsfw_hal/linux/spi/SpiCookie.h b/hal/src/fsfw_hal/linux/spi/SpiCookie.h index d6d4078f..2104e2eb 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiCookie.h +++ b/hal/src/fsfw_hal/linux/spi/SpiCookie.h @@ -2,6 +2,8 @@ #define LINUX_SPI_SPICOOKIE_H_ #include +#include +#include #include #include "../../common/gpio/gpioDefinitions.h" @@ -20,6 +22,8 @@ */ class SpiCookie : public CookieIF { public: + static constexpr dur_millis_t DEFAULT_MUTEX_TIMEOUT = 20; + /** * Each SPI device will have a corresponding cookie. The cookie is used by the communication * interface and contains device specific information like the largest expected size to be @@ -137,9 +141,42 @@ class SpiCookie : public CookieIF { */ void activateCsDeselect(bool deselectCs, uint16_t delayUsecs); + void getMutexParams(MutexIF::TimeoutType& csTimeoutType, dur_millis_t& csTimeout) const; + void setMutexParams(MutexIF::TimeoutType csTimeoutType, dur_millis_t csTimeout); + + void setCsLockManual(bool enable); + bool getCsLockManual() const; + spi_ioc_transfer* getTransferStructHandle(); private: + address_t spiAddress; + gpioId_t chipSelectPin; + + spi::SpiComIfModes comIfMode; + + // Required for regular mode + const size_t maxSize; + spi::SpiModes spiMode; + /** + * If this is set to true, the SPI ComIF will not perform any mutex locking for the + * CS mechanism. The user is responsible to locking and unlocking the mutex for the + * whole duration of the transfers. + */ + bool manualCsLock = false; + uint32_t spiSpeed; + bool halfDuplex = false; + + MutexIF::TimeoutType csTimeoutType = MutexIF::TimeoutType::WAITING; + dur_millis_t csTimeout = DEFAULT_MUTEX_TIMEOUT; + + // Required for callback mode + spi::send_callback_function_t sendCallback = nullptr; + void* callbackArgs = nullptr; + + struct spi_ioc_transfer spiTransferStruct = {}; + UncommonParameters uncommonParameters; + /** * Internal constructor which initializes every field * @param spiAddress @@ -154,24 +191,6 @@ class SpiCookie : public CookieIF { SpiCookie(spi::SpiComIfModes comIfMode, address_t spiAddress, gpioId_t chipSelect, const size_t maxSize, spi::SpiModes spiMode, uint32_t spiSpeed, spi::send_callback_function_t callback, void* args); - - address_t spiAddress; - gpioId_t chipSelectPin; - - spi::SpiComIfModes comIfMode; - - // Required for regular mode - const size_t maxSize; - spi::SpiModes spiMode; - uint32_t spiSpeed; - bool halfDuplex = false; - - // Required for callback mode - spi::send_callback_function_t sendCallback = nullptr; - void* callbackArgs = nullptr; - - struct spi_ioc_transfer spiTransferStruct = {}; - UncommonParameters uncommonParameters; }; #endif /* LINUX_SPI_SPICOOKIE_H_ */ From 55ed7ab93e816ea083a961018a9383dc6c8ff33f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 14 May 2022 16:58:28 +0200 Subject: [PATCH 134/404] important fix --- hal/src/fsfw_hal/linux/spi/ManualCsLockGuard.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/hal/src/fsfw_hal/linux/spi/ManualCsLockGuard.h b/hal/src/fsfw_hal/linux/spi/ManualCsLockGuard.h index 99131393..b282bcc0 100644 --- a/hal/src/fsfw_hal/linux/spi/ManualCsLockGuard.h +++ b/hal/src/fsfw_hal/linux/spi/ManualCsLockGuard.h @@ -23,12 +23,13 @@ class ManualCsLockWrapper : public HasReturnvaluesIF { } ~ManualCsLockWrapper() { - if (lockResult == RETURN_OK) { - lock->unlockMutex(); - } if (gpioResult == RETURN_OK) { gpioIF->pullHigh(cookie->getChipSelectPin()); } + cookie->setCsLockManual(false); + if (lockResult == RETURN_OK) { + lock->unlockMutex(); + } } ReturnValue_t lockResult; ReturnValue_t gpioResult; From 14a1b4a7ac57080758b2a1931786ea69d82db780 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 17 May 2022 13:31:33 +0200 Subject: [PATCH 135/404] made auto-formatter even more re-usable --- scripts/auto-formatter.sh | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/scripts/auto-formatter.sh b/scripts/auto-formatter.sh index 7b67ee9d..33f3fabc 100755 --- a/scripts/auto-formatter.sh +++ b/scripts/auto-formatter.sh @@ -12,11 +12,17 @@ else fi cpp_format="clang-format" +folder_list=( + "./src" + "./hal" + "./tests" +) file_selectors="-iname *.h -o -iname *.cpp -o -iname *.c -o -iname *.tpp" if command -v ${cpp_format} &> /dev/null; then - find ./src ${file_selectors} | xargs clang-format --style=file -i - find ./hal ${file_selectors} | xargs clang-format --style=file -i - find ./tests ${file_selectors} | xargs clang-format --style=file -i + echo "Auto-formatting ${dir} recursively" + for dir in ${allThreads[@]}; do + find ${dir} ${file_selectors} | xargs clang-format --style=file -i + done else echo "No ${cpp_format} tool found, not formatting C++/C files" fi From e8023886f60ba3af0a63279da8dbc0d3eb0939a7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 17 May 2022 13:31:56 +0200 Subject: [PATCH 136/404] made auto-formatter even more usable --- scripts/auto-formatter.sh | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/scripts/auto-formatter.sh b/scripts/auto-formatter.sh index 33f3fabc..cec0b680 100755 --- a/scripts/auto-formatter.sh +++ b/scripts/auto-formatter.sh @@ -3,6 +3,12 @@ if [[ ! -f README.md ]]; then cd .. fi +folder_list=( + "./src" + "./hal" + "./tests" +) + cmake_fmt="cmake-format" if command -v ${cmake_fmt} &> /dev/null; then cmake_fmt_cmd="${cmake_fmt} -i CMakeLists.txt" @@ -12,15 +18,10 @@ else fi cpp_format="clang-format" -folder_list=( - "./src" - "./hal" - "./tests" -) file_selectors="-iname *.h -o -iname *.cpp -o -iname *.c -o -iname *.tpp" if command -v ${cpp_format} &> /dev/null; then - echo "Auto-formatting ${dir} recursively" - for dir in ${allThreads[@]}; do + for dir in ${folder_list[@]}; do + echo "Auto-formatting ${dir} recursively" find ${dir} ${file_selectors} | xargs clang-format --style=file -i done else From f9c42d3583555a03ef9cb7477689c69c422d9a0d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 17 May 2022 18:12:05 +0200 Subject: [PATCH 137/404] vector as core container is ok --- src/fsfw/osal/linux/PeriodicPosixTask.cpp | 7 ++++--- src/fsfw/osal/linux/PeriodicPosixTask.h | 4 ++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.cpp b/src/fsfw/osal/linux/PeriodicPosixTask.cpp index 26b6f53e..44d669df 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.cpp +++ b/src/fsfw/osal/linux/PeriodicPosixTask.cpp @@ -1,6 +1,7 @@ #include "fsfw/osal/linux/PeriodicPosixTask.h" -#include +#include +#include #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/serviceinterface/ServiceInterface.h" @@ -23,7 +24,7 @@ void* PeriodicPosixTask::taskEntryPoint(void* arg) { PeriodicPosixTask* originalTask(reinterpret_cast(arg)); // The task's functionality is called. originalTask->taskFunctionality(); - return NULL; + return nullptr; } ReturnValue_t PeriodicPosixTask::addComponent(object_id_t object, uint8_t opCode) { @@ -43,7 +44,7 @@ ReturnValue_t PeriodicPosixTask::addComponent(ExecutableObjectIF* object, uint8_ #endif return HasReturnvaluesIF::RETURN_FAILED; } - objectList.emplace(object, opCode); + objectList.push_back({object, opCode}); object->setTaskIF(this); return HasReturnvaluesIF::RETURN_OK; diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.h b/src/fsfw/osal/linux/PeriodicPosixTask.h index 1142c854..a3c6b187 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.h +++ b/src/fsfw/osal/linux/PeriodicPosixTask.h @@ -1,7 +1,7 @@ #ifndef FRAMEWORK_OSAL_LINUX_PERIODICPOSIXTASK_H_ #define FRAMEWORK_OSAL_LINUX_PERIODICPOSIXTASK_H_ -#include +#include #include "../../objectmanager/ObjectManagerIF.h" #include "../../tasks/ExecutableObjectIF.h" @@ -61,7 +61,7 @@ class PeriodicPosixTask : public PosixThread, public PeriodicTaskIF { private: //! Typedef for the List of objects. Will contain the objects to execute and their respective //! op codes - using ObjectList = std::multiset>; + using ObjectList = std::vector>; /** * @brief This attribute holds a list of objects to be executed. */ From 18b342e94b59123b986fca3242ba375be3c6aefd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 17 May 2022 18:12:05 +0200 Subject: [PATCH 138/404] vector as core container is ok --- src/fsfw/osal/linux/PeriodicPosixTask.cpp | 7 ++++--- src/fsfw/osal/linux/PeriodicPosixTask.h | 4 ++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.cpp b/src/fsfw/osal/linux/PeriodicPosixTask.cpp index 26b6f53e..44d669df 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.cpp +++ b/src/fsfw/osal/linux/PeriodicPosixTask.cpp @@ -1,6 +1,7 @@ #include "fsfw/osal/linux/PeriodicPosixTask.h" -#include +#include +#include #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/serviceinterface/ServiceInterface.h" @@ -23,7 +24,7 @@ void* PeriodicPosixTask::taskEntryPoint(void* arg) { PeriodicPosixTask* originalTask(reinterpret_cast(arg)); // The task's functionality is called. originalTask->taskFunctionality(); - return NULL; + return nullptr; } ReturnValue_t PeriodicPosixTask::addComponent(object_id_t object, uint8_t opCode) { @@ -43,7 +44,7 @@ ReturnValue_t PeriodicPosixTask::addComponent(ExecutableObjectIF* object, uint8_ #endif return HasReturnvaluesIF::RETURN_FAILED; } - objectList.emplace(object, opCode); + objectList.push_back({object, opCode}); object->setTaskIF(this); return HasReturnvaluesIF::RETURN_OK; diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.h b/src/fsfw/osal/linux/PeriodicPosixTask.h index 1142c854..a3c6b187 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.h +++ b/src/fsfw/osal/linux/PeriodicPosixTask.h @@ -1,7 +1,7 @@ #ifndef FRAMEWORK_OSAL_LINUX_PERIODICPOSIXTASK_H_ #define FRAMEWORK_OSAL_LINUX_PERIODICPOSIXTASK_H_ -#include +#include #include "../../objectmanager/ObjectManagerIF.h" #include "../../tasks/ExecutableObjectIF.h" @@ -61,7 +61,7 @@ class PeriodicPosixTask : public PosixThread, public PeriodicTaskIF { private: //! Typedef for the List of objects. Will contain the objects to execute and their respective //! op codes - using ObjectList = std::multiset>; + using ObjectList = std::vector>; /** * @brief This attribute holds a list of objects to be executed. */ From 7b3de873644150fe82cb54399b35c59c7134cd3c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 18 May 2022 13:19:43 +0200 Subject: [PATCH 139/404] removed some changes which belong in separate PR --- hal/src/fsfw_hal/linux/CommandExecutor.cpp | 2 -- hal/src/fsfw_hal/linux/spi/SpiComIF.cpp | 27 +++---------------- hal/src/fsfw_hal/linux/spi/SpiComIF.h | 30 ++++------------------ 3 files changed, 8 insertions(+), 51 deletions(-) diff --git a/hal/src/fsfw_hal/linux/CommandExecutor.cpp b/hal/src/fsfw_hal/linux/CommandExecutor.cpp index dcdd10ee..49c44ebf 100644 --- a/hal/src/fsfw_hal/linux/CommandExecutor.cpp +++ b/hal/src/fsfw_hal/linux/CommandExecutor.cpp @@ -205,5 +205,3 @@ ReturnValue_t CommandExecutor::executeBlocking() { } return HasReturnvaluesIF::RETURN_OK; } - -const std::vector& CommandExecutor::getReadVector() const { return readVec; } diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp index 3c257f1f..dcf92b5d 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -401,33 +401,12 @@ void SpiComIF::setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed) if (retval != 0) { utility::handleIoctlError("SpiComIF::setSpiSpeedAndMode: Setting SPI speed failed"); } -} - -void SpiComIF::getSpiSpeedAndMode(int spiFd, spi::SpiModes& mode, uint32_t& speed) const { - uint8_t tmpMode = 0; - int retval = ioctl(spiFd, SPI_IOC_RD_MODE, &tmpMode); - if (retval != 0) { - utility::handleIoctlError("SpiComIF::getSpiSpeedAndMode: Reading SPI mode failed"); - } - mode = static_cast(tmpMode); - - retval = ioctl(spiFd, SPI_IOC_RD_MAX_SPEED_HZ, &speed); - if (retval != 0) { - utility::handleIoctlError("SpiComIF::getSpiSpeedAndMode: Getting SPI speed failed"); - } -} - -const std::string& SpiComIF::getSpiDev() const { return dev; } - -void SpiComIF::updateLinePolarity(int spiFd) { + // This updates the SPI clock default polarity. Only setting the mode does not update + // the line state, which can be an issue on mode switches because the clock line will + // switch the state after the chip select is pulled low clockUpdateTransfer.len = 0; retval = ioctl(spiFd, SPI_IOC_MESSAGE(1), &clockUpdateTransfer); if (retval != 0) { utility::handleIoctlError("SpiComIF::setSpiSpeedAndMode: Updating SPI default clock failed"); } } - -void SpiComIF::setMutexParams(MutexIF::TimeoutType timeoutType_, uint32_t timeoutMs_) { - timeoutType = timeoutType_; - timeoutMs = timeoutMs_; -} diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.h b/hal/src/fsfw_hal/linux/spi/SpiComIF.h index 1400dcfc..357afa2f 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.h +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.h @@ -22,17 +22,15 @@ class SpiCookie; */ class SpiComIF : public DeviceCommunicationIF, public SystemObject { public: - static constexpr dur_millis_t DEFAULT_MUTEX_TIMEOUT = 20; - - static constexpr uint8_t CLASS_ID = CLASS_ID::HAL_SPI; + static constexpr uint8_t spiRetvalId = CLASS_ID::HAL_SPI; static constexpr ReturnValue_t OPENING_FILE_FAILED = - HasReturnvaluesIF::makeReturnCode(CLASS_ID, 0); + HasReturnvaluesIF::makeReturnCode(spiRetvalId, 0); /* Full duplex (ioctl) transfer failure */ static constexpr ReturnValue_t FULL_DUPLEX_TRANSFER_FAILED = - HasReturnvaluesIF::makeReturnCode(CLASS_ID, 1); + HasReturnvaluesIF::makeReturnCode(spiRetvalId, 1); /* Half duplex (read/write) transfer failure */ static constexpr ReturnValue_t HALF_DUPLEX_TRANSFER_FAILED = - HasReturnvaluesIF::makeReturnCode(CLASS_ID, 2); + HasReturnvaluesIF::makeReturnCode(spiRetvalId, 2); SpiComIF(object_id_t objectId, GpioIF* gpioComIF); @@ -47,7 +45,6 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { * the chip select must be driven from outside of the com if. */ MutexIF* getMutex(MutexIF::TimeoutType* timeoutType = nullptr, uint32_t* timeoutMs = nullptr); - void setMutexParams(MutexIF::TimeoutType timeoutType, uint32_t timeoutMs); /** * Perform a regular send operation using Linux iotcl. This is public so it can be used @@ -62,23 +59,6 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { GpioIF* getGpioInterface(); void setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed); -<<<<<<< Updated upstream -======= - void getSpiSpeedAndMode(int spiFd, spi::SpiModes& mode, uint32_t& speed) const; - - /** - * This updates the SPI clock default polarity. Only setting the mode does not update - * the line state, which can be an issue on mode switches because the clock line will - * switch the state after the chip select is pulled low. - * - * It is recommended to call this function after #setSpiSpeedAndMode and after locking the - * CS mutex if the SPI bus has multiple SPI devices with different speed and SPI modes attached. - * @param spiFd - */ - void updateLinePolarity(int spiFd); - - const std::string& getSpiDev() const; ->>>>>>> Stashed changes void performSpiWiretapping(SpiCookie* spiCookie); ReturnValue_t getReadBuffer(address_t spiAddress, uint8_t** buffer); @@ -93,7 +73,7 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { MutexIF* spiMutex = nullptr; MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; - uint32_t timeoutMs = DEFAULT_MUTEX_TIMEOUT; + uint32_t timeoutMs = 20; spi_ioc_transfer clockUpdateTransfer = {}; using SpiDeviceMap = std::unordered_map; From e87b5a0207070ebe3125cd675a108cae29d5c2f0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 18 May 2022 14:32:35 +0200 Subject: [PATCH 140/404] new base class for periodic tasks --- src/fsfw/osal/linux/PeriodicPosixTask.cpp | 57 ++--------------------- src/fsfw/osal/linux/PeriodicPosixTask.h | 55 ++++------------------ src/fsfw/tasks/PeriodicTaskBase.cpp | 57 +++++++++++++++++++++++ src/fsfw/tasks/PeriodicTaskBase.h | 52 +++++++++++++++++++++ src/fsfw/tasks/PeriodicTaskIF.h | 24 +++++----- src/fsfw/tasks/TaskFactory.h | 2 +- src/fsfw/tasks/Typedef.h | 13 ------ src/fsfw/tasks/definitions.h | 13 ++++++ 8 files changed, 148 insertions(+), 125 deletions(-) create mode 100644 src/fsfw/tasks/PeriodicTaskBase.cpp create mode 100644 src/fsfw/tasks/PeriodicTaskBase.h delete mode 100644 src/fsfw/tasks/Typedef.h create mode 100644 src/fsfw/tasks/definitions.h diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.cpp b/src/fsfw/osal/linux/PeriodicPosixTask.cpp index 44d669df..510ec59c 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.cpp +++ b/src/fsfw/osal/linux/PeriodicPosixTask.cpp @@ -8,12 +8,10 @@ #include "fsfw/tasks/ExecutableObjectIF.h" PeriodicPosixTask::PeriodicPosixTask(const char* name_, int priority_, size_t stackSize_, - uint32_t period_, void(deadlineMissedFunc_)()) + uint32_t period_, TaskDeadlineMissedFunction dlMissedFunc_) : PosixThread(name_, priority_, stackSize_), - objectList(), - started(false), - periodMs(period_), - deadlineMissedFunc(deadlineMissedFunc_) {} + PeriodicTaskBase(period_, dlMissedFunc_), + started(false) {} PeriodicPosixTask::~PeriodicPosixTask() { // Not Implemented @@ -27,31 +25,8 @@ void* PeriodicPosixTask::taskEntryPoint(void* arg) { return nullptr; } -ReturnValue_t PeriodicPosixTask::addComponent(object_id_t object, uint8_t opCode) { - ExecutableObjectIF* newObject = ObjectManager::instance()->get(object); - return addComponent(newObject, opCode); -} - -ReturnValue_t PeriodicPosixTask::addComponent(ExecutableObjectIF* object, uint8_t opCode) { - if (object == nullptr) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PeriodicTask::addComponent: Invalid object. Make sure" - << " it implements ExecutableObjectIF!" << std::endl; -#else - sif::printError( - "PeriodicTask::addComponent: Invalid object. Make sure it " - "implements ExecutableObjectIF!\n"); -#endif - return HasReturnvaluesIF::RETURN_FAILED; - } - objectList.push_back({object, opCode}); - object->setTaskIF(this); - - return HasReturnvaluesIF::RETURN_OK; -} - ReturnValue_t PeriodicPosixTask::sleepFor(uint32_t ms) { - return PosixThread::sleep((uint64_t)ms * 1000000); + return PosixThread::sleep(static_cast(ms * 1000000)); } ReturnValue_t PeriodicPosixTask::startTask(void) { @@ -84,27 +59,3 @@ void PeriodicPosixTask::taskFunctionality(void) { } } } - -uint32_t PeriodicPosixTask::getPeriodMs() const { return periodMs; } - -bool PeriodicPosixTask::isEmpty() const { return objectList.empty(); } - -ReturnValue_t PeriodicPosixTask::initObjsAfterTaskCreation() { - std::multiset uniqueObjects; - ReturnValue_t status = HasReturnvaluesIF::RETURN_OK; - uint32_t count = 0; - for (const auto& obj : objectList) { - // Ensure that each unique object is initialized once. - if (uniqueObjects.find(obj.first) == uniqueObjects.end()) { - ReturnValue_t result = obj.first->initializeAfterTaskCreation(); - if (result != HasReturnvaluesIF::RETURN_OK) { - count++; - status = result; - } - uniqueObjects.emplace(obj.first); - } - } - if (count > 0) { - } - return status; -} diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.h b/src/fsfw/osal/linux/PeriodicPosixTask.h index a3c6b187..3dcc6bcc 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.h +++ b/src/fsfw/osal/linux/PeriodicPosixTask.h @@ -1,14 +1,17 @@ #ifndef FRAMEWORK_OSAL_LINUX_PERIODICPOSIXTASK_H_ #define FRAMEWORK_OSAL_LINUX_PERIODICPOSIXTASK_H_ -#include - -#include "../../objectmanager/ObjectManagerIF.h" -#include "../../tasks/ExecutableObjectIF.h" -#include "../../tasks/PeriodicTaskIF.h" #include "PosixThread.h" -class PeriodicPosixTask : public PosixThread, public PeriodicTaskIF { +#include + +#include "fsfw/objectmanager/ObjectManagerIF.h" +#include "fsfw/tasks/ExecutableObjectIF.h" +#include "fsfw/tasks/PeriodicTaskIF.h" +#include "fsfw/tasks/PeriodicTaskBase.h" + + +class PeriodicPosixTask : public PosixThread, public PeriodicTaskBase { public: /** * Create a generic periodic task. @@ -34,48 +37,16 @@ class PeriodicPosixTask : public PosixThread, public PeriodicTaskIF { * to the system call. */ ReturnValue_t startTask() override; - /** - * Adds an object to the list of objects to be executed. - * The objects are executed in the order added. - * @param object Id of the object to add. - * @return RETURN_OK on success, RETURN_FAILED if the object could not be added. - */ - ReturnValue_t addComponent(object_id_t object, uint8_t opCode) override; - - /** - * Adds an object to the list of objects to be executed. - * The objects are executed in the order added. - * @param object pointer to the object to add. - * @return RETURN_OK on success, RETURN_FAILED if the object could not be added. - */ - ReturnValue_t addComponent(ExecutableObjectIF* object, uint8_t opCode) override; - - uint32_t getPeriodMs() const override; ReturnValue_t sleepFor(uint32_t ms) override; - ReturnValue_t initObjsAfterTaskCreation(); - - bool isEmpty() const override; - private: - //! Typedef for the List of objects. Will contain the objects to execute and their respective - //! op codes - using ObjectList = std::vector>; - /** - * @brief This attribute holds a list of objects to be executed. - */ - ObjectList objectList; /** * @brief Flag to indicate that the task was started and is allowed to run */ bool started; - /** - * @brief Period of the task in milliseconds - */ - uint32_t periodMs; /** * @brief The function containing the actual functionality of the task. * @details The method sets and starts @@ -92,14 +63,6 @@ class PeriodicPosixTask : public PosixThread, public PeriodicTaskIF { * of the child class. Needs a valid pointer to the derived class. */ static void* taskEntryPoint(void* arg); - /** - * @brief The pointer to the deadline-missed function. - * @details This pointer stores the function that is executed if the task's deadline is missed. - * So, each may react individually on a timing failure. The pointer may be - * NULL, then nothing happens on missing the deadline. The deadline is equal to the next execution - * of the periodic task. - */ - void (*deadlineMissedFunc)(); }; #endif /* FRAMEWORK_OSAL_LINUX_PERIODICPOSIXTASK_H_ */ diff --git a/src/fsfw/tasks/PeriodicTaskBase.cpp b/src/fsfw/tasks/PeriodicTaskBase.cpp new file mode 100644 index 00000000..606fee1e --- /dev/null +++ b/src/fsfw/tasks/PeriodicTaskBase.cpp @@ -0,0 +1,57 @@ +#include +#include "PeriodicTaskBase.h" + +#include + +PeriodicTaskBase::PeriodicTaskBase(uint32_t periodMs_, + TaskDeadlineMissedFunction deadlineMissedFunc_) + : periodMs(periodMs_), deadlineMissedFunc(deadlineMissedFunc_) {} + +uint32_t PeriodicTaskBase::getPeriodMs() const { return periodMs; } + +bool PeriodicTaskBase::isEmpty() const override { + return objectList.empty(); +} + +ReturnValue_t PeriodicTaskBase::initObjsAfterTaskCreation() { + std::multiset uniqueObjects; + ReturnValue_t status = HasReturnvaluesIF::RETURN_OK; + uint32_t count = 0; + for (const auto& obj : objectList) { + // Ensure that each unique object is initialized once. + if (uniqueObjects.find(obj.first) == uniqueObjects.end()) { + ReturnValue_t result = obj.first->initializeAfterTaskCreation(); + if (result != HasReturnvaluesIF::RETURN_OK) { + count++; + status = result; + } + uniqueObjects.emplace(obj.first); + } + } + if (count > 0) { + } + return status; +} + +ReturnValue_t PeriodicTaskBase::addComponent(object_id_t object, uint8_t opCode) { + ExecutableObjectIF* newObject = ObjectManager::instance()->get(object); + return addComponent(newObject, opCode); +} + +ReturnValue_t PeriodicTaskBase::addComponent(ExecutableObjectIF* object, uint8_t opCode) { + if (object == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "PeriodicTask::addComponent: Invalid object. Make sure" + << " it implements ExecutableObjectIF!" << std::endl; +#else + sif::printError( + "PeriodicTask::addComponent: Invalid object. Make sure it " + "implements ExecutableObjectIF!\n"); +#endif + return HasReturnvaluesIF::RETURN_FAILED; + } + objectList.push_back({object, opCode}); + object->setTaskIF(this); + + return HasReturnvaluesIF::RETURN_OK; +} diff --git a/src/fsfw/tasks/PeriodicTaskBase.h b/src/fsfw/tasks/PeriodicTaskBase.h new file mode 100644 index 00000000..4b330426 --- /dev/null +++ b/src/fsfw/tasks/PeriodicTaskBase.h @@ -0,0 +1,52 @@ +#ifndef FSFW_SRC_FSFW_TASKS_PERIODICTASKBASE_H_ +#define FSFW_SRC_FSFW_TASKS_PERIODICTASKBASE_H_ + +#include "fsfw/tasks/PeriodicTaskIF.h" +#include "fsfw/tasks/definitions.h" +#include +#include + +class ExecutableObjectIF; + +class PeriodicTaskBase: public PeriodicTaskIF { +public: + PeriodicTaskBase(uint32_t periodMs, TaskDeadlineMissedFunction deadlineMissedFunc = nullptr); + + ReturnValue_t addComponent(object_id_t object, uint8_t opCode) override; + ReturnValue_t addComponent(ExecutableObjectIF* object, uint8_t opCode) override; + + + uint32_t getPeriodMs() const override; + + bool isEmpty() const override; + + ReturnValue_t initObjsAfterTaskCreation(); + +protected: + + //! Typedef for the List of objects. Will contain the objects to execute and their respective + //! operation codes + using ObjectList = std::vector>; + /** + * @brief This attribute holds a list of objects to be executed. + */ + ObjectList objectList; + + /** + * @brief Period of the task in milliseconds + */ + uint32_t periodMs; + + /** + * @brief The pointer to the deadline-missed function. + * @details This pointer stores the function that is executed if the task's deadline is missed. + * So, each may react individually on a timing failure. The pointer may be + * NULL, then nothing happens on missing the deadline. The deadline is equal to the next execution + * of the periodic task. + */ + TaskDeadlineMissedFunction deadlineMissedFunc = nullptr; +}; + + + +#endif /* FSFW_SRC_FSFW_TASKS_PERIODICTASKBASE_H_ */ diff --git a/src/fsfw/tasks/PeriodicTaskIF.h b/src/fsfw/tasks/PeriodicTaskIF.h index 2ae268fc..076ef56e 100644 --- a/src/fsfw/tasks/PeriodicTaskIF.h +++ b/src/fsfw/tasks/PeriodicTaskIF.h @@ -1,11 +1,10 @@ #ifndef FRAMEWORK_TASK_PERIODICTASKIF_H_ #define FRAMEWORK_TASK_PERIODICTASKIF_H_ -#include +#include "fsfw/objectmanager/SystemObjectIF.h" +#include "fsfw/tasks/ExecutableObjectIF.h" -#include "../objectmanager/SystemObjectIF.h" -#include "../timemanager/Clock.h" -class ExecutableObjectIF; +#include /** * New version of TaskIF @@ -26,20 +25,21 @@ class PeriodicTaskIF { virtual ReturnValue_t startTask() = 0; /** - * Add a component (object) to a periodic task. - * @param object - * Add an object to the task. The object needs to implement ExecutableObjectIF - * @return + * Adds an object to the list of objects to be executed. + * The objects are executed in the order added. The object needs to implement + * ExecutableObjectIF + * @param object Id of the object to add. + * @return RETURN_OK on success, RETURN_FAILED if the object could not be added. */ virtual ReturnValue_t addComponent(object_id_t object, uint8_t opCode = 0) { return HasReturnvaluesIF::RETURN_FAILED; }; /** - * Add an object to a periodic task. - * @param object - * Add an object to the task. - * @return + * Adds an object to the list of objects to be executed. + * The objects are executed in the order added. + * @param object pointer to the object to add. + * @return RETURN_OK on success, RETURN_FAILED if the object could not be added. */ virtual ReturnValue_t addComponent(ExecutableObjectIF* object, uint8_t opCode = 0) { return HasReturnvaluesIF::RETURN_FAILED; diff --git a/src/fsfw/tasks/TaskFactory.h b/src/fsfw/tasks/TaskFactory.h index fcd62678..828c533e 100644 --- a/src/fsfw/tasks/TaskFactory.h +++ b/src/fsfw/tasks/TaskFactory.h @@ -4,7 +4,7 @@ #include #include "FixedTimeslotTaskIF.h" -#include "Typedef.h" +#include "definitions.h" /** * Singleton Class that produces Tasks. diff --git a/src/fsfw/tasks/Typedef.h b/src/fsfw/tasks/Typedef.h deleted file mode 100644 index 1bb75131..00000000 --- a/src/fsfw/tasks/Typedef.h +++ /dev/null @@ -1,13 +0,0 @@ -#ifndef FSFW_TASKS_TYPEDEF_H_ -#define FSFW_TASKS_TYPEDEF_H_ - -#include -#include - -typedef const char* TaskName; -typedef uint32_t TaskPriority; -typedef size_t TaskStackSize; -typedef double TaskPeriod; -typedef void (*TaskDeadlineMissedFunction)(); - -#endif /* FSFW_TASKS_TYPEDEF_H_ */ diff --git a/src/fsfw/tasks/definitions.h b/src/fsfw/tasks/definitions.h new file mode 100644 index 00000000..bca9b768 --- /dev/null +++ b/src/fsfw/tasks/definitions.h @@ -0,0 +1,13 @@ +#ifndef FSFW_TASKS_TYPEDEF_H_ +#define FSFW_TASKS_TYPEDEF_H_ + +#include +#include + +using TaskName = const char*; +using TaskPriority = uint32_t; +using TaskStackSize = size_t; +using TaskPeriod = double; +using TaskDeadlineMissedFunction = void (*)(); + +#endif /* FSFW_TASKS_TYPEDEF_H_ */ From 86ca4f246bbf2a8adb3e265f8c4f0b47fde0807d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 18 May 2022 14:32:35 +0200 Subject: [PATCH 141/404] new base class for periodic tasks --- src/fsfw/osal/linux/PeriodicPosixTask.cpp | 57 ++--------------------- src/fsfw/osal/linux/PeriodicPosixTask.h | 55 ++++------------------ src/fsfw/tasks/PeriodicTaskBase.cpp | 57 +++++++++++++++++++++++ src/fsfw/tasks/PeriodicTaskBase.h | 52 +++++++++++++++++++++ src/fsfw/tasks/PeriodicTaskIF.h | 24 +++++----- src/fsfw/tasks/TaskFactory.h | 2 +- src/fsfw/tasks/Typedef.h | 13 ------ src/fsfw/tasks/definitions.h | 13 ++++++ 8 files changed, 148 insertions(+), 125 deletions(-) create mode 100644 src/fsfw/tasks/PeriodicTaskBase.cpp create mode 100644 src/fsfw/tasks/PeriodicTaskBase.h delete mode 100644 src/fsfw/tasks/Typedef.h create mode 100644 src/fsfw/tasks/definitions.h diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.cpp b/src/fsfw/osal/linux/PeriodicPosixTask.cpp index 44d669df..510ec59c 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.cpp +++ b/src/fsfw/osal/linux/PeriodicPosixTask.cpp @@ -8,12 +8,10 @@ #include "fsfw/tasks/ExecutableObjectIF.h" PeriodicPosixTask::PeriodicPosixTask(const char* name_, int priority_, size_t stackSize_, - uint32_t period_, void(deadlineMissedFunc_)()) + uint32_t period_, TaskDeadlineMissedFunction dlMissedFunc_) : PosixThread(name_, priority_, stackSize_), - objectList(), - started(false), - periodMs(period_), - deadlineMissedFunc(deadlineMissedFunc_) {} + PeriodicTaskBase(period_, dlMissedFunc_), + started(false) {} PeriodicPosixTask::~PeriodicPosixTask() { // Not Implemented @@ -27,31 +25,8 @@ void* PeriodicPosixTask::taskEntryPoint(void* arg) { return nullptr; } -ReturnValue_t PeriodicPosixTask::addComponent(object_id_t object, uint8_t opCode) { - ExecutableObjectIF* newObject = ObjectManager::instance()->get(object); - return addComponent(newObject, opCode); -} - -ReturnValue_t PeriodicPosixTask::addComponent(ExecutableObjectIF* object, uint8_t opCode) { - if (object == nullptr) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PeriodicTask::addComponent: Invalid object. Make sure" - << " it implements ExecutableObjectIF!" << std::endl; -#else - sif::printError( - "PeriodicTask::addComponent: Invalid object. Make sure it " - "implements ExecutableObjectIF!\n"); -#endif - return HasReturnvaluesIF::RETURN_FAILED; - } - objectList.push_back({object, opCode}); - object->setTaskIF(this); - - return HasReturnvaluesIF::RETURN_OK; -} - ReturnValue_t PeriodicPosixTask::sleepFor(uint32_t ms) { - return PosixThread::sleep((uint64_t)ms * 1000000); + return PosixThread::sleep(static_cast(ms * 1000000)); } ReturnValue_t PeriodicPosixTask::startTask(void) { @@ -84,27 +59,3 @@ void PeriodicPosixTask::taskFunctionality(void) { } } } - -uint32_t PeriodicPosixTask::getPeriodMs() const { return periodMs; } - -bool PeriodicPosixTask::isEmpty() const { return objectList.empty(); } - -ReturnValue_t PeriodicPosixTask::initObjsAfterTaskCreation() { - std::multiset uniqueObjects; - ReturnValue_t status = HasReturnvaluesIF::RETURN_OK; - uint32_t count = 0; - for (const auto& obj : objectList) { - // Ensure that each unique object is initialized once. - if (uniqueObjects.find(obj.first) == uniqueObjects.end()) { - ReturnValue_t result = obj.first->initializeAfterTaskCreation(); - if (result != HasReturnvaluesIF::RETURN_OK) { - count++; - status = result; - } - uniqueObjects.emplace(obj.first); - } - } - if (count > 0) { - } - return status; -} diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.h b/src/fsfw/osal/linux/PeriodicPosixTask.h index a3c6b187..3dcc6bcc 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.h +++ b/src/fsfw/osal/linux/PeriodicPosixTask.h @@ -1,14 +1,17 @@ #ifndef FRAMEWORK_OSAL_LINUX_PERIODICPOSIXTASK_H_ #define FRAMEWORK_OSAL_LINUX_PERIODICPOSIXTASK_H_ -#include - -#include "../../objectmanager/ObjectManagerIF.h" -#include "../../tasks/ExecutableObjectIF.h" -#include "../../tasks/PeriodicTaskIF.h" #include "PosixThread.h" -class PeriodicPosixTask : public PosixThread, public PeriodicTaskIF { +#include + +#include "fsfw/objectmanager/ObjectManagerIF.h" +#include "fsfw/tasks/ExecutableObjectIF.h" +#include "fsfw/tasks/PeriodicTaskIF.h" +#include "fsfw/tasks/PeriodicTaskBase.h" + + +class PeriodicPosixTask : public PosixThread, public PeriodicTaskBase { public: /** * Create a generic periodic task. @@ -34,48 +37,16 @@ class PeriodicPosixTask : public PosixThread, public PeriodicTaskIF { * to the system call. */ ReturnValue_t startTask() override; - /** - * Adds an object to the list of objects to be executed. - * The objects are executed in the order added. - * @param object Id of the object to add. - * @return RETURN_OK on success, RETURN_FAILED if the object could not be added. - */ - ReturnValue_t addComponent(object_id_t object, uint8_t opCode) override; - - /** - * Adds an object to the list of objects to be executed. - * The objects are executed in the order added. - * @param object pointer to the object to add. - * @return RETURN_OK on success, RETURN_FAILED if the object could not be added. - */ - ReturnValue_t addComponent(ExecutableObjectIF* object, uint8_t opCode) override; - - uint32_t getPeriodMs() const override; ReturnValue_t sleepFor(uint32_t ms) override; - ReturnValue_t initObjsAfterTaskCreation(); - - bool isEmpty() const override; - private: - //! Typedef for the List of objects. Will contain the objects to execute and their respective - //! op codes - using ObjectList = std::vector>; - /** - * @brief This attribute holds a list of objects to be executed. - */ - ObjectList objectList; /** * @brief Flag to indicate that the task was started and is allowed to run */ bool started; - /** - * @brief Period of the task in milliseconds - */ - uint32_t periodMs; /** * @brief The function containing the actual functionality of the task. * @details The method sets and starts @@ -92,14 +63,6 @@ class PeriodicPosixTask : public PosixThread, public PeriodicTaskIF { * of the child class. Needs a valid pointer to the derived class. */ static void* taskEntryPoint(void* arg); - /** - * @brief The pointer to the deadline-missed function. - * @details This pointer stores the function that is executed if the task's deadline is missed. - * So, each may react individually on a timing failure. The pointer may be - * NULL, then nothing happens on missing the deadline. The deadline is equal to the next execution - * of the periodic task. - */ - void (*deadlineMissedFunc)(); }; #endif /* FRAMEWORK_OSAL_LINUX_PERIODICPOSIXTASK_H_ */ diff --git a/src/fsfw/tasks/PeriodicTaskBase.cpp b/src/fsfw/tasks/PeriodicTaskBase.cpp new file mode 100644 index 00000000..606fee1e --- /dev/null +++ b/src/fsfw/tasks/PeriodicTaskBase.cpp @@ -0,0 +1,57 @@ +#include +#include "PeriodicTaskBase.h" + +#include + +PeriodicTaskBase::PeriodicTaskBase(uint32_t periodMs_, + TaskDeadlineMissedFunction deadlineMissedFunc_) + : periodMs(periodMs_), deadlineMissedFunc(deadlineMissedFunc_) {} + +uint32_t PeriodicTaskBase::getPeriodMs() const { return periodMs; } + +bool PeriodicTaskBase::isEmpty() const override { + return objectList.empty(); +} + +ReturnValue_t PeriodicTaskBase::initObjsAfterTaskCreation() { + std::multiset uniqueObjects; + ReturnValue_t status = HasReturnvaluesIF::RETURN_OK; + uint32_t count = 0; + for (const auto& obj : objectList) { + // Ensure that each unique object is initialized once. + if (uniqueObjects.find(obj.first) == uniqueObjects.end()) { + ReturnValue_t result = obj.first->initializeAfterTaskCreation(); + if (result != HasReturnvaluesIF::RETURN_OK) { + count++; + status = result; + } + uniqueObjects.emplace(obj.first); + } + } + if (count > 0) { + } + return status; +} + +ReturnValue_t PeriodicTaskBase::addComponent(object_id_t object, uint8_t opCode) { + ExecutableObjectIF* newObject = ObjectManager::instance()->get(object); + return addComponent(newObject, opCode); +} + +ReturnValue_t PeriodicTaskBase::addComponent(ExecutableObjectIF* object, uint8_t opCode) { + if (object == nullptr) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "PeriodicTask::addComponent: Invalid object. Make sure" + << " it implements ExecutableObjectIF!" << std::endl; +#else + sif::printError( + "PeriodicTask::addComponent: Invalid object. Make sure it " + "implements ExecutableObjectIF!\n"); +#endif + return HasReturnvaluesIF::RETURN_FAILED; + } + objectList.push_back({object, opCode}); + object->setTaskIF(this); + + return HasReturnvaluesIF::RETURN_OK; +} diff --git a/src/fsfw/tasks/PeriodicTaskBase.h b/src/fsfw/tasks/PeriodicTaskBase.h new file mode 100644 index 00000000..4b330426 --- /dev/null +++ b/src/fsfw/tasks/PeriodicTaskBase.h @@ -0,0 +1,52 @@ +#ifndef FSFW_SRC_FSFW_TASKS_PERIODICTASKBASE_H_ +#define FSFW_SRC_FSFW_TASKS_PERIODICTASKBASE_H_ + +#include "fsfw/tasks/PeriodicTaskIF.h" +#include "fsfw/tasks/definitions.h" +#include +#include + +class ExecutableObjectIF; + +class PeriodicTaskBase: public PeriodicTaskIF { +public: + PeriodicTaskBase(uint32_t periodMs, TaskDeadlineMissedFunction deadlineMissedFunc = nullptr); + + ReturnValue_t addComponent(object_id_t object, uint8_t opCode) override; + ReturnValue_t addComponent(ExecutableObjectIF* object, uint8_t opCode) override; + + + uint32_t getPeriodMs() const override; + + bool isEmpty() const override; + + ReturnValue_t initObjsAfterTaskCreation(); + +protected: + + //! Typedef for the List of objects. Will contain the objects to execute and their respective + //! operation codes + using ObjectList = std::vector>; + /** + * @brief This attribute holds a list of objects to be executed. + */ + ObjectList objectList; + + /** + * @brief Period of the task in milliseconds + */ + uint32_t periodMs; + + /** + * @brief The pointer to the deadline-missed function. + * @details This pointer stores the function that is executed if the task's deadline is missed. + * So, each may react individually on a timing failure. The pointer may be + * NULL, then nothing happens on missing the deadline. The deadline is equal to the next execution + * of the periodic task. + */ + TaskDeadlineMissedFunction deadlineMissedFunc = nullptr; +}; + + + +#endif /* FSFW_SRC_FSFW_TASKS_PERIODICTASKBASE_H_ */ diff --git a/src/fsfw/tasks/PeriodicTaskIF.h b/src/fsfw/tasks/PeriodicTaskIF.h index 2ae268fc..076ef56e 100644 --- a/src/fsfw/tasks/PeriodicTaskIF.h +++ b/src/fsfw/tasks/PeriodicTaskIF.h @@ -1,11 +1,10 @@ #ifndef FRAMEWORK_TASK_PERIODICTASKIF_H_ #define FRAMEWORK_TASK_PERIODICTASKIF_H_ -#include +#include "fsfw/objectmanager/SystemObjectIF.h" +#include "fsfw/tasks/ExecutableObjectIF.h" -#include "../objectmanager/SystemObjectIF.h" -#include "../timemanager/Clock.h" -class ExecutableObjectIF; +#include /** * New version of TaskIF @@ -26,20 +25,21 @@ class PeriodicTaskIF { virtual ReturnValue_t startTask() = 0; /** - * Add a component (object) to a periodic task. - * @param object - * Add an object to the task. The object needs to implement ExecutableObjectIF - * @return + * Adds an object to the list of objects to be executed. + * The objects are executed in the order added. The object needs to implement + * ExecutableObjectIF + * @param object Id of the object to add. + * @return RETURN_OK on success, RETURN_FAILED if the object could not be added. */ virtual ReturnValue_t addComponent(object_id_t object, uint8_t opCode = 0) { return HasReturnvaluesIF::RETURN_FAILED; }; /** - * Add an object to a periodic task. - * @param object - * Add an object to the task. - * @return + * Adds an object to the list of objects to be executed. + * The objects are executed in the order added. + * @param object pointer to the object to add. + * @return RETURN_OK on success, RETURN_FAILED if the object could not be added. */ virtual ReturnValue_t addComponent(ExecutableObjectIF* object, uint8_t opCode = 0) { return HasReturnvaluesIF::RETURN_FAILED; diff --git a/src/fsfw/tasks/TaskFactory.h b/src/fsfw/tasks/TaskFactory.h index fcd62678..828c533e 100644 --- a/src/fsfw/tasks/TaskFactory.h +++ b/src/fsfw/tasks/TaskFactory.h @@ -4,7 +4,7 @@ #include #include "FixedTimeslotTaskIF.h" -#include "Typedef.h" +#include "definitions.h" /** * Singleton Class that produces Tasks. diff --git a/src/fsfw/tasks/Typedef.h b/src/fsfw/tasks/Typedef.h deleted file mode 100644 index 1bb75131..00000000 --- a/src/fsfw/tasks/Typedef.h +++ /dev/null @@ -1,13 +0,0 @@ -#ifndef FSFW_TASKS_TYPEDEF_H_ -#define FSFW_TASKS_TYPEDEF_H_ - -#include -#include - -typedef const char* TaskName; -typedef uint32_t TaskPriority; -typedef size_t TaskStackSize; -typedef double TaskPeriod; -typedef void (*TaskDeadlineMissedFunction)(); - -#endif /* FSFW_TASKS_TYPEDEF_H_ */ diff --git a/src/fsfw/tasks/definitions.h b/src/fsfw/tasks/definitions.h new file mode 100644 index 00000000..bca9b768 --- /dev/null +++ b/src/fsfw/tasks/definitions.h @@ -0,0 +1,13 @@ +#ifndef FSFW_TASKS_TYPEDEF_H_ +#define FSFW_TASKS_TYPEDEF_H_ + +#include +#include + +using TaskName = const char*; +using TaskPriority = uint32_t; +using TaskStackSize = size_t; +using TaskPeriod = double; +using TaskDeadlineMissedFunction = void (*)(); + +#endif /* FSFW_TASKS_TYPEDEF_H_ */ From b1e30ae9ff5c89ab6c628758a01d3fd274dd1d37 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 18 May 2022 14:39:37 +0200 Subject: [PATCH 142/404] minor bugfix --- src/fsfw/tasks/CMakeLists.txt | 1 + src/fsfw/tasks/PeriodicTaskBase.cpp | 2 +- src/fsfw/tasks/PeriodicTaskBase.h | 1 - 3 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/fsfw/tasks/CMakeLists.txt b/src/fsfw/tasks/CMakeLists.txt index 1964bb4e..5c2d6b1c 100644 --- a/src/fsfw/tasks/CMakeLists.txt +++ b/src/fsfw/tasks/CMakeLists.txt @@ -2,4 +2,5 @@ target_sources(${LIB_FSFW_NAME} PRIVATE FixedSequenceSlot.cpp FixedSlotSequence.cpp + PeriodicTaskBase.cpp ) \ No newline at end of file diff --git a/src/fsfw/tasks/PeriodicTaskBase.cpp b/src/fsfw/tasks/PeriodicTaskBase.cpp index 606fee1e..87745b14 100644 --- a/src/fsfw/tasks/PeriodicTaskBase.cpp +++ b/src/fsfw/tasks/PeriodicTaskBase.cpp @@ -9,7 +9,7 @@ PeriodicTaskBase::PeriodicTaskBase(uint32_t periodMs_, uint32_t PeriodicTaskBase::getPeriodMs() const { return periodMs; } -bool PeriodicTaskBase::isEmpty() const override { +bool PeriodicTaskBase::isEmpty() const { return objectList.empty(); } diff --git a/src/fsfw/tasks/PeriodicTaskBase.h b/src/fsfw/tasks/PeriodicTaskBase.h index 4b330426..cc4eae4b 100644 --- a/src/fsfw/tasks/PeriodicTaskBase.h +++ b/src/fsfw/tasks/PeriodicTaskBase.h @@ -15,7 +15,6 @@ public: ReturnValue_t addComponent(object_id_t object, uint8_t opCode) override; ReturnValue_t addComponent(ExecutableObjectIF* object, uint8_t opCode) override; - uint32_t getPeriodMs() const override; bool isEmpty() const override; From b47eb0a7ff7445f078413c63588c7145ef2891fb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 18 May 2022 14:39:37 +0200 Subject: [PATCH 143/404] minor bugfix --- src/fsfw/tasks/CMakeLists.txt | 3 ++- src/fsfw/tasks/PeriodicTaskBase.cpp | 2 +- src/fsfw/tasks/PeriodicTaskBase.h | 1 - 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fsfw/tasks/CMakeLists.txt b/src/fsfw/tasks/CMakeLists.txt index df69520a..537320b7 100644 --- a/src/fsfw/tasks/CMakeLists.txt +++ b/src/fsfw/tasks/CMakeLists.txt @@ -1,2 +1,3 @@ target_sources(${LIB_FSFW_NAME} PRIVATE FixedSequenceSlot.cpp - FixedSlotSequence.cpp) + FixedSlotSequence.cpp + PeriodicTaskBase.cpp) diff --git a/src/fsfw/tasks/PeriodicTaskBase.cpp b/src/fsfw/tasks/PeriodicTaskBase.cpp index 606fee1e..87745b14 100644 --- a/src/fsfw/tasks/PeriodicTaskBase.cpp +++ b/src/fsfw/tasks/PeriodicTaskBase.cpp @@ -9,7 +9,7 @@ PeriodicTaskBase::PeriodicTaskBase(uint32_t periodMs_, uint32_t PeriodicTaskBase::getPeriodMs() const { return periodMs; } -bool PeriodicTaskBase::isEmpty() const override { +bool PeriodicTaskBase::isEmpty() const { return objectList.empty(); } diff --git a/src/fsfw/tasks/PeriodicTaskBase.h b/src/fsfw/tasks/PeriodicTaskBase.h index 4b330426..cc4eae4b 100644 --- a/src/fsfw/tasks/PeriodicTaskBase.h +++ b/src/fsfw/tasks/PeriodicTaskBase.h @@ -15,7 +15,6 @@ public: ReturnValue_t addComponent(object_id_t object, uint8_t opCode) override; ReturnValue_t addComponent(ExecutableObjectIF* object, uint8_t opCode) override; - uint32_t getPeriodMs() const override; bool isEmpty() const override; From 1886da0d3f8360693d8bc60eebc39f3169805dbb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 18 May 2022 15:42:18 +0200 Subject: [PATCH 144/404] refactoring host osal --- src/fsfw/osal/host/PeriodicTask.cpp | 32 +++---------- src/fsfw/osal/host/PeriodicTask.h | 55 +++-------------------- src/fsfw/osal/linux/FixedTimeslotTask.cpp | 12 ++--- src/fsfw/osal/linux/FixedTimeslotTask.h | 11 +++-- src/fsfw/osal/linux/PeriodicPosixTask.cpp | 15 ++++--- src/fsfw/osal/linux/PeriodicPosixTask.h | 11 +++-- src/fsfw/osal/linux/PosixThread.h | 30 ++++++------- src/fsfw/osal/linux/TaskFactory.cpp | 4 +- src/fsfw/tasks/CMakeLists.txt | 6 +-- src/fsfw/tasks/PeriodicTaskBase.cpp | 14 +++--- src/fsfw/tasks/PeriodicTaskBase.h | 31 +++++++------ src/fsfw/tasks/PeriodicTaskIF.h | 4 +- 12 files changed, 83 insertions(+), 142 deletions(-) diff --git a/src/fsfw/osal/host/PeriodicTask.cpp b/src/fsfw/osal/host/PeriodicTask.cpp index cdcfafa6..ed4ef3f1 100644 --- a/src/fsfw/osal/host/PeriodicTask.cpp +++ b/src/fsfw/osal/host/PeriodicTask.cpp @@ -20,8 +20,8 @@ #endif PeriodicTask::PeriodicTask(const char* name, TaskPriority setPriority, TaskStackSize setStack, - TaskPeriod setPeriod, void (*setDeadlineMissedFunc)()) - : started(false), taskName(name), period(setPeriod), deadlineMissedFunc(setDeadlineMissedFunc) { + TaskPeriod setPeriod, TaskDeadlineMissedFunction dlmFunc_) + : PeriodicTaskBase(setPeriod, dlmFunc_), started(false), taskName(name) { // It is probably possible to set task priorities by using the native // task handles for Windows / Linux mainThread = std::thread(&PeriodicTask::taskEntryPoint, this, this); @@ -75,9 +75,7 @@ ReturnValue_t PeriodicTask::sleepFor(uint32_t ms) { } void PeriodicTask::taskFunctionality() { - for (const auto& object : objectList) { - object->initializeAfterTaskCreation(); - } + initObjsAfterTaskCreation(); std::chrono::milliseconds periodChrono(static_cast(period * 1000)); auto currentStartTime{std::chrono::duration_cast( @@ -89,33 +87,17 @@ void PeriodicTask::taskFunctionality() { if (terminateThread.load()) { break; } - for (const auto& object : objectList) { - object->performOperation(); + for (const auto& objectPair : objectList) { + objectPair.first->performOperation(objectPair.second); } if (not delayForInterval(¤tStartTime, periodChrono)) { - if (deadlineMissedFunc != nullptr) { - this->deadlineMissedFunc(); + if (dlmFunc != nullptr) { + this->dlmFunc(); } } } } -ReturnValue_t PeriodicTask::addComponent(object_id_t object) { - ExecutableObjectIF* newObject = ObjectManager::instance()->get(object); - return addComponent(newObject); -} - -ReturnValue_t PeriodicTask::addComponent(ExecutableObjectIF* object) { - if (object == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - object->setTaskIF(this); - objectList.push_back(object); - return HasReturnvaluesIF::RETURN_OK; -} - -uint32_t PeriodicTask::getPeriodMs() const { return period * 1000; } - bool PeriodicTask::delayForInterval(chron_ms* previousWakeTimeMs, const chron_ms interval) { bool shouldDelay = false; // Get current wakeup time diff --git a/src/fsfw/osal/host/PeriodicTask.h b/src/fsfw/osal/host/PeriodicTask.h index 6c4d5e8b..a565c584 100644 --- a/src/fsfw/osal/host/PeriodicTask.h +++ b/src/fsfw/osal/host/PeriodicTask.h @@ -6,9 +6,9 @@ #include #include -#include "../../objectmanager/ObjectManagerIF.h" -#include "../../tasks/PeriodicTaskIF.h" -#include "../../tasks/Typedef.h" +#include "fsfw/objectmanager/ObjectManagerIF.h" +#include "fsfw/tasks/PeriodicTaskBase.h" +#include "fsfw/tasks/definitions.h" class ExecutableObjectIF; @@ -19,7 +19,7 @@ class ExecutableObjectIF; * * @ingroup task_handling */ -class PeriodicTask : public PeriodicTaskIF { +class PeriodicTask : public PeriodicTaskBase { public: /** * @brief Standard constructor of the class. @@ -34,7 +34,7 @@ class PeriodicTask : public PeriodicTaskIF { * assigned. */ PeriodicTask(const char* name, TaskPriority setPriority, TaskStackSize setStack, - TaskPeriod setPeriod, void (*setDeadlineMissedFunc)()); + TaskPeriod setPeriod, TaskDeadlineMissedFunction dlmFunc); /** * @brief Currently, the executed object's lifetime is not coupled with * the task object's lifetime, so the destructor is empty. @@ -49,62 +49,19 @@ class PeriodicTask : public PeriodicTaskIF { * to the system call. */ ReturnValue_t startTask(void); - /** - * Adds an object to the list of objects to be executed. - * The objects are executed in the order added. - * @param object Id of the object to add. - * @return - * -@c RETURN_OK on success - * -@c RETURN_FAILED if the object could not be added. - */ - ReturnValue_t addComponent(object_id_t object); - - /** - * Adds an object to the list of objects to be executed. - * The objects are executed in the order added. - * @param object pointer to the object to add. - * @return - * -@c RETURN_OK on success - * -@c RETURN_FAILED if the object could not be added. - */ - ReturnValue_t addComponent(ExecutableObjectIF* object); - - uint32_t getPeriodMs() const; ReturnValue_t sleepFor(uint32_t ms); protected: using chron_ms = std::chrono::milliseconds; bool started; - //!< Typedef for the List of objects. - typedef std::vector ObjectList; std::thread mainThread; std::atomic terminateThread{false}; - /** - * @brief This attribute holds a list of objects to be executed. - */ - ObjectList objectList; - std::condition_variable initCondition; std::mutex initMutex; std::string taskName; - /** - * @brief The period of the task. - * @details - * The period determines the frequency of the task's execution. - * It is expressed in clock ticks. - */ - TaskPeriod period; - /** - * @brief The pointer to the deadline-missed function. - * @details - * This pointer stores the function that is executed if the task's deadline - * is missed. So, each may react individually on a timing failure. - * The pointer may be NULL, then nothing happens on missing the deadline. - * The deadline is equal to the next execution of the periodic task. - */ - void (*deadlineMissedFunc)(void); + /** * @brief This is the function executed in the new task's context. * @details diff --git a/src/fsfw/osal/linux/FixedTimeslotTask.cpp b/src/fsfw/osal/linux/FixedTimeslotTask.cpp index d1fccdf9..4d912a40 100644 --- a/src/fsfw/osal/linux/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/linux/FixedTimeslotTask.cpp @@ -9,8 +9,10 @@ uint32_t FixedTimeslotTask::deadlineMissedCount = 0; const size_t PeriodicTaskIF::MINIMUM_STACK_SIZE = PTHREAD_STACK_MIN; FixedTimeslotTask::FixedTimeslotTask(const char* name_, int priority_, size_t stackSize_, - uint32_t periodMs_) - : PosixThread(name_, priority_, stackSize_), pst(periodMs_), started(false) {} + TaskPeriod periodSeconds_) + : posixThread(name_, priority_, stackSize_), + pst(static_cast(periodSeconds_ * 1000)), + started(false) {} FixedTimeslotTask::~FixedTimeslotTask() {} @@ -26,7 +28,7 @@ void* FixedTimeslotTask::taskEntryPoint(void* arg) { ReturnValue_t FixedTimeslotTask::startTask() { started = true; - createTask(&taskEntryPoint, this); + posixThread.createTask(&taskEntryPoint, this); return HasReturnvaluesIF::RETURN_OK; } @@ -57,13 +59,13 @@ ReturnValue_t FixedTimeslotTask::checkSequence() { return pst.checkSequence(); } void FixedTimeslotTask::taskFunctionality() { // Like FreeRTOS pthreads are running as soon as they are created if (!started) { - suspend(); + posixThread.suspend(); } pst.intializeSequenceAfterTaskCreation(); // The start time for the first entry is read. - uint64_t lastWakeTime = getCurrentMonotonicTimeMs(); + uint64_t lastWakeTime = posixThread.getCurrentMonotonicTimeMs(); uint64_t interval = pst.getIntervalToNextSlotMs(); // The task's "infinite" inner loop is entered. diff --git a/src/fsfw/osal/linux/FixedTimeslotTask.h b/src/fsfw/osal/linux/FixedTimeslotTask.h index a5dc9032..ecce7321 100644 --- a/src/fsfw/osal/linux/FixedTimeslotTask.h +++ b/src/fsfw/osal/linux/FixedTimeslotTask.h @@ -3,11 +3,12 @@ #include -#include "../../tasks/FixedSlotSequence.h" -#include "../../tasks/FixedTimeslotTaskIF.h" #include "PosixThread.h" +#include "fsfw/tasks/FixedSlotSequence.h" +#include "fsfw/tasks/FixedTimeslotTaskIF.h" +#include "fsfw/tasks/definitions.h" -class FixedTimeslotTask : public FixedTimeslotTaskIF, public PosixThread { +class FixedTimeslotTask : public FixedTimeslotTaskIF { public: /** * Create a generic periodic task. @@ -21,7 +22,7 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF, public PosixThread { * @param period_ * @param deadlineMissedFunc_ */ - FixedTimeslotTask(const char* name_, int priority_, size_t stackSize_, uint32_t periodMs_); + FixedTimeslotTask(const char* name_, int priority_, size_t stackSize_, TaskPeriod periodSeconds_); virtual ~FixedTimeslotTask(); ReturnValue_t startTask() override; @@ -59,6 +60,8 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF, public PosixThread { virtual void taskFunctionality(); private: + PosixThread posixThread; + /** * @brief This is the entry point in a new thread. * diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.cpp b/src/fsfw/osal/linux/PeriodicPosixTask.cpp index 510ec59c..f52067f1 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.cpp +++ b/src/fsfw/osal/linux/PeriodicPosixTask.cpp @@ -1,16 +1,16 @@ #include "fsfw/osal/linux/PeriodicPosixTask.h" -#include #include +#include #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/serviceinterface/ServiceInterface.h" #include "fsfw/tasks/ExecutableObjectIF.h" PeriodicPosixTask::PeriodicPosixTask(const char* name_, int priority_, size_t stackSize_, - uint32_t period_, TaskDeadlineMissedFunction dlMissedFunc_) - : PosixThread(name_, priority_, stackSize_), - PeriodicTaskBase(period_, dlMissedFunc_), + TaskPeriod period_, TaskDeadlineMissedFunction dlMissedFunc_) + : PeriodicTaskBase(period_, dlMissedFunc_), + posixThread(name_, priority_, stackSize_), started(false) {} PeriodicPosixTask::~PeriodicPosixTask() { @@ -34,18 +34,19 @@ ReturnValue_t PeriodicPosixTask::startTask(void) { return HasReturnvaluesIF::RETURN_FAILED; } started = true; - PosixThread::createTask(&taskEntryPoint, this); + posixThread.createTask(&taskEntryPoint, this); return HasReturnvaluesIF::RETURN_OK; } void PeriodicPosixTask::taskFunctionality(void) { if (not started) { - suspend(); + posixThread.suspend(); } initObjsAfterTaskCreation(); - uint64_t lastWakeTime = getCurrentMonotonicTimeMs(); + uint64_t lastWakeTime = posixThread.getCurrentMonotonicTimeMs(); + uint64_t periodMs = getPeriodMs(); // The task's "infinite" inner loop is entered. while (1) { for (auto const& objOpCodePair : objectList) { diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.h b/src/fsfw/osal/linux/PeriodicPosixTask.h index 3dcc6bcc..3ab79ed0 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.h +++ b/src/fsfw/osal/linux/PeriodicPosixTask.h @@ -1,17 +1,15 @@ #ifndef FRAMEWORK_OSAL_LINUX_PERIODICPOSIXTASK_H_ #define FRAMEWORK_OSAL_LINUX_PERIODICPOSIXTASK_H_ -#include "PosixThread.h" - #include +#include "PosixThread.h" #include "fsfw/objectmanager/ObjectManagerIF.h" #include "fsfw/tasks/ExecutableObjectIF.h" -#include "fsfw/tasks/PeriodicTaskIF.h" #include "fsfw/tasks/PeriodicTaskBase.h" +#include "fsfw/tasks/PeriodicTaskIF.h" - -class PeriodicPosixTask : public PosixThread, public PeriodicTaskBase { +class PeriodicPosixTask : public PeriodicTaskBase { public: /** * Create a generic periodic task. @@ -25,7 +23,7 @@ class PeriodicPosixTask : public PosixThread, public PeriodicTaskBase { * @param period_ * @param deadlineMissedFunc_ */ - PeriodicPosixTask(const char* name_, int priority_, size_t stackSize_, uint32_t period_, + PeriodicPosixTask(const char* name_, int priority_, size_t stackSize_, TaskPeriod period_, void (*deadlineMissedFunc_)()); virtual ~PeriodicPosixTask(); @@ -41,6 +39,7 @@ class PeriodicPosixTask : public PosixThread, public PeriodicTaskBase { ReturnValue_t sleepFor(uint32_t ms) override; private: + PosixThread posixThread; /** * @brief Flag to indicate that the task was started and is allowed to run diff --git a/src/fsfw/osal/linux/PosixThread.h b/src/fsfw/osal/linux/PosixThread.h index 69c6c5b7..78fdfa2b 100644 --- a/src/fsfw/osal/linux/PosixThread.h +++ b/src/fsfw/osal/linux/PosixThread.h @@ -35,6 +35,21 @@ class PosixThread { */ void resume(); + /** + * @brief Function that has to be called by derived class because the + * derived class pointer has to be valid as argument. + * @details + * This function creates a pthread with the given parameters. As the + * function requires a pointer to the derived object it has to be called + * after the this pointer of the derived object is valid. + * Sets the taskEntryPoint as function to be called by new a thread. + * @param fnc_ Function which will be executed by the thread. + * @param arg_ + * argument of the taskEntryPoint function, needs to be this pointer + * of derived class + */ + void createTask(void* (*fnc_)(void*), void* arg_); + /** * Delay function similar to FreeRtos delayUntil function * @@ -55,21 +70,6 @@ class PosixThread { protected: pthread_t thread; - /** - * @brief Function that has to be called by derived class because the - * derived class pointer has to be valid as argument. - * @details - * This function creates a pthread with the given parameters. As the - * function requires a pointer to the derived object it has to be called - * after the this pointer of the derived object is valid. - * Sets the taskEntryPoint as function to be called by new a thread. - * @param fnc_ Function which will be executed by the thread. - * @param arg_ - * argument of the taskEntryPoint function, needs to be this pointer - * of derived class - */ - void createTask(void* (*fnc_)(void*), void* arg_); - private: char name[PTHREAD_MAX_NAMELEN]; int priority; diff --git a/src/fsfw/osal/linux/TaskFactory.cpp b/src/fsfw/osal/linux/TaskFactory.cpp index 8503039f..b9e7eb28 100644 --- a/src/fsfw/osal/linux/TaskFactory.cpp +++ b/src/fsfw/osal/linux/TaskFactory.cpp @@ -15,14 +15,14 @@ TaskFactory* TaskFactory::instance() { return TaskFactory::factoryInstance; } PeriodicTaskIF* TaskFactory::createPeriodicTask( TaskName name_, TaskPriority taskPriority_, TaskStackSize stackSize_, TaskPeriod periodInSeconds_, TaskDeadlineMissedFunction deadLineMissedFunction_) { - return new PeriodicPosixTask(name_, taskPriority_, stackSize_, periodInSeconds_ * 1000, + return new PeriodicPosixTask(name_, taskPriority_, stackSize_, periodInSeconds_, deadLineMissedFunction_); } FixedTimeslotTaskIF* TaskFactory::createFixedTimeslotTask( TaskName name_, TaskPriority taskPriority_, TaskStackSize stackSize_, TaskPeriod periodInSeconds_, TaskDeadlineMissedFunction deadLineMissedFunction_) { - return new FixedTimeslotTask(name_, taskPriority_, stackSize_, periodInSeconds_ * 1000); + return new FixedTimeslotTask(name_, taskPriority_, stackSize_, periodInSeconds_); } ReturnValue_t TaskFactory::deleteTask(PeriodicTaskIF* task) { diff --git a/src/fsfw/tasks/CMakeLists.txt b/src/fsfw/tasks/CMakeLists.txt index 537320b7..6128c272 100644 --- a/src/fsfw/tasks/CMakeLists.txt +++ b/src/fsfw/tasks/CMakeLists.txt @@ -1,3 +1,3 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE FixedSequenceSlot.cpp - FixedSlotSequence.cpp - PeriodicTaskBase.cpp) +target_sources( + ${LIB_FSFW_NAME} PRIVATE FixedSequenceSlot.cpp FixedSlotSequence.cpp + PeriodicTaskBase.cpp) diff --git a/src/fsfw/tasks/PeriodicTaskBase.cpp b/src/fsfw/tasks/PeriodicTaskBase.cpp index 87745b14..021ba7b5 100644 --- a/src/fsfw/tasks/PeriodicTaskBase.cpp +++ b/src/fsfw/tasks/PeriodicTaskBase.cpp @@ -1,17 +1,15 @@ -#include #include "PeriodicTaskBase.h" +#include + #include -PeriodicTaskBase::PeriodicTaskBase(uint32_t periodMs_, - TaskDeadlineMissedFunction deadlineMissedFunc_) - : periodMs(periodMs_), deadlineMissedFunc(deadlineMissedFunc_) {} +PeriodicTaskBase::PeriodicTaskBase(TaskPeriod period_, TaskDeadlineMissedFunction dlmFunc_) + : period(period), dlmFunc(dlmFunc_) {} -uint32_t PeriodicTaskBase::getPeriodMs() const { return periodMs; } +uint32_t PeriodicTaskBase::getPeriodMs() const { return static_cast(period * 1000); } -bool PeriodicTaskBase::isEmpty() const { - return objectList.empty(); -} +bool PeriodicTaskBase::isEmpty() const { return objectList.empty(); } ReturnValue_t PeriodicTaskBase::initObjsAfterTaskCreation() { std::multiset uniqueObjects; diff --git a/src/fsfw/tasks/PeriodicTaskBase.h b/src/fsfw/tasks/PeriodicTaskBase.h index cc4eae4b..a262a2d4 100644 --- a/src/fsfw/tasks/PeriodicTaskBase.h +++ b/src/fsfw/tasks/PeriodicTaskBase.h @@ -1,16 +1,17 @@ #ifndef FSFW_SRC_FSFW_TASKS_PERIODICTASKBASE_H_ #define FSFW_SRC_FSFW_TASKS_PERIODICTASKBASE_H_ +#include +#include + #include "fsfw/tasks/PeriodicTaskIF.h" #include "fsfw/tasks/definitions.h" -#include -#include class ExecutableObjectIF; -class PeriodicTaskBase: public PeriodicTaskIF { -public: - PeriodicTaskBase(uint32_t periodMs, TaskDeadlineMissedFunction deadlineMissedFunc = nullptr); +class PeriodicTaskBase : public PeriodicTaskIF { + public: + PeriodicTaskBase(TaskPeriod period, TaskDeadlineMissedFunction deadlineMissedFunc = nullptr); ReturnValue_t addComponent(object_id_t object, uint8_t opCode) override; ReturnValue_t addComponent(ExecutableObjectIF* object, uint8_t opCode) override; @@ -21,8 +22,7 @@ public: ReturnValue_t initObjsAfterTaskCreation(); -protected: - + protected: //! Typedef for the List of objects. Will contain the objects to execute and their respective //! operation codes using ObjectList = std::vector>; @@ -32,20 +32,19 @@ protected: ObjectList objectList; /** - * @brief Period of the task in milliseconds + * @brief Period of task in floating point seconds */ - uint32_t periodMs; + TaskPeriod period; /** * @brief The pointer to the deadline-missed function. - * @details This pointer stores the function that is executed if the task's deadline is missed. - * So, each may react individually on a timing failure. The pointer may be - * NULL, then nothing happens on missing the deadline. The deadline is equal to the next execution - * of the periodic task. + * @details + * This pointer stores the function that is executed if the task's deadline + * is missed. So, each may react individually on a timing failure. + * The pointer may be NULL, then nothing happens on missing the deadline. + * The deadline is equal to the next execution of the periodic task. */ - TaskDeadlineMissedFunction deadlineMissedFunc = nullptr; + TaskDeadlineMissedFunction dlmFunc = nullptr; }; - - #endif /* FSFW_SRC_FSFW_TASKS_PERIODICTASKBASE_H_ */ diff --git a/src/fsfw/tasks/PeriodicTaskIF.h b/src/fsfw/tasks/PeriodicTaskIF.h index 076ef56e..ec25faa8 100644 --- a/src/fsfw/tasks/PeriodicTaskIF.h +++ b/src/fsfw/tasks/PeriodicTaskIF.h @@ -1,11 +1,11 @@ #ifndef FRAMEWORK_TASK_PERIODICTASKIF_H_ #define FRAMEWORK_TASK_PERIODICTASKIF_H_ +#include + #include "fsfw/objectmanager/SystemObjectIF.h" #include "fsfw/tasks/ExecutableObjectIF.h" -#include - /** * New version of TaskIF * Follows RAII principles, i.e. there's no create or delete method. From 64e7d4bb5e61c5ce2ece0bedddaffde086d7fe99 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 18 May 2022 18:15:31 +0200 Subject: [PATCH 145/404] continued refactoring --- src/fsfw/osal/host/FixedTimeslotTask.cpp | 19 ++++++------ src/fsfw/osal/host/FixedTimeslotTask.h | 38 ++++++++++-------------- 2 files changed, 25 insertions(+), 32 deletions(-) diff --git a/src/fsfw/osal/host/FixedTimeslotTask.cpp b/src/fsfw/osal/host/FixedTimeslotTask.cpp index 07853938..0c4acf8e 100644 --- a/src/fsfw/osal/host/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/host/FixedTimeslotTask.cpp @@ -22,12 +22,12 @@ FixedTimeslotTask::FixedTimeslotTask(const char* name, TaskPriority setPriority, TaskStackSize setStack, TaskPeriod setPeriod, - void (*setDeadlineMissedFunc)()) + TaskDeadlineMissedFunction dlmFunc_) : started(false), - pollingSeqTable(setPeriod * 1000), + pollingSeqTable(static_cast(setPeriod * 1000)), taskName(name), period(setPeriod), - deadlineMissedFunc(setDeadlineMissedFunc) { + dlmFunc(dlmFunc_) { // It is propably possible to set task priorities by using the native // task handles for Windows / Linux mainThread = std::thread(&FixedTimeslotTask::taskEntryPoint, this, this); @@ -39,7 +39,7 @@ FixedTimeslotTask::FixedTimeslotTask(const char* name, TaskPriority setPriority, tasks::insertTaskName(mainThread.get_id(), taskName); } -FixedTimeslotTask::~FixedTimeslotTask(void) { +FixedTimeslotTask::~FixedTimeslotTask() { // Do not delete objects, we were responsible for ptrs only. terminateThread = true; if (mainThread.joinable()) { @@ -48,7 +48,7 @@ FixedTimeslotTask::~FixedTimeslotTask(void) { } void FixedTimeslotTask::taskEntryPoint(void* argument) { - FixedTimeslotTask* originalTask(reinterpret_cast(argument)); + auto* originalTask(reinterpret_cast(argument)); if (not originalTask->started) { // we have to suspend/block here until the task is started. @@ -114,8 +114,7 @@ void FixedTimeslotTask::taskFunctionality() { ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId, uint32_t slotTimeMs, int8_t executionStep) { - ExecutableObjectIF* executableObject = - ObjectManager::instance()->get(componentId); + auto* executableObject = ObjectManager::instance()->get(componentId); if (executableObject != nullptr) { pollingSeqTable.addSlot(componentId, slotTimeMs, executionStep, executableObject, this); return HasReturnvaluesIF::RETURN_OK; @@ -133,9 +132,9 @@ ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId, uint32_t slotT return HasReturnvaluesIF::RETURN_FAILED; } -ReturnValue_t FixedTimeslotTask::checkSequence() const { return pollingSeqTable.checkSequence(); } +ReturnValue_t FixedTimeslotTask::checkSequence() { return pollingSeqTable.checkSequence(); } -uint32_t FixedTimeslotTask::getPeriodMs() const { return period * 1000; } +uint32_t FixedTimeslotTask::getPeriodMs() const { return static_cast(period * 1000); } bool FixedTimeslotTask::delayForInterval(chron_ms* previousWakeTimeMs, const chron_ms interval) { bool shouldDelay = false; @@ -176,3 +175,5 @@ bool FixedTimeslotTask::delayForInterval(chron_ms* previousWakeTimeMs, const chr (*previousWakeTimeMs) = currentStartTime; return false; } + +bool FixedTimeslotTask::isEmpty() const { return pollingSeqTable.isEmpty() }; diff --git a/src/fsfw/osal/host/FixedTimeslotTask.h b/src/fsfw/osal/host/FixedTimeslotTask.h index cdbc6f23..69c84c75 100644 --- a/src/fsfw/osal/host/FixedTimeslotTask.h +++ b/src/fsfw/osal/host/FixedTimeslotTask.h @@ -6,10 +6,10 @@ #include #include -#include "../../objectmanager/ObjectManagerIF.h" -#include "../../tasks/FixedSlotSequence.h" -#include "../../tasks/FixedTimeslotTaskIF.h" -#include "../../tasks/Typedef.h" +#include "fsfw/objectmanager/ObjectManagerIF.h" +#include "fsfw/tasks/FixedSlotSequence.h" +#include "fsfw/tasks/FixedTimeslotTaskIF.h" +#include "fsfw/tasks/definitions.h" class ExecutableObjectIF; @@ -39,7 +39,7 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF { * @brief Currently, the executed object's lifetime is not coupled with * the task object's lifetime, so the destructor is empty. */ - virtual ~FixedTimeslotTask(void); + ~FixedTimeslotTask() override; /** * @brief The method to start the task. @@ -48,7 +48,7 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF { * The address of the task object is passed as an argument * to the system call. */ - ReturnValue_t startTask(void); + ReturnValue_t startTask() override; /** * Add timeslot to the polling sequence table. @@ -57,22 +57,23 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF { * @param executionStep * @return */ - ReturnValue_t addSlot(object_id_t componentId, uint32_t slotTimeMs, int8_t executionStep); + ReturnValue_t addSlot(object_id_t componentId, uint32_t slotTimeMs, int8_t executionStep) override; - ReturnValue_t checkSequence() const override; + ReturnValue_t checkSequence() override; - uint32_t getPeriodMs() const; + ReturnValue_t sleepFor(uint32_t ms) override; + uint32_t getPeriodMs() const override; - ReturnValue_t sleepFor(uint32_t ms); + bool isEmpty() const override; protected: using chron_ms = std::chrono::milliseconds; bool started; - //!< Typedef for the List of objects. - typedef std::vector ObjectList; + std::thread mainThread; std::atomic terminateThread{false}; + TaskDeadlineMissedFunction dlmFunc = nullptr; //! Polling sequence table which contains the object to execute //! and information like the timeslots and the passed execution step. @@ -89,15 +90,6 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF { */ TaskPeriod period; - /** - * @brief The pointer to the deadline-missed function. - * @details - * This pointer stores the function that is executed if the task's deadline - * is missed. So, each may react individually on a timing failure. - * The pointer may be NULL, then nothing happens on missing the deadline. - * The deadline is equal to the next execution of the periodic task. - */ - void (*deadlineMissedFunc)(void); /** * @brief This is the function executed in the new task's context. * @details @@ -117,9 +109,9 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF { * the checkAndRestartPeriod system call blocks the task until the next * period. On missing the deadline, the deadlineMissedFunction is executed. */ - void taskFunctionality(void); + void taskFunctionality(); - bool delayForInterval(chron_ms* previousWakeTimeMs, const chron_ms interval); + bool delayForInterval(chron_ms* previousWakeTimeMs, chron_ms interval); }; #endif /* FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ */ From 08f1ebf9fc1fac2bdada7005b8dad54f55bf2429 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 18 May 2022 23:45:38 +0200 Subject: [PATCH 146/404] continued refactoring --- src/fsfw/osal/host/FixedTimeslotTask.cpp | 2 +- src/fsfw/osal/host/FixedTimeslotTask.h | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/fsfw/osal/host/FixedTimeslotTask.cpp b/src/fsfw/osal/host/FixedTimeslotTask.cpp index 0c4acf8e..02570f68 100644 --- a/src/fsfw/osal/host/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/host/FixedTimeslotTask.cpp @@ -176,4 +176,4 @@ bool FixedTimeslotTask::delayForInterval(chron_ms* previousWakeTimeMs, const chr return false; } -bool FixedTimeslotTask::isEmpty() const { return pollingSeqTable.isEmpty() }; +bool FixedTimeslotTask::isEmpty() const { return pollingSeqTable.isEmpty(); } diff --git a/src/fsfw/osal/host/FixedTimeslotTask.h b/src/fsfw/osal/host/FixedTimeslotTask.h index 69c84c75..fa7c688d 100644 --- a/src/fsfw/osal/host/FixedTimeslotTask.h +++ b/src/fsfw/osal/host/FixedTimeslotTask.h @@ -73,7 +73,6 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF { std::thread mainThread; std::atomic terminateThread{false}; - TaskDeadlineMissedFunction dlmFunc = nullptr; //! Polling sequence table which contains the object to execute //! and information like the timeslots and the passed execution step. @@ -89,7 +88,7 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF { * It is expressed in clock ticks. */ TaskPeriod period; - + TaskDeadlineMissedFunction dlmFunc = nullptr; /** * @brief This is the function executed in the new task's context. * @details @@ -111,7 +110,7 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF { */ void taskFunctionality(); - bool delayForInterval(chron_ms* previousWakeTimeMs, chron_ms interval); + static bool delayForInterval(chron_ms* previousWakeTimeMs, chron_ms interval); }; #endif /* FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ */ From 3a162907076f6b0ce592f0282cb786e71ba3eae4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 19 May 2022 00:44:34 +0200 Subject: [PATCH 147/404] refactored and tested hosted and linux task IF --- src/fsfw/osal/host/FixedTimeslotTask.cpp | 23 +++------- src/fsfw/osal/host/FixedTimeslotTask.h | 27 +++--------- src/fsfw/osal/host/PeriodicTask.cpp | 9 +--- src/fsfw/osal/host/PeriodicTask.h | 12 ++--- src/fsfw/osal/host/TaskFactory.cpp | 4 +- src/fsfw/osal/host/taskHelpers.cpp | 2 +- src/fsfw/osal/host/taskHelpers.h | 2 +- src/fsfw/osal/linux/FixedTimeslotTask.cpp | 53 ++++++----------------- src/fsfw/osal/linux/FixedTimeslotTask.h | 24 +++------- src/fsfw/osal/linux/PeriodicPosixTask.cpp | 32 +++++--------- src/fsfw/osal/linux/PeriodicPosixTask.h | 6 +-- src/fsfw/osal/linux/TaskFactory.cpp | 5 ++- src/fsfw/tasks/CMakeLists.txt | 2 +- src/fsfw/tasks/FixedSlotSequence.h | 2 +- src/fsfw/tasks/FixedTimeslotTaskBase.cpp | 29 +++++++++++++ src/fsfw/tasks/FixedTimeslotTaskBase.h | 44 +++++++++++++++++++ src/fsfw/tasks/FixedTimeslotTaskIF.h | 2 +- src/fsfw/tasks/PeriodicTaskBase.cpp | 18 ++++++-- src/fsfw/tasks/PeriodicTaskBase.h | 7 +-- src/fsfw/tasks/PeriodicTaskIF.h | 2 +- src/fsfw/tasks/definitions.h | 2 +- 21 files changed, 160 insertions(+), 147 deletions(-) create mode 100644 src/fsfw/tasks/FixedTimeslotTaskBase.cpp create mode 100644 src/fsfw/tasks/FixedTimeslotTaskBase.h diff --git a/src/fsfw/osal/host/FixedTimeslotTask.cpp b/src/fsfw/osal/host/FixedTimeslotTask.cpp index 02570f68..931e6a22 100644 --- a/src/fsfw/osal/host/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/host/FixedTimeslotTask.cpp @@ -3,9 +3,7 @@ #include #include -#include "fsfw/ipc/MutexFactory.h" #include "fsfw/objectmanager/ObjectManager.h" -#include "fsfw/osal/host/FixedTimeslotTask.h" #include "fsfw/osal/host/Mutex.h" #include "fsfw/osal/host/taskHelpers.h" #include "fsfw/platform.h" @@ -23,11 +21,7 @@ FixedTimeslotTask::FixedTimeslotTask(const char* name, TaskPriority setPriority, TaskStackSize setStack, TaskPeriod setPeriod, TaskDeadlineMissedFunction dlmFunc_) - : started(false), - pollingSeqTable(static_cast(setPeriod * 1000)), - taskName(name), - period(setPeriod), - dlmFunc(dlmFunc_) { + : FixedTimeslotTaskBase(setPeriod, dlmFunc_), started(false), taskName(name) { // It is propably possible to set task priorities by using the native // task handles for Windows / Linux mainThread = std::thread(&FixedTimeslotTask::taskEntryPoint, this, this); @@ -80,7 +74,7 @@ ReturnValue_t FixedTimeslotTask::sleepFor(uint32_t ms) { return HasReturnvaluesIF::RETURN_OK; } -void FixedTimeslotTask::taskFunctionality() { +[[noreturn]] void FixedTimeslotTask::taskFunctionality() { pollingSeqTable.intializeSequenceAfterTaskCreation(); // A local iterator for the Polling Sequence Table is created to @@ -106,8 +100,11 @@ void FixedTimeslotTask::taskFunctionality() { // we need to wait before executing the current slot // this gives us the time to wait: interval = chron_ms(this->pollingSeqTable.getIntervalToPreviousSlotMs()); - delayForInterval(¤tStartTime, interval); - // TODO deadline missed check + if (not delayForInterval(¤tStartTime, interval)) { + if (dlmFunc != nullptr) { + dlmFunc(); + } + } } } } @@ -132,10 +129,6 @@ ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId, uint32_t slotT return HasReturnvaluesIF::RETURN_FAILED; } -ReturnValue_t FixedTimeslotTask::checkSequence() { return pollingSeqTable.checkSequence(); } - -uint32_t FixedTimeslotTask::getPeriodMs() const { return static_cast(period * 1000); } - bool FixedTimeslotTask::delayForInterval(chron_ms* previousWakeTimeMs, const chron_ms interval) { bool shouldDelay = false; // Get current wakeup time @@ -175,5 +168,3 @@ bool FixedTimeslotTask::delayForInterval(chron_ms* previousWakeTimeMs, const chr (*previousWakeTimeMs) = currentStartTime; return false; } - -bool FixedTimeslotTask::isEmpty() const { return pollingSeqTable.isEmpty(); } diff --git a/src/fsfw/osal/host/FixedTimeslotTask.h b/src/fsfw/osal/host/FixedTimeslotTask.h index fa7c688d..d85ad34c 100644 --- a/src/fsfw/osal/host/FixedTimeslotTask.h +++ b/src/fsfw/osal/host/FixedTimeslotTask.h @@ -8,7 +8,7 @@ #include "fsfw/objectmanager/ObjectManagerIF.h" #include "fsfw/tasks/FixedSlotSequence.h" -#include "fsfw/tasks/FixedTimeslotTaskIF.h" +#include "fsfw/tasks/FixedTimeslotTaskBase.h" #include "fsfw/tasks/definitions.h" class ExecutableObjectIF; @@ -19,7 +19,7 @@ class ExecutableObjectIF; * @details * @ingroup task_handling */ -class FixedTimeslotTask : public FixedTimeslotTaskIF { +class FixedTimeslotTask : public FixedTimeslotTaskBase { public: /** * @brief Standard constructor of the class. @@ -57,14 +57,10 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF { * @param executionStep * @return */ - ReturnValue_t addSlot(object_id_t componentId, uint32_t slotTimeMs, int8_t executionStep) override; - - ReturnValue_t checkSequence() override; + ReturnValue_t addSlot(object_id_t componentId, uint32_t slotTimeMs, + int8_t executionStep) override; ReturnValue_t sleepFor(uint32_t ms) override; - uint32_t getPeriodMs() const override; - - bool isEmpty() const override; protected: using chron_ms = std::chrono::milliseconds; @@ -74,21 +70,10 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF { std::thread mainThread; std::atomic terminateThread{false}; - //! Polling sequence table which contains the object to execute - //! and information like the timeslots and the passed execution step. - FixedSlotSequence pollingSeqTable; - std::condition_variable initCondition; std::mutex initMutex; std::string taskName; - /** - * @brief The period of the task. - * @details - * The period determines the frequency of the task's execution. - * It is expressed in clock ticks. - */ - TaskPeriod period; - TaskDeadlineMissedFunction dlmFunc = nullptr; + /** * @brief This is the function executed in the new task's context. * @details @@ -108,7 +93,7 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF { * the checkAndRestartPeriod system call blocks the task until the next * period. On missing the deadline, the deadlineMissedFunction is executed. */ - void taskFunctionality(); + [[noreturn]] void taskFunctionality(); static bool delayForInterval(chron_ms* previousWakeTimeMs, chron_ms interval); }; diff --git a/src/fsfw/osal/host/PeriodicTask.cpp b/src/fsfw/osal/host/PeriodicTask.cpp index ed4ef3f1..1f18d335 100644 --- a/src/fsfw/osal/host/PeriodicTask.cpp +++ b/src/fsfw/osal/host/PeriodicTask.cpp @@ -3,13 +3,10 @@ #include #include -#include "fsfw/ipc/MutexFactory.h" -#include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/osal/host/Mutex.h" #include "fsfw/osal/host/taskHelpers.h" #include "fsfw/platform.h" #include "fsfw/serviceinterface/ServiceInterface.h" -#include "fsfw/tasks/ExecutableObjectIF.h" #if defined(PLATFORM_WIN) #include @@ -33,7 +30,7 @@ PeriodicTask::PeriodicTask(const char* name, TaskPriority setPriority, TaskStack tasks::insertTaskName(mainThread.get_id(), taskName); } -PeriodicTask::~PeriodicTask(void) { +PeriodicTask::~PeriodicTask() { // Do not delete objects, we were responsible for ptrs only. terminateThread = true; if (mainThread.joinable()) { @@ -42,7 +39,7 @@ PeriodicTask::~PeriodicTask(void) { } void PeriodicTask::taskEntryPoint(void* argument) { - PeriodicTask* originalTask(reinterpret_cast(argument)); + auto* originalTask(reinterpret_cast(argument)); if (not originalTask->started) { // we have to suspend/block here until the task is started. @@ -80,8 +77,6 @@ void PeriodicTask::taskFunctionality() { std::chrono::milliseconds periodChrono(static_cast(period * 1000)); auto currentStartTime{std::chrono::duration_cast( std::chrono::system_clock::now().time_since_epoch())}; - auto nextStartTime{currentStartTime}; - /* Enter the loop that defines the task behavior. */ for (;;) { if (terminateThread.load()) { diff --git a/src/fsfw/osal/host/PeriodicTask.h b/src/fsfw/osal/host/PeriodicTask.h index a565c584..6fdaae4e 100644 --- a/src/fsfw/osal/host/PeriodicTask.h +++ b/src/fsfw/osal/host/PeriodicTask.h @@ -39,7 +39,7 @@ class PeriodicTask : public PeriodicTaskBase { * @brief Currently, the executed object's lifetime is not coupled with * the task object's lifetime, so the destructor is empty. */ - virtual ~PeriodicTask(void); + ~PeriodicTask() override; /** * @brief The method to start the task. @@ -48,9 +48,9 @@ class PeriodicTask : public PeriodicTaskBase { * The address of the task object is passed as an argument * to the system call. */ - ReturnValue_t startTask(void); + ReturnValue_t startTask() override; - ReturnValue_t sleepFor(uint32_t ms); + ReturnValue_t sleepFor(uint32_t ms) override; protected: using chron_ms = std::chrono::milliseconds; @@ -81,9 +81,9 @@ class PeriodicTask : public PeriodicTaskBase { * the checkAndRestartPeriod system call blocks the task until the next * period. On missing the deadline, the deadlineMissedFunction is executed. */ - void taskFunctionality(void); + void taskFunctionality(); - bool delayForInterval(chron_ms* previousWakeTimeMs, const chron_ms interval); + static bool delayForInterval(chron_ms* previousWakeTimeMs, chron_ms interval); }; -#endif /* PERIODICTASK_H_ */ +#endif /* FRAMEWORK_OSAL_HOST_PERIODICTASK_H_ */ diff --git a/src/fsfw/osal/host/TaskFactory.cpp b/src/fsfw/osal/host/TaskFactory.cpp index 6e74fd57..ec4c1554 100644 --- a/src/fsfw/osal/host/TaskFactory.cpp +++ b/src/fsfw/osal/host/TaskFactory.cpp @@ -14,9 +14,9 @@ TaskFactory* TaskFactory::factoryInstance = new TaskFactory(); // Not used for the host implementation for now because C++ thread abstraction is used const size_t PeriodicTaskIF::MINIMUM_STACK_SIZE = 0; -TaskFactory::TaskFactory() {} +TaskFactory::TaskFactory() = default; -TaskFactory::~TaskFactory() {} +TaskFactory::~TaskFactory() = default; TaskFactory* TaskFactory::instance() { return TaskFactory::factoryInstance; } diff --git a/src/fsfw/osal/host/taskHelpers.cpp b/src/fsfw/osal/host/taskHelpers.cpp index aba2948a..432cf30c 100644 --- a/src/fsfw/osal/host/taskHelpers.cpp +++ b/src/fsfw/osal/host/taskHelpers.cpp @@ -6,7 +6,7 @@ std::mutex nameMapLock; std::map taskNameMap; -ReturnValue_t tasks::insertTaskName(std::thread::id threadId, std::string taskName) { +ReturnValue_t tasks::insertTaskName(std::thread::id threadId, const std::string& taskName) { std::lock_guard lg(nameMapLock); auto returnPair = taskNameMap.emplace(threadId, taskName); if (not returnPair.second) { diff --git a/src/fsfw/osal/host/taskHelpers.h b/src/fsfw/osal/host/taskHelpers.h index cf553011..13a71d16 100644 --- a/src/fsfw/osal/host/taskHelpers.h +++ b/src/fsfw/osal/host/taskHelpers.h @@ -7,7 +7,7 @@ namespace tasks { -ReturnValue_t insertTaskName(std::thread::id threadId, std::string taskName); +ReturnValue_t insertTaskName(std::thread::id threadId, const std::string& taskName); std::string getTaskName(std::thread::id threadId); } // namespace tasks diff --git a/src/fsfw/osal/linux/FixedTimeslotTask.cpp b/src/fsfw/osal/linux/FixedTimeslotTask.cpp index 4d912a40..c2823b00 100644 --- a/src/fsfw/osal/linux/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/linux/FixedTimeslotTask.cpp @@ -1,26 +1,21 @@ #include "fsfw/osal/linux/FixedTimeslotTask.h" -#include +#include -#include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/serviceinterface/ServiceInterface.h" uint32_t FixedTimeslotTask::deadlineMissedCount = 0; const size_t PeriodicTaskIF::MINIMUM_STACK_SIZE = PTHREAD_STACK_MIN; -FixedTimeslotTask::FixedTimeslotTask(const char* name_, int priority_, size_t stackSize_, - TaskPeriod periodSeconds_) - : posixThread(name_, priority_, stackSize_), - pst(static_cast(periodSeconds_ * 1000)), +FixedTimeslotTask::FixedTimeslotTask(const char* name_, TaskPriority priority_, size_t stackSize_, + TaskPeriod periodSeconds_, TaskDeadlineMissedFunction dlmFunc_) + : FixedTimeslotTaskBase(periodSeconds_, dlmFunc_), + posixThread(name_, priority_, stackSize_), started(false) {} -FixedTimeslotTask::~FixedTimeslotTask() {} - -bool FixedTimeslotTask::isEmpty() const { return pst.isEmpty(); } - void* FixedTimeslotTask::taskEntryPoint(void* arg) { // The argument is re-interpreted as PollingTask. - FixedTimeslotTask* originalTask(reinterpret_cast(arg)); + auto* originalTask(reinterpret_cast(arg)); // The task's functionality is called. originalTask->taskFunctionality(); return nullptr; @@ -36,45 +31,25 @@ ReturnValue_t FixedTimeslotTask::sleepFor(uint32_t ms) { return PosixThread::sleep((uint64_t)ms * 1000000); } -uint32_t FixedTimeslotTask::getPeriodMs() const { return pst.getLengthMs(); } - -ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId, uint32_t slotTimeMs, - int8_t executionStep) { - ExecutableObjectIF* executableObject = - ObjectManager::instance()->get(componentId); - if (executableObject != nullptr) { - pst.addSlot(componentId, slotTimeMs, executionStep, executableObject, this); - return HasReturnvaluesIF::RETURN_OK; - } - -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "Component " << std::hex << componentId << " not found, not adding it to pst" - << std::dec << std::endl; -#endif - return HasReturnvaluesIF::RETURN_FAILED; -} - -ReturnValue_t FixedTimeslotTask::checkSequence() { return pst.checkSequence(); } - -void FixedTimeslotTask::taskFunctionality() { +[[noreturn]] void FixedTimeslotTask::taskFunctionality() { // Like FreeRTOS pthreads are running as soon as they are created if (!started) { posixThread.suspend(); } - pst.intializeSequenceAfterTaskCreation(); + pollingSeqTable.intializeSequenceAfterTaskCreation(); // The start time for the first entry is read. - uint64_t lastWakeTime = posixThread.getCurrentMonotonicTimeMs(); - uint64_t interval = pst.getIntervalToNextSlotMs(); + uint64_t lastWakeTime = PosixThread::getCurrentMonotonicTimeMs(); + uint32_t interval = 0; // The task's "infinite" inner loop is entered. - while (1) { - if (pst.slotFollowsImmediately()) { + while (true) { + if (pollingSeqTable.slotFollowsImmediately()) { // Do nothing } else { // The interval for the next polling slot is selected. - interval = this->pst.getIntervalToPreviousSlotMs(); + interval = pollingSeqTable.getIntervalToPreviousSlotMs(); // The period is checked and restarted with the new interval. // If the deadline was missed, the deadlineMissedFunc is called. if (!PosixThread::delayUntil(&lastWakeTime, interval)) { @@ -83,7 +58,7 @@ void FixedTimeslotTask::taskFunctionality() { } } // The device handler for this slot is executed and the next one is chosen. - this->pst.executeAndAdvance(); + pollingSeqTable.executeAndAdvance(); } } diff --git a/src/fsfw/osal/linux/FixedTimeslotTask.h b/src/fsfw/osal/linux/FixedTimeslotTask.h index ecce7321..d6c7c0fb 100644 --- a/src/fsfw/osal/linux/FixedTimeslotTask.h +++ b/src/fsfw/osal/linux/FixedTimeslotTask.h @@ -5,10 +5,10 @@ #include "PosixThread.h" #include "fsfw/tasks/FixedSlotSequence.h" -#include "fsfw/tasks/FixedTimeslotTaskIF.h" +#include "fsfw/tasks/FixedTimeslotTaskBase.h" #include "fsfw/tasks/definitions.h" -class FixedTimeslotTask : public FixedTimeslotTaskIF { +class FixedTimeslotTask : public FixedTimeslotTaskBase { public: /** * Create a generic periodic task. @@ -22,22 +22,14 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF { * @param period_ * @param deadlineMissedFunc_ */ - FixedTimeslotTask(const char* name_, int priority_, size_t stackSize_, TaskPeriod periodSeconds_); - virtual ~FixedTimeslotTask(); + FixedTimeslotTask(const char* name_, TaskPriority priority_, size_t stackSize_, + TaskPeriod periodSeconds_, TaskDeadlineMissedFunction dlmFunc_); + ~FixedTimeslotTask() override = default; ReturnValue_t startTask() override; ReturnValue_t sleepFor(uint32_t ms) override; - uint32_t getPeriodMs() const override; - - ReturnValue_t addSlot(object_id_t componentId, uint32_t slotTimeMs, - int8_t executionStep) override; - - ReturnValue_t checkSequence() override; - - bool isEmpty() const override; - /** * This static function can be used as #deadlineMissedFunc. * It counts missedDeadlines and prints the number of missed deadlines every 10th time. @@ -57,10 +49,11 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF { * It links the functionalities provided by FixedSlotSequence with the * OS's System Calls to keep the timing of the periods. */ - virtual void taskFunctionality(); + [[noreturn]] virtual void taskFunctionality(); private: PosixThread posixThread; + bool started; /** * @brief This is the entry point in a new thread. @@ -74,9 +67,6 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF { * arbitrary data. */ static void* taskEntryPoint(void* arg); - FixedSlotSequence pst; - - bool started; }; #endif /* FSFW_OSAL_LINUX_FIXEDTIMESLOTTASK_H_ */ diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.cpp b/src/fsfw/osal/linux/PeriodicPosixTask.cpp index f52067f1..09b106ed 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.cpp +++ b/src/fsfw/osal/linux/PeriodicPosixTask.cpp @@ -1,35 +1,27 @@ -#include "fsfw/osal/linux/PeriodicPosixTask.h" +#include "PeriodicPosixTask.h" -#include -#include - -#include "fsfw/objectmanager/ObjectManager.h" -#include "fsfw/serviceinterface/ServiceInterface.h" +#include "fsfw/serviceinterface.h" #include "fsfw/tasks/ExecutableObjectIF.h" PeriodicPosixTask::PeriodicPosixTask(const char* name_, int priority_, size_t stackSize_, - TaskPeriod period_, TaskDeadlineMissedFunction dlMissedFunc_) - : PeriodicTaskBase(period_, dlMissedFunc_), + TaskPeriod period_, TaskDeadlineMissedFunction dlmFunc_) + : PeriodicTaskBase(period_, dlmFunc_), posixThread(name_, priority_, stackSize_), started(false) {} -PeriodicPosixTask::~PeriodicPosixTask() { - // Not Implemented -} - void* PeriodicPosixTask::taskEntryPoint(void* arg) { // The argument is re-interpreted as PollingTask. - PeriodicPosixTask* originalTask(reinterpret_cast(arg)); + auto* originalTask(reinterpret_cast(arg)); // The task's functionality is called. originalTask->taskFunctionality(); return nullptr; } ReturnValue_t PeriodicPosixTask::sleepFor(uint32_t ms) { - return PosixThread::sleep(static_cast(ms * 1000000)); + return PosixThread::sleep(static_cast(ms) * 1000000); } -ReturnValue_t PeriodicPosixTask::startTask(void) { +ReturnValue_t PeriodicPosixTask::startTask() { if (isEmpty()) { return HasReturnvaluesIF::RETURN_FAILED; } @@ -38,24 +30,24 @@ ReturnValue_t PeriodicPosixTask::startTask(void) { return HasReturnvaluesIF::RETURN_OK; } -void PeriodicPosixTask::taskFunctionality(void) { +[[noreturn]] void PeriodicPosixTask::taskFunctionality() { if (not started) { posixThread.suspend(); } initObjsAfterTaskCreation(); - uint64_t lastWakeTime = posixThread.getCurrentMonotonicTimeMs(); + uint64_t lastWakeTime = PosixThread::getCurrentMonotonicTimeMs(); uint64_t periodMs = getPeriodMs(); // The task's "infinite" inner loop is entered. - while (1) { + while (true) { for (auto const& objOpCodePair : objectList) { objOpCodePair.first->performOperation(objOpCodePair.second); } if (not PosixThread::delayUntil(&lastWakeTime, periodMs)) { - if (this->deadlineMissedFunc != nullptr) { - this->deadlineMissedFunc(); + if (dlmFunc != nullptr) { + dlmFunc(); } } } diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.h b/src/fsfw/osal/linux/PeriodicPosixTask.h index 3ab79ed0..085c10b9 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.h +++ b/src/fsfw/osal/linux/PeriodicPosixTask.h @@ -24,8 +24,8 @@ class PeriodicPosixTask : public PeriodicTaskBase { * @param deadlineMissedFunc_ */ PeriodicPosixTask(const char* name_, int priority_, size_t stackSize_, TaskPeriod period_, - void (*deadlineMissedFunc_)()); - virtual ~PeriodicPosixTask(); + TaskDeadlineMissedFunction dlmFunc_); + ~PeriodicPosixTask() override = default; /** * @brief The method to start the task. @@ -54,7 +54,7 @@ class PeriodicPosixTask : public PeriodicTaskBase { * will be blocked until the next period. On missing the deadline, the deadlineMissedFunction is * executed. */ - virtual void taskFunctionality(void); + [[noreturn]] virtual void taskFunctionality(); /** * @brief This is the entry point in a new thread. * diff --git a/src/fsfw/osal/linux/TaskFactory.cpp b/src/fsfw/osal/linux/TaskFactory.cpp index b9e7eb28..a28e685d 100644 --- a/src/fsfw/osal/linux/TaskFactory.cpp +++ b/src/fsfw/osal/linux/TaskFactory.cpp @@ -8,7 +8,7 @@ // TODO: Different variant than the lazy loading in QueueFactory. What's better and why? TaskFactory* TaskFactory::factoryInstance = new TaskFactory(); -TaskFactory::~TaskFactory() {} +TaskFactory::~TaskFactory() = default; TaskFactory* TaskFactory::instance() { return TaskFactory::factoryInstance; } @@ -22,7 +22,8 @@ PeriodicTaskIF* TaskFactory::createPeriodicTask( FixedTimeslotTaskIF* TaskFactory::createFixedTimeslotTask( TaskName name_, TaskPriority taskPriority_, TaskStackSize stackSize_, TaskPeriod periodInSeconds_, TaskDeadlineMissedFunction deadLineMissedFunction_) { - return new FixedTimeslotTask(name_, taskPriority_, stackSize_, periodInSeconds_); + return new FixedTimeslotTask(name_, taskPriority_, stackSize_, periodInSeconds_, + deadLineMissedFunction_); } ReturnValue_t TaskFactory::deleteTask(PeriodicTaskIF* task) { diff --git a/src/fsfw/tasks/CMakeLists.txt b/src/fsfw/tasks/CMakeLists.txt index 6128c272..1d4ab4e1 100644 --- a/src/fsfw/tasks/CMakeLists.txt +++ b/src/fsfw/tasks/CMakeLists.txt @@ -1,3 +1,3 @@ target_sources( ${LIB_FSFW_NAME} PRIVATE FixedSequenceSlot.cpp FixedSlotSequence.cpp - PeriodicTaskBase.cpp) + PeriodicTaskBase.cpp FixedTimeslotTaskBase.cpp) diff --git a/src/fsfw/tasks/FixedSlotSequence.h b/src/fsfw/tasks/FixedSlotSequence.h index 5ece7126..838963c1 100644 --- a/src/fsfw/tasks/FixedSlotSequence.h +++ b/src/fsfw/tasks/FixedSlotSequence.h @@ -35,7 +35,7 @@ class FixedSlotSequence { * @brief The constructor of the FixedSlotSequence object. * @param setLength The period length, expressed in ms. */ - FixedSlotSequence(uint32_t setLengthMs); + explicit FixedSlotSequence(uint32_t setLengthMs); /** * @brief The destructor of the FixedSlotSequence object. diff --git a/src/fsfw/tasks/FixedTimeslotTaskBase.cpp b/src/fsfw/tasks/FixedTimeslotTaskBase.cpp new file mode 100644 index 00000000..26726582 --- /dev/null +++ b/src/fsfw/tasks/FixedTimeslotTaskBase.cpp @@ -0,0 +1,29 @@ +#include "FixedTimeslotTaskBase.h" + +#include "fsfw/objectmanager/ObjectManager.h" + +FixedTimeslotTaskBase::FixedTimeslotTaskBase(TaskPeriod period_, + TaskDeadlineMissedFunction dlmFunc_) + : pollingSeqTable(getPeriodMs()), period(period_), dlmFunc(dlmFunc_) {} +uint32_t FixedTimeslotTaskBase::getPeriodMs() const { return static_cast(period * 1000); } + +bool FixedTimeslotTaskBase::isEmpty() const { return pollingSeqTable.isEmpty(); } + +ReturnValue_t FixedTimeslotTaskBase::checkSequence() { return pollingSeqTable.checkSequence(); } + +ReturnValue_t FixedTimeslotTaskBase::addSlot(object_id_t componentId, uint32_t slotTimeMs, + int8_t executionStep) { + auto* executableObject = ObjectManager::instance()->get(componentId); + if (executableObject != nullptr) { + pollingSeqTable.addSlot(componentId, slotTimeMs, executionStep, executableObject, this); + return HasReturnvaluesIF::RETURN_OK; + } + +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "Component 0x" << std::hex << std::setw(8) << std::setfill('0') << componentId + << std::setfill(' ') << " not found, not adding it to PST" << std::dec << std::endl; +#else + sif::printError("Component 0x%08x not found, not adding it to PST\n"); +#endif + return HasReturnvaluesIF::RETURN_FAILED; +} diff --git a/src/fsfw/tasks/FixedTimeslotTaskBase.h b/src/fsfw/tasks/FixedTimeslotTaskBase.h new file mode 100644 index 00000000..91a4f649 --- /dev/null +++ b/src/fsfw/tasks/FixedTimeslotTaskBase.h @@ -0,0 +1,44 @@ +#ifndef FSFW_EXAMPLE_HOSTED_FIXEDTIMESLOTTASKBASE_H +#define FSFW_EXAMPLE_HOSTED_FIXEDTIMESLOTTASKBASE_H + +#include "FixedSlotSequence.h" +#include "FixedTimeslotTaskIF.h" +#include "definitions.h" + +class FixedTimeslotTaskBase : public FixedTimeslotTaskIF { + public: + explicit FixedTimeslotTaskBase(TaskPeriod period, TaskDeadlineMissedFunction dlmFunc = nullptr); + ~FixedTimeslotTaskBase() override = default; + ; + + protected: + //! Polling sequence table which contains the object to execute + //! and information like the timeslots and the passed execution step. + FixedSlotSequence pollingSeqTable; + + /** + * @brief Period of task in floating point seconds + */ + TaskPeriod period; + + /** + * @brief The pointer to the deadline-missed function. + * @details + * This pointer stores the function that is executed if the task's deadline + * is missed. So, each may react individually on a timing failure. + * The pointer may be NULL, then nothing happens on missing the deadline. + * The deadline is equal to the next execution of the periodic task. + */ + TaskDeadlineMissedFunction dlmFunc = nullptr; + + ReturnValue_t checkSequence() override; + + [[nodiscard]] uint32_t getPeriodMs() const override; + + [[nodiscard]] bool isEmpty() const override; + + ReturnValue_t addSlot(object_id_t componentId, uint32_t slotTimeMs, + int8_t executionStep) override; +}; + +#endif // FSFW_EXAMPLE_HOSTED_FIXEDTIMESLOTTASKBASE_H diff --git a/src/fsfw/tasks/FixedTimeslotTaskIF.h b/src/fsfw/tasks/FixedTimeslotTaskIF.h index 9d85ac4a..dec382c3 100644 --- a/src/fsfw/tasks/FixedTimeslotTaskIF.h +++ b/src/fsfw/tasks/FixedTimeslotTaskIF.h @@ -11,7 +11,7 @@ */ class FixedTimeslotTaskIF : public PeriodicTaskIF { public: - virtual ~FixedTimeslotTaskIF() {} + ~FixedTimeslotTaskIF() override = default; static constexpr ReturnValue_t SLOT_LIST_EMPTY = HasReturnvaluesIF::makeReturnCode(CLASS_ID::FIXED_SLOT_TASK_IF, 0); diff --git a/src/fsfw/tasks/PeriodicTaskBase.cpp b/src/fsfw/tasks/PeriodicTaskBase.cpp index 021ba7b5..cc8784d9 100644 --- a/src/fsfw/tasks/PeriodicTaskBase.cpp +++ b/src/fsfw/tasks/PeriodicTaskBase.cpp @@ -1,11 +1,21 @@ #include "PeriodicTaskBase.h" -#include - #include +#include "fsfw/objectmanager/ObjectManager.h" +#include "fsfw/serviceinterface.h" + PeriodicTaskBase::PeriodicTaskBase(TaskPeriod period_, TaskDeadlineMissedFunction dlmFunc_) - : period(period), dlmFunc(dlmFunc_) {} + : period(period_), dlmFunc(dlmFunc_) { + // Hints at configuration error + if (PeriodicTaskBase::getPeriodMs() <= 1) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "Passed task period 0 or smaller than 1 ms" << std::endl; +#else + sif::printWarning("Passed task period 0 or smaller than 1ms\n"); +#endif + } +} uint32_t PeriodicTaskBase::getPeriodMs() const { return static_cast(period * 1000); } @@ -32,7 +42,7 @@ ReturnValue_t PeriodicTaskBase::initObjsAfterTaskCreation() { } ReturnValue_t PeriodicTaskBase::addComponent(object_id_t object, uint8_t opCode) { - ExecutableObjectIF* newObject = ObjectManager::instance()->get(object); + auto* newObject = ObjectManager::instance()->get(object); return addComponent(newObject, opCode); } diff --git a/src/fsfw/tasks/PeriodicTaskBase.h b/src/fsfw/tasks/PeriodicTaskBase.h index a262a2d4..68791fb8 100644 --- a/src/fsfw/tasks/PeriodicTaskBase.h +++ b/src/fsfw/tasks/PeriodicTaskBase.h @@ -11,14 +11,15 @@ class ExecutableObjectIF; class PeriodicTaskBase : public PeriodicTaskIF { public: - PeriodicTaskBase(TaskPeriod period, TaskDeadlineMissedFunction deadlineMissedFunc = nullptr); + explicit PeriodicTaskBase(TaskPeriod period, + TaskDeadlineMissedFunction deadlineMissedFunc = nullptr); ReturnValue_t addComponent(object_id_t object, uint8_t opCode) override; ReturnValue_t addComponent(ExecutableObjectIF* object, uint8_t opCode) override; - uint32_t getPeriodMs() const override; + [[nodiscard]] uint32_t getPeriodMs() const override; - bool isEmpty() const override; + [[nodiscard]] bool isEmpty() const override; ReturnValue_t initObjsAfterTaskCreation(); diff --git a/src/fsfw/tasks/PeriodicTaskIF.h b/src/fsfw/tasks/PeriodicTaskIF.h index ec25faa8..a6d6a6d6 100644 --- a/src/fsfw/tasks/PeriodicTaskIF.h +++ b/src/fsfw/tasks/PeriodicTaskIF.h @@ -17,7 +17,7 @@ class PeriodicTaskIF { /** * @brief A virtual destructor as it is mandatory for interfaces. */ - virtual ~PeriodicTaskIF() {} + virtual ~PeriodicTaskIF() = default; /** * @brief With the startTask method, a created task can be started * for the first time. diff --git a/src/fsfw/tasks/definitions.h b/src/fsfw/tasks/definitions.h index bca9b768..586884b6 100644 --- a/src/fsfw/tasks/definitions.h +++ b/src/fsfw/tasks/definitions.h @@ -5,7 +5,7 @@ #include using TaskName = const char*; -using TaskPriority = uint32_t; +using TaskPriority = int; using TaskStackSize = size_t; using TaskPeriod = double; using TaskDeadlineMissedFunction = void (*)(); From c0292f072e77beaf7760753c86d0e57bbfa90409 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 20 May 2022 20:52:36 +0200 Subject: [PATCH 148/404] warning printout correction --- src/fsfw/osal/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/osal/CMakeLists.txt b/src/fsfw/osal/CMakeLists.txt index f3c5cfad..8fa353c6 100644 --- a/src/fsfw/osal/CMakeLists.txt +++ b/src/fsfw/osal/CMakeLists.txt @@ -18,7 +18,7 @@ elseif(FSFW_OSAL MATCHES "host") else() - message(WARNING "The OS_FSFW variable was not set. Assuming host OS..") + message(WARNING "${MSG_PREFIX} The FSFW_OSAL variable was not set. Assuming host OS..") # Not set. Assumuing this is a host build, try to determine host OS if (WIN32) add_subdirectory(host) From b8b7756a3e4f92ff1d8abd80f4be3eb5566f71fd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 22 May 2022 14:32:48 +0200 Subject: [PATCH 149/404] fix host OSAL --- src/fsfw/osal/host/FixedTimeslotTask.cpp | 4 +++- src/fsfw/osal/host/FixedTimeslotTask.h | 4 +++- src/fsfw/osal/host/PeriodicTask.cpp | 2 ++ src/fsfw/osal/host/PeriodicTask.h | 1 + 4 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/fsfw/osal/host/FixedTimeslotTask.cpp b/src/fsfw/osal/host/FixedTimeslotTask.cpp index 07853938..bb3ffe35 100644 --- a/src/fsfw/osal/host/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/host/FixedTimeslotTask.cpp @@ -133,7 +133,7 @@ ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId, uint32_t slotT return HasReturnvaluesIF::RETURN_FAILED; } -ReturnValue_t FixedTimeslotTask::checkSequence() const { return pollingSeqTable.checkSequence(); } +ReturnValue_t FixedTimeslotTask::checkSequence() { return pollingSeqTable.checkSequence(); } uint32_t FixedTimeslotTask::getPeriodMs() const { return period * 1000; } @@ -176,3 +176,5 @@ bool FixedTimeslotTask::delayForInterval(chron_ms* previousWakeTimeMs, const chr (*previousWakeTimeMs) = currentStartTime; return false; } + +bool FixedTimeslotTask::isEmpty() const { return pollingSeqTable.isEmpty(); } diff --git a/src/fsfw/osal/host/FixedTimeslotTask.h b/src/fsfw/osal/host/FixedTimeslotTask.h index cdbc6f23..dac33871 100644 --- a/src/fsfw/osal/host/FixedTimeslotTask.h +++ b/src/fsfw/osal/host/FixedTimeslotTask.h @@ -59,12 +59,14 @@ class FixedTimeslotTask : public FixedTimeslotTaskIF { */ ReturnValue_t addSlot(object_id_t componentId, uint32_t slotTimeMs, int8_t executionStep); - ReturnValue_t checkSequence() const override; + ReturnValue_t checkSequence() override; uint32_t getPeriodMs() const; ReturnValue_t sleepFor(uint32_t ms); + bool isEmpty() const override; + protected: using chron_ms = std::chrono::milliseconds; diff --git a/src/fsfw/osal/host/PeriodicTask.cpp b/src/fsfw/osal/host/PeriodicTask.cpp index cdcfafa6..23736b96 100644 --- a/src/fsfw/osal/host/PeriodicTask.cpp +++ b/src/fsfw/osal/host/PeriodicTask.cpp @@ -156,3 +156,5 @@ bool PeriodicTask::delayForInterval(chron_ms* previousWakeTimeMs, const chron_ms (*previousWakeTimeMs) = currentStartTime; return false; } + +bool PeriodicTask::isEmpty() const { return objectList.empty(); } diff --git a/src/fsfw/osal/host/PeriodicTask.h b/src/fsfw/osal/host/PeriodicTask.h index 6c4d5e8b..463ab379 100644 --- a/src/fsfw/osal/host/PeriodicTask.h +++ b/src/fsfw/osal/host/PeriodicTask.h @@ -73,6 +73,7 @@ class PeriodicTask : public PeriodicTaskIF { ReturnValue_t sleepFor(uint32_t ms); + bool isEmpty() const override; protected: using chron_ms = std::chrono::milliseconds; bool started; From 24069dfd78a5905b3e327fab3528baacbc913757 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 24 May 2022 16:22:27 +0200 Subject: [PATCH 150/404] removed [[maybe_unused]] --- src/fsfw/pus/Service11TelecommandScheduling.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/pus/Service11TelecommandScheduling.h b/src/fsfw/pus/Service11TelecommandScheduling.h index 9a8be603..45a7c26b 100644 --- a/src/fsfw/pus/Service11TelecommandScheduling.h +++ b/src/fsfw/pus/Service11TelecommandScheduling.h @@ -45,7 +45,7 @@ class Service11TelecommandScheduling final : public PusServiceBase { HasReturnvaluesIF::makeReturnCode(CLASS_ID, 3); // The types of PUS-11 subservices - enum [[maybe_unused]] Subservice : uint8_t{ENABLE_SCHEDULING = 1, + enum Subservice : uint8_t{ENABLE_SCHEDULING = 1, DISABLE_SCHEDULING = 2, RESET_SCHEDULING = 3, INSERT_ACTIVITY = 4, From c8355251967009559ea790b63c3ebfc67a27efef Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 25 May 2022 09:56:32 +0200 Subject: [PATCH 151/404] added cast for PUS11 --- src/fsfw/pus/Service11TelecommandScheduling.tpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/pus/Service11TelecommandScheduling.tpp b/src/fsfw/pus/Service11TelecommandScheduling.tpp index 5ee08194..564a11fc 100644 --- a/src/fsfw/pus/Service11TelecommandScheduling.tpp +++ b/src/fsfw/pus/Service11TelecommandScheduling.tpp @@ -67,7 +67,7 @@ inline ReturnValue_t Service11TelecommandScheduling::performService // NOTE: The iterator is increased in the loop here. Increasing the iterator as for-loop arg // does not work in this case as we are deleting the current element here. for (auto it = telecommandMap.begin(); it != telecommandMap.end();) { - if (it->first <= tNow.tv_sec) { + if (it->first <= static_cast(tNow.tv_sec)) { // release tc TmTcMessage releaseMsg(it->second.storeAddr); auto sendRet = this->requestQueue->sendMessage(recipientMsgQueueId, &releaseMsg, false); From 8cfe848dfef41bfc845b4b5c2e6273ac3735400e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 25 May 2022 14:30:00 +0200 Subject: [PATCH 152/404] service 3 and local HK man improvements --- src/fsfw/datapoollocal/LocalDataPoolManager.cpp | 5 +++++ src/fsfw/ipc/CommandMessageIF.h | 2 +- src/fsfw/pus/Service3Housekeeping.cpp | 15 ++++++++++----- 3 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/fsfw/datapoollocal/LocalDataPoolManager.cpp b/src/fsfw/datapoollocal/LocalDataPoolManager.cpp index 781d8f71..dec64b81 100644 --- a/src/fsfw/datapoollocal/LocalDataPoolManager.cpp +++ b/src/fsfw/datapoollocal/LocalDataPoolManager.cpp @@ -577,6 +577,9 @@ ReturnValue_t LocalDataPoolManager::handleHousekeepingMessage(CommandMessage* me CommandMessage reply; if (result != HasReturnvaluesIF::RETURN_OK) { + if(result == WRONG_HK_PACKET_TYPE) { + printWarningOrError(sif::OutputTypes::OUT_WARNING, "handleHousekeepingMessage", WRONG_HK_PACKET_TYPE); + } HousekeepingMessage::setHkRequestFailureReply(&reply, sid, result); } else { HousekeepingMessage::setHkRequestSuccessReply(&reply, sid); @@ -834,6 +837,8 @@ void LocalDataPoolManager::printWarningOrError(sif::OutputTypes outputType, errorPrint = "Dataset not found"; } else if (error == POOLOBJECT_NOT_FOUND) { errorPrint = "Pool Object not found"; + } else if (error == WRONG_HK_PACKET_TYPE) { + errorPrint = "Wrong Packet Type"; } else if (error == HasReturnvaluesIF::RETURN_FAILED) { if (outputType == sif::OutputTypes::OUT_WARNING) { errorPrint = "Generic Warning"; diff --git a/src/fsfw/ipc/CommandMessageIF.h b/src/fsfw/ipc/CommandMessageIF.h index aea08203..3c31a184 100644 --- a/src/fsfw/ipc/CommandMessageIF.h +++ b/src/fsfw/ipc/CommandMessageIF.h @@ -34,7 +34,7 @@ class CommandMessageIF { static const Command_t CMD_NONE = MAKE_COMMAND_ID(0); static const Command_t REPLY_COMMAND_OK = MAKE_COMMAND_ID(1); //! Reply indicating that the current command was rejected, - //! par1 should contain the error code + //! Parameter 1 should contain the error code static const Command_t REPLY_REJECTED = MAKE_COMMAND_ID(2); virtual ~CommandMessageIF(){}; diff --git a/src/fsfw/pus/Service3Housekeeping.cpp b/src/fsfw/pus/Service3Housekeeping.cpp index cce8fc91..85c3762f 100644 --- a/src/fsfw/pus/Service3Housekeeping.cpp +++ b/src/fsfw/pus/Service3Housekeeping.cpp @@ -208,7 +208,7 @@ ReturnValue_t Service3Housekeeping::handleReply(const CommandMessage* reply, ReturnValue_t error = HasReturnvaluesIF::RETURN_FAILED; HousekeepingMessage::getHkRequestFailureReply(reply, &error); failureParameter2 = error; - return CommandingServiceBase::EXECUTION_COMPLETE; + return RETURN_FAILED; } default: @@ -248,19 +248,23 @@ void Service3Housekeeping::handleUnrequestedReply(CommandMessage* reply) { case (HousekeepingMessage::HK_REQUEST_FAILURE): { break; } + case(CommandMessage::REPLY_REJECTED): { + sif::warning << "Service3Housekeeping::handleUnrequestedReply: Unexpected reply " + "rejected with error code" << reply->getParameter() << std::endl; + break; + } default: { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "Service3Housekeeping::handleUnrequestedReply: Invalid reply with reply " - "command " - << command << "!" << std::endl; + "command " << command << "" << std::endl; #else sif::printWarning( "Service3Housekeeping::handleUnrequestedReply: Invalid reply with " - "reply command %hu!\n", + "reply command %hu\n", command); #endif - return; + break; } } @@ -275,6 +279,7 @@ void Service3Housekeeping::handleUnrequestedReply(CommandMessage* reply) { "Could not generate reply!\n"); #endif } + CommandingServiceBase::handleUnrequestedReply(reply); } MessageQueueId_t Service3Housekeeping::getHkQueue() const { return commandQueue->getId(); } From ac62443f318f62a15812eb2b382a38b3ec84d960 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 25 May 2022 14:30:58 +0200 Subject: [PATCH 153/404] use better type for stored limit --- src/fsfw/tmtcservices/TmTcBridge.cpp | 2 +- src/fsfw/tmtcservices/TmTcBridge.h | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/fsfw/tmtcservices/TmTcBridge.cpp b/src/fsfw/tmtcservices/TmTcBridge.cpp index cc6ec599..4fa07c14 100644 --- a/src/fsfw/tmtcservices/TmTcBridge.cpp +++ b/src/fsfw/tmtcservices/TmTcBridge.cpp @@ -36,7 +36,7 @@ ReturnValue_t TmTcBridge::setNumberOfSentPacketsPerCycle(uint8_t sentPacketsPerC } } -ReturnValue_t TmTcBridge::setMaxNumberOfPacketsStored(uint8_t maxNumberOfPacketsStored) { +ReturnValue_t TmTcBridge::setMaxNumberOfPacketsStored(unsigned int maxNumberOfPacketsStored) { if (maxNumberOfPacketsStored <= LIMIT_DOWNLINK_PACKETS_STORED) { this->maxNumberOfPacketsStored = maxNumberOfPacketsStored; return RETURN_OK; diff --git a/src/fsfw/tmtcservices/TmTcBridge.h b/src/fsfw/tmtcservices/TmTcBridge.h index 81d8e5d8..679ab2ef 100644 --- a/src/fsfw/tmtcservices/TmTcBridge.h +++ b/src/fsfw/tmtcservices/TmTcBridge.h @@ -18,7 +18,7 @@ class TmTcBridge : public AcceptsTelemetryIF, public: static constexpr uint8_t TMTC_RECEPTION_QUEUE_DEPTH = 20; static constexpr uint8_t LIMIT_STORED_DATA_SENT_PER_CYCLE = 15; - static constexpr uint8_t LIMIT_DOWNLINK_PACKETS_STORED = 200; + static constexpr unsigned int LIMIT_DOWNLINK_PACKETS_STORED = 1000; static constexpr uint8_t DEFAULT_STORED_DATA_SENT_PER_CYCLE = 5; static constexpr uint8_t DEFAULT_DOWNLINK_PACKETS_STORED = 10; @@ -43,7 +43,7 @@ class TmTcBridge : public AcceptsTelemetryIF, * @return -@c RETURN_OK if value was set successfully * -@c RETURN_FAILED otherwise, stored value stays the same */ - ReturnValue_t setMaxNumberOfPacketsStored(uint8_t maxNumberOfPacketsStored); + ReturnValue_t setMaxNumberOfPacketsStored(unsigned int maxNumberOfPacketsStored); /** * This will set up the bridge to overwrite old data in the FIFO. @@ -152,7 +152,7 @@ class TmTcBridge : public AcceptsTelemetryIF, */ DynamicFIFO* tmFifo = nullptr; uint8_t sentPacketsPerCycle = DEFAULT_STORED_DATA_SENT_PER_CYCLE; - uint8_t maxNumberOfPacketsStored = DEFAULT_DOWNLINK_PACKETS_STORED; + unsigned int maxNumberOfPacketsStored = DEFAULT_DOWNLINK_PACKETS_STORED; }; #endif /* FSFW_TMTCSERVICES_TMTCBRIDGE_H_ */ From 3749f31ab46d32d4c9fd95bef031fe5f3bf39d3e Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Thu, 26 May 2022 02:03:39 +0200 Subject: [PATCH 154/404] disable pending commands and replies in MODE_OFF transition --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 25 +++++++++++++++++++ src/fsfw/devicehandlers/DeviceHandlerBase.h | 5 ++++ 2 files changed, 30 insertions(+) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 69baf54f..e0b50733 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -572,6 +572,9 @@ void DeviceHandlerBase::setMode(Mode_t newMode, uint8_t newSubmode) { mode = newMode; modeChanged(); setNormalDatapoolEntriesInvalid(); + if (newMode == MODE_OFF) { + disableCommandsAndReplies(); + } if (!isTransitionalMode()) { modeHelper.modeChanged(newMode, newSubmode); announceMode(false); @@ -1482,6 +1485,9 @@ ReturnValue_t DeviceHandlerBase::initializeAfterTaskCreation() { LocalPoolDataSetBase* DeviceHandlerBase::getDataSetHandle(sid_t sid) { auto iter = deviceReplyMap.find(sid.ownerSetId); + if (sid.objectId == 0x44140014) { + sif::debug << "HK message for IMTQ" << std::endl; + } if (iter != deviceReplyMap.end()) { return iter->second.dataSet; } else { @@ -1567,3 +1573,22 @@ void DeviceHandlerBase::setParent(object_id_t parent) { this->parent = parent; } void DeviceHandlerBase::setPowerSwitcher(PowerSwitchIF* switcher) { this->powerSwitcher = switcher; } + +void DeviceHandlerBase::disableCommandsAndReplies() { + for (auto& command : deviceCommandMap) { + if (command.second.isExecuting) { + command.second.isExecuting = false; + } + } + for (auto& reply : deviceReplyMap) { + if (!reply.second.periodic) { + if (reply.second.countdown != nullptr) { + reply.second.countdown->timeOut(); + } + else { + reply.second.delayCycles = 0; + } + reply.second.active = false; + } + } +} diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index 34171067..58e54da0 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -1325,6 +1325,11 @@ class DeviceHandlerBase : public DeviceHandlerIF, void printWarningOrError(sif::OutputTypes errorType, const char *functionName, ReturnValue_t errorCode = HasReturnvaluesIF::RETURN_FAILED, const char *errorPrint = nullptr); + + /** + * @brief Disables all commands and replies when device is set to MODE_OFF + */ + void disableCommandsAndReplies(); }; #endif /* FSFW_DEVICEHANDLERS_DEVICEHANDLERBASE_H_ */ From ab68817e9a8254c793239313e169862ceb65b9c4 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Thu, 26 May 2022 02:06:05 +0200 Subject: [PATCH 155/404] removed debugging printout --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index e0b50733..f6e8578b 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1485,9 +1485,6 @@ ReturnValue_t DeviceHandlerBase::initializeAfterTaskCreation() { LocalPoolDataSetBase* DeviceHandlerBase::getDataSetHandle(sid_t sid) { auto iter = deviceReplyMap.find(sid.ownerSetId); - if (sid.objectId == 0x44140014) { - sif::debug << "HK message for IMTQ" << std::endl; - } if (iter != deviceReplyMap.end()) { return iter->second.dataSet; } else { From 95a64e1da3bb6d334c58c9ba78747bcdaccd5a8b Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Fri, 27 May 2022 13:04:21 +0200 Subject: [PATCH 156/404] wrong initialization order --- src/fsfw/tasks/FixedTimeslotTaskBase.cpp | 2 +- src/fsfw/tasks/FixedTimeslotTaskBase.h | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/fsfw/tasks/FixedTimeslotTaskBase.cpp b/src/fsfw/tasks/FixedTimeslotTaskBase.cpp index 26726582..05c08109 100644 --- a/src/fsfw/tasks/FixedTimeslotTaskBase.cpp +++ b/src/fsfw/tasks/FixedTimeslotTaskBase.cpp @@ -4,7 +4,7 @@ FixedTimeslotTaskBase::FixedTimeslotTaskBase(TaskPeriod period_, TaskDeadlineMissedFunction dlmFunc_) - : pollingSeqTable(getPeriodMs()), period(period_), dlmFunc(dlmFunc_) {} + : period(period_), pollingSeqTable(getPeriodMs()), dlmFunc(dlmFunc_) {} uint32_t FixedTimeslotTaskBase::getPeriodMs() const { return static_cast(period * 1000); } bool FixedTimeslotTaskBase::isEmpty() const { return pollingSeqTable.isEmpty(); } diff --git a/src/fsfw/tasks/FixedTimeslotTaskBase.h b/src/fsfw/tasks/FixedTimeslotTaskBase.h index 91a4f649..6f08e3fe 100644 --- a/src/fsfw/tasks/FixedTimeslotTaskBase.h +++ b/src/fsfw/tasks/FixedTimeslotTaskBase.h @@ -12,15 +12,15 @@ class FixedTimeslotTaskBase : public FixedTimeslotTaskIF { ; protected: - //! Polling sequence table which contains the object to execute - //! and information like the timeslots and the passed execution step. - FixedSlotSequence pollingSeqTable; - /** * @brief Period of task in floating point seconds */ TaskPeriod period; + //! Polling sequence table which contains the object to execute + //! and information like the timeslots and the passed execution step. + FixedSlotSequence pollingSeqTable; + /** * @brief The pointer to the deadline-missed function. * @details From cda81fc8415c3873c035aa7ebbfa3fe93d519f08 Mon Sep 17 00:00:00 2001 From: Cleanroom Laptop L15 Date: Fri, 3 Jun 2022 18:05:38 +0200 Subject: [PATCH 157/404] enable hk --- hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp | 1 + hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp | 1 + hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp | 1 + 3 files changed, 3 insertions(+) diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp index 94e1331c..3dd19275 100644 --- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp @@ -252,6 +252,7 @@ ReturnValue_t GyroHandlerL3GD20H::initializeLocalDataPool(localpool::DataPool &l localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, new PoolEntry({0.0})); localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, new PoolEntry({0.0})); localDataPoolMap.emplace(L3GD20H::TEMPERATURE, new PoolEntry({0.0})); + poolManager.subscribeForPeriodicPacket(dataset.getSid(), false, 10.0, false); return HasReturnvaluesIF::RETURN_OK; } diff --git a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp index a13ae791..ee45056a 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp @@ -475,6 +475,7 @@ ReturnValue_t MgmLIS3MDLHandler::initializeLocalDataPool(localpool::DataPool &lo localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTH_Y, new PoolEntry({0.0})); localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTH_Z, new PoolEntry({0.0})); localDataPoolMap.emplace(MGMLIS3MDL::TEMPERATURE_CELCIUS, new PoolEntry({0.0})); + poolManager.subscribeForPeriodicPacket(dataset.getSid(), false, 10.0, false); return HasReturnvaluesIF::RETURN_OK; } diff --git a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp b/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp index f9929d63..3396ea15 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp @@ -312,6 +312,7 @@ ReturnValue_t MgmRM3100Handler::initializeLocalDataPool(localpool::DataPool &loc localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_X, new PoolEntry({0.0})); localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_Y, new PoolEntry({0.0})); localDataPoolMap.emplace(RM3100::FIELD_STRENGTH_Z, new PoolEntry({0.0})); + poolManager.subscribeForPeriodicPacket(primaryDataset.getSid(), false, 10.0, false); return HasReturnvaluesIF::RETURN_OK; } From af890c62187f4c7833ec60847bdd793babb00137 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Thu, 16 Jun 2022 07:55:57 +0200 Subject: [PATCH 158/404] corrected warning text --- src/fsfw/datapoollocal/LocalDataPoolManager.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/fsfw/datapoollocal/LocalDataPoolManager.cpp b/src/fsfw/datapoollocal/LocalDataPoolManager.cpp index dec64b81..c76134b3 100644 --- a/src/fsfw/datapoollocal/LocalDataPoolManager.cpp +++ b/src/fsfw/datapoollocal/LocalDataPoolManager.cpp @@ -699,9 +699,9 @@ void LocalDataPoolManager::performPeriodicHkGeneration(HkReceiver& receiver) { if (result != HasReturnvaluesIF::RETURN_OK) { /* Configuration error */ #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "LocalDataPoolManager::performHkOperation: HK generation failed." << std::endl; + sif::warning << "LocalDataPoolManager::performPeriodicHkOperation: HK generation failed." << std::endl; #else - sif::printWarning("LocalDataPoolManager::performHkOperation: HK generation failed.\n"); + sif::printWarning("LocalDataPoolManager::performPeriodicHkOperation: HK generation failed.\n"); #endif } } From 1910a7838c712f1cf34031d2a8b53e2f7cd24f66 Mon Sep 17 00:00:00 2001 From: Irini Kosmidou Date: Mon, 20 Jun 2022 11:04:06 +0200 Subject: [PATCH 159/404] compile DleParser --- src/fsfw/globalfunctions/CMakeLists.txt | 1 + src/fsfw/globalfunctions/DleParser.cpp | 1 - src/fsfw/globalfunctions/DleParser.h | 5 +---- 3 files changed, 2 insertions(+), 5 deletions(-) diff --git a/src/fsfw/globalfunctions/CMakeLists.txt b/src/fsfw/globalfunctions/CMakeLists.txt index cfa02696..cf8a25d5 100644 --- a/src/fsfw/globalfunctions/CMakeLists.txt +++ b/src/fsfw/globalfunctions/CMakeLists.txt @@ -4,6 +4,7 @@ target_sources( AsciiConverter.cpp CRC.cpp DleEncoder.cpp + DleParser.cpp PeriodicOperationDivider.cpp timevalOperations.cpp Type.cpp diff --git a/src/fsfw/globalfunctions/DleParser.cpp b/src/fsfw/globalfunctions/DleParser.cpp index 71da7e6a..f326f837 100644 --- a/src/fsfw/globalfunctions/DleParser.cpp +++ b/src/fsfw/globalfunctions/DleParser.cpp @@ -1,6 +1,5 @@ #include "DleParser.h" -#include #include #include diff --git a/src/fsfw/globalfunctions/DleParser.h b/src/fsfw/globalfunctions/DleParser.h index 32fe38cb..e8ee61d9 100644 --- a/src/fsfw/globalfunctions/DleParser.h +++ b/src/fsfw/globalfunctions/DleParser.h @@ -1,5 +1,4 @@ -#ifndef MISSION_DEVICES_DLEPARSER_H_ -#define MISSION_DEVICES_DLEPARSER_H_ +#pragma once #include #include @@ -123,5 +122,3 @@ class DleParser : public HasReturnvaluesIF { Context ctx; bool startFound = false; }; - -#endif /* MISSION_DEVICES_DLEPARSER_H_ */ From 5abbf42e9f9b48cc244faabe88dc845e66cd0d24 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 21 Jun 2022 00:49:58 +0200 Subject: [PATCH 160/404] some form updates --- CMakeLists.txt | 5 ++++- src/fsfw/datapoollocal/LocalDataPoolManager.cpp | 8 +++++--- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 3 +-- src/fsfw/osal/CMakeLists.txt | 4 +++- src/fsfw/osal/host/PeriodicTask.h | 1 + src/fsfw/power/PowerSwitchIF.h | 6 +++--- src/fsfw/pus/Service3Housekeeping.cpp | 8 +++++--- src/fsfw/thermal/ThermalComponentIF.h | 6 +++--- 8 files changed, 25 insertions(+), 16 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 6b7634b2..65a675df 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -182,7 +182,10 @@ if(FSFW_BUILD_UNITTESTS) endif() endif() -message(STATUS "${MSG_PREFIX} Finding and/or providing etl library with version ${FSFW_ETL_LIB_MAJOR_VERSION}") +message( + STATUS + "${MSG_PREFIX} Finding and/or providing etl library with version ${FSFW_ETL_LIB_MAJOR_VERSION}" +) # Check whether the user has already installed ETL first find_package(${FSFW_ETL_LIB_NAME} ${FSFW_ETL_LIB_MAJOR_VERSION} CONFIG QUIET) diff --git a/src/fsfw/datapoollocal/LocalDataPoolManager.cpp b/src/fsfw/datapoollocal/LocalDataPoolManager.cpp index c76134b3..11faa4d0 100644 --- a/src/fsfw/datapoollocal/LocalDataPoolManager.cpp +++ b/src/fsfw/datapoollocal/LocalDataPoolManager.cpp @@ -577,8 +577,9 @@ ReturnValue_t LocalDataPoolManager::handleHousekeepingMessage(CommandMessage* me CommandMessage reply; if (result != HasReturnvaluesIF::RETURN_OK) { - if(result == WRONG_HK_PACKET_TYPE) { - printWarningOrError(sif::OutputTypes::OUT_WARNING, "handleHousekeepingMessage", WRONG_HK_PACKET_TYPE); + if (result == WRONG_HK_PACKET_TYPE) { + printWarningOrError(sif::OutputTypes::OUT_WARNING, "handleHousekeepingMessage", + WRONG_HK_PACKET_TYPE); } HousekeepingMessage::setHkRequestFailureReply(&reply, sid, result); } else { @@ -699,7 +700,8 @@ void LocalDataPoolManager::performPeriodicHkGeneration(HkReceiver& receiver) { if (result != HasReturnvaluesIF::RETURN_OK) { /* Configuration error */ #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "LocalDataPoolManager::performPeriodicHkOperation: HK generation failed." << std::endl; + sif::warning << "LocalDataPoolManager::performPeriodicHkOperation: HK generation failed." + << std::endl; #else sif::printWarning("LocalDataPoolManager::performPeriodicHkOperation: HK generation failed.\n"); #endif diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index f6e8578b..a7ce2870 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1581,8 +1581,7 @@ void DeviceHandlerBase::disableCommandsAndReplies() { if (!reply.second.periodic) { if (reply.second.countdown != nullptr) { reply.second.countdown->timeOut(); - } - else { + } else { reply.second.delayCycles = 0; } reply.second.active = false; diff --git a/src/fsfw/osal/CMakeLists.txt b/src/fsfw/osal/CMakeLists.txt index 62f2a63a..d0aea96a 100644 --- a/src/fsfw/osal/CMakeLists.txt +++ b/src/fsfw/osal/CMakeLists.txt @@ -16,7 +16,9 @@ elseif(FSFW_OSAL MATCHES "host") else() - message(WARNING "${MSG_PREFIX} The FSFW_OSAL variable was not set. Assuming host OS..") + message( + WARNING + "${MSG_PREFIX} The FSFW_OSAL variable was not set. Assuming host OS..") # Not set. Assumuing this is a host build, try to determine host OS if(WIN32) add_subdirectory(host) diff --git a/src/fsfw/osal/host/PeriodicTask.h b/src/fsfw/osal/host/PeriodicTask.h index 63d72b7a..fad588a0 100644 --- a/src/fsfw/osal/host/PeriodicTask.h +++ b/src/fsfw/osal/host/PeriodicTask.h @@ -53,6 +53,7 @@ class PeriodicTask : public PeriodicTaskBase { ReturnValue_t sleepFor(uint32_t ms) override; bool isEmpty() const override; + protected: using chron_ms = std::chrono::milliseconds; bool started; diff --git a/src/fsfw/power/PowerSwitchIF.h b/src/fsfw/power/PowerSwitchIF.h index bc883fbc..c2727158 100644 --- a/src/fsfw/power/PowerSwitchIF.h +++ b/src/fsfw/power/PowerSwitchIF.h @@ -29,9 +29,9 @@ class PowerSwitchIF : public HasReturnvaluesIF { static const ReturnValue_t FUSE_ON = MAKE_RETURN_CODE(3); static const ReturnValue_t FUSE_OFF = MAKE_RETURN_CODE(4); static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PCDU_2; - static const Event SWITCH_WENT_OFF = MAKE_EVENT( - 0, severity::LOW); //!< Someone detected that a switch went off which shouldn't. Severity: - //!< Low, Parameter1: switchId1, Parameter2: switchId2 + //!< Someone detected that a switch went off which shouldn't. Severity: + //!< Low, Parameter1: switchId1, Parameter2: switchId2 + static const Event SWITCH_WENT_OFF = MAKE_EVENT(0, severity::LOW); /** * send a direct command to the Power Unit to enable/disable the specified switch. * diff --git a/src/fsfw/pus/Service3Housekeeping.cpp b/src/fsfw/pus/Service3Housekeeping.cpp index 85c3762f..97addd7c 100644 --- a/src/fsfw/pus/Service3Housekeeping.cpp +++ b/src/fsfw/pus/Service3Housekeeping.cpp @@ -248,16 +248,18 @@ void Service3Housekeeping::handleUnrequestedReply(CommandMessage* reply) { case (HousekeepingMessage::HK_REQUEST_FAILURE): { break; } - case(CommandMessage::REPLY_REJECTED): { + case (CommandMessage::REPLY_REJECTED): { sif::warning << "Service3Housekeeping::handleUnrequestedReply: Unexpected reply " - "rejected with error code" << reply->getParameter() << std::endl; + "rejected with error code" + << reply->getParameter() << std::endl; break; } default: { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "Service3Housekeeping::handleUnrequestedReply: Invalid reply with reply " - "command " << command << "" << std::endl; + "command " + << command << "" << std::endl; #else sif::printWarning( "Service3Housekeeping::handleUnrequestedReply: Invalid reply with " diff --git a/src/fsfw/thermal/ThermalComponentIF.h b/src/fsfw/thermal/ThermalComponentIF.h index 0c50fbad..3a9f3f2d 100644 --- a/src/fsfw/thermal/ThermalComponentIF.h +++ b/src/fsfw/thermal/ThermalComponentIF.h @@ -13,9 +13,9 @@ class ThermalComponentIF : public HasParametersIF { static const Event COMPONENT_TEMP_HIGH = MAKE_EVENT(2, severity::LOW); static const Event COMPONENT_TEMP_OOL_LOW = MAKE_EVENT(3, severity::LOW); static const Event COMPONENT_TEMP_OOL_HIGH = MAKE_EVENT(4, severity::LOW); - static const Event TEMP_NOT_IN_OP_RANGE = MAKE_EVENT( - 5, severity::LOW); //!< Is thrown when a device should start-up, but the temperature is out - //!< of OP range. P1: thermalState of the component, P2: 0 + //!< Is thrown when a device should start-up, but the temperature is out + //!< of OP range. P1: thermalState of the component, P2: 0 + static const Event TEMP_NOT_IN_OP_RANGE = MAKE_EVENT(5, severity::LOW); static const uint8_t INTERFACE_ID = CLASS_ID::THERMAL_COMPONENT_IF; static const ReturnValue_t INVALID_TARGET_STATE = MAKE_RETURN_CODE(1); From 682abd1b5bf9c130ed69fd98ea435d7d6309687a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 4 Jul 2022 10:55:09 +0200 Subject: [PATCH 161/404] new clion folder in misc for .run configs --- {.run => misc/clion/.run}/fsfw-tests_coverage.run.xml | 0 {.run => misc/clion/.run}/fsfw.run.xml | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) rename {.run => misc/clion/.run}/fsfw-tests_coverage.run.xml (100%) rename {.run => misc/clion/.run}/fsfw.run.xml (72%) diff --git a/.run/fsfw-tests_coverage.run.xml b/misc/clion/.run/fsfw-tests_coverage.run.xml similarity index 100% rename from .run/fsfw-tests_coverage.run.xml rename to misc/clion/.run/fsfw-tests_coverage.run.xml diff --git a/.run/fsfw.run.xml b/misc/clion/.run/fsfw.run.xml similarity index 72% rename from .run/fsfw.run.xml rename to misc/clion/.run/fsfw.run.xml index 72f74939..5e35a788 100644 --- a/.run/fsfw.run.xml +++ b/misc/clion/.run/fsfw.run.xml @@ -1,5 +1,5 @@ - + From c5eb09314f3e6e7111762c7e36a773bddd48ed6b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 4 Jul 2022 11:04:37 +0200 Subject: [PATCH 162/404] delete run configs --- misc/clion/.run/fsfw-tests_coverage.run.xml | 7 ------- misc/clion/.run/fsfw.run.xml | 7 ------- 2 files changed, 14 deletions(-) delete mode 100644 misc/clion/.run/fsfw-tests_coverage.run.xml delete mode 100644 misc/clion/.run/fsfw.run.xml diff --git a/misc/clion/.run/fsfw-tests_coverage.run.xml b/misc/clion/.run/fsfw-tests_coverage.run.xml deleted file mode 100644 index 49d9b135..00000000 --- a/misc/clion/.run/fsfw-tests_coverage.run.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/misc/clion/.run/fsfw.run.xml b/misc/clion/.run/fsfw.run.xml deleted file mode 100644 index 5e35a788..00000000 --- a/misc/clion/.run/fsfw.run.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - \ No newline at end of file From 03fa77e2b305d66b65d6db8f1181972b7b2643d2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 Aug 2022 12:29:10 +0200 Subject: [PATCH 163/404] get current uptime correctly --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 8efac940..da977fdc 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -377,6 +377,8 @@ void DeviceHandlerBase::doStateMachine() { setMode(MODE_OFF); break; } + uint32_t currentUptime; + Clock::getUptime(¤tUptime); if (currentUptime - timeoutStart >= powerSwitcher->getSwitchDelayMs()) { triggerEvent(MODE_TRANSITION_FAILED, PowerSwitchIF::SWITCH_TIMEOUT, 0); setMode(MODE_ERROR_ON); From fdcfd89ed271118ddb2daf0de3db5d69bdbc95d4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 12 Aug 2022 12:55:31 +0200 Subject: [PATCH 164/404] add some Linux HAL options --- CMakeLists.txt | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7de32501..79fd3e59 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -118,6 +118,11 @@ option(FSFW_ADD_INTERNAL_TESTS "Add internal unit tests" ON) option(FSFW_ADD_UNITTESTS "Add regular unittests. Requires Catch2" OFF) option(FSFW_ADD_HAL "Add Hardware Abstraction Layer" ON) +if(UNIX) + option(FSFW_HAL_LINUX_ADD_PERIPHERAL_DRIVERS "Add Linux peripheral drivers" OFF) + option(FSFW_HAL_LINUX_ADD_LIBGPIOD "Attempt to add Linux GPIOD drivers" OFF) +endif() + # Optional sources option(FSFW_ADD_PUS "Compile with PUS sources" ON) option(FSFW_ADD_MONITORING "Compile with monitoring components" ON) From f4c4f9946c7fb9fd050178baa4865da654366b70 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 Aug 2022 11:18:53 +0200 Subject: [PATCH 165/404] printout preproc block --- src/fsfw/pus/Service3Housekeeping.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/fsfw/pus/Service3Housekeeping.cpp b/src/fsfw/pus/Service3Housekeeping.cpp index 97addd7c..aa1bf824 100644 --- a/src/fsfw/pus/Service3Housekeeping.cpp +++ b/src/fsfw/pus/Service3Housekeeping.cpp @@ -249,9 +249,12 @@ void Service3Housekeeping::handleUnrequestedReply(CommandMessage* reply) { break; } case (CommandMessage::REPLY_REJECTED): { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "Service3Housekeeping::handleUnrequestedReply: Unexpected reply " "rejected with error code" << reply->getParameter() << std::endl; +#else +#endif break; } From 007f958a0b787a572bf1bd98b348c3a632799bf7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 Aug 2022 11:38:52 +0200 Subject: [PATCH 166/404] fsfw fixes for merge --- src/fsfw/datapoollocal/LocalDataPoolManager.h | 5 +++-- src/fsfw/pus/Service5EventReporting.cpp | 2 +- src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp | 3 ++- src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp | 3 ++- src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp | 3 ++- 5 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/fsfw/datapoollocal/LocalDataPoolManager.h b/src/fsfw/datapoollocal/LocalDataPoolManager.h index 998ca03b..413e00f6 100644 --- a/src/fsfw/datapoollocal/LocalDataPoolManager.h +++ b/src/fsfw/datapoollocal/LocalDataPoolManager.h @@ -243,13 +243,14 @@ class LocalDataPoolManager : public ProvidesDataPoolSubscriptionIF, public Acces LocalDataPoolManager* getPoolManagerHandle() override; ReturnValue_t subscribeForRegularPeriodicPacket(subdp::RegularHkPeriodicParams params) override; ReturnValue_t subscribeForDiagPeriodicPacket(subdp::DiagnosticsHkPeriodicParams params) override; - ReturnValue_t subscribeForPeriodicPacket(subdp::ParamsBase& params); ReturnValue_t subscribeForRegularUpdatePacket(subdp::RegularHkUpdateParams params) override; ReturnValue_t subscribeForDiagUpdatePacket(subdp::DiagnosticsHkUpdateParams params) override; - ReturnValue_t subscribeForUpdatePacket(subdp::ParamsBase& params); protected: + ReturnValue_t subscribeForPeriodicPacket(subdp::ParamsBase& params); + ReturnValue_t subscribeForUpdatePacket(subdp::ParamsBase& params); + /** Core data structure for the actual pool data */ localpool::DataPool localPoolMap; /** Every housekeeping data manager has a mutex to protect access diff --git a/src/fsfw/pus/Service5EventReporting.cpp b/src/fsfw/pus/Service5EventReporting.cpp index 5203ff3d..13bf29c3 100644 --- a/src/fsfw/pus/Service5EventReporting.cpp +++ b/src/fsfw/pus/Service5EventReporting.cpp @@ -13,7 +13,7 @@ Service5EventReporting::Service5EventReporting(PsbParams params, size_t maxNumbe storeHelper(params.apid), tmHelper(params.serviceId, storeHelper, sendHelper), maxNumberReportsPerCycle(maxNumberReportsPerCycle) { - auto mqArgs = MqArgs(objectId, static_cast(this)); + auto mqArgs = MqArgs(getObjectId(), static_cast(this)); eventQueue = QueueFactory::instance()->createMessageQueue( messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } diff --git a/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp b/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp index 3dd19275..5ee7f978 100644 --- a/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp +++ b/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp @@ -252,7 +252,8 @@ ReturnValue_t GyroHandlerL3GD20H::initializeLocalDataPool(localpool::DataPool &l localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, new PoolEntry({0.0})); localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, new PoolEntry({0.0})); localDataPoolMap.emplace(L3GD20H::TEMPERATURE, new PoolEntry({0.0})); - poolManager.subscribeForPeriodicPacket(dataset.getSid(), false, 10.0, false); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(dataset.getSid(), false, 10.0)); return HasReturnvaluesIF::RETURN_OK; } diff --git a/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp b/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp index e1b1294f..e366760d 100644 --- a/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp +++ b/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp @@ -469,7 +469,8 @@ ReturnValue_t MgmLIS3MDLHandler::initializeLocalDataPool(localpool::DataPool &lo LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTHS, &mgmXYZ); localDataPoolMap.emplace(MGMLIS3MDL::TEMPERATURE_CELCIUS, &temperature); - poolManager.subscribeForPeriodicPacket(dataset.getSid(), false, 10.0, false); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(dataset.getSid(), false, 10.0)); return HasReturnvaluesIF::RETURN_OK; } diff --git a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp index c329f5a6..b39ac852 100644 --- a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp +++ b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp @@ -310,7 +310,8 @@ void MgmRM3100Handler::modeChanged(void) { internalState = InternalState::NONE; ReturnValue_t MgmRM3100Handler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { localDataPoolMap.emplace(RM3100::FIELD_STRENGTHS, &mgmXYZ); - poolManager.subscribeForPeriodicPacket(primaryDataset.getSid(), false, 10.0, false); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(primaryDataset.getSid(), false, 10.0)); return HasReturnvaluesIF::RETURN_OK; } From 4d82d0e4c15091aceafc8279790517702d5941f4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 Aug 2022 17:24:48 +0200 Subject: [PATCH 167/404] update source sequence counter code --- src/fsfw/tmtcservices/SourceSequenceCounter.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fsfw/tmtcservices/SourceSequenceCounter.h b/src/fsfw/tmtcservices/SourceSequenceCounter.h index 4b33fb0e..9b8e85d1 100644 --- a/src/fsfw/tmtcservices/SourceSequenceCounter.h +++ b/src/fsfw/tmtcservices/SourceSequenceCounter.h @@ -10,14 +10,14 @@ class SourceSequenceCounter { public: SourceSequenceCounter(uint16_t initialSequenceCount = 0) : sequenceCount(initialSequenceCount) {} void increment() { - sequenceCount = (sequenceCount + 1) % (SpacePacketBase::LIMIT_SEQUENCE_COUNT); + sequenceCount = (sequenceCount + 1) % (ccsds::LIMIT_SEQUENCE_COUNT); } void decrement() { - sequenceCount = (sequenceCount - 1) % (SpacePacketBase::LIMIT_SEQUENCE_COUNT); + sequenceCount = (sequenceCount - 1) % (ccsds::LIMIT_SEQUENCE_COUNT); } uint16_t get() { return this->sequenceCount; } void reset(uint16_t toValue = 0) { - sequenceCount = toValue % (SpacePacketBase::LIMIT_SEQUENCE_COUNT); + sequenceCount = toValue % (ccsds::LIMIT_SEQUENCE_COUNT); } SourceSequenceCounter& operator++(int) { this->increment(); From d815f422c394bb87ccf9e9d306cb42bd01f162bc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 16 Aug 2022 17:09:22 +0200 Subject: [PATCH 168/404] improve verif reporter API --- src/fsfw/tmtcservices/VerificationReporter.cpp | 2 +- src/fsfw/tmtcservices/VerificationReporter.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fsfw/tmtcservices/VerificationReporter.cpp b/src/fsfw/tmtcservices/VerificationReporter.cpp index 2eb0c771..cce8f95d 100644 --- a/src/fsfw/tmtcservices/VerificationReporter.cpp +++ b/src/fsfw/tmtcservices/VerificationReporter.cpp @@ -7,7 +7,7 @@ object_id_t VerificationReporter::DEFAULT_RECEIVER = objects::PUS_SERVICE_1_VERIFICATION; object_id_t VerificationReporter::DEFAULT_REPORTER = objects::TC_VERIFICATOR; -VerificationReporter::VerificationReporter(AcceptsVerifyMessageIF* receiver, object_id_t objectId) +VerificationReporter::VerificationReporter(object_id_t objectId, AcceptsVerifyMessageIF* receiver) : SystemObject(objectId) { if (receiver != nullptr) { acknowledgeQueue = receiver->getVerificationQueue(); diff --git a/src/fsfw/tmtcservices/VerificationReporter.h b/src/fsfw/tmtcservices/VerificationReporter.h index 78c60962..5fb67f74 100644 --- a/src/fsfw/tmtcservices/VerificationReporter.h +++ b/src/fsfw/tmtcservices/VerificationReporter.h @@ -27,8 +27,8 @@ class VerificationReporter : public SystemObject, public VerificationReporterIF friend void Factory::setStaticFrameworkObjectIds(); public: - explicit VerificationReporter(AcceptsVerifyMessageIF* receiver, - object_id_t objectId = DEFAULT_REPORTER); + explicit VerificationReporter(object_id_t objectId = DEFAULT_REPORTER, + AcceptsVerifyMessageIF* receiver = nullptr); ~VerificationReporter() override; void setReceiver(AcceptsVerifyMessageIF& receiver); From 72058853578c051aca09f17107a5a11f18f865ac Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 16 Aug 2022 17:47:56 +0200 Subject: [PATCH 169/404] added additional API for HK subscription params --- src/fsfw/datapoollocal/ProvidesDataPoolSubscriptionIF.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/fsfw/datapoollocal/ProvidesDataPoolSubscriptionIF.h b/src/fsfw/datapoollocal/ProvidesDataPoolSubscriptionIF.h index c350bb60..de964876 100644 --- a/src/fsfw/datapoollocal/ProvidesDataPoolSubscriptionIF.h +++ b/src/fsfw/datapoollocal/ProvidesDataPoolSubscriptionIF.h @@ -28,11 +28,17 @@ struct ParamsBase { }; struct RegularHkPeriodicParams : public ParamsBase { + RegularHkPeriodicParams(sid_t sid, float collectionInterval) + : ParamsBase(sid, false, collectionInterval, false) {} + RegularHkPeriodicParams(sid_t sid, bool enableReporting, float collectionInterval) : ParamsBase(sid, enableReporting, collectionInterval, false) {} }; struct DiagnosticsHkPeriodicParams : public ParamsBase { + DiagnosticsHkPeriodicParams(sid_t sid, float collectionInterval) + : ParamsBase(sid, false, collectionInterval, true) {} + DiagnosticsHkPeriodicParams(sid_t sid, bool enableReporting, float collectionInterval) : ParamsBase(sid, enableReporting, collectionInterval, true) {} }; From 7881f5bab86212035b94f4995551d75e843174b5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 18 Aug 2022 11:19:42 +0200 Subject: [PATCH 170/404] important bugfix for verif reporter --- src/fsfw/tcdistribution/PusDistributor.cpp | 5 ++--- src/fsfw/tmtcservices/VerificationReporter.h | 5 +---- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/src/fsfw/tcdistribution/PusDistributor.cpp b/src/fsfw/tcdistribution/PusDistributor.cpp index bb8708cd..0ae36c88 100644 --- a/src/fsfw/tcdistribution/PusDistributor.cpp +++ b/src/fsfw/tcdistribution/PusDistributor.cpp @@ -105,14 +105,13 @@ ReturnValue_t PusDistributor::callbackAfterSending(ReturnValue_t queueStatus) { tcStatus = queueStatus; } if (tcStatus != RETURN_OK) { - verifyChannel->sendFailureReport( - VerifFailureParams(tcverif::ACCEPTANCE_FAILURE, reader, tcStatus)); + verifyChannel->sendFailureReport({tcverif::ACCEPTANCE_FAILURE, reader, tcStatus}); // A failed packet is deleted immediately after reporting, // otherwise it will block memory. store->deleteData(currentMessage.getStorageId()); return RETURN_FAILED; } else { - verifyChannel->sendSuccessReport(VerifSuccessParams(tcverif::ACCEPTANCE_SUCCESS, reader)); + verifyChannel->sendSuccessReport({tcverif::ACCEPTANCE_SUCCESS, reader}); return RETURN_OK; } } diff --git a/src/fsfw/tmtcservices/VerificationReporter.h b/src/fsfw/tmtcservices/VerificationReporter.h index 5fb67f74..f6152614 100644 --- a/src/fsfw/tmtcservices/VerificationReporter.h +++ b/src/fsfw/tmtcservices/VerificationReporter.h @@ -33,10 +33,7 @@ class VerificationReporter : public SystemObject, public VerificationReporterIF void setReceiver(AcceptsVerifyMessageIF& receiver); - // TODO: The API is a little bit bloated. It might be better to group all the parameters - // into a dedicated struct ReturnValue_t sendSuccessReport(VerifSuccessParams params) override; - ReturnValue_t sendFailureReport(VerifFailureParams params) override; static object_id_t DEFAULT_REPORTER; @@ -44,7 +41,7 @@ class VerificationReporter : public SystemObject, public VerificationReporterIF ReturnValue_t initialize() override; private: - MessageQueueId_t acknowledgeQueue; + MessageQueueId_t acknowledgeQueue = MessageQueueIF::NO_QUEUE; }; #endif /* FSFW_TMTCSERVICES_VERIFICATIONREPORTER_H_ */ From f5866ddacee6cd0f381fb1a69f1d0cf22b5b310a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 24 Aug 2022 17:25:45 +0200 Subject: [PATCH 171/404] missing replacements --- src/fsfw/events/EventManager.cpp | 2 +- src/fsfw/globalfunctions/DleParser.cpp | 14 +++++++------- src/fsfw/globalfunctions/DleParser.h | 4 ++-- src/fsfw/health/HasHealthIF.h | 6 +++--- src/fsfw/parameters/HasParametersIF.h | 2 +- src/fsfw/pus/Service3Housekeeping.cpp | 2 +- src/fsfw/tmtcservices/CommandingServiceBase.cpp | 2 +- src/fsfw_hal/linux/spi/ManualCsLockGuard.h | 10 +++++----- .../integration/devices/TestDeviceHandler.cpp | 2 +- unittests/datapoollocal/testDataSet.cpp | 2 +- unittests/osal/TestSemaphore.cpp | 4 ++-- 11 files changed, 25 insertions(+), 25 deletions(-) diff --git a/src/fsfw/events/EventManager.cpp b/src/fsfw/events/EventManager.cpp index 366a4265..3cb80776 100644 --- a/src/fsfw/events/EventManager.cpp +++ b/src/fsfw/events/EventManager.cpp @@ -51,7 +51,7 @@ void EventManager::notifyListeners(EventMessage* message) { if (listener.second.match(message)) { ReturnValue_t result = MessageQueueSenderIF::sendMessage(listener.first, message, message->getSender()); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::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) diff --git a/src/fsfw/globalfunctions/DleParser.cpp b/src/fsfw/globalfunctions/DleParser.cpp index f326f837..44c75bab 100644 --- a/src/fsfw/globalfunctions/DleParser.cpp +++ b/src/fsfw/globalfunctions/DleParser.cpp @@ -23,12 +23,12 @@ DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPa ReturnValue_t DleParser::passData(uint8_t* data, size_t len) { if (data == nullptr or len == 0 or handler == nullptr) { - return RETURN_FAILED; + return returnvalue::FAILED; } size_t copyIntoRingBufFromHere = 0; size_t copyAmount = len; size_t startIdx = 0; - ReturnValue_t result = RETURN_OK; + ReturnValue_t result = returnvalue::OK; bool startFoundInThisPacket = false; for (size_t idx = 0; idx < len; idx++) { if (data[idx] == DleEncoder::STX_CHAR) { @@ -54,7 +54,7 @@ ReturnValue_t DleParser::passData(uint8_t* data, size_t len) { size_t decodedLen = 0; result = decoder.decode(data + startIdx, idx + 1 - startIdx, &readLen, decodedBuf.first, decodedBuf.second, &decodedLen); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { ctx.setType(ContextType::PACKET_FOUND); ctx.decodedPacket.first = decodedBuf.first; ctx.decodedPacket.second = decodedLen; @@ -81,7 +81,7 @@ ReturnValue_t DleParser::passData(uint8_t* data, size_t len) { // ETX found but STX was found in another mini packet. Reconstruct the full packet // to decode it result = decodeRingBuf.writeData(data, idx + 1); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { ErrorInfo info; info.res = result; prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info); @@ -100,7 +100,7 @@ ReturnValue_t DleParser::passData(uint8_t* data, size_t len) { decodeRingBuf.readData(encodedBuf.first, fullEncodedLen, true); result = decoder.decode(encodedBuf.first, fullEncodedLen, &readLen, decodedBuf.first, decodedBuf.second, &decodedLen); - if (result == HasReturnvaluesIF::RETURN_OK) { + if (result == returnvalue::OK) { if (this->handler != nullptr) { ctx.setType(ContextType::PACKET_FOUND); ctx.decodedPacket.first = decodedBuf.first; @@ -148,14 +148,14 @@ ReturnValue_t DleParser::passData(uint8_t* data, size_t len) { } if (copyAmount > 0) { result = decodeRingBuf.writeData(data + copyIntoRingBufFromHere, copyAmount); - if (result != HasReturnvaluesIF::RETURN_OK) { + if (result != returnvalue::OK) { ErrorInfo info; info.res = result; prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info); handler(ctx); } } - return RETURN_OK; + return returnvalue::OK; } void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* args) { diff --git a/src/fsfw/globalfunctions/DleParser.h b/src/fsfw/globalfunctions/DleParser.h index e8ee61d9..f1a4f193 100644 --- a/src/fsfw/globalfunctions/DleParser.h +++ b/src/fsfw/globalfunctions/DleParser.h @@ -2,7 +2,7 @@ #include #include -#include +#include #include #include @@ -16,7 +16,7 @@ * overriding two provided virtual methods. This also allows detecting multiple DLE packets * inside one passed packet. */ -class DleParser : public HasReturnvaluesIF { +class DleParser { public: using BufPair = std::pair; diff --git a/src/fsfw/health/HasHealthIF.h b/src/fsfw/health/HasHealthIF.h index 3fd94840..bf696e15 100644 --- a/src/fsfw/health/HasHealthIF.h +++ b/src/fsfw/health/HasHealthIF.h @@ -17,11 +17,11 @@ class HasHealthIF { static const uint8_t INTERFACE_ID = CLASS_ID::HAS_HEALTH_IF; static constexpr ReturnValue_t OBJECT_NOT_HEALTHY = - HasReturnvaluesIF::makeReturnCode(INTERFACE_ID, 1); + returnvalue::makeCode(INTERFACE_ID, 1); static constexpr ReturnValue_t INVALID_HEALTH_STATE = - HasReturnvaluesIF::makeReturnCode(INTERFACE_ID, 2); + returnvalue::makeCode(INTERFACE_ID, 2); static constexpr ReturnValue_t IS_EXTERNALLY_CONTROLLED = - HasReturnvaluesIF::makeReturnCode(INTERFACE_ID, 3); + returnvalue::makeCode(INTERFACE_ID, 3); static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SYSTEM_MANAGER_1; //! P1: New Health, P2: Old Health diff --git a/src/fsfw/parameters/HasParametersIF.h b/src/fsfw/parameters/HasParametersIF.h index 5125d248..5d70c328 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_OK if parameter is valid and a set function of the parameter wrapper was called. + * @return returnvalue::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/pus/Service3Housekeeping.cpp b/src/fsfw/pus/Service3Housekeeping.cpp index 82b4e848..5c887572 100644 --- a/src/fsfw/pus/Service3Housekeeping.cpp +++ b/src/fsfw/pus/Service3Housekeeping.cpp @@ -208,7 +208,7 @@ ReturnValue_t Service3Housekeeping::handleReply(const CommandMessage* reply, ReturnValue_t error = returnvalue::FAILED; HousekeepingMessage::getHkRequestFailureReply(reply, &error); failureParameter2 = error; - return RETURN_FAILED; + return returnvalue::FAILED; } default: diff --git a/src/fsfw/tmtcservices/CommandingServiceBase.cpp b/src/fsfw/tmtcservices/CommandingServiceBase.cpp index c6f8085b..d3314fa6 100644 --- a/src/fsfw/tmtcservices/CommandingServiceBase.cpp +++ b/src/fsfw/tmtcservices/CommandingServiceBase.cpp @@ -223,7 +223,7 @@ void CommandingServiceBase::handleReplyHandlerResult(ReturnValue_t result, Comma // In case a new command is to be sent immediately, this is performed here. // If no new command is sent, only analyse reply result by initializing - // sendResult as RETURN_OK + // sendResult as returnvalue::OK ReturnValue_t sendResult = returnvalue::OK; if (nextCommand->getCommand() != CommandMessage::CMD_NONE) { sendResult = commandQueue->sendMessage(reply->getSender(), nextCommand); diff --git a/src/fsfw_hal/linux/spi/ManualCsLockGuard.h b/src/fsfw_hal/linux/spi/ManualCsLockGuard.h index b282bcc0..feb6dd83 100644 --- a/src/fsfw_hal/linux/spi/ManualCsLockGuard.h +++ b/src/fsfw_hal/linux/spi/ManualCsLockGuard.h @@ -1,10 +1,10 @@ #pragma once #include "fsfw/ipc/MutexIF.h" -#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/returnvalues/returnvalue.h" #include "fsfw_hal/common/gpio/GpioIF.h" -class ManualCsLockWrapper : public HasReturnvaluesIF { +class ManualCsLockWrapper { public: ManualCsLockWrapper(MutexIF* lock, GpioIF* gpioIF, SpiCookie* cookie, MutexIF::TimeoutType type = MutexIF::TimeoutType::BLOCKING, @@ -16,18 +16,18 @@ class ManualCsLockWrapper : public HasReturnvaluesIF { } cookie->setCsLockManual(true); lockResult = lock->lockMutex(type, timeoutMs); - if (lockResult != RETURN_OK) { + if (lockResult != returnvalue::OK) { return; } gpioResult = gpioIF->pullLow(cookie->getChipSelectPin()); } ~ManualCsLockWrapper() { - if (gpioResult == RETURN_OK) { + if (gpioResult == returnvalue::OK) { gpioIF->pullHigh(cookie->getChipSelectPin()); } cookie->setCsLockManual(false); - if (lockResult == RETURN_OK) { + if (lockResult == returnvalue::OK) { lock->unlockMutex(); } } diff --git a/src/fsfw_tests/integration/devices/TestDeviceHandler.cpp b/src/fsfw_tests/integration/devices/TestDeviceHandler.cpp index 01f0494b..fdf02a70 100644 --- a/src/fsfw_tests/integration/devices/TestDeviceHandler.cpp +++ b/src/fsfw_tests/integration/devices/TestDeviceHandler.cpp @@ -646,7 +646,7 @@ ReturnValue_t TestDevice::initializeLocalDataPool(localpool::DataPool& localData /* Subscribe for periodic HK packets but do not enable reporting for now. Non-diangostic with a period of one second */ poolManager.subscribeForRegularPeriodicPacket({sid, false, 1.0}); - return HasReturnvaluesIF::RETURN_OK; + return returnvalue::OK; } ReturnValue_t TestDevice::getParameter(uint8_t domainId, uint8_t uniqueId, diff --git a/unittests/datapoollocal/testDataSet.cpp b/unittests/datapoollocal/testDataSet.cpp index def92ac3..8bc0abb7 100644 --- a/unittests/datapoollocal/testDataSet.cpp +++ b/unittests/datapoollocal/testDataSet.cpp @@ -269,7 +269,7 @@ TEST_CASE("DataSetTest", "[DataSetTest]") { { // PoolReadGuard rg(&sharedSet); - // CHECK(rg.getReadResult() == result::OK); + // CHECK(rg.getReadResult() == returnvalue::OK); localSet.localPoolVarUint8.value = 5; localSet.localPoolUint16Vec.value[0] = 1; localSet.localPoolUint16Vec.value[1] = 2; diff --git a/unittests/osal/TestSemaphore.cpp b/unittests/osal/TestSemaphore.cpp index 376b08db..550c60b7 100644 --- a/unittests/osal/TestSemaphore.cpp +++ b/unittests/osal/TestSemaphore.cpp @@ -19,7 +19,7 @@ TEST_CASE("Binary Semaphore Test" , "[BinSemaphore]") { REQUIRE(binSemaph->release() == static_cast(SemaphoreIF::SEMAPHORE_NOT_OWNED)); REQUIRE(binSemaph->acquire(SemaphoreIF::POLLING) == - result::OK); + returnvalue::OK); { // not precise enough on linux.. should use clock instead.. //Stopwatch stopwatch(false); @@ -29,7 +29,7 @@ TEST_CASE("Binary Semaphore Test" , "[BinSemaphore]") { //CHECK(time == 5); } REQUIRE(binSemaph->getSemaphoreCounter() == 0); - REQUIRE(binSemaph->release() == result::OK); + REQUIRE(binSemaph->release() == returnvalue::OK); } SemaphoreFactory::instance()->deleteSemaphore(binSemaph); // perform tear-down here From 2a75440b325b70b7ca90269159440a1f9a4a6e2a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 27 Aug 2022 01:01:29 +0200 Subject: [PATCH 172/404] allow device tm in raw format --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 61 +++++++++++++++++++ src/fsfw/devicehandlers/DeviceHandlerBase.h | 4 +- src/fsfw/health/HasHealthIF.h | 9 +-- src/fsfw/parameters/HasParametersIF.h | 3 +- src/fsfw/tmtcservices/SourceSequenceCounter.h | 12 +--- 5 files changed, 71 insertions(+), 18 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 1b64e1e2..15dbca97 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1325,6 +1325,67 @@ void DeviceHandlerBase::handleDeviceTM(SerializeIF* dataSet, DeviceCommandId_t r } } +void DeviceHandlerBase::handleDeviceTM(const uint8_t* data, size_t dataSize, + DeviceCommandId_t replyId, bool forceDirectTm) { + // TODO: Horrible duplicate code. Avoiding this would require using the Serializable union + // type. Furthermore, DeviceTmReportingWrapper needs to be extended to allow using raw + // buffers. + if (data == nullptr) { + return; + } + + DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId); + if (iter == deviceReplyMap.end()) { + triggerEvent(DEVICE_UNKNOWN_REPLY, replyId); + return; + } + + /* Regular replies to a command */ + if (iter->second.command != deviceCommandMap.end()) { + MessageQueueId_t queueId = iter->second.command->second.sendReplyTo; + + if (queueId != NO_COMMANDER) { + /* This may fail, but we'll ignore the fault. */ + actionHelper.reportData(queueId, replyId, data, dataSize); + } + + /* This check should make sure we get any TM but don't get anything doubled. */ + // TODO: The wrapper does not support this.. + // if (wiretappingMode == TM && (requestedRawTraffic != queueId)) { + // DeviceTmReportingWrapper wrapper(getObjectId(), replyId, data, dataSize); + // actionHelper.reportData(requestedRawTraffic, replyId, &wrapper); + // } + + else if (forceDirectTm and (defaultRawReceiver != queueId) and + (defaultRawReceiver != MessageQueueIF::NO_QUEUE)) { + // hiding of sender needed so the service will handle it as + // unexpected Data, no matter what state (progress or completed) + // it is in + actionHelper.reportData(defaultRawReceiver, replyId, data, dataSize, true); + } + } + /* Unrequested or aperiodic replies */ + // TODO: The wrapper does not support this.. + // else { + // DeviceTmReportingWrapper wrapper(getObjectId(), replyId, data, dataSize); + // if (wiretappingMode == TM) { + // actionHelper.reportData(requestedRawTraffic, replyId, &wrapper); + // } + // if (forceDirectTm and defaultRawReceiver != MessageQueueIF::NO_QUEUE) { + // // sid_t setSid = sid_t(this->getObjectId(), replyId); + // // LocalPoolDataSetBase* dataset = getDataSetHandle(setSid); + // // if(dataset != nullptr) { + // // poolManager.generateHousekeepingPacket(setSid, dataset, true); + // // } + // + // // hiding of sender needed so the service will handle it as + // // unexpected Data, no matter what state (progress or completed) + // // it is in + // actionHelper.reportData(defaultRawReceiver, replyId, &wrapper, true); + // } + // } +} + ReturnValue_t DeviceHandlerBase::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, size_t size) { ReturnValue_t result = acceptExternalDeviceCommands(); diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index 8864c821..7f7c2751 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -1070,8 +1070,8 @@ class DeviceHandlerBase : public DeviceHandlerIF, bool isAwaitingReply(); void handleDeviceTM(SerializeIF *dataSet, DeviceCommandId_t replyId, bool forceDirectTm = false); - // void handleDeviceTM(uint8_t* data, size_t dataSize, DeviceCommandId_t replyId, - // bool forceDirectTm); + void handleDeviceTM(const uint8_t *data, size_t dataSize, DeviceCommandId_t replyId, + bool forceDirectTm = false); virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, uint32_t *msToReachTheMode); diff --git a/src/fsfw/health/HasHealthIF.h b/src/fsfw/health/HasHealthIF.h index bf696e15..16666bbc 100644 --- a/src/fsfw/health/HasHealthIF.h +++ b/src/fsfw/health/HasHealthIF.h @@ -16,12 +16,9 @@ class HasHealthIF { }; static const uint8_t INTERFACE_ID = CLASS_ID::HAS_HEALTH_IF; - static constexpr ReturnValue_t OBJECT_NOT_HEALTHY = - returnvalue::makeCode(INTERFACE_ID, 1); - static constexpr ReturnValue_t INVALID_HEALTH_STATE = - returnvalue::makeCode(INTERFACE_ID, 2); - static constexpr ReturnValue_t IS_EXTERNALLY_CONTROLLED = - returnvalue::makeCode(INTERFACE_ID, 3); + static constexpr ReturnValue_t OBJECT_NOT_HEALTHY = returnvalue::makeCode(INTERFACE_ID, 1); + static constexpr ReturnValue_t INVALID_HEALTH_STATE = returnvalue::makeCode(INTERFACE_ID, 2); + static constexpr ReturnValue_t IS_EXTERNALLY_CONTROLLED = returnvalue::makeCode(INTERFACE_ID, 3); static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::SYSTEM_MANAGER_1; //! P1: New Health, P2: Old Health diff --git a/src/fsfw/parameters/HasParametersIF.h b/src/fsfw/parameters/HasParametersIF.h index 5d70c328..8dc99644 100644 --- a/src/fsfw/parameters/HasParametersIF.h +++ b/src/fsfw/parameters/HasParametersIF.h @@ -66,7 +66,8 @@ class HasParametersIF { * @param newValues * @param startAtIndex Linear index, runs left to right, top to bottom for * matrix indexes. - * @return returnvalue::OK if parameter is valid and a set function of the parameter wrapper was called. + * @return returnvalue::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/tmtcservices/SourceSequenceCounter.h b/src/fsfw/tmtcservices/SourceSequenceCounter.h index 9b8e85d1..43b6945c 100644 --- a/src/fsfw/tmtcservices/SourceSequenceCounter.h +++ b/src/fsfw/tmtcservices/SourceSequenceCounter.h @@ -9,16 +9,10 @@ class SourceSequenceCounter { public: SourceSequenceCounter(uint16_t initialSequenceCount = 0) : sequenceCount(initialSequenceCount) {} - void increment() { - sequenceCount = (sequenceCount + 1) % (ccsds::LIMIT_SEQUENCE_COUNT); - } - void decrement() { - sequenceCount = (sequenceCount - 1) % (ccsds::LIMIT_SEQUENCE_COUNT); - } + void increment() { sequenceCount = (sequenceCount + 1) % (ccsds::LIMIT_SEQUENCE_COUNT); } + void decrement() { sequenceCount = (sequenceCount - 1) % (ccsds::LIMIT_SEQUENCE_COUNT); } uint16_t get() { return this->sequenceCount; } - void reset(uint16_t toValue = 0) { - sequenceCount = toValue % (ccsds::LIMIT_SEQUENCE_COUNT); - } + void reset(uint16_t toValue = 0) { sequenceCount = toValue % (ccsds::LIMIT_SEQUENCE_COUNT); } SourceSequenceCounter& operator++(int) { this->increment(); return *this; From 9a590a3fcd81ed4dd48de503522b6d71f64205d2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 29 Aug 2022 12:01:44 +0200 Subject: [PATCH 173/404] additional safety check --- src/fsfw/pus/Service11TelecommandScheduling.tpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/fsfw/pus/Service11TelecommandScheduling.tpp b/src/fsfw/pus/Service11TelecommandScheduling.tpp index 3e48be0c..47135155 100644 --- a/src/fsfw/pus/Service11TelecommandScheduling.tpp +++ b/src/fsfw/pus/Service11TelecommandScheduling.tpp @@ -581,10 +581,12 @@ inline ReturnValue_t Service11TelecommandScheduling::getMapFilterFr } // additional security check, this should never be true - if (itBegin->first > itEnd->first) { + if ((itBegin != telecommandMap.end() and itEnd != telecommandMap.end()) and + (itBegin->first > itEnd->first)) { #if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "11::getMapFilterFromData: itBegin > itEnd\n" << std::endl; #else - sif::printError("11::GetMapFilterFromData: itBegin > itEnd\n"); + sif::printError("11::getMapFilterFromData: itBegin > itEnd\n"); #endif return returnvalue::FAILED; } From 3a47062f2a5432be95bdce8f380eaab7347e03fd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 Aug 2022 13:39:21 +0200 Subject: [PATCH 174/404] refactored dhb TM handler --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 38 ++++++++++--------- src/fsfw/devicehandlers/DeviceHandlerBase.h | 6 +-- .../DeviceTmReportingWrapper.cpp | 33 +++++++++------- .../devicehandlers/DeviceTmReportingWrapper.h | 26 +++++++------ 4 files changed, 57 insertions(+), 46 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 525a3dcc..66b673c8 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1257,30 +1257,40 @@ ReturnValue_t DeviceHandlerBase::letChildHandleMessage(CommandMessage* message) return returnvalue::FAILED; } -void DeviceHandlerBase::handleDeviceTM(SerializeIF* dataSet, DeviceCommandId_t replyId, +void DeviceHandlerBase::handleDeviceTm(util::DataWrapper dataWrapper, DeviceCommandId_t replyId, bool forceDirectTm) { - if (dataSet == nullptr) { + if (dataWrapper.isNull()) { return; } - DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId); + auto iter = deviceReplyMap.find(replyId); if (iter == deviceReplyMap.end()) { triggerEvent(DEVICE_UNKNOWN_REPLY, replyId); return; } - /* Regular replies to a command */ + auto reportData = [&](MessageQueueId_t queueId) { + if (dataWrapper.type == util::DataTypes::SERIALIZABLE) { + return actionHelper.reportData(queueId, replyId, dataWrapper.dataUnion.serializable); + } else if (dataWrapper.type == util::DataTypes::RAW) { + return actionHelper.reportData(queueId, replyId, dataWrapper.dataUnion.raw.data, + dataWrapper.dataUnion.raw.len); + } + return returnvalue::FAILED; + }; + + // Regular replies to a command if (iter->second.command != deviceCommandMap.end()) { MessageQueueId_t queueId = iter->second.command->second.sendReplyTo; + // This may fail, but we'll ignore the fault. if (queueId != NO_COMMANDER) { - /* This may fail, but we'll ignore the fault. */ - actionHelper.reportData(queueId, replyId, dataSet); + reportData(queueId); } - /* This check should make sure we get any TM but don't get anything doubled. */ + // This check should make sure we get any TM but don't get anything doubled. if (wiretappingMode == TM && (requestedRawTraffic != queueId)) { - DeviceTmReportingWrapper wrapper(getObjectId(), replyId, dataSet); + DeviceTmReportingWrapper wrapper(getObjectId(), replyId, dataWrapper); actionHelper.reportData(requestedRawTraffic, replyId, &wrapper); } @@ -1289,22 +1299,16 @@ void DeviceHandlerBase::handleDeviceTM(SerializeIF* dataSet, DeviceCommandId_t r // hiding of sender needed so the service will handle it as // unexpected Data, no matter what state (progress or completed) // it is in - actionHelper.reportData(defaultRawReceiver, replyId, dataSet, true); + reportData(defaultRawReceiver); } } - /* Unrequested or aperiodic replies */ + // Unrequested or aperiodic replies else { - DeviceTmReportingWrapper wrapper(getObjectId(), replyId, dataSet); + DeviceTmReportingWrapper wrapper(getObjectId(), replyId, dataWrapper); if (wiretappingMode == TM) { actionHelper.reportData(requestedRawTraffic, replyId, &wrapper); } if (forceDirectTm and defaultRawReceiver != MessageQueueIF::NO_QUEUE) { - // sid_t setSid = sid_t(this->getObjectId(), replyId); - // LocalPoolDataSetBase* dataset = getDataSetHandle(setSid); - // if(dataset != nullptr) { - // poolManager.generateHousekeepingPacket(setSid, dataset, true); - // } - // hiding of sender needed so the service will handle it as // unexpected Data, no matter what state (progress or completed) // it is in diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index a20eae0c..48f9fdbf 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -23,6 +23,7 @@ #include "fsfw/serviceinterface/serviceInterfaceDefintions.h" #include "fsfw/tasks/ExecutableObjectIF.h" #include "fsfw/tasks/PeriodicTaskIF.h" +#include "fsfw/util/dataWrapper.h" namespace Factory { void setStaticFrameworkObjectIds(); @@ -1052,9 +1053,8 @@ class DeviceHandlerBase : public DeviceHandlerIF, bool isAwaitingReply(); - void handleDeviceTM(SerializeIF *dataSet, DeviceCommandId_t replyId, bool forceDirectTm = false); - // void handleDeviceTM(uint8_t* data, size_t dataSize, DeviceCommandId_t replyId, - // bool forceDirectTm); + void handleDeviceTm(util::DataWrapper dataWrapper, DeviceCommandId_t replyId, + bool forceDirectTm = false); virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, uint32_t *msToReachTheMode); diff --git a/src/fsfw/devicehandlers/DeviceTmReportingWrapper.cpp b/src/fsfw/devicehandlers/DeviceTmReportingWrapper.cpp index dc987e6d..1f661cd3 100644 --- a/src/fsfw/devicehandlers/DeviceTmReportingWrapper.cpp +++ b/src/fsfw/devicehandlers/DeviceTmReportingWrapper.cpp @@ -3,10 +3,10 @@ #include "fsfw/serialize/SerializeAdapter.h" DeviceTmReportingWrapper::DeviceTmReportingWrapper(object_id_t objectId, ActionId_t actionId, - SerializeIF* data) - : objectId(objectId), actionId(actionId), data(data) {} + util::DataWrapper data) + : objectId(objectId), actionId(actionId), dataWrapper(data) {} -DeviceTmReportingWrapper::~DeviceTmReportingWrapper() {} +DeviceTmReportingWrapper::~DeviceTmReportingWrapper() = default; ReturnValue_t DeviceTmReportingWrapper::serialize(uint8_t** buffer, size_t* size, size_t maxSize, Endianness streamEndianness) const { @@ -19,22 +19,27 @@ ReturnValue_t DeviceTmReportingWrapper::serialize(uint8_t** buffer, size_t* size if (result != returnvalue::OK) { return result; } - return data->serialize(buffer, size, maxSize, streamEndianness); + if (dataWrapper.isNull()) { + return returnvalue::FAILED; + } + if (dataWrapper.type == util::DataTypes::SERIALIZABLE) { + return dataWrapper.dataUnion.serializable->serialize(buffer, size, maxSize, streamEndianness); + } else if (dataWrapper.type == util::DataTypes::RAW) { + if (*size + dataWrapper.dataUnion.raw.len > maxSize) { + return SerializeIF::BUFFER_TOO_SHORT; + } + std::memcpy(*buffer, dataWrapper.dataUnion.raw.data, dataWrapper.dataUnion.raw.len); + *buffer += dataWrapper.dataUnion.raw.len; + *size += dataWrapper.dataUnion.raw.len; + } + return returnvalue::OK; } size_t DeviceTmReportingWrapper::getSerializedSize() const { - return sizeof(objectId) + sizeof(ActionId_t) + data->getSerializedSize(); + return sizeof(objectId) + sizeof(ActionId_t) + dataWrapper.getLength(); } ReturnValue_t DeviceTmReportingWrapper::deSerialize(const uint8_t** buffer, size_t* size, Endianness streamEndianness) { - ReturnValue_t result = SerializeAdapter::deSerialize(&objectId, buffer, size, streamEndianness); - if (result != returnvalue::OK) { - return result; - } - result = SerializeAdapter::deSerialize(&actionId, buffer, size, streamEndianness); - if (result != returnvalue::OK) { - return result; - } - return data->deSerialize(buffer, size, streamEndianness); + return returnvalue::FAILED; } diff --git a/src/fsfw/devicehandlers/DeviceTmReportingWrapper.h b/src/fsfw/devicehandlers/DeviceTmReportingWrapper.h index 71c64453..236e2d1e 100644 --- a/src/fsfw/devicehandlers/DeviceTmReportingWrapper.h +++ b/src/fsfw/devicehandlers/DeviceTmReportingWrapper.h @@ -1,27 +1,29 @@ #ifndef FSFW_DEVICEHANDLERS_DEVICETMREPORTINGWRAPPER_H_ #define FSFW_DEVICEHANDLERS_DEVICETMREPORTINGWRAPPER_H_ -#include "../action/HasActionsIF.h" -#include "../objectmanager/SystemObjectIF.h" -#include "../serialize/SerializeIF.h" +#include "fsfw/action/HasActionsIF.h" +#include "fsfw/objectmanager/SystemObjectIF.h" +#include "fsfw/serialize/SerializeIF.h" +#include "fsfw/util/dataWrapper.h" class DeviceTmReportingWrapper : public SerializeIF { public: - DeviceTmReportingWrapper(object_id_t objectId, ActionId_t actionId, SerializeIF* data); - virtual ~DeviceTmReportingWrapper(); + DeviceTmReportingWrapper(object_id_t objectId, ActionId_t actionId, util::DataWrapper data); + ~DeviceTmReportingWrapper() override; - virtual ReturnValue_t serialize(uint8_t** buffer, size_t* size, size_t maxSize, - Endianness streamEndianness) const override; + ReturnValue_t serialize(uint8_t** buffer, size_t* size, size_t maxSize, + Endianness streamEndianness) const override; - virtual size_t getSerializedSize() const override; - - virtual ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size, - Endianness streamEndianness) override; + [[nodiscard]] size_t getSerializedSize() const override; private: object_id_t objectId; ActionId_t actionId; - SerializeIF* data; + util::DataWrapper dataWrapper; + + // Deserialization forbidden + ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size, + Endianness streamEndianness) override; }; #endif /* FSFW_DEVICEHANDLERS_DEVICETMREPORTINGWRAPPER_H_ */ From bdd79d060df5ddde24da71b3a7df83ffbf486be9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 Aug 2022 14:02:58 +0200 Subject: [PATCH 175/404] basic data wrapper unittests --- src/fsfw/util/dataWrapper.h | 5 ++-- unittests/util/CMakeLists.txt | 1 + unittests/util/testDataWrapper.cpp | 31 ++++++++++++++++++++++++ unittests/util/testUnsignedByteField.cpp | 2 +- 4 files changed, 36 insertions(+), 3 deletions(-) create mode 100644 unittests/util/testDataWrapper.cpp diff --git a/src/fsfw/util/dataWrapper.h b/src/fsfw/util/dataWrapper.h index 8bb373e6..440f9501 100644 --- a/src/fsfw/util/dataWrapper.h +++ b/src/fsfw/util/dataWrapper.h @@ -10,6 +10,7 @@ namespace util { struct RawData { + RawData() = default; const uint8_t* data = nullptr; size_t len = 0; }; @@ -17,8 +18,8 @@ struct RawData { enum DataTypes { NONE, RAW, SERIALIZABLE }; union DataUnion { - RawData raw; - SerializeIF* serializable = nullptr; + RawData raw{}; + SerializeIF* serializable; }; struct DataWrapper { diff --git a/unittests/util/CMakeLists.txt b/unittests/util/CMakeLists.txt index d4caa4d5..dd9f6dc1 100644 --- a/unittests/util/CMakeLists.txt +++ b/unittests/util/CMakeLists.txt @@ -1,3 +1,4 @@ target_sources(${FSFW_TEST_TGT} PRIVATE testUnsignedByteField.cpp + testDataWrapper.cpp ) diff --git a/unittests/util/testDataWrapper.cpp b/unittests/util/testDataWrapper.cpp new file mode 100644 index 00000000..5a6b26d7 --- /dev/null +++ b/unittests/util/testDataWrapper.cpp @@ -0,0 +1,31 @@ +#include +#include + +#include "fsfw/util/dataWrapper.h" +#include "mocks/SimpleSerializable.h" + +TEST_CASE("Data Wrapper", "[util]") { + util::DataWrapper wrapper; + SECTION("State") { + REQUIRE(wrapper.isNull()); + } + + SECTION("Set Raw Data") { + REQUIRE(wrapper.isNull()); + std::array data = {1, 2, 3, 4}; + wrapper.setRawData({data.data(), data.size()}); + REQUIRE(not wrapper.isNull()); + REQUIRE(wrapper.type == util::DataTypes::RAW); + REQUIRE(wrapper.dataUnion.raw.data == data.data()); + REQUIRE(wrapper.dataUnion.raw.len == data.size()); + } + + SECTION("Simple Serializable") { + REQUIRE(wrapper.isNull()); + SimpleSerializable serializable; + wrapper.setSerializable(serializable); + REQUIRE(not wrapper.isNull()); + REQUIRE(wrapper.type == util::DataTypes::SERIALIZABLE); + REQUIRE(wrapper.dataUnion.serializable == &serializable); + } +} \ No newline at end of file diff --git a/unittests/util/testUnsignedByteField.cpp b/unittests/util/testUnsignedByteField.cpp index df1ff483..58a87a7d 100644 --- a/unittests/util/testUnsignedByteField.cpp +++ b/unittests/util/testUnsignedByteField.cpp @@ -4,7 +4,7 @@ #include "fsfw/util/UnsignedByteField.h" -TEST_CASE("Unsigned Byte Field", "[unsigned-byte-field]") { +TEST_CASE("Unsigned Byte Field", "[util]") { auto testByteField = UnsignedByteField(10); auto u32ByteField = U32ByteField(10); auto u16ByteField = U16ByteField(5); From 192255df1cc9b75559d9bd57d44f467bc49a3b83 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 Aug 2022 14:03:33 +0200 Subject: [PATCH 176/404] additional test --- unittests/util/testDataWrapper.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/unittests/util/testDataWrapper.cpp b/unittests/util/testDataWrapper.cpp index 5a6b26d7..14ba287a 100644 --- a/unittests/util/testDataWrapper.cpp +++ b/unittests/util/testDataWrapper.cpp @@ -8,6 +8,7 @@ TEST_CASE("Data Wrapper", "[util]") { util::DataWrapper wrapper; SECTION("State") { REQUIRE(wrapper.isNull()); + REQUIRE(wrapper.type == util::DataTypes::NONE); } SECTION("Set Raw Data") { From 6b8c83be29c683e73ff179f0ad99ebf77e783ccb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 Aug 2022 14:40:02 +0200 Subject: [PATCH 177/404] update changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 1c78318c..06c02e96 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,10 @@ and this project adheres to [Semantic Versioning](http://semver.org/). ## Added +- DHB TM handler `handleDeviceTM` renamed to `handleDeviceTm` and now takes + `util::DataWrapper` as the data input argument. This allows more flexibility in the possible + types of telemetry. + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/669 - Add new `UnsignedByteField` class PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/660 From d675a789a29c18bf1fd57f5dbc89870da54db31b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 Aug 2022 14:41:37 +0200 Subject: [PATCH 178/404] update changelog --- CHANGELOG.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 1c78318c..34053f16 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,8 @@ and this project adheres to [Semantic Versioning](http://semver.org/). # [unreleased] +# [v6.0.0] + ## Changes - Removed `HasReturnvaluesIF` class in favor of `returnvalue` namespace with `OK` and `FAILED` @@ -16,6 +18,10 @@ and this project adheres to [Semantic Versioning](http://semver.org/). ## Added +- Add `util::DataWrapper` class inside the `util` module. This is a tagged union which allows + to specify raw data either as a classic C-style raw pointer and size or as a `SerializeIF` + pointer. + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/668 - Add new `UnsignedByteField` class PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/660 From efd2994dc575c249f9e71375a6ed2497d6ba63c4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 Aug 2022 14:59:59 +0200 Subject: [PATCH 179/404] dump compiler erorrs.. --- src/fsfw/datapoollocal/LocalDataPoolManager.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/fsfw/datapoollocal/LocalDataPoolManager.h b/src/fsfw/datapoollocal/LocalDataPoolManager.h index 2f1476ab..8f369ea0 100644 --- a/src/fsfw/datapoollocal/LocalDataPoolManager.h +++ b/src/fsfw/datapoollocal/LocalDataPoolManager.h @@ -243,11 +243,9 @@ class LocalDataPoolManager : public ProvidesDataPoolSubscriptionIF, public Acces LocalDataPoolManager* getPoolManagerHandle() override; ReturnValue_t subscribeForRegularPeriodicPacket(subdp::RegularHkPeriodicParams params) override; ReturnValue_t subscribeForDiagPeriodicPacket(subdp::DiagnosticsHkPeriodicParams params) override; - ReturnValue_t subscribeForPeriodicPacket(subdp::ParamsBase& params); ReturnValue_t subscribeForRegularUpdatePacket(subdp::RegularHkUpdateParams params) override; ReturnValue_t subscribeForDiagUpdatePacket(subdp::DiagnosticsHkUpdateParams params) override; - ReturnValue_t subscribeForUpdatePacket(subdp::ParamsBase& params); protected: ReturnValue_t subscribeForPeriodicPacket(subdp::ParamsBase& params); From ae40543e3aff4002b074e1d733a2ba9caa6ca599 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 Aug 2022 15:16:54 +0200 Subject: [PATCH 180/404] this is annoying --- src/fsfw/pus/Service11TelecommandScheduling.h | 5 +++-- src/fsfw/pus/Service11TelecommandScheduling.tpp | 14 +++----------- 2 files changed, 6 insertions(+), 13 deletions(-) diff --git a/src/fsfw/pus/Service11TelecommandScheduling.h b/src/fsfw/pus/Service11TelecommandScheduling.h index 85d615e3..aa958193 100644 --- a/src/fsfw/pus/Service11TelecommandScheduling.h +++ b/src/fsfw/pus/Service11TelecommandScheduling.h @@ -38,8 +38,9 @@ class Service11TelecommandScheduling final : public PusServiceBase { static constexpr uint8_t CLASS_ID = CLASS_ID::PUS_SERVICE_11; static constexpr ReturnValue_t INVALID_TYPE_TIME_WINDOW = returnvalue::makeCode(CLASS_ID, 1); - static constexpr ReturnValue_t TIMESHIFTING_NOT_POSSIBLE = returnvalue::makeCode(CLASS_ID, 2); - static constexpr ReturnValue_t INVALID_RELATIVE_TIME = returnvalue::makeCode(CLASS_ID, 3); + static constexpr ReturnValue_t INVALID_TIME_WINDOW = returnvalue::makeCode(CLASS_ID, 2); + static constexpr ReturnValue_t TIMESHIFTING_NOT_POSSIBLE = returnvalue::makeCode(CLASS_ID, 3); + static constexpr ReturnValue_t INVALID_RELATIVE_TIME = returnvalue::makeCode(CLASS_ID, 4); static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_11; diff --git a/src/fsfw/pus/Service11TelecommandScheduling.tpp b/src/fsfw/pus/Service11TelecommandScheduling.tpp index dfb6ea92..315d8950 100644 --- a/src/fsfw/pus/Service11TelecommandScheduling.tpp +++ b/src/fsfw/pus/Service11TelecommandScheduling.tpp @@ -564,6 +564,9 @@ inline ReturnValue_t Service11TelecommandScheduling::getMapFilterFr if (result != returnvalue::OK) { return result; } + if(fromTimestamp > toTimestamp) { + return INVALID_TIME_WINDOW; + } itBegin = telecommandMap.begin(); itEnd = telecommandMap.begin(); @@ -579,17 +582,6 @@ inline ReturnValue_t Service11TelecommandScheduling::getMapFilterFr default: return returnvalue::FAILED; } - - // additional security check, this should never be true - if (itBegin > itEnd) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "11::getMapFilterFromData: itBegin > itEnd\n" << std::endl; -#else - sif::printError("11::getMapFilterFromData: itBegin > itEnd\n"); -#endif - return returnvalue::FAILED; - } - // the map range should now be set according to the sent filter. return returnvalue::OK; } From 21ac86619e2b1fecf71bd2aa0f09ecbd471c8531 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 Aug 2022 15:52:34 +0200 Subject: [PATCH 181/404] now its getting interesting --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 61 ------------------- src/fsfw/util/dataWrapper.h | 18 +++++- 2 files changed, 17 insertions(+), 62 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index bdfd7efa..5aebe917 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1329,67 +1329,6 @@ void DeviceHandlerBase::handleDeviceTm(util::DataWrapper dataWrapper, DeviceComm } } -void DeviceHandlerBase::handleDeviceTM(const uint8_t* data, size_t dataSize, - DeviceCommandId_t replyId, bool forceDirectTm) { - // TODO: Horrible duplicate code. Avoiding this would require using the Serializable union - // type. Furthermore, DeviceTmReportingWrapper needs to be extended to allow using raw - // buffers. - if (data == nullptr) { - return; - } - - DeviceReplyMap::iterator iter = deviceReplyMap.find(replyId); - if (iter == deviceReplyMap.end()) { - triggerEvent(DEVICE_UNKNOWN_REPLY, replyId); - return; - } - - /* Regular replies to a command */ - if (iter->second.command != deviceCommandMap.end()) { - MessageQueueId_t queueId = iter->second.command->second.sendReplyTo; - - if (queueId != NO_COMMANDER) { - /* This may fail, but we'll ignore the fault. */ - actionHelper.reportData(queueId, replyId, data, dataSize); - } - - /* This check should make sure we get any TM but don't get anything doubled. */ - // TODO: The wrapper does not support this.. - // if (wiretappingMode == TM && (requestedRawTraffic != queueId)) { - // DeviceTmReportingWrapper wrapper(getObjectId(), replyId, data, dataSize); - // actionHelper.reportData(requestedRawTraffic, replyId, &wrapper); - // } - - else if (forceDirectTm and (defaultRawReceiver != queueId) and - (defaultRawReceiver != MessageQueueIF::NO_QUEUE)) { - // hiding of sender needed so the service will handle it as - // unexpected Data, no matter what state (progress or completed) - // it is in - actionHelper.reportData(defaultRawReceiver, replyId, data, dataSize, true); - } - } - /* Unrequested or aperiodic replies */ - // TODO: The wrapper does not support this.. - // else { - // DeviceTmReportingWrapper wrapper(getObjectId(), replyId, data, dataSize); - // if (wiretappingMode == TM) { - // actionHelper.reportData(requestedRawTraffic, replyId, &wrapper); - // } - // if (forceDirectTm and defaultRawReceiver != MessageQueueIF::NO_QUEUE) { - // // sid_t setSid = sid_t(this->getObjectId(), replyId); - // // LocalPoolDataSetBase* dataset = getDataSetHandle(setSid); - // // if(dataset != nullptr) { - // // poolManager.generateHousekeepingPacket(setSid, dataset, true); - // // } - // - // // hiding of sender needed so the service will handle it as - // // unexpected Data, no matter what state (progress or completed) - // // it is in - // actionHelper.reportData(defaultRawReceiver, replyId, &wrapper, true); - // } - // } -} - ReturnValue_t DeviceHandlerBase::executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, size_t size) { ReturnValue_t result = acceptExternalDeviceCommands(); diff --git a/src/fsfw/util/dataWrapper.h b/src/fsfw/util/dataWrapper.h index fe973290..6a3fd5dc 100644 --- a/src/fsfw/util/dataWrapper.h +++ b/src/fsfw/util/dataWrapper.h @@ -23,9 +23,25 @@ union DataUnion { }; struct DataWrapper { + using BufPairT = std::pair; + + DataWrapper() = default; + + DataWrapper(const uint8_t* data, size_t size): type(DataTypes::RAW) { + setRawData({data, size}); + } + + explicit DataWrapper(BufPairT raw): type(DataTypes::RAW) { + setRawData(raw); + } + + explicit DataWrapper(SerializeIF& serializable): type(DataTypes::SERIALIZABLE) { + setSerializable(serializable); + } + DataTypes type = DataTypes::NONE; DataUnion dataUnion; - using BufPairT = std::pair; + [[nodiscard]] size_t getLength() const { if (type == DataTypes::RAW) { From 8d1777fa0c7b330fedfdc834fa15da50bcd9df17 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 Aug 2022 16:02:50 +0200 Subject: [PATCH 182/404] additional tests --- src/fsfw/util/dataWrapper.h | 21 +++++++++++-- unittests/util/testDataWrapper.cpp | 47 +++++++++++++++++++++++------- 2 files changed, 56 insertions(+), 12 deletions(-) diff --git a/src/fsfw/util/dataWrapper.h b/src/fsfw/util/dataWrapper.h index 440f9501..ac4404f1 100644 --- a/src/fsfw/util/dataWrapper.h +++ b/src/fsfw/util/dataWrapper.h @@ -9,6 +9,8 @@ namespace util { +using BufPair = std::pair; + struct RawData { RawData() = default; const uint8_t* data = nullptr; @@ -23,9 +25,24 @@ union DataUnion { }; struct DataWrapper { + + + DataWrapper() = default; + + DataWrapper(const uint8_t* data, size_t size): type(DataTypes::RAW) { + setRawData({data, size}); + } + + explicit DataWrapper(BufPair raw): type(DataTypes::RAW) { + setRawData(raw); + } + + explicit DataWrapper(SerializeIF& serializable): type(DataTypes::SERIALIZABLE) { + setSerializable(serializable); + } + DataTypes type = DataTypes::NONE; DataUnion dataUnion; - using BufPairT = std::pair; [[nodiscard]] size_t getLength() const { if (type == DataTypes::RAW) { @@ -43,7 +60,7 @@ struct DataWrapper { } return false; } - void setRawData(BufPairT bufPair) { + void setRawData(BufPair bufPair) { type = DataTypes::RAW; dataUnion.raw.data = bufPair.first; dataUnion.raw.len = bufPair.second; diff --git a/unittests/util/testDataWrapper.cpp b/unittests/util/testDataWrapper.cpp index 14ba287a..72931d90 100644 --- a/unittests/util/testDataWrapper.cpp +++ b/unittests/util/testDataWrapper.cpp @@ -12,21 +12,48 @@ TEST_CASE("Data Wrapper", "[util]") { } SECTION("Set Raw Data") { + util::DataWrapper* instance = &wrapper; + bool deleteInst = false; REQUIRE(wrapper.isNull()); std::array data = {1, 2, 3, 4}; - wrapper.setRawData({data.data(), data.size()}); - REQUIRE(not wrapper.isNull()); - REQUIRE(wrapper.type == util::DataTypes::RAW); - REQUIRE(wrapper.dataUnion.raw.data == data.data()); - REQUIRE(wrapper.dataUnion.raw.len == data.size()); + SECTION("Setter") { + wrapper.setRawData({data.data(), data.size()}); + } + SECTION("Direct Construction Pair") { + instance = new util::DataWrapper(util::BufPair(data.data(), data.size())); + deleteInst = true; + } + SECTION("Direct Construction Single Args") { + instance = new util::DataWrapper(data.data(), data.size()); + deleteInst = true; + } + REQUIRE(not instance->isNull()); + REQUIRE(instance->type == util::DataTypes::RAW); + REQUIRE(instance->dataUnion.raw.data == data.data()); + REQUIRE(instance->dataUnion.raw.len == data.size()); + if(deleteInst) { + delete instance; + } } SECTION("Simple Serializable") { - REQUIRE(wrapper.isNull()); + util::DataWrapper* instance = &wrapper; + bool deleteInst = false; + REQUIRE(instance->isNull()); SimpleSerializable serializable; - wrapper.setSerializable(serializable); - REQUIRE(not wrapper.isNull()); - REQUIRE(wrapper.type == util::DataTypes::SERIALIZABLE); - REQUIRE(wrapper.dataUnion.serializable == &serializable); + SECTION("Setter") { + wrapper.setSerializable(serializable); + } + SECTION("Direct Construction") { + instance = new util::DataWrapper(serializable); + deleteInst = true; + } + + REQUIRE(not instance->isNull()); + REQUIRE(instance->type == util::DataTypes::SERIALIZABLE); + REQUIRE(instance->dataUnion.serializable == &serializable); + if(deleteInst) { + delete instance; + } } } \ No newline at end of file From 20f0707813b12b5a4f96b312b9825c357b45ab5e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 30 Aug 2022 16:04:20 +0200 Subject: [PATCH 183/404] remove newline --- src/fsfw/util/dataWrapper.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/fsfw/util/dataWrapper.h b/src/fsfw/util/dataWrapper.h index ac4404f1..6c953281 100644 --- a/src/fsfw/util/dataWrapper.h +++ b/src/fsfw/util/dataWrapper.h @@ -25,8 +25,7 @@ union DataUnion { }; struct DataWrapper { - - + DataWrapper() = default; DataWrapper(const uint8_t* data, size_t size): type(DataTypes::RAW) { From cfca27542a3cabf58f9b7cab238c89ab9ad9beea Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 31 Aug 2022 16:26:18 +0200 Subject: [PATCH 184/404] small fix which allows sending action reply immediately --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 0b6138b8..cc6f99a8 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1331,18 +1331,22 @@ ReturnValue_t DeviceHandlerBase::executeAction(ActionId_t actionId, MessageQueue return result; } DeviceCommandMap::iterator iter = deviceCommandMap.find(actionId); + MessageQueueId_t prevRecipient = MessageQueueIF::NO_QUEUE; if (iter == deviceCommandMap.end()) { result = COMMAND_NOT_SUPPORTED; } else if (iter->second.isExecuting) { result = COMMAND_ALREADY_SENT; } else { + prevRecipient = iter->second.sendReplyTo; + iter->second.sendReplyTo = commandedBy; result = buildCommandFromCommand(actionId, data, size); } if (result == returnvalue::OK) { - iter->second.sendReplyTo = commandedBy; iter->second.isExecuting = true; cookieInfo.pendingCommand = iter; cookieInfo.state = COOKIE_WRITE_READY; + } else { + iter->second.sendReplyTo = prevRecipient; } return result; } From 496dac89e4c094e03578f40d8651485f7f378cb2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 31 Aug 2022 22:47:58 +0200 Subject: [PATCH 185/404] important bugfix for TCP TMTC server --- src/fsfw/osal/common/TcpTmTcServer.cpp | 19 ++++---- .../pus/Service11TelecommandScheduling.tpp | 2 +- src/fsfw/tcdistribution/PusDistributor.cpp | 4 +- src/fsfw/tmtcservices/SpacePacketParser.cpp | 45 ++++++++++--------- src/fsfw/tmtcservices/SpacePacketParser.h | 18 ++++++-- src/fsfw/util/dataWrapper.h | 11 ++--- unittests/util/testDataWrapper.cpp | 12 ++--- 7 files changed, 56 insertions(+), 55 deletions(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index f9cb923e..f1174734 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -161,7 +161,7 @@ void TcpTmTcServer::handleServerOperation(socket_t& connSocket) { while (true) { ssize_t retval = recv(connSocket, reinterpret_cast(receptionBuffer.data()), - receptionBuffer.capacity(), tcpConfig.tcpFlags); + receptionBuffer.size(), tcpConfig.tcpFlags); if (retval == 0) { size_t availableReadData = ringBuffer.getAvailableReadData(); if (availableReadData > lastRingBufferSize) { @@ -335,31 +335,28 @@ ReturnValue_t TcpTmTcServer::handleTcRingBufferData(size_t availableReadData) { } ringBuffer.readData(receptionBuffer.data(), readAmount, true); const uint8_t* bufPtr = receptionBuffer.data(); - const uint8_t** bufPtrPtr = &bufPtr; - size_t startIdx = 0; - size_t foundSize = 0; - size_t readLen = 0; - while (readLen < readAmount) { + FoundPacketInfo info; + ParsingState parseState; + while (parseState.amountRead < readAmount) { if (spacePacketParser == nullptr) { return returnvalue::FAILED; } - result = - spacePacketParser->parseSpacePackets(bufPtrPtr, readAmount, startIdx, foundSize, readLen); + result = spacePacketParser->parseSpacePackets(&bufPtr, readAmount, info, parseState); switch (result) { case (SpacePacketParser::NO_PACKET_FOUND): case (SpacePacketParser::SPLIT_PACKET): { break; } case (returnvalue::OK): { - result = handleTcReception(receptionBuffer.data() + startIdx, foundSize); + result = handleTcReception(receptionBuffer.data() + info.startIdx, info.sizeFound); if (result != returnvalue::OK) { status = result; } } } - ringBuffer.deleteData(foundSize); + ringBuffer.deleteData(info.sizeFound); lastRingBufferSize = ringBuffer.getAvailableReadData(); - std::memset(receptionBuffer.data() + startIdx, 0, foundSize); + // std::memset(receptionBuffer.data() + startIdx, 0, foundSize); } return status; } diff --git a/src/fsfw/pus/Service11TelecommandScheduling.tpp b/src/fsfw/pus/Service11TelecommandScheduling.tpp index 315d8950..9a3cea57 100644 --- a/src/fsfw/pus/Service11TelecommandScheduling.tpp +++ b/src/fsfw/pus/Service11TelecommandScheduling.tpp @@ -564,7 +564,7 @@ inline ReturnValue_t Service11TelecommandScheduling::getMapFilterFr if (result != returnvalue::OK) { return result; } - if(fromTimestamp > toTimestamp) { + if (fromTimestamp > toTimestamp) { return INVALID_TIME_WINDOW; } itBegin = telecommandMap.begin(); diff --git a/src/fsfw/tcdistribution/PusDistributor.cpp b/src/fsfw/tcdistribution/PusDistributor.cpp index c94d5736..37e09d56 100644 --- a/src/fsfw/tcdistribution/PusDistributor.cpp +++ b/src/fsfw/tcdistribution/PusDistributor.cpp @@ -146,7 +146,7 @@ ReturnValue_t PusDistributor::initialize() { void PusDistributor::checkerFailurePrinter() const { #if FSFW_VERBOSE_LEVEL >= 1 - const char* keyword = "unnamed error"; + const char* keyword = "unnamed"; if (tcStatus == tcdistrib::INCORRECT_CHECKSUM) { keyword = "checksum"; } else if (tcStatus == tcdistrib::INCORRECT_PRIMARY_HEADER) { @@ -157,6 +157,8 @@ void PusDistributor::checkerFailurePrinter() const { keyword = "incorrect secondary header"; } else if (tcStatus == tcdistrib::INCOMPLETE_PACKET) { keyword = "incomplete packet"; + } else if (tcStatus == tcdistrib::INVALID_SEC_HEADER_FIELD) { + keyword = "invalid secondary header field"; } #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "PUSDistributor::handlePacket: Packet format invalid, " << keyword << " error" diff --git a/src/fsfw/tmtcservices/SpacePacketParser.cpp b/src/fsfw/tmtcservices/SpacePacketParser.cpp index b8364138..16ee5dac 100644 --- a/src/fsfw/tmtcservices/SpacePacketParser.cpp +++ b/src/fsfw/tmtcservices/SpacePacketParser.cpp @@ -7,16 +7,16 @@ SpacePacketParser::SpacePacketParser(std::vector validPacketIds) : validPacketIds(validPacketIds) {} ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t* buffer, const size_t maxSize, - size_t& startIndex, size_t& foundSize) { + FoundPacketInfo& packetInfo, + ParsingState& parsingState) { const uint8_t** tempPtr = &buffer; - size_t readLen = 0; - return parseSpacePackets(tempPtr, maxSize, startIndex, foundSize, readLen); + return parseSpacePackets(tempPtr, maxSize, packetInfo, parsingState); } ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t** buffer, const size_t maxSize, - size_t& startIndex, size_t& foundSize, - size_t& readLen) { - if (buffer == nullptr or maxSize < 5) { + FoundPacketInfo& packetInfo, + ParsingState& parsingState) { + if (buffer == nullptr or parsingState.nextStartIdx > maxSize) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "SpacePacketParser::parseSpacePackets: Frame invalid" << std::endl; #else @@ -26,35 +26,36 @@ ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t** buffer, const } const uint8_t* bufPtr = *buffer; - auto verifyLengthField = [&](size_t idx) { - uint16_t lengthField = bufPtr[idx + 4] << 8 | bufPtr[idx + 5]; + auto verifyLengthField = [&](size_t localIdx) { + uint16_t lengthField = (bufPtr[localIdx + 4] << 8) | bufPtr[localIdx + 5]; size_t packetSize = lengthField + 7; - startIndex = idx; ReturnValue_t result = returnvalue::OK; if (lengthField == 0) { // Skip whole header for now - foundSize = 6; + packetInfo.sizeFound = 6; result = NO_PACKET_FOUND; - } else if (packetSize + idx > maxSize) { + } else if (packetSize + localIdx + parsingState.amountRead > maxSize) { // Don't increment buffer and read length here, user has to decide what to do - foundSize = packetSize; + packetInfo.sizeFound = packetSize; return SPLIT_PACKET; } else { - foundSize = packetSize; + packetInfo.sizeFound = packetSize; } - *buffer += foundSize; - readLen += idx + foundSize; + *buffer += packetInfo.sizeFound; + packetInfo.startIdx = localIdx + parsingState.amountRead; + parsingState.nextStartIdx = localIdx + parsingState.amountRead + packetInfo.sizeFound; + parsingState.amountRead = parsingState.nextStartIdx; return result; }; size_t idx = 0; // Space packet ID as start marker if (validPacketIds.size() > 0) { - while (idx < maxSize - 5) { - uint16_t currentPacketId = bufPtr[idx] << 8 | bufPtr[idx + 1]; + while (idx + parsingState.amountRead < maxSize - 5) { + uint16_t currentPacketId = (bufPtr[idx] << 8) | bufPtr[idx + 1]; if (std::find(validPacketIds.begin(), validPacketIds.end(), currentPacketId) != validPacketIds.end()) { - if (idx + 5 >= maxSize) { + if (idx + parsingState.amountRead >= maxSize - 5) { return SPLIT_PACKET; } return verifyLengthField(idx); @@ -62,10 +63,10 @@ ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t** buffer, const idx++; } } - startIndex = 0; - foundSize = maxSize; - *buffer += foundSize; - readLen += foundSize; + parsingState.nextStartIdx = maxSize; + packetInfo.sizeFound = maxSize; + parsingState.amountRead = maxSize; + *buffer += maxSize; return NO_PACKET_FOUND; } // Assume that the user verified a valid start of a space packet diff --git a/src/fsfw/tmtcservices/SpacePacketParser.h b/src/fsfw/tmtcservices/SpacePacketParser.h index 93f90afe..394083b2 100644 --- a/src/fsfw/tmtcservices/SpacePacketParser.h +++ b/src/fsfw/tmtcservices/SpacePacketParser.h @@ -7,6 +7,16 @@ #include "fsfw/container/DynamicFIFO.h" #include "fsfw/returnvalues/FwClassIds.h" +struct FoundPacketInfo { + size_t startIdx = 0; + size_t sizeFound = 0; +}; + +struct ParsingState { + size_t nextStartIdx = 0; + size_t amountRead = 0; +}; + /** * @brief This small helper class scans a given buffer for space packets. * Can be used if space packets are serialized in a tightly packed frame. @@ -53,8 +63,8 @@ class SpacePacketParser { * will be assigned. * -@c returnvalue::OK if a packet was found */ - ReturnValue_t parseSpacePackets(const uint8_t** buffer, const size_t maxSize, size_t& startIndex, - size_t& foundSize, size_t& readLen); + ReturnValue_t parseSpacePackets(const uint8_t** buffer, const size_t maxSize, + FoundPacketInfo& packetInfo, ParsingState& parsingState); /** * Parse a given frame for space packets @@ -69,8 +79,8 @@ class SpacePacketParser { * detected packet * -@c returnvalue::OK if a packet was found */ - ReturnValue_t parseSpacePackets(const uint8_t* buffer, const size_t maxSize, size_t& startIndex, - size_t& foundSize); + ReturnValue_t parseSpacePackets(const uint8_t* buffer, const size_t maxSize, + FoundPacketInfo& packetInfo, ParsingState& parsingState); private: std::vector validPacketIds; diff --git a/src/fsfw/util/dataWrapper.h b/src/fsfw/util/dataWrapper.h index 84a9814f..f2620556 100644 --- a/src/fsfw/util/dataWrapper.h +++ b/src/fsfw/util/dataWrapper.h @@ -25,18 +25,13 @@ union DataUnion { }; struct DataWrapper { - DataWrapper() = default; - DataWrapper(const uint8_t* data, size_t size): type(DataTypes::RAW) { - setRawData({data, size}); - } + DataWrapper(const uint8_t* data, size_t size) : type(DataTypes::RAW) { setRawData({data, size}); } - explicit DataWrapper(BufPair raw): type(DataTypes::RAW) { - setRawData(raw); - } + explicit DataWrapper(BufPair raw) : type(DataTypes::RAW) { setRawData(raw); } - explicit DataWrapper(SerializeIF& serializable): type(DataTypes::SERIALIZABLE) { + explicit DataWrapper(SerializeIF& serializable) : type(DataTypes::SERIALIZABLE) { setSerializable(serializable); } diff --git a/unittests/util/testDataWrapper.cpp b/unittests/util/testDataWrapper.cpp index 72931d90..ef381a62 100644 --- a/unittests/util/testDataWrapper.cpp +++ b/unittests/util/testDataWrapper.cpp @@ -16,9 +16,7 @@ TEST_CASE("Data Wrapper", "[util]") { bool deleteInst = false; REQUIRE(wrapper.isNull()); std::array data = {1, 2, 3, 4}; - SECTION("Setter") { - wrapper.setRawData({data.data(), data.size()}); - } + SECTION("Setter") { wrapper.setRawData({data.data(), data.size()}); } SECTION("Direct Construction Pair") { instance = new util::DataWrapper(util::BufPair(data.data(), data.size())); deleteInst = true; @@ -31,7 +29,7 @@ TEST_CASE("Data Wrapper", "[util]") { REQUIRE(instance->type == util::DataTypes::RAW); REQUIRE(instance->dataUnion.raw.data == data.data()); REQUIRE(instance->dataUnion.raw.len == data.size()); - if(deleteInst) { + if (deleteInst) { delete instance; } } @@ -41,9 +39,7 @@ TEST_CASE("Data Wrapper", "[util]") { bool deleteInst = false; REQUIRE(instance->isNull()); SimpleSerializable serializable; - SECTION("Setter") { - wrapper.setSerializable(serializable); - } + SECTION("Setter") { wrapper.setSerializable(serializable); } SECTION("Direct Construction") { instance = new util::DataWrapper(serializable); deleteInst = true; @@ -52,7 +48,7 @@ TEST_CASE("Data Wrapper", "[util]") { REQUIRE(not instance->isNull()); REQUIRE(instance->type == util::DataTypes::SERIALIZABLE); REQUIRE(instance->dataUnion.serializable == &serializable); - if(deleteInst) { + if (deleteInst) { delete instance; } } From cf8fe7ea728bea077b9936bcf0db96845bc6419e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 1 Sep 2022 10:51:09 +0200 Subject: [PATCH 186/404] more simplfications --- src/fsfw/osal/common/TcpTmTcServer.cpp | 15 +++-- src/fsfw/tmtcservices/SpacePacketParser.cpp | 32 ++++------ src/fsfw/tmtcservices/SpacePacketParser.h | 66 +++++++-------------- 3 files changed, 40 insertions(+), 73 deletions(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index f1174734..dff959ba 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -335,13 +335,13 @@ ReturnValue_t TcpTmTcServer::handleTcRingBufferData(size_t availableReadData) { } ringBuffer.readData(receptionBuffer.data(), readAmount, true); const uint8_t* bufPtr = receptionBuffer.data(); - FoundPacketInfo info; - ParsingState parseState; - while (parseState.amountRead < readAmount) { - if (spacePacketParser == nullptr) { - return returnvalue::FAILED; - } - result = spacePacketParser->parseSpacePackets(&bufPtr, readAmount, info, parseState); + SpacePacketParser::FoundPacketInfo info; + if (spacePacketParser == nullptr) { + return returnvalue::FAILED; + } + spacePacketParser->reset(); + while (spacePacketParser->getAmountRead() < readAmount) { + result = spacePacketParser->parseSpacePackets(&bufPtr, readAmount, info); switch (result) { case (SpacePacketParser::NO_PACKET_FOUND): case (SpacePacketParser::SPLIT_PACKET): { @@ -356,7 +356,6 @@ ReturnValue_t TcpTmTcServer::handleTcRingBufferData(size_t availableReadData) { } ringBuffer.deleteData(info.sizeFound); lastRingBufferSize = ringBuffer.getAvailableReadData(); - // std::memset(receptionBuffer.data() + startIdx, 0, foundSize); } return status; } diff --git a/src/fsfw/tmtcservices/SpacePacketParser.cpp b/src/fsfw/tmtcservices/SpacePacketParser.cpp index 16ee5dac..75db7a4e 100644 --- a/src/fsfw/tmtcservices/SpacePacketParser.cpp +++ b/src/fsfw/tmtcservices/SpacePacketParser.cpp @@ -6,17 +6,9 @@ SpacePacketParser::SpacePacketParser(std::vector validPacketIds) : validPacketIds(validPacketIds) {} -ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t* buffer, const size_t maxSize, - FoundPacketInfo& packetInfo, - ParsingState& parsingState) { - const uint8_t** tempPtr = &buffer; - return parseSpacePackets(tempPtr, maxSize, packetInfo, parsingState); -} - ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t** buffer, const size_t maxSize, - FoundPacketInfo& packetInfo, - ParsingState& parsingState) { - if (buffer == nullptr or parsingState.nextStartIdx > maxSize) { + FoundPacketInfo& packetInfo) { + if (buffer == nullptr or nextStartIdx > maxSize) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "SpacePacketParser::parseSpacePackets: Frame invalid" << std::endl; #else @@ -30,11 +22,7 @@ ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t** buffer, const uint16_t lengthField = (bufPtr[localIdx + 4] << 8) | bufPtr[localIdx + 5]; size_t packetSize = lengthField + 7; ReturnValue_t result = returnvalue::OK; - if (lengthField == 0) { - // Skip whole header for now - packetInfo.sizeFound = 6; - result = NO_PACKET_FOUND; - } else if (packetSize + localIdx + parsingState.amountRead > maxSize) { + if (packetSize + localIdx + amountRead > maxSize) { // Don't increment buffer and read length here, user has to decide what to do packetInfo.sizeFound = packetSize; return SPLIT_PACKET; @@ -42,20 +30,20 @@ ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t** buffer, const packetInfo.sizeFound = packetSize; } *buffer += packetInfo.sizeFound; - packetInfo.startIdx = localIdx + parsingState.amountRead; - parsingState.nextStartIdx = localIdx + parsingState.amountRead + packetInfo.sizeFound; - parsingState.amountRead = parsingState.nextStartIdx; + packetInfo.startIdx = localIdx + amountRead; + nextStartIdx = localIdx + amountRead + packetInfo.sizeFound; + amountRead = nextStartIdx; return result; }; size_t idx = 0; // Space packet ID as start marker if (validPacketIds.size() > 0) { - while (idx + parsingState.amountRead < maxSize - 5) { + while (idx + amountRead < maxSize - 5) { uint16_t currentPacketId = (bufPtr[idx] << 8) | bufPtr[idx + 1]; if (std::find(validPacketIds.begin(), validPacketIds.end(), currentPacketId) != validPacketIds.end()) { - if (idx + parsingState.amountRead >= maxSize - 5) { + if (idx + amountRead >= maxSize - 5) { return SPLIT_PACKET; } return verifyLengthField(idx); @@ -63,9 +51,9 @@ ReturnValue_t SpacePacketParser::parseSpacePackets(const uint8_t** buffer, const idx++; } } - parsingState.nextStartIdx = maxSize; + nextStartIdx = maxSize; packetInfo.sizeFound = maxSize; - parsingState.amountRead = maxSize; + amountRead = maxSize; *buffer += maxSize; return NO_PACKET_FOUND; } diff --git a/src/fsfw/tmtcservices/SpacePacketParser.h b/src/fsfw/tmtcservices/SpacePacketParser.h index 394083b2..3a1b4d16 100644 --- a/src/fsfw/tmtcservices/SpacePacketParser.h +++ b/src/fsfw/tmtcservices/SpacePacketParser.h @@ -7,16 +7,6 @@ #include "fsfw/container/DynamicFIFO.h" #include "fsfw/returnvalues/FwClassIds.h" -struct FoundPacketInfo { - size_t startIdx = 0; - size_t sizeFound = 0; -}; - -struct ParsingState { - size_t nextStartIdx = 0; - size_t amountRead = 0; -}; - /** * @brief This small helper class scans a given buffer for space packets. * Can be used if space packets are serialized in a tightly packed frame. @@ -27,9 +17,11 @@ struct ParsingState { */ class SpacePacketParser { public: - //! The first entry is the index inside the buffer while the second index - //! is the size of the PUS packet starting at that index. - using IndexSizePair = std::pair; + + struct FoundPacketInfo { + size_t startIdx = 0; + size_t sizeFound = 0; + }; static constexpr uint8_t INTERFACE_ID = CLASS_ID::SPACE_PACKET_PARSER; static constexpr ReturnValue_t NO_PACKET_FOUND = MAKE_RETURN_CODE(0x00); @@ -46,44 +38,32 @@ class SpacePacketParser { SpacePacketParser(std::vector validPacketIds); /** - * Parse a given frame for space packets but also increment the given buffer and assign the - * total number of bytes read so far + * Parse a given frame for space packets but also increments the given buffer. * @param buffer Parser will look for space packets in this buffer * @param maxSize Maximum size of the buffer - * @param startIndex Start index of a found space packet - * @param foundSize Found size of the space packet - * @param readLen Length read so far. This value is incremented by the number of parsed - * bytes which also includes the size of a found packet - * -@c NO_PACKET_FOUND if no packet was found in the given buffer or the length field is - * invalid. foundSize will be set to the size of the space packet header. buffer and - * readLen will be incremented accordingly. - * -@c SPLIT_PACKET if a packet was found but the detected size exceeds maxSize. foundSize - * will be set to the detected packet size and startIndex will be set to the start of the - * detected packet. buffer and read length will not be incremented but the found length - * will be assigned. - * -@c returnvalue::OK if a packet was found + * @param packetInfo Information about packets found. + * -@c NO_PACKET_FOUND if no packet was found in the given buffer + * -@c SPLIT_PACKET if a packet was found but the detected size exceeds maxSize. packetInfo + * will contain the detected packet size and start index. + * -@c returnvalue::OK if a packet was found. Packet size and start index will be set in + * packetInfo */ ReturnValue_t parseSpacePackets(const uint8_t** buffer, const size_t maxSize, - FoundPacketInfo& packetInfo, ParsingState& parsingState); + FoundPacketInfo& packetInfo); - /** - * Parse a given frame for space packets - * @param buffer Parser will look for space packets in this buffer - * @param maxSize Maximum size of the buffer - * @param startIndex Start index of a found space packet - * @param foundSize Found size of the space packet - * -@c NO_PACKET_FOUND if no packet was found in the given buffer or the length field is - * invalid. foundSize will be set to the size of the space packet header - * -@c SPLIT_PACKET if a packet was found but the detected size exceeds maxSize. foundSize - * will be set to the detected packet size and startIndex will be set to the start of the - * detected packet - * -@c returnvalue::OK if a packet was found - */ - ReturnValue_t parseSpacePackets(const uint8_t* buffer, const size_t maxSize, - FoundPacketInfo& packetInfo, ParsingState& parsingState); + size_t getAmountRead() { + return amountRead; + } + + void reset() { + nextStartIdx = 0; + amountRead = 0; + } private: std::vector validPacketIds; + size_t nextStartIdx = 0; + size_t amountRead = 0; }; #endif /* FRAMEWORK_TMTCSERVICES_PUSPARSER_H_ */ From 385a0ffd73b7bb38e5543efcd3996042730c41bd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 14 Sep 2022 19:29:43 +0200 Subject: [PATCH 187/404] add CFDP handlers manually --- src/fsfw/cfdp.h | 1 + src/fsfw/cfdp/handler/CMakeLists.txt | 3 +- src/fsfw/cfdp/handler/DestHandler.cpp | 480 +++++++++++++++++++ src/fsfw/cfdp/handler/DestHandler.h | 202 ++++++++ src/fsfw/cfdp/handler/SourceHandler.cpp | 1 + src/fsfw/cfdp/handler/SourceHandler.h | 6 + unittests/cfdp/handler/CMakeLists.txt | 5 +- unittests/cfdp/handler/testDestHandler.cpp | 244 ++++++++++ unittests/cfdp/handler/testSourceHandler.cpp | 3 + 9 files changed, 942 insertions(+), 3 deletions(-) create mode 100644 src/fsfw/cfdp/handler/DestHandler.cpp create mode 100644 src/fsfw/cfdp/handler/DestHandler.h create mode 100644 src/fsfw/cfdp/handler/SourceHandler.cpp create mode 100644 src/fsfw/cfdp/handler/SourceHandler.h create mode 100644 unittests/cfdp/handler/testDestHandler.cpp create mode 100644 unittests/cfdp/handler/testSourceHandler.cpp diff --git a/src/fsfw/cfdp.h b/src/fsfw/cfdp.h index 86086432..28ddfec2 100644 --- a/src/fsfw/cfdp.h +++ b/src/fsfw/cfdp.h @@ -2,6 +2,7 @@ #define FSFW_CFDP_H #include "cfdp/definitions.h" +#include "cfdp/handler/DestHandler.h" #include "cfdp/handler/FaultHandlerBase.h" #include "cfdp/tlv/Lv.h" #include "cfdp/tlv/StringLv.h" diff --git a/src/fsfw/cfdp/handler/CMakeLists.txt b/src/fsfw/cfdp/handler/CMakeLists.txt index 90130806..7ad995c0 100644 --- a/src/fsfw/cfdp/handler/CMakeLists.txt +++ b/src/fsfw/cfdp/handler/CMakeLists.txt @@ -1 +1,2 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE FaultHandlerBase.cpp UserBase.cpp) +target_sources(${LIB_FSFW_NAME} PRIVATE SourceHandler.cpp DestHandler.cpp + FaultHandlerBase.cpp UserBase.cpp) diff --git a/src/fsfw/cfdp/handler/DestHandler.cpp b/src/fsfw/cfdp/handler/DestHandler.cpp new file mode 100644 index 00000000..d43d1482 --- /dev/null +++ b/src/fsfw/cfdp/handler/DestHandler.cpp @@ -0,0 +1,480 @@ +#include "DestHandler.h" + +#include + +#include + +#include "fsfw/FSFW.h" +#include "fsfw/cfdp/pdu/EofPduReader.h" +#include "fsfw/cfdp/pdu/FileDataReader.h" +#include "fsfw/cfdp/pdu/FinishedPduCreator.h" +#include "fsfw/cfdp/pdu/PduHeaderReader.h" +#include "fsfw/objectmanager.h" +#include "fsfw/tmtcservices/TmTcMessage.h" + +using namespace returnvalue; + +cfdp::DestHandler::DestHandler(DestHandlerParams params, FsfwParams fsfwParams) + : tlvVec(params.maxTlvsInOnePdu), + userTlvVec(params.maxTlvsInOnePdu), + dp(std::move(params)), + fp(fsfwParams), + tp(params.maxFilenameLen) { + tp.pduConf.direction = cfdp::Direction::TOWARDS_SENDER; +} + +const cfdp::DestHandler::FsmResult& cfdp::DestHandler::performStateMachine() { + ReturnValue_t result; + uint8_t errorIdx = 0; + fsmRes.resetOfIteration(); + if (fsmRes.step == TransactionStep::IDLE) { + for (auto infoIter = dp.packetListRef.begin(); infoIter != dp.packetListRef.end();) { + if (infoIter->pduType == PduTypes::FILE_DIRECTIVE and + infoIter->directiveType == FileDirectives::METADATA) { + result = handleMetadataPdu(*infoIter); + checkAndHandleError(result, errorIdx); + // Store data was deleted in PDU handler because a store guard is used + dp.packetListRef.erase(infoIter++); + } else { + infoIter++; + } + } + if (fsmRes.step == TransactionStep::IDLE) { + // To decrease the already high complexity of the software, all packets arriving before + // a metadata PDU are deleted. + for (auto infoIter = dp.packetListRef.begin(); infoIter != dp.packetListRef.end();) { + fp.tcStore->deleteData(infoIter->storeId); + infoIter++; + } + dp.packetListRef.clear(); + } + + if (fsmRes.step != TransactionStep::IDLE) { + fsmRes.callStatus = CallStatus::CALL_AGAIN; + } + return updateFsmRes(errorIdx); + } + if (fsmRes.state == CfdpStates::BUSY_CLASS_1_NACKED) { + if (fsmRes.step == TransactionStep::RECEIVING_FILE_DATA_PDUS) { + for (auto infoIter = dp.packetListRef.begin(); infoIter != dp.packetListRef.end();) { + if (infoIter->pduType == PduTypes::FILE_DATA) { + result = handleFileDataPdu(*infoIter); + checkAndHandleError(result, errorIdx); + // Store data was deleted in PDU handler because a store guard is used + dp.packetListRef.erase(infoIter++); + } else if (infoIter->pduType == PduTypes::FILE_DIRECTIVE and + infoIter->directiveType == FileDirectives::EOF_DIRECTIVE) { + // TODO: Support for check timer missing + result = handleEofPdu(*infoIter); + checkAndHandleError(result, errorIdx); + // Store data was deleted in PDU handler because a store guard is used + dp.packetListRef.erase(infoIter++); + } else { + infoIter++; + } + } + } + if (fsmRes.step == TransactionStep::TRANSFER_COMPLETION) { + result = handleTransferCompletion(); + checkAndHandleError(result, errorIdx); + } + if (fsmRes.step == TransactionStep::SENDING_FINISHED_PDU) { + result = sendFinishedPdu(); + checkAndHandleError(result, errorIdx); + finish(); + } + return updateFsmRes(errorIdx); + } + if (fsmRes.state == CfdpStates::BUSY_CLASS_2_ACKED) { + // TODO: Will be implemented at a later stage +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "CFDP state machine for acknowledged mode not implemented yet" << std::endl; +#endif + } + return updateFsmRes(errorIdx); +} + +ReturnValue_t cfdp::DestHandler::passPacket(PacketInfo packet) { + if (dp.packetListRef.full()) { + return FAILED; + } + dp.packetListRef.push_back(packet); + return OK; +} + +ReturnValue_t cfdp::DestHandler::initialize() { + if (fp.tmStore == nullptr) { + fp.tmStore = ObjectManager::instance()->get(objects::TM_STORE); + if (fp.tmStore == nullptr) { + return FAILED; + } + } + + if (fp.tcStore == nullptr) { + fp.tcStore = ObjectManager::instance()->get(objects::TC_STORE); + if (fp.tcStore == nullptr) { + return FAILED; + } + } + + if (fp.msgQueue == nullptr) { + return FAILED; + } + return OK; +} + +ReturnValue_t cfdp::DestHandler::handleMetadataPdu(const PacketInfo& info) { + // Process metadata PDU + auto constAccessorPair = fp.tcStore->getData(info.storeId); + if (constAccessorPair.first != OK) { + // TODO: This is not a CFDP error. Event and/or warning? + return constAccessorPair.first; + } + cfdp::StringLv sourceFileName; + cfdp::StringLv destFileName; + MetadataInfo metadataInfo(tp.fileSize, sourceFileName, destFileName); + cfdp::Tlv* tlvArrayAsPtr = tlvVec.data(); + metadataInfo.setOptionsArray(&tlvArrayAsPtr, std::nullopt, tlvVec.size()); + MetadataPduReader reader(constAccessorPair.second.data(), constAccessorPair.second.size(), + metadataInfo); + ReturnValue_t result = reader.parseData(); + // TODO: The standard does not really specify what happens if this kind of error happens + // I think it might be a good idea to cache some sort of error code, which + // is translated into a warning and/or event by an upper layer + if (result != OK) { + return handleMetadataParseError(result, constAccessorPair.second.data(), + constAccessorPair.second.size()); + } + return startTransaction(reader, metadataInfo); +} + +ReturnValue_t cfdp::DestHandler::handleFileDataPdu(const cfdp::PacketInfo& info) { + // Process file data PDU + auto constAccessorPair = fp.tcStore->getData(info.storeId); + if (constAccessorPair.first != OK) { + // TODO: This is not a CFDP error. Event and/or warning? + return constAccessorPair.first; + } + cfdp::FileSize offset; + FileDataInfo fdInfo(offset); + FileDataReader reader(constAccessorPair.second.data(), constAccessorPair.second.size(), fdInfo); + ReturnValue_t result = reader.parseData(); + if (result != OK) { + return result; + } + size_t fileSegmentLen = 0; + const uint8_t* fileData = fdInfo.getFileData(&fileSegmentLen); + FileOpParams fileOpParams(tp.destName.data(), fileSegmentLen); + fileOpParams.offset = offset.value(); + if (dp.cfg.indicCfg.fileSegmentRecvIndicRequired) { + FileSegmentRecvdParams segParams; + segParams.offset = offset.value(); + segParams.id = tp.transactionId; + segParams.length = fileSegmentLen; + segParams.recContState = fdInfo.getRecordContinuationState(); + size_t segmentMetadatLen = 0; + auto* segMetadata = fdInfo.getSegmentMetadata(&segmentMetadatLen); + segParams.segmentMetadata = {segMetadata, segmentMetadatLen}; + dp.user.fileSegmentRecvdIndication(segParams); + } + result = dp.user.vfs.writeToFile(fileOpParams, fileData); + if (offset.value() + fileSegmentLen > tp.progress) { + tp.progress = offset.value() + fileSegmentLen; + } + if (result != returnvalue::OK) { + // TODO: Proper Error handling +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "File write error" << std::endl; +#endif + } else { + tp.deliveryStatus = FileDeliveryStatus::RETAINED_IN_FILESTORE; + } + return result; +} + +ReturnValue_t cfdp::DestHandler::handleEofPdu(const cfdp::PacketInfo& info) { + // Process EOF PDU + auto constAccessorPair = fp.tcStore->getData(info.storeId); + if (constAccessorPair.first != OK) { + // TODO: This is not a CFDP error. Event and/or warning? + return constAccessorPair.first; + } + EofInfo eofInfo(nullptr); + EofPduReader reader(constAccessorPair.second.data(), constAccessorPair.second.size(), eofInfo); + ReturnValue_t result = reader.parseData(); + if (result != OK) { + return result; + } + // TODO: Error handling + if (eofInfo.getConditionCode() == ConditionCode::NO_ERROR) { + tp.crc = eofInfo.getChecksum(); + uint64_t fileSizeFromEof = eofInfo.getFileSize().value(); + // CFDP 4.6.1.2.9: Declare file size error if progress exceeds file size + if (fileSizeFromEof > tp.progress) { + // TODO: File size error + } + tp.fileSize.setFileSize(fileSizeFromEof, std::nullopt); + } + if (dp.cfg.indicCfg.eofRecvIndicRequired) { + dp.user.eofRecvIndication(getTransactionId()); + } + if (fsmRes.step == TransactionStep::RECEIVING_FILE_DATA_PDUS) { + if (fsmRes.state == CfdpStates::BUSY_CLASS_1_NACKED) { + fsmRes.step = TransactionStep::TRANSFER_COMPLETION; + } else if (fsmRes.state == CfdpStates::BUSY_CLASS_2_ACKED) { + fsmRes.step = TransactionStep::SENDING_ACK_PDU; + } + } + return returnvalue::OK; +} + +ReturnValue_t cfdp::DestHandler::handleMetadataParseError(ReturnValue_t result, + const uint8_t* rawData, size_t maxSize) { + // TODO: try to extract destination ID for error + // TODO: Invalid metadata PDU. +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "Parsing Metadata PDU failed with code " << result << std::endl; +#else +#endif + PduHeaderReader headerReader(rawData, maxSize); + result = headerReader.parseData(); + if (result != OK) { + // TODO: Now this really should not happen. Warning or error, + // yield or cache appropriate returnvalue +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "Parsing Header failed" << std::endl; +#else +#endif + // TODO: Trigger appropriate event + return result; + } + cfdp::EntityId destId; + headerReader.getDestId(destId); + RemoteEntityCfg* remoteCfg; + if (not dp.remoteCfgTable.getRemoteCfg(destId, &remoteCfg)) { +// TODO: No remote config for dest ID. I consider this a configuration error, which is not +// covered by the standard. +// Warning or error, yield or cache appropriate returnvalue +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "No remote config exists for destination ID" << std::endl; +#else +#endif + // TODO: Trigger appropriate event + } + // TODO: Appropriate returnvalue? + return returnvalue::FAILED; +} + +ReturnValue_t cfdp::DestHandler::startTransaction(MetadataPduReader& reader, MetadataInfo& info) { + if (fsmRes.state != CfdpStates::IDLE) { + // According to standard, discard metadata PDU if we are busy + return OK; + } + ReturnValue_t result = OK; + fsmRes.step = TransactionStep::TRANSACTION_START; + if (reader.getTransmissionMode() == TransmissionModes::UNACKNOWLEDGED) { + fsmRes.state = CfdpStates::BUSY_CLASS_1_NACKED; + } else if (reader.getTransmissionMode() == TransmissionModes::ACKNOWLEDGED) { + fsmRes.state = CfdpStates::BUSY_CLASS_2_ACKED; + } + tp.checksumType = info.getChecksumType(); + tp.closureRequested = info.isClosureRequested(); + size_t sourceNameSize = 0; + const uint8_t* sourceNamePtr = info.getSourceFileName().getValue(&sourceNameSize); + if (sourceNameSize > tp.sourceName.size()) { + // TODO: Warning, event etc. + return FAILED; + } + std::memcpy(tp.sourceName.data(), sourceNamePtr, sourceNameSize); + tp.sourceName[sourceNameSize] = '\0'; + size_t destNameSize = 0; + const uint8_t* destNamePtr = info.getDestFileName().getValue(&destNameSize); + if (destNameSize > tp.destName.size()) { + // TODO: Warning, event etc. + return FAILED; + } + std::memcpy(tp.destName.data(), destNamePtr, destNameSize); + tp.destName[destNameSize] = '\0'; + reader.fillConfig(tp.pduConf); + tp.pduConf.direction = Direction::TOWARDS_SENDER; + tp.transactionId.entityId = tp.pduConf.sourceId; + tp.transactionId.seqNum = tp.pduConf.seqNum; + if (not dp.remoteCfgTable.getRemoteCfg(tp.pduConf.sourceId, &tp.remoteCfg)) { + // TODO: Warning, event etc. +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "cfdp::DestHandler" << __func__ + << ": No remote configuration found for destination ID " + << tp.pduConf.sourceId.getValue() << std::endl; +#endif + return FAILED; + } + // If both dest name size and source name size are 0, we are dealing with a metadata only PDU, + // so there is no need to create a file or truncate an existing file + if (destNameSize > 0 and sourceNameSize > 0) { + FilesystemParams fparams(tp.destName.data()); + // TODO: Filesystem errors? + if (dp.user.vfs.fileExists(fparams)) { + dp.user.vfs.truncateFile(fparams); + } else { + result = dp.user.vfs.createFile(fparams); + if (result != OK) { + // TODO: Handle FS error. This is probably a case for the filestore rejection mechanism of + // CFDP. + // In any case, it does not really make sense to continue here + } + } + } + fsmRes.step = TransactionStep::RECEIVING_FILE_DATA_PDUS; + MetadataRecvdParams params(tp.transactionId, tp.pduConf.sourceId); + params.fileSize = tp.fileSize.getSize(); + params.destFileName = tp.destName.data(); + params.sourceFileName = tp.sourceName.data(); + params.msgsToUserArray = dynamic_cast(userTlvVec.data()); + params.msgsToUserLen = info.getOptionsLen(); + dp.user.metadataRecvdIndication(params); + return result; +} + +cfdp::CfdpStates cfdp::DestHandler::getCfdpState() const { return fsmRes.state; } + +ReturnValue_t cfdp::DestHandler::handleTransferCompletion() { + ReturnValue_t result; + if (tp.checksumType != ChecksumTypes::NULL_CHECKSUM) { + result = checksumVerification(); + if (result != OK) { + // TODO: Warning / error handling? + } + } else { + tp.conditionCode = ConditionCode::NO_ERROR; + } + result = noticeOfCompletion(); + if (result != OK) { + } + if (fsmRes.state == CfdpStates::BUSY_CLASS_1_NACKED) { + if (tp.closureRequested) { + fsmRes.step = TransactionStep::SENDING_FINISHED_PDU; + } else { + finish(); + } + } else if (fsmRes.state == CfdpStates::BUSY_CLASS_2_ACKED) { + fsmRes.step = TransactionStep::SENDING_FINISHED_PDU; + } + return OK; +} + +void cfdp::DestHandler::finish() { + tp.reset(); + dp.packetListRef.clear(); + fsmRes.state = CfdpStates::IDLE; + fsmRes.step = TransactionStep::IDLE; +} + +ReturnValue_t cfdp::DestHandler::checksumVerification() { + std::array buf{}; + // TODO: Checksum verification and notice of completion + etl::crc32 crcCalc; + uint64_t currentOffset = 0; + FileOpParams params(tp.destName.data(), tp.fileSize.value()); + while (currentOffset < tp.fileSize.value()) { + uint64_t readLen; + if (currentOffset + buf.size() > tp.fileSize.value()) { + readLen = tp.fileSize.value() - currentOffset; + } else { + readLen = buf.size(); + } + if (readLen > 0) { + params.offset = currentOffset; + params.size = readLen; + auto result = dp.user.vfs.readFromFile(params, buf.data(), buf.size()); + if (result != OK) { + // TODO: I think this is a case for a filestore rejection, but it might sense to print + // a warning or trigger an event because this should generally not happen + return FAILED; + } + crcCalc.add(buf.begin(), buf.begin() + readLen); + } + currentOffset += readLen; + } + + uint32_t value = crcCalc.value(); + if (value == tp.crc) { + tp.conditionCode = ConditionCode::NO_ERROR; + tp.deliveryCode = FileDeliveryCode::DATA_COMPLETE; + } else { + // TODO: Proper error handling +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "CRC check for file " << tp.destName.data() << " failed" << std::endl; +#endif + tp.conditionCode = ConditionCode::FILE_CHECKSUM_FAILURE; + } + return OK; +} + +ReturnValue_t cfdp::DestHandler::noticeOfCompletion() { + if (dp.cfg.indicCfg.transactionFinishedIndicRequired) { + TransactionFinishedParams params(tp.transactionId, tp.conditionCode, tp.deliveryCode, + tp.deliveryStatus); + dp.user.transactionFinishedIndication(params); + } + return OK; +} + +ReturnValue_t cfdp::DestHandler::sendFinishedPdu() { + FinishedInfo info(tp.conditionCode, tp.deliveryCode, tp.deliveryStatus); + FinishPduCreator finishedPdu(tp.pduConf, info); + store_address_t storeId; + uint8_t* dataPtr = nullptr; + ReturnValue_t result = + fp.tcStore->getFreeElement(&storeId, finishedPdu.getSerializedSize(), &dataPtr); + if (result != OK) { + // TODO: Error handling and event, this is a non CFDP specific error (most likely store is full) + return result; + } + size_t serLen = 0; + result = finishedPdu.serialize(dataPtr, serLen, finishedPdu.getSerializedSize()); + if (result != OK) { + // TODO: Error printout, this really should not happen + return result; + } + TmTcMessage msg(storeId); + result = fp.msgQueue->sendMessage(fp.packetDest.getReportReceptionQueue(), &msg); + if (result != OK) { + // TODO: Error handling and event, this is a non CFDP specific error (most likely store is full) + return result; + } + fsmRes.packetsSent++; + return OK; +} + +cfdp::DestHandler::TransactionStep cfdp::DestHandler::getTransactionStep() const { + return fsmRes.step; +} + +const cfdp::DestHandler::FsmResult& cfdp::DestHandler::updateFsmRes(uint8_t errors) { + fsmRes.errors = errors; + fsmRes.result = OK; + if (fsmRes.errors > 0) { + fsmRes.result = FAILED; + } + return fsmRes; +} + +const cfdp::TransactionId& cfdp::DestHandler::getTransactionId() const { return tp.transactionId; } + +void cfdp::DestHandler::checkAndHandleError(ReturnValue_t result, uint8_t& errorIdx) { + if (result != OK and errorIdx < 3) { + fsmRes.errorCodes[errorIdx] = result; + errorIdx++; + } +} + +void cfdp::DestHandler::setMsgQueue(MessageQueueIF& queue) { fp.msgQueue = &queue; } + +void cfdp::DestHandler::setEventReporter(EventReportingProxyIF& reporter) { + fp.eventReporter = &reporter; +} + +const cfdp::DestHandlerParams& cfdp::DestHandler::getDestHandlerParams() const { return dp; } + +StorageManagerIF* cfdp::DestHandler::getTmStore() const { return fp.tmStore; } +StorageManagerIF* cfdp::DestHandler::getTcStore() const { return fp.tcStore; } diff --git a/src/fsfw/cfdp/handler/DestHandler.h b/src/fsfw/cfdp/handler/DestHandler.h new file mode 100644 index 00000000..000fc2c0 --- /dev/null +++ b/src/fsfw/cfdp/handler/DestHandler.h @@ -0,0 +1,202 @@ +#ifndef FSFW_CFDP_CFDPDESTHANDLER_H +#define FSFW_CFDP_CFDPDESTHANDLER_H + +#include +#include + +#include +#include + +#include "RemoteConfigTableIF.h" +#include "UserBase.h" +#include "defs.h" +#include "fsfw/cfdp/handler/mib.h" +#include "fsfw/cfdp/pdu/MetadataPduReader.h" +#include "fsfw/cfdp/pdu/PduConfig.h" +#include "fsfw/container/DynamicFIFO.h" +#include "fsfw/storagemanager/StorageManagerIF.h" +#include "fsfw/storagemanager/storeAddress.h" +#include "fsfw/tmtcservices/AcceptsTelemetryIF.h" + +namespace cfdp { + +struct PacketInfo { + PacketInfo(PduTypes type, store_address_t storeId, + std::optional directive = std::nullopt) + : pduType(type), directiveType(directive), storeId(storeId) {} + + PduTypes pduType = PduTypes::FILE_DATA; + std::optional directiveType = FileDirectives::INVALID_DIRECTIVE; + store_address_t storeId = store_address_t::invalid(); + PacketInfo() = default; +}; + +template +using LostSegmentsList = etl::set, SIZE>; +template +using PacketInfoList = etl::list; +using LostSegmentsListBase = etl::iset>; +using PacketInfoListBase = etl::ilist; + +struct DestHandlerParams { + DestHandlerParams(LocalEntityCfg cfg, UserBase& user, RemoteConfigTableIF& remoteCfgTable, + PacketInfoListBase& packetList, + // TODO: This container can potentially take tons of space. For a better + // memory efficient implementation, an additional abstraction could be + // be used so users can use uint32_t as the pair type + LostSegmentsListBase& lostSegmentsContainer) + : cfg(std::move(cfg)), + user(user), + remoteCfgTable(remoteCfgTable), + packetListRef(packetList), + lostSegmentsContainer(lostSegmentsContainer) {} + + LocalEntityCfg cfg; + UserBase& user; + RemoteConfigTableIF& remoteCfgTable; + + PacketInfoListBase& packetListRef; + LostSegmentsListBase& lostSegmentsContainer; + uint8_t maxTlvsInOnePdu = 10; + size_t maxFilenameLen = 255; +}; + +struct FsfwParams { + FsfwParams(AcceptsTelemetryIF& packetDest, MessageQueueIF* msgQueue, + EventReportingProxyIF* eventReporter, StorageManagerIF& tcStore, + StorageManagerIF& tmStore) + : FsfwParams(packetDest, msgQueue, eventReporter) { + this->tcStore = &tcStore; + this->tmStore = &tmStore; + } + + FsfwParams(AcceptsTelemetryIF& packetDest, MessageQueueIF* msgQueue, + EventReportingProxyIF* eventReporter) + : packetDest(packetDest), msgQueue(msgQueue), eventReporter(eventReporter) {} + AcceptsTelemetryIF& packetDest; + MessageQueueIF* msgQueue; + EventReportingProxyIF* eventReporter = nullptr; + StorageManagerIF* tcStore = nullptr; + StorageManagerIF* tmStore = nullptr; +}; + +enum class CallStatus { DONE, CALL_AFTER_DELAY, CALL_AGAIN }; + +class DestHandler { + public: + enum class TransactionStep { + IDLE = 0, + TRANSACTION_START = 1, + RECEIVING_FILE_DATA_PDUS = 2, + SENDING_ACK_PDU = 3, + TRANSFER_COMPLETION = 4, + SENDING_FINISHED_PDU = 5 + }; + + struct FsmResult { + public: + ReturnValue_t result = returnvalue::OK; + CallStatus callStatus = CallStatus::CALL_AFTER_DELAY; + TransactionStep step = TransactionStep::IDLE; + CfdpStates state = CfdpStates::IDLE; + uint32_t packetsSent = 0; + uint8_t errors = 0; + std::array errorCodes = {}; + void resetOfIteration() { + result = returnvalue::OK; + callStatus = CallStatus::CALL_AFTER_DELAY; + packetsSent = 0; + errors = 0; + errorCodes.fill(returnvalue::OK); + } + }; + /** + * Will be returned if it is advisable to call the state machine operation call again + */ + ReturnValue_t PARTIAL_SUCCESS = returnvalue::makeCode(0, 2); + ReturnValue_t FAILURE = returnvalue::makeCode(0, 3); + explicit DestHandler(DestHandlerParams handlerParams, FsfwParams fsfwParams); + + /** + * + * @return + * - @c returnvalue::OK State machine OK for this execution cycle + * - @c CALL_FSM_AGAIN State machine should be called again. + */ + const FsmResult& performStateMachine(); + void setMsgQueue(MessageQueueIF& queue); + void setEventReporter(EventReportingProxyIF& reporter); + + ReturnValue_t passPacket(PacketInfo packet); + + ReturnValue_t initialize(); + + [[nodiscard]] CfdpStates getCfdpState() const; + [[nodiscard]] TransactionStep getTransactionStep() const; + [[nodiscard]] const TransactionId& getTransactionId() const; + [[nodiscard]] const DestHandlerParams& getDestHandlerParams() const; + [[nodiscard]] StorageManagerIF* getTcStore() const; + [[nodiscard]] StorageManagerIF* getTmStore() const; + + private: + struct TransactionParams { + // Initialize char vectors with length + 1 for 0 termination + explicit TransactionParams(size_t maxFileNameLen) + : sourceName(maxFileNameLen + 1), destName(maxFileNameLen + 1) {} + + void reset() { + pduConf = PduConfig(); + transactionId = TransactionId(); + std::fill(sourceName.begin(), sourceName.end(), '\0'); + std::fill(destName.begin(), destName.end(), '\0'); + fileSize.setFileSize(0, false); + conditionCode = ConditionCode::NO_ERROR; + deliveryCode = FileDeliveryCode::DATA_INCOMPLETE; + deliveryStatus = FileDeliveryStatus::DISCARDED_DELIBERATELY; + crc = 0; + progress = 0; + remoteCfg = nullptr; + closureRequested = false; + checksumType = ChecksumTypes::NULL_CHECKSUM; + } + + ChecksumTypes checksumType = ChecksumTypes::NULL_CHECKSUM; + bool closureRequested = false; + std::vector sourceName; + std::vector destName; + cfdp::FileSize fileSize; + TransactionId transactionId; + PduConfig pduConf; + ConditionCode conditionCode = ConditionCode::NO_ERROR; + FileDeliveryCode deliveryCode = FileDeliveryCode::DATA_INCOMPLETE; + FileDeliveryStatus deliveryStatus = FileDeliveryStatus::DISCARDED_DELIBERATELY; + uint32_t crc = 0; + uint64_t progress = 0; + RemoteEntityCfg* remoteCfg = nullptr; + }; + + std::vector tlvVec; + std::vector userTlvVec; + DestHandlerParams dp; + FsfwParams fp; + TransactionParams tp; + FsmResult fsmRes; + + ReturnValue_t startTransaction(MetadataPduReader& reader, MetadataInfo& info); + ReturnValue_t handleMetadataPdu(const PacketInfo& info); + ReturnValue_t handleFileDataPdu(const PacketInfo& info); + ReturnValue_t handleEofPdu(const PacketInfo& info); + ReturnValue_t handleMetadataParseError(ReturnValue_t result, const uint8_t* rawData, + size_t maxSize); + ReturnValue_t handleTransferCompletion(); + ReturnValue_t sendFinishedPdu(); + ReturnValue_t noticeOfCompletion(); + ReturnValue_t checksumVerification(); + const FsmResult& updateFsmRes(uint8_t errors); + void checkAndHandleError(ReturnValue_t result, uint8_t& errorIdx); + void finish(); +}; + +} // namespace cfdp + +#endif // FSFW_CFDP_CFDPDESTHANDLER_H diff --git a/src/fsfw/cfdp/handler/SourceHandler.cpp b/src/fsfw/cfdp/handler/SourceHandler.cpp new file mode 100644 index 00000000..513b25f3 --- /dev/null +++ b/src/fsfw/cfdp/handler/SourceHandler.cpp @@ -0,0 +1 @@ +#include "SourceHandler.h" diff --git a/src/fsfw/cfdp/handler/SourceHandler.h b/src/fsfw/cfdp/handler/SourceHandler.h new file mode 100644 index 00000000..319cf258 --- /dev/null +++ b/src/fsfw/cfdp/handler/SourceHandler.h @@ -0,0 +1,6 @@ +#ifndef FSFW_CFDP_CFDPSOURCEHANDLER_H +#define FSFW_CFDP_CFDPSOURCEHANDLER_H + +class SourceHandler {}; + +#endif // FSFW_CFDP_CFDPSOURCEHANDLER_H diff --git a/unittests/cfdp/handler/CMakeLists.txt b/unittests/cfdp/handler/CMakeLists.txt index 0993a398..f70e5dfb 100644 --- a/unittests/cfdp/handler/CMakeLists.txt +++ b/unittests/cfdp/handler/CMakeLists.txt @@ -1,2 +1,3 @@ -target_sources(${FSFW_TEST_TGT} PRIVATE testDistributor.cpp - testFaultHandler.cpp) +target_sources( + ${FSFW_TEST_TGT} PRIVATE testDistributor.cpp testDestHandler.cpp + testSourceHandler.cpp testFaultHandler.cpp) diff --git a/unittests/cfdp/handler/testDestHandler.cpp b/unittests/cfdp/handler/testDestHandler.cpp new file mode 100644 index 00000000..30bb9fc5 --- /dev/null +++ b/unittests/cfdp/handler/testDestHandler.cpp @@ -0,0 +1,244 @@ +#include + +#include +#include +#include + +#include "fsfw/cfdp.h" +#include "fsfw/cfdp/pdu/EofPduCreator.h" +#include "fsfw/cfdp/pdu/FileDataCreator.h" +#include "fsfw/cfdp/pdu/MetadataPduCreator.h" +#include "mocks/AcceptsTmMock.h" +#include "mocks/EventReportingProxyMock.h" +#include "mocks/FilesystemMock.h" +#include "mocks/MessageQueueMock.h" +#include "mocks/StorageManagerMock.h" +#include "mocks/cfdp/FaultHandlerMock.h" +#include "mocks/cfdp/RemoteConfigTableMock.h" +#include "mocks/cfdp/UserMock.h" + +TEST_CASE("CFDP Dest Handler", "[cfdp]") { + using namespace cfdp; + using namespace returnvalue; + MessageQueueId_t destQueueId = 2; + AcceptsTmMock tmReceiver(destQueueId); + MessageQueueMock mqMock(destQueueId); + EntityId localId = EntityId(UnsignedByteField(2)); + EntityId remoteId = EntityId(UnsignedByteField(3)); + FaultHandlerMock fhMock; + LocalEntityCfg localEntityCfg(localId, IndicationCfg(), fhMock); + FilesystemMock fsMock; + UserMock userMock(fsMock); + RemoteConfigTableMock remoteCfgTableMock; + PacketInfoList<64> packetInfoList; + LostSegmentsList<128> lostSegmentsList; + DestHandlerParams dp(localEntityCfg, userMock, remoteCfgTableMock, packetInfoList, + lostSegmentsList); + EventReportingProxyMock eventReporterMock; + LocalPool::LocalPoolConfig storeCfg = {{10, 32}, {10, 64}, {10, 128}, {10, 1024}}; + StorageManagerMock tcStore(2, storeCfg); + StorageManagerMock tmStore(3, storeCfg); + FsfwParams fp(tmReceiver, &mqMock, &eventReporterMock); + RemoteEntityCfg cfg(remoteId); + remoteCfgTableMock.addRemoteConfig(cfg); + fp.tcStore = &tcStore; + fp.tmStore = &tmStore; + uint8_t* buf = nullptr; + size_t serLen = 0; + store_address_t storeId; + PduConfig conf; + auto destHandler = DestHandler(dp, fp); + CHECK(destHandler.initialize() == OK); + + auto metadataPreparation = [&](FileSize cfdpFileSize, ChecksumTypes checksumType) { + std::string srcNameString = "hello.txt"; + std::string destNameString = "hello-cpy.txt"; + StringLv srcName(srcNameString); + StringLv destName(destNameString); + MetadataInfo info(false, checksumType, cfdpFileSize, srcName, destName); + TransactionSeqNum seqNum(UnsignedByteField(1)); + conf.sourceId = remoteId; + conf.destId = localId; + conf.mode = TransmissionModes::UNACKNOWLEDGED; + conf.seqNum = seqNum; + MetadataPduCreator metadataCreator(conf, info); + REQUIRE(tcStore.getFreeElement(&storeId, metadataCreator.getSerializedSize(), &buf) == OK); + REQUIRE(metadataCreator.serialize(buf, serLen, metadataCreator.getSerializedSize()) == OK); + PacketInfo packetInfo(metadataCreator.getPduType(), storeId, + metadataCreator.getDirectiveCode()); + packetInfoList.push_back(packetInfo); + }; + + auto metadataCheck = [&](const cfdp::DestHandler::FsmResult& res, const char* sourceName, + const char* destName, size_t fileLen) { + REQUIRE(res.result == OK); + REQUIRE(res.callStatus == CallStatus::CALL_AGAIN); + REQUIRE(res.errors == 0); + // Assert that the packet was deleted after handling + REQUIRE(not tcStore.hasDataAtId(storeId)); + REQUIRE(packetInfoList.empty()); + REQUIRE(userMock.metadataRecvd.size() == 1); + auto& idMetadataPair = userMock.metadataRecvd.back(); + REQUIRE(idMetadataPair.first == destHandler.getTransactionId()); + REQUIRE(idMetadataPair.second.sourceId.getValue() == 3); + REQUIRE(idMetadataPair.second.fileSize == fileLen); + REQUIRE(strcmp(idMetadataPair.second.destFileName, destName) == 0); + REQUIRE(strcmp(idMetadataPair.second.sourceFileName, sourceName) == 0); + userMock.metadataRecvd.pop(); + REQUIRE(fsMock.fileMap.find(destName) != fsMock.fileMap.end()); + REQUIRE(res.result == OK); + REQUIRE(res.state == CfdpStates::BUSY_CLASS_1_NACKED); + REQUIRE(res.step == DestHandler::TransactionStep::RECEIVING_FILE_DATA_PDUS); + }; + + auto eofPreparation = [&](FileSize cfdpFileSize, uint32_t crc) { + EofInfo eofInfo(cfdp::ConditionCode::NO_ERROR, crc, std::move(cfdpFileSize)); + EofPduCreator eofCreator(conf, eofInfo); + REQUIRE(tcStore.getFreeElement(&storeId, eofCreator.getSerializedSize(), &buf) == OK); + REQUIRE(eofCreator.serialize(buf, serLen, eofCreator.getSerializedSize()) == OK); + PacketInfo packetInfo(eofCreator.getPduType(), storeId, eofCreator.getDirectiveCode()); + packetInfoList.push_back(packetInfo); + }; + + auto eofCheck = [&](const cfdp::DestHandler::FsmResult& res, const TransactionId& id) { + REQUIRE(res.result == OK); + REQUIRE(res.state == CfdpStates::IDLE); + REQUIRE(res.errors == 0); + REQUIRE(res.step == DestHandler::TransactionStep::IDLE); + // Assert that the packet was deleted after handling + REQUIRE(not tcStore.hasDataAtId(storeId)); + REQUIRE(packetInfoList.empty()); + REQUIRE(userMock.eofsRevd.size() == 1); + auto& eofId = userMock.eofsRevd.back(); + CHECK(eofId == id); + REQUIRE(userMock.finishedRecvd.size() == 1); + auto& idParamPair = userMock.finishedRecvd.back(); + CHECK(idParamPair.first == id); + CHECK(idParamPair.second.condCode == ConditionCode::NO_ERROR); + }; + + auto fileDataPduCheck = [&](const cfdp::DestHandler::FsmResult& res, + const std::vector& idsToCheck) { + REQUIRE(res.result == OK); + REQUIRE(res.state == CfdpStates::BUSY_CLASS_1_NACKED); + REQUIRE(res.step == DestHandler::TransactionStep::RECEIVING_FILE_DATA_PDUS); + for (const auto id : idsToCheck) { + REQUIRE(not tcStore.hasDataAtId(id)); + } + REQUIRE(packetInfoList.empty()); + }; + + SECTION("State") { + CHECK(destHandler.getCfdpState() == CfdpStates::IDLE); + CHECK(destHandler.getTransactionStep() == DestHandler::TransactionStep::IDLE); + } + + SECTION("Idle State Machine Iteration") { + auto res = destHandler.performStateMachine(); + CHECK(res.result == OK); + CHECK(res.callStatus == CallStatus::CALL_AFTER_DELAY); + CHECK(res.errors == 0); + CHECK(destHandler.getCfdpState() == CfdpStates::IDLE); + CHECK(destHandler.getTransactionStep() == DestHandler::TransactionStep::IDLE); + } + + SECTION("Empty File Transfer") { + const DestHandler::FsmResult& res = destHandler.performStateMachine(); + CHECK(res.result == OK); + FileSize cfdpFileSize(0); + metadataPreparation(cfdpFileSize, ChecksumTypes::NULL_CHECKSUM); + destHandler.performStateMachine(); + metadataCheck(res, "hello.txt", "hello-cpy.txt", 0); + destHandler.performStateMachine(); + REQUIRE(res.callStatus == CallStatus::CALL_AFTER_DELAY); + auto transactionId = destHandler.getTransactionId(); + eofPreparation(cfdpFileSize, 0); + // After EOF, operation is done because no closure was requested + destHandler.performStateMachine(); + eofCheck(res, transactionId); + } + + SECTION("Small File Transfer") { + const DestHandler::FsmResult& res = destHandler.performStateMachine(); + CHECK(res.result == OK); + std::string fileData = "hello test data"; + etl::crc32 crcCalc; + crcCalc.add(fileData.begin(), fileData.end()); + uint32_t crc32 = crcCalc.value(); + FileSize cfdpFileSize(fileData.size()); + metadataPreparation(cfdpFileSize, ChecksumTypes::CRC_32); + destHandler.performStateMachine(); + metadataCheck(res, "hello.txt", "hello-cpy.txt", fileData.size()); + destHandler.performStateMachine(); + REQUIRE(res.callStatus == CallStatus::CALL_AFTER_DELAY); + auto transactionId = destHandler.getTransactionId(); + FileSize offset(0); + FileDataInfo fdPduInfo(offset, reinterpret_cast(fileData.data()), + fileData.size()); + FileDataCreator fdPduCreator(conf, fdPduInfo); + REQUIRE(tcStore.getFreeElement(&storeId, fdPduCreator.getSerializedSize(), &buf) == OK); + REQUIRE(fdPduCreator.serialize(buf, serLen, fdPduCreator.getSerializedSize()) == OK); + PacketInfo packetInfo(fdPduCreator.getPduType(), storeId, std::nullopt); + packetInfoList.push_back(packetInfo); + destHandler.performStateMachine(); + fileDataPduCheck(res, {storeId}); + eofPreparation(cfdpFileSize, crc32); + // After EOF, operation is done because no closure was requested + destHandler.performStateMachine(); + eofCheck(res, transactionId); + } + + SECTION("Segmented File Transfer") { + const DestHandler::FsmResult& res = destHandler.performStateMachine(); + CHECK(res.result == OK); + std::random_device dev; + std::mt19937 rng(dev()); + std::uniform_int_distribution distU8(0, 255); + std::array largerFileData{}; + for (auto& val : largerFileData) { + val = distU8(rng); + } + etl::crc32 crcCalc; + crcCalc.add(largerFileData.begin(), largerFileData.end()); + uint32_t crc32 = crcCalc.value(); + FileSize cfdpFileSize(largerFileData.size()); + metadataPreparation(cfdpFileSize, ChecksumTypes::CRC_32); + destHandler.performStateMachine(); + metadataCheck(res, "hello.txt", "hello-cpy.txt", largerFileData.size()); + destHandler.performStateMachine(); + REQUIRE(res.callStatus == CallStatus::CALL_AFTER_DELAY); + auto transactionId = destHandler.getTransactionId(); + + std::vector idsToCheck; + { + FileSize offset(0); + FileDataInfo fdPduInfo(offset, reinterpret_cast(largerFileData.data()), + largerFileData.size() / 2); + FileDataCreator fdPduCreator(conf, fdPduInfo); + REQUIRE(tcStore.getFreeElement(&storeId, fdPduCreator.getSerializedSize(), &buf) == OK); + REQUIRE(fdPduCreator.serialize(buf, serLen, fdPduCreator.getSerializedSize()) == OK); + PacketInfo packetInfo(fdPduCreator.getPduType(), storeId, std::nullopt); + idsToCheck.push_back(storeId); + packetInfoList.push_back(packetInfo); + } + + { + FileSize offset(512); + FileDataInfo fdPduInfo(offset, reinterpret_cast(largerFileData.data() + 512), + largerFileData.size() / 2); + FileDataCreator fdPduCreator(conf, fdPduInfo); + REQUIRE(tcStore.getFreeElement(&storeId, fdPduCreator.getSerializedSize(), &buf) == OK); + REQUIRE(fdPduCreator.serialize(buf, serLen, fdPduCreator.getSerializedSize()) == OK); + PacketInfo packetInfo(fdPduCreator.getPduType(), storeId, std::nullopt); + idsToCheck.push_back(storeId); + packetInfoList.push_back(packetInfo); + } + + destHandler.performStateMachine(); + fileDataPduCheck(res, idsToCheck); + eofPreparation(cfdpFileSize, crc32); + // After EOF, operation is done because no closure was requested + destHandler.performStateMachine(); + eofCheck(res, transactionId); + } +} \ No newline at end of file diff --git a/unittests/cfdp/handler/testSourceHandler.cpp b/unittests/cfdp/handler/testSourceHandler.cpp new file mode 100644 index 00000000..570ecb08 --- /dev/null +++ b/unittests/cfdp/handler/testSourceHandler.cpp @@ -0,0 +1,3 @@ +#include + +TEST_CASE("CFDP Source Handler", "[cfdp]") {} \ No newline at end of file From ee1c6a3f04a7fe982f4549b79afe696134c9c43b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 15 Sep 2022 13:37:10 +0200 Subject: [PATCH 188/404] better error printout --- src/fsfw/datapoollocal/LocalPoolObjectBase.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fsfw/datapoollocal/LocalPoolObjectBase.cpp b/src/fsfw/datapoollocal/LocalPoolObjectBase.cpp index 82aefc18..9edf02c5 100644 --- a/src/fsfw/datapoollocal/LocalPoolObjectBase.cpp +++ b/src/fsfw/datapoollocal/LocalPoolObjectBase.cpp @@ -48,12 +48,12 @@ LocalPoolObjectBase::LocalPoolObjectBase(object_id_t poolOwner, lp_id_t poolId, if (hkOwner == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "LocalPoolVariable: The supplied pool owner 0x" << std::hex << poolOwner - << std::dec << " did not implement the correct interface " + << std::dec << " does not exists or did not implement the correct interface " << "HasLocalDataPoolIF" << std::endl; #else sif::printError( - "LocalPoolVariable: The supplied pool owner 0x%08x did not implement the correct " - "interface HasLocalDataPoolIF\n", + "LocalPoolVariable: The supplied pool owner 0x%08x does not exists or does not implement " + "the correct interface HasLocalDataPoolIF\n", poolOwner); #endif return; From 7e0a5d5a9e4f38c6d818bbdd5b44d34d8007eb1e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 15 Sep 2022 13:40:12 +0200 Subject: [PATCH 189/404] printout tweak --- src/fsfw/datapoollocal/LocalPoolObjectBase.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/fsfw/datapoollocal/LocalPoolObjectBase.cpp b/src/fsfw/datapoollocal/LocalPoolObjectBase.cpp index 9edf02c5..5575c3d8 100644 --- a/src/fsfw/datapoollocal/LocalPoolObjectBase.cpp +++ b/src/fsfw/datapoollocal/LocalPoolObjectBase.cpp @@ -48,11 +48,11 @@ LocalPoolObjectBase::LocalPoolObjectBase(object_id_t poolOwner, lp_id_t poolId, if (hkOwner == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "LocalPoolVariable: The supplied pool owner 0x" << std::hex << poolOwner - << std::dec << " does not exists or did not implement the correct interface " + << std::dec << " does not exist or does not implement the correct interface " << "HasLocalDataPoolIF" << std::endl; #else sif::printError( - "LocalPoolVariable: The supplied pool owner 0x%08x does not exists or does not implement " + "LocalPoolVariable: The supplied pool owner 0x%08x does not exist or does not implement " "the correct interface HasLocalDataPoolIF\n", poolOwner); #endif From 753d5ff39ec0e4799128768a903bca35d01f9171 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 15 Sep 2022 16:47:09 +0200 Subject: [PATCH 190/404] adaptions for enum renaming --- src/fsfw/cfdp/handler/DestHandler.cpp | 8 ++++---- src/fsfw/cfdp/handler/DestHandler.h | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/fsfw/cfdp/handler/DestHandler.cpp b/src/fsfw/cfdp/handler/DestHandler.cpp index d43d1482..c63d4711 100644 --- a/src/fsfw/cfdp/handler/DestHandler.cpp +++ b/src/fsfw/cfdp/handler/DestHandler.cpp @@ -206,7 +206,7 @@ ReturnValue_t cfdp::DestHandler::handleEofPdu(const cfdp::PacketInfo& info) { return result; } // TODO: Error handling - if (eofInfo.getConditionCode() == ConditionCode::NO_ERROR) { + if (eofInfo.getConditionCode() == ConditionCodes::NO_ERROR) { tp.crc = eofInfo.getChecksum(); uint64_t fileSizeFromEof = eofInfo.getFileSize().value(); // CFDP 4.6.1.2.9: Declare file size error if progress exceeds file size @@ -345,7 +345,7 @@ ReturnValue_t cfdp::DestHandler::handleTransferCompletion() { // TODO: Warning / error handling? } } else { - tp.conditionCode = ConditionCode::NO_ERROR; + tp.conditionCode = ConditionCodes::NO_ERROR; } result = noticeOfCompletion(); if (result != OK) { @@ -398,14 +398,14 @@ ReturnValue_t cfdp::DestHandler::checksumVerification() { uint32_t value = crcCalc.value(); if (value == tp.crc) { - tp.conditionCode = ConditionCode::NO_ERROR; + tp.conditionCode = ConditionCodes::NO_ERROR; tp.deliveryCode = FileDeliveryCode::DATA_COMPLETE; } else { // TODO: Proper error handling #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "CRC check for file " << tp.destName.data() << " failed" << std::endl; #endif - tp.conditionCode = ConditionCode::FILE_CHECKSUM_FAILURE; + tp.conditionCode = ConditionCodes::FILE_CHECKSUM_FAILURE; } return OK; } diff --git a/src/fsfw/cfdp/handler/DestHandler.h b/src/fsfw/cfdp/handler/DestHandler.h index 000fc2c0..91618db2 100644 --- a/src/fsfw/cfdp/handler/DestHandler.h +++ b/src/fsfw/cfdp/handler/DestHandler.h @@ -150,7 +150,7 @@ class DestHandler { std::fill(sourceName.begin(), sourceName.end(), '\0'); std::fill(destName.begin(), destName.end(), '\0'); fileSize.setFileSize(0, false); - conditionCode = ConditionCode::NO_ERROR; + conditionCode = ConditionCodes::NO_ERROR; deliveryCode = FileDeliveryCode::DATA_INCOMPLETE; deliveryStatus = FileDeliveryStatus::DISCARDED_DELIBERATELY; crc = 0; @@ -167,7 +167,7 @@ class DestHandler { cfdp::FileSize fileSize; TransactionId transactionId; PduConfig pduConf; - ConditionCode conditionCode = ConditionCode::NO_ERROR; + ConditionCodes conditionCode = ConditionCodes::NO_ERROR; FileDeliveryCode deliveryCode = FileDeliveryCode::DATA_INCOMPLETE; FileDeliveryStatus deliveryStatus = FileDeliveryStatus::DISCARDED_DELIBERATELY; uint32_t crc = 0; From c549914efb845756800cc32d33b03d87e386b11e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 15 Sep 2022 16:50:06 +0200 Subject: [PATCH 191/404] include helpers --- src/fsfw/cfdp.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/fsfw/cfdp.h b/src/fsfw/cfdp.h index 28ddfec2..b2645978 100644 --- a/src/fsfw/cfdp.h +++ b/src/fsfw/cfdp.h @@ -4,6 +4,7 @@ #include "cfdp/definitions.h" #include "cfdp/handler/DestHandler.h" #include "cfdp/handler/FaultHandlerBase.h" +#include "cfdp/helpers.h" #include "cfdp/tlv/Lv.h" #include "cfdp/tlv/StringLv.h" #include "cfdp/tlv/Tlv.h" From ed68268c0cd69fd0932ed09fd34000c7d129371d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 15 Sep 2022 16:53:04 +0200 Subject: [PATCH 192/404] generic cfdp handler wrapper --- src/fsfw/cfdp.h | 1 + src/fsfw/cfdp/handler/CMakeLists.txt | 2 +- src/fsfw/cfdp/handler/CfdpHandler.cpp | 135 ++++++++++++++++++++++++++ src/fsfw/cfdp/handler/CfdpHandler.h | 65 +++++++++++++ 4 files changed, 202 insertions(+), 1 deletion(-) create mode 100644 src/fsfw/cfdp/handler/CfdpHandler.cpp create mode 100644 src/fsfw/cfdp/handler/CfdpHandler.h diff --git a/src/fsfw/cfdp.h b/src/fsfw/cfdp.h index b2645978..f6c01ad0 100644 --- a/src/fsfw/cfdp.h +++ b/src/fsfw/cfdp.h @@ -2,6 +2,7 @@ #define FSFW_CFDP_H #include "cfdp/definitions.h" +#include "cfdp/handler/CfdpHandler.h" #include "cfdp/handler/DestHandler.h" #include "cfdp/handler/FaultHandlerBase.h" #include "cfdp/helpers.h" diff --git a/src/fsfw/cfdp/handler/CMakeLists.txt b/src/fsfw/cfdp/handler/CMakeLists.txt index 7ad995c0..2e7dceef 100644 --- a/src/fsfw/cfdp/handler/CMakeLists.txt +++ b/src/fsfw/cfdp/handler/CMakeLists.txt @@ -1,2 +1,2 @@ target_sources(${LIB_FSFW_NAME} PRIVATE SourceHandler.cpp DestHandler.cpp - FaultHandlerBase.cpp UserBase.cpp) + FaultHandlerBase.cpp UserBase.cpp CfdpHandler.cpp) diff --git a/src/fsfw/cfdp/handler/CfdpHandler.cpp b/src/fsfw/cfdp/handler/CfdpHandler.cpp new file mode 100644 index 00000000..904f06b2 --- /dev/null +++ b/src/fsfw/cfdp/handler/CfdpHandler.cpp @@ -0,0 +1,135 @@ +#include "CfdpHandler.h" + +#include "fsfw/cfdp/pdu/AckPduReader.h" +#include "fsfw/cfdp/pdu/PduHeaderReader.h" +#include "fsfw/globalfunctions/arrayprinter.h" +#include "fsfw/ipc/QueueFactory.h" +#include "fsfw/tmtcservices/TmTcMessage.h" + +using namespace returnvalue; +using namespace cfdp; + +CfdpHandler::CfdpHandler(const FsfwHandlerParams& fsfwParams, const CfdpHandlerCfg& cfdpCfg) + : SystemObject(fsfwParams.objectId), + destHandler( + DestHandlerParams(LocalEntityCfg(cfdpCfg.id, cfdpCfg.indicCfg, cfdpCfg.faultHandler), + cfdpCfg.userHandler, cfdpCfg.remoteCfgProvider, cfdpCfg.packetInfoList, + cfdpCfg.lostSegmentsList), + FsfwParams(fsfwParams.packetDest, nullptr, this, fsfwParams.tcStore, + fsfwParams.tmStore)) { + // TODO: Make queue params configurable, or better yet, expect it to be passed externally + msgQueue = QueueFactory::instance()->createMessageQueue(); + destHandler.setMsgQueue(*msgQueue); +} + +[[nodiscard]] const char* CfdpHandler::getName() const { return "CFDP Handler"; } + +[[nodiscard]] uint32_t CfdpHandler::getIdentifier() const { + return destHandler.getDestHandlerParams().cfg.localId.getValue(); +} + +[[nodiscard]] MessageQueueId_t CfdpHandler::getRequestQueue() const { return msgQueue->getId(); } + +ReturnValue_t CfdpHandler::initialize() { + ReturnValue_t result = destHandler.initialize(); + if (result != OK) { + return result; + } + tcStore = destHandler.getTcStore(); + tmStore = destHandler.getTmStore(); + + return SystemObject::initialize(); +} + +ReturnValue_t CfdpHandler::performOperation(uint8_t operationCode) { + // TODO: Receive TC packets and route them to source and dest handler, depending on which is + // correct or more appropriate + ReturnValue_t status; + ReturnValue_t result = OK; + TmTcMessage tmtcMsg; + for (status = msgQueue->receiveMessage(&tmtcMsg); status == returnvalue::OK; + status = msgQueue->receiveMessage(&tmtcMsg)) { + result = handleCfdpPacket(tmtcMsg); + if (result != OK) { + status = result; + } + } + auto& fsmRes = destHandler.performStateMachine(); + // TODO: Error handling? + while (fsmRes.callStatus == CallStatus::CALL_AGAIN) { + destHandler.performStateMachine(); + // TODO: Error handling? + } + return status; +} + +ReturnValue_t CfdpHandler::handleCfdpPacket(TmTcMessage& msg) { + auto accessorPair = tcStore->getData(msg.getStorageId()); + if (accessorPair.first != OK) { + return accessorPair.first; + } + PduHeaderReader reader(accessorPair.second.data(), accessorPair.second.size()); + ReturnValue_t result = reader.parseData(); + if (result != returnvalue::OK) { + return INVALID_PDU_FORMAT; + } + // The CFDP distributor should have taken care of ensuring the destination ID is correct + PduTypes type = reader.getPduType(); + // Only the destination handler can process these PDUs + if (type == PduTypes::FILE_DATA) { + // Disable auto-deletion of packet + accessorPair.second.release(); + PacketInfo info(type, msg.getStorageId()); + result = destHandler.passPacket(info); + } else { + // Route depending on PDU type and directive type if applicable. It retrieves directive type + // from the raw stream for better performance (with sanity and directive code check). + // The routing is based on section 4.5 of the CFDP standard which specifies the PDU forwarding + // procedure. + + // PDU header only. Invalid supplied data. A directive packet should have a valid data field + // with at least one byte being the directive code + const uint8_t* pduDataField = reader.getPduDataField(); + if (pduDataField == nullptr) { + return INVALID_PDU_FORMAT; + } + if (not FileDirectiveReader::checkFileDirective(pduDataField[0])) { + return INVALID_DIRECTIVE_FIELD; + } + auto directive = static_cast(pduDataField[0]); + + auto passToDestHandler = [&]() { + accessorPair.second.release(); + PacketInfo info(type, msg.getStorageId(), directive); + result = destHandler.passPacket(info); + }; + auto passToSourceHandler = [&]() { + + }; + if (directive == FileDirectives::METADATA or directive == FileDirectives::EOF_DIRECTIVE or + directive == FileDirectives::PROMPT) { + // Section b) of 4.5.3: These PDUs should always be targeted towards the file receiver a.k.a. + // the destination handler + passToDestHandler(); + } else if (directive == FileDirectives::FINISH or directive == FileDirectives::NAK or + directive == FileDirectives::KEEP_ALIVE) { + // Section c) of 4.5.3: These PDUs should always be targeted towards the file sender a.k.a. + // the source handler + passToSourceHandler(); + } else if (directive == FileDirectives::ACK) { + // Section a): Recipient depends of the type of PDU that is being acknowledged. We can simply + // extract the PDU type from the raw stream. If it is an EOF PDU, this packet is passed to + // the source handler, for a Finished PDU, it is passed to the destination handler. + FileDirectives ackedDirective; + if (not AckPduReader::checkAckedDirectiveField(pduDataField[1], ackedDirective)) { + return INVALID_ACK_DIRECTIVE_FIELDS; + } + if (ackedDirective == FileDirectives::EOF_DIRECTIVE) { + passToSourceHandler(); + } else if (ackedDirective == FileDirectives::FINISH) { + passToDestHandler(); + } + } + } + return result; +} diff --git a/src/fsfw/cfdp/handler/CfdpHandler.h b/src/fsfw/cfdp/handler/CfdpHandler.h new file mode 100644 index 00000000..d7472905 --- /dev/null +++ b/src/fsfw/cfdp/handler/CfdpHandler.h @@ -0,0 +1,65 @@ +#ifndef FSFW_EXAMPLE_HOSTED_CFDPHANDLER_H +#define FSFW_EXAMPLE_HOSTED_CFDPHANDLER_H + +#include + +#include "fsfw/cfdp/handler/DestHandler.h" +#include "fsfw/objectmanager/SystemObject.h" +#include "fsfw/tasks/ExecutableObjectIF.h" +#include "fsfw/tmtcservices/AcceptsTelecommandsIF.h" +#include "fsfw/tmtcservices/TmTcMessage.h" + +struct FsfwHandlerParams { + FsfwHandlerParams(object_id_t objectId, HasFileSystemIF& vfs, AcceptsTelemetryIF& packetDest, + StorageManagerIF& tcStore, StorageManagerIF& tmStore) + : objectId(objectId), vfs(vfs), packetDest(packetDest), tcStore(tcStore), tmStore(tmStore) {} + object_id_t objectId{}; + HasFileSystemIF& vfs; + AcceptsTelemetryIF& packetDest; + StorageManagerIF& tcStore; + StorageManagerIF& tmStore; +}; + +struct CfdpHandlerCfg { + CfdpHandlerCfg(cfdp::EntityId localId, cfdp::IndicationCfg indicationCfg, + cfdp::UserBase& userHandler, cfdp::FaultHandlerBase& userFaultHandler, + cfdp::PacketInfoListBase& packetInfo, cfdp::LostSegmentsListBase& lostSegmentsList, + cfdp::RemoteConfigTableIF& remoteCfgProvider) + : id(std::move(localId)), + indicCfg(indicationCfg), + packetInfoList(packetInfo), + lostSegmentsList(lostSegmentsList), + remoteCfgProvider(remoteCfgProvider), + userHandler(userHandler), + faultHandler(userFaultHandler) {} + + cfdp::EntityId id; + cfdp::IndicationCfg indicCfg; + cfdp::PacketInfoListBase& packetInfoList; + cfdp::LostSegmentsListBase& lostSegmentsList; + cfdp::RemoteConfigTableIF& remoteCfgProvider; + cfdp::UserBase& userHandler; + cfdp::FaultHandlerBase& faultHandler; +}; + +class CfdpHandler : public SystemObject, public ExecutableObjectIF, public AcceptsTelecommandsIF { + public: + explicit CfdpHandler(const FsfwHandlerParams& fsfwParams, const CfdpHandlerCfg& cfdpCfg); + + [[nodiscard]] const char* getName() const override; + [[nodiscard]] uint32_t getIdentifier() const override; + [[nodiscard]] MessageQueueId_t getRequestQueue() const override; + + ReturnValue_t initialize() override; + ReturnValue_t performOperation(uint8_t operationCode) override; + + private: + MessageQueueIF* msgQueue = nullptr; + cfdp::DestHandler destHandler; + StorageManagerIF* tcStore = nullptr; + StorageManagerIF* tmStore = nullptr; + + ReturnValue_t handleCfdpPacket(TmTcMessage& msg); +}; + +#endif // FSFW_EXAMPLE_HOSTED_CFDPHANDLER_H From c38088c64bd4ba63246e5c44f5dae6d7c2efc0e2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 15 Sep 2022 18:44:00 +0200 Subject: [PATCH 193/404] adaptions for enum renaming --- src/fsfw/cfdp/handler/CfdpHandler.cpp | 22 ++++++++++---------- src/fsfw/cfdp/handler/DestHandler.cpp | 24 +++++++++++----------- src/fsfw/cfdp/handler/DestHandler.h | 16 +++++++-------- unittests/cfdp/handler/testDestHandler.cpp | 10 ++++----- 4 files changed, 36 insertions(+), 36 deletions(-) diff --git a/src/fsfw/cfdp/handler/CfdpHandler.cpp b/src/fsfw/cfdp/handler/CfdpHandler.cpp index 904f06b2..1ea5501b 100644 --- a/src/fsfw/cfdp/handler/CfdpHandler.cpp +++ b/src/fsfw/cfdp/handler/CfdpHandler.cpp @@ -74,9 +74,9 @@ ReturnValue_t CfdpHandler::handleCfdpPacket(TmTcMessage& msg) { return INVALID_PDU_FORMAT; } // The CFDP distributor should have taken care of ensuring the destination ID is correct - PduTypes type = reader.getPduType(); + PduType type = reader.getPduType(); // Only the destination handler can process these PDUs - if (type == PduTypes::FILE_DATA) { + if (type == PduType::FILE_DATA) { // Disable auto-deletion of packet accessorPair.second.release(); PacketInfo info(type, msg.getStorageId()); @@ -96,7 +96,7 @@ ReturnValue_t CfdpHandler::handleCfdpPacket(TmTcMessage& msg) { if (not FileDirectiveReader::checkFileDirective(pduDataField[0])) { return INVALID_DIRECTIVE_FIELD; } - auto directive = static_cast(pduDataField[0]); + auto directive = static_cast(pduDataField[0]); auto passToDestHandler = [&]() { accessorPair.second.release(); @@ -106,27 +106,27 @@ ReturnValue_t CfdpHandler::handleCfdpPacket(TmTcMessage& msg) { auto passToSourceHandler = [&]() { }; - if (directive == FileDirectives::METADATA or directive == FileDirectives::EOF_DIRECTIVE or - directive == FileDirectives::PROMPT) { + if (directive == FileDirective::METADATA or directive == FileDirective::EOF_DIRECTIVE or + directive == FileDirective::PROMPT) { // Section b) of 4.5.3: These PDUs should always be targeted towards the file receiver a.k.a. // the destination handler passToDestHandler(); - } else if (directive == FileDirectives::FINISH or directive == FileDirectives::NAK or - directive == FileDirectives::KEEP_ALIVE) { + } else if (directive == FileDirective::FINISH or directive == FileDirective::NAK or + directive == FileDirective::KEEP_ALIVE) { // Section c) of 4.5.3: These PDUs should always be targeted towards the file sender a.k.a. // the source handler passToSourceHandler(); - } else if (directive == FileDirectives::ACK) { + } else if (directive == FileDirective::ACK) { // Section a): Recipient depends of the type of PDU that is being acknowledged. We can simply // extract the PDU type from the raw stream. If it is an EOF PDU, this packet is passed to // the source handler, for a Finished PDU, it is passed to the destination handler. - FileDirectives ackedDirective; + FileDirective ackedDirective; if (not AckPduReader::checkAckedDirectiveField(pduDataField[1], ackedDirective)) { return INVALID_ACK_DIRECTIVE_FIELDS; } - if (ackedDirective == FileDirectives::EOF_DIRECTIVE) { + if (ackedDirective == FileDirective::EOF_DIRECTIVE) { passToSourceHandler(); - } else if (ackedDirective == FileDirectives::FINISH) { + } else if (ackedDirective == FileDirective::FINISH) { passToDestHandler(); } } diff --git a/src/fsfw/cfdp/handler/DestHandler.cpp b/src/fsfw/cfdp/handler/DestHandler.cpp index c63d4711..8049cc0a 100644 --- a/src/fsfw/cfdp/handler/DestHandler.cpp +++ b/src/fsfw/cfdp/handler/DestHandler.cpp @@ -29,8 +29,8 @@ const cfdp::DestHandler::FsmResult& cfdp::DestHandler::performStateMachine() { fsmRes.resetOfIteration(); if (fsmRes.step == TransactionStep::IDLE) { for (auto infoIter = dp.packetListRef.begin(); infoIter != dp.packetListRef.end();) { - if (infoIter->pduType == PduTypes::FILE_DIRECTIVE and - infoIter->directiveType == FileDirectives::METADATA) { + if (infoIter->pduType == PduType::FILE_DIRECTIVE and + infoIter->directiveType == FileDirective::METADATA) { result = handleMetadataPdu(*infoIter); checkAndHandleError(result, errorIdx); // Store data was deleted in PDU handler because a store guard is used @@ -57,13 +57,13 @@ const cfdp::DestHandler::FsmResult& cfdp::DestHandler::performStateMachine() { if (fsmRes.state == CfdpStates::BUSY_CLASS_1_NACKED) { if (fsmRes.step == TransactionStep::RECEIVING_FILE_DATA_PDUS) { for (auto infoIter = dp.packetListRef.begin(); infoIter != dp.packetListRef.end();) { - if (infoIter->pduType == PduTypes::FILE_DATA) { + if (infoIter->pduType == PduType::FILE_DATA) { result = handleFileDataPdu(*infoIter); checkAndHandleError(result, errorIdx); // Store data was deleted in PDU handler because a store guard is used dp.packetListRef.erase(infoIter++); - } else if (infoIter->pduType == PduTypes::FILE_DIRECTIVE and - infoIter->directiveType == FileDirectives::EOF_DIRECTIVE) { + } else if (infoIter->pduType == PduType::FILE_DIRECTIVE and + infoIter->directiveType == FileDirective::EOF_DIRECTIVE) { // TODO: Support for check timer missing result = handleEofPdu(*infoIter); checkAndHandleError(result, errorIdx); @@ -206,7 +206,7 @@ ReturnValue_t cfdp::DestHandler::handleEofPdu(const cfdp::PacketInfo& info) { return result; } // TODO: Error handling - if (eofInfo.getConditionCode() == ConditionCodes::NO_ERROR) { + if (eofInfo.getConditionCode() == ConditionCode::NO_ERROR) { tp.crc = eofInfo.getChecksum(); uint64_t fileSizeFromEof = eofInfo.getFileSize().value(); // CFDP 4.6.1.2.9: Declare file size error if progress exceeds file size @@ -272,9 +272,9 @@ ReturnValue_t cfdp::DestHandler::startTransaction(MetadataPduReader& reader, Met } ReturnValue_t result = OK; fsmRes.step = TransactionStep::TRANSACTION_START; - if (reader.getTransmissionMode() == TransmissionModes::UNACKNOWLEDGED) { + if (reader.getTransmissionMode() == TransmissionMode::UNACKNOWLEDGED) { fsmRes.state = CfdpStates::BUSY_CLASS_1_NACKED; - } else if (reader.getTransmissionMode() == TransmissionModes::ACKNOWLEDGED) { + } else if (reader.getTransmissionMode() == TransmissionMode::ACKNOWLEDGED) { fsmRes.state = CfdpStates::BUSY_CLASS_2_ACKED; } tp.checksumType = info.getChecksumType(); @@ -339,13 +339,13 @@ cfdp::CfdpStates cfdp::DestHandler::getCfdpState() const { return fsmRes.state; ReturnValue_t cfdp::DestHandler::handleTransferCompletion() { ReturnValue_t result; - if (tp.checksumType != ChecksumTypes::NULL_CHECKSUM) { + if (tp.checksumType != ChecksumType::NULL_CHECKSUM) { result = checksumVerification(); if (result != OK) { // TODO: Warning / error handling? } } else { - tp.conditionCode = ConditionCodes::NO_ERROR; + tp.conditionCode = ConditionCode::NO_ERROR; } result = noticeOfCompletion(); if (result != OK) { @@ -398,14 +398,14 @@ ReturnValue_t cfdp::DestHandler::checksumVerification() { uint32_t value = crcCalc.value(); if (value == tp.crc) { - tp.conditionCode = ConditionCodes::NO_ERROR; + tp.conditionCode = ConditionCode::NO_ERROR; tp.deliveryCode = FileDeliveryCode::DATA_COMPLETE; } else { // TODO: Proper error handling #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "CRC check for file " << tp.destName.data() << " failed" << std::endl; #endif - tp.conditionCode = ConditionCodes::FILE_CHECKSUM_FAILURE; + tp.conditionCode = ConditionCode::FILE_CHECKSUM_FAILURE; } return OK; } diff --git a/src/fsfw/cfdp/handler/DestHandler.h b/src/fsfw/cfdp/handler/DestHandler.h index 91618db2..5cef88d4 100644 --- a/src/fsfw/cfdp/handler/DestHandler.h +++ b/src/fsfw/cfdp/handler/DestHandler.h @@ -21,12 +21,12 @@ namespace cfdp { struct PacketInfo { - PacketInfo(PduTypes type, store_address_t storeId, - std::optional directive = std::nullopt) + PacketInfo(PduType type, store_address_t storeId, + std::optional directive = std::nullopt) : pduType(type), directiveType(directive), storeId(storeId) {} - PduTypes pduType = PduTypes::FILE_DATA; - std::optional directiveType = FileDirectives::INVALID_DIRECTIVE; + PduType pduType = PduType::FILE_DATA; + std::optional directiveType = FileDirective::INVALID_DIRECTIVE; store_address_t storeId = store_address_t::invalid(); PacketInfo() = default; }; @@ -150,24 +150,24 @@ class DestHandler { std::fill(sourceName.begin(), sourceName.end(), '\0'); std::fill(destName.begin(), destName.end(), '\0'); fileSize.setFileSize(0, false); - conditionCode = ConditionCodes::NO_ERROR; + conditionCode = ConditionCode::NO_ERROR; deliveryCode = FileDeliveryCode::DATA_INCOMPLETE; deliveryStatus = FileDeliveryStatus::DISCARDED_DELIBERATELY; crc = 0; progress = 0; remoteCfg = nullptr; closureRequested = false; - checksumType = ChecksumTypes::NULL_CHECKSUM; + checksumType = ChecksumType::NULL_CHECKSUM; } - ChecksumTypes checksumType = ChecksumTypes::NULL_CHECKSUM; + ChecksumType checksumType = ChecksumType::NULL_CHECKSUM; bool closureRequested = false; std::vector sourceName; std::vector destName; cfdp::FileSize fileSize; TransactionId transactionId; PduConfig pduConf; - ConditionCodes conditionCode = ConditionCodes::NO_ERROR; + ConditionCode conditionCode = ConditionCode::NO_ERROR; FileDeliveryCode deliveryCode = FileDeliveryCode::DATA_INCOMPLETE; FileDeliveryStatus deliveryStatus = FileDeliveryStatus::DISCARDED_DELIBERATELY; uint32_t crc = 0; diff --git a/unittests/cfdp/handler/testDestHandler.cpp b/unittests/cfdp/handler/testDestHandler.cpp index 30bb9fc5..0224a20b 100644 --- a/unittests/cfdp/handler/testDestHandler.cpp +++ b/unittests/cfdp/handler/testDestHandler.cpp @@ -50,7 +50,7 @@ TEST_CASE("CFDP Dest Handler", "[cfdp]") { auto destHandler = DestHandler(dp, fp); CHECK(destHandler.initialize() == OK); - auto metadataPreparation = [&](FileSize cfdpFileSize, ChecksumTypes checksumType) { + auto metadataPreparation = [&](FileSize cfdpFileSize, ChecksumType checksumType) { std::string srcNameString = "hello.txt"; std::string destNameString = "hello-cpy.txt"; StringLv srcName(srcNameString); @@ -59,7 +59,7 @@ TEST_CASE("CFDP Dest Handler", "[cfdp]") { TransactionSeqNum seqNum(UnsignedByteField(1)); conf.sourceId = remoteId; conf.destId = localId; - conf.mode = TransmissionModes::UNACKNOWLEDGED; + conf.mode = TransmissionMode::UNACKNOWLEDGED; conf.seqNum = seqNum; MetadataPduCreator metadataCreator(conf, info); REQUIRE(tcStore.getFreeElement(&storeId, metadataCreator.getSerializedSize(), &buf) == OK); @@ -146,7 +146,7 @@ TEST_CASE("CFDP Dest Handler", "[cfdp]") { const DestHandler::FsmResult& res = destHandler.performStateMachine(); CHECK(res.result == OK); FileSize cfdpFileSize(0); - metadataPreparation(cfdpFileSize, ChecksumTypes::NULL_CHECKSUM); + metadataPreparation(cfdpFileSize, ChecksumType::NULL_CHECKSUM); destHandler.performStateMachine(); metadataCheck(res, "hello.txt", "hello-cpy.txt", 0); destHandler.performStateMachine(); @@ -166,7 +166,7 @@ TEST_CASE("CFDP Dest Handler", "[cfdp]") { crcCalc.add(fileData.begin(), fileData.end()); uint32_t crc32 = crcCalc.value(); FileSize cfdpFileSize(fileData.size()); - metadataPreparation(cfdpFileSize, ChecksumTypes::CRC_32); + metadataPreparation(cfdpFileSize, ChecksumType::CRC_32); destHandler.performStateMachine(); metadataCheck(res, "hello.txt", "hello-cpy.txt", fileData.size()); destHandler.performStateMachine(); @@ -202,7 +202,7 @@ TEST_CASE("CFDP Dest Handler", "[cfdp]") { crcCalc.add(largerFileData.begin(), largerFileData.end()); uint32_t crc32 = crcCalc.value(); FileSize cfdpFileSize(largerFileData.size()); - metadataPreparation(cfdpFileSize, ChecksumTypes::CRC_32); + metadataPreparation(cfdpFileSize, ChecksumType::CRC_32); destHandler.performStateMachine(); metadataCheck(res, "hello.txt", "hello-cpy.txt", largerFileData.size()); destHandler.performStateMachine(); From e2c115833762f1887804fef76baebedf914e36d2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 16 Sep 2022 16:27:57 +0200 Subject: [PATCH 194/404] pass message queue externally --- src/fsfw/cfdp/handler/CfdpHandler.cpp | 13 ++++++------- src/fsfw/cfdp/handler/CfdpHandler.h | 8 +++++--- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/src/fsfw/cfdp/handler/CfdpHandler.cpp b/src/fsfw/cfdp/handler/CfdpHandler.cpp index 1ea5501b..9d20cc5e 100644 --- a/src/fsfw/cfdp/handler/CfdpHandler.cpp +++ b/src/fsfw/cfdp/handler/CfdpHandler.cpp @@ -11,15 +11,14 @@ using namespace cfdp; CfdpHandler::CfdpHandler(const FsfwHandlerParams& fsfwParams, const CfdpHandlerCfg& cfdpCfg) : SystemObject(fsfwParams.objectId), + msgQueue(fsfwParams.msgQueue), destHandler( DestHandlerParams(LocalEntityCfg(cfdpCfg.id, cfdpCfg.indicCfg, cfdpCfg.faultHandler), cfdpCfg.userHandler, cfdpCfg.remoteCfgProvider, cfdpCfg.packetInfoList, cfdpCfg.lostSegmentsList), FsfwParams(fsfwParams.packetDest, nullptr, this, fsfwParams.tcStore, - fsfwParams.tmStore)) { - // TODO: Make queue params configurable, or better yet, expect it to be passed externally - msgQueue = QueueFactory::instance()->createMessageQueue(); - destHandler.setMsgQueue(*msgQueue); + fsfwParams.tmStore)) { + destHandler.setMsgQueue(msgQueue); } [[nodiscard]] const char* CfdpHandler::getName() const { return "CFDP Handler"; } @@ -28,7 +27,7 @@ CfdpHandler::CfdpHandler(const FsfwHandlerParams& fsfwParams, const CfdpHandlerC return destHandler.getDestHandlerParams().cfg.localId.getValue(); } -[[nodiscard]] MessageQueueId_t CfdpHandler::getRequestQueue() const { return msgQueue->getId(); } +[[nodiscard]] MessageQueueId_t CfdpHandler::getRequestQueue() const { return msgQueue.getId(); } ReturnValue_t CfdpHandler::initialize() { ReturnValue_t result = destHandler.initialize(); @@ -47,8 +46,8 @@ ReturnValue_t CfdpHandler::performOperation(uint8_t operationCode) { ReturnValue_t status; ReturnValue_t result = OK; TmTcMessage tmtcMsg; - for (status = msgQueue->receiveMessage(&tmtcMsg); status == returnvalue::OK; - status = msgQueue->receiveMessage(&tmtcMsg)) { + for (status = msgQueue.receiveMessage(&tmtcMsg); status == returnvalue::OK; + status = msgQueue.receiveMessage(&tmtcMsg)) { result = handleCfdpPacket(tmtcMsg); if (result != OK) { status = result; diff --git a/src/fsfw/cfdp/handler/CfdpHandler.h b/src/fsfw/cfdp/handler/CfdpHandler.h index d7472905..147ffc70 100644 --- a/src/fsfw/cfdp/handler/CfdpHandler.h +++ b/src/fsfw/cfdp/handler/CfdpHandler.h @@ -11,13 +11,15 @@ struct FsfwHandlerParams { FsfwHandlerParams(object_id_t objectId, HasFileSystemIF& vfs, AcceptsTelemetryIF& packetDest, - StorageManagerIF& tcStore, StorageManagerIF& tmStore) - : objectId(objectId), vfs(vfs), packetDest(packetDest), tcStore(tcStore), tmStore(tmStore) {} + StorageManagerIF& tcStore, StorageManagerIF& tmStore, MessageQueueIF& msgQueue) + : objectId(objectId), vfs(vfs), packetDest(packetDest), tcStore(tcStore), tmStore(tmStore), + msgQueue(msgQueue) {} object_id_t objectId{}; HasFileSystemIF& vfs; AcceptsTelemetryIF& packetDest; StorageManagerIF& tcStore; StorageManagerIF& tmStore; + MessageQueueIF& msgQueue; }; struct CfdpHandlerCfg { @@ -54,7 +56,7 @@ class CfdpHandler : public SystemObject, public ExecutableObjectIF, public Accep ReturnValue_t performOperation(uint8_t operationCode) override; private: - MessageQueueIF* msgQueue = nullptr; + MessageQueueIF& msgQueue; cfdp::DestHandler destHandler; StorageManagerIF* tcStore = nullptr; StorageManagerIF* tmStore = nullptr; From bdbe0cc9da5edcaa8b01af4f6462a2f46d4628bd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 16 Sep 2022 16:27:57 +0200 Subject: [PATCH 195/404] pass message queue externally --- src/fsfw/cfdp/handler/CfdpHandler.cpp | 13 ++++++------- src/fsfw/cfdp/handler/CfdpHandler.h | 8 +++++--- 2 files changed, 11 insertions(+), 10 deletions(-) diff --git a/src/fsfw/cfdp/handler/CfdpHandler.cpp b/src/fsfw/cfdp/handler/CfdpHandler.cpp index 1ea5501b..9d20cc5e 100644 --- a/src/fsfw/cfdp/handler/CfdpHandler.cpp +++ b/src/fsfw/cfdp/handler/CfdpHandler.cpp @@ -11,15 +11,14 @@ using namespace cfdp; CfdpHandler::CfdpHandler(const FsfwHandlerParams& fsfwParams, const CfdpHandlerCfg& cfdpCfg) : SystemObject(fsfwParams.objectId), + msgQueue(fsfwParams.msgQueue), destHandler( DestHandlerParams(LocalEntityCfg(cfdpCfg.id, cfdpCfg.indicCfg, cfdpCfg.faultHandler), cfdpCfg.userHandler, cfdpCfg.remoteCfgProvider, cfdpCfg.packetInfoList, cfdpCfg.lostSegmentsList), FsfwParams(fsfwParams.packetDest, nullptr, this, fsfwParams.tcStore, - fsfwParams.tmStore)) { - // TODO: Make queue params configurable, or better yet, expect it to be passed externally - msgQueue = QueueFactory::instance()->createMessageQueue(); - destHandler.setMsgQueue(*msgQueue); + fsfwParams.tmStore)) { + destHandler.setMsgQueue(msgQueue); } [[nodiscard]] const char* CfdpHandler::getName() const { return "CFDP Handler"; } @@ -28,7 +27,7 @@ CfdpHandler::CfdpHandler(const FsfwHandlerParams& fsfwParams, const CfdpHandlerC return destHandler.getDestHandlerParams().cfg.localId.getValue(); } -[[nodiscard]] MessageQueueId_t CfdpHandler::getRequestQueue() const { return msgQueue->getId(); } +[[nodiscard]] MessageQueueId_t CfdpHandler::getRequestQueue() const { return msgQueue.getId(); } ReturnValue_t CfdpHandler::initialize() { ReturnValue_t result = destHandler.initialize(); @@ -47,8 +46,8 @@ ReturnValue_t CfdpHandler::performOperation(uint8_t operationCode) { ReturnValue_t status; ReturnValue_t result = OK; TmTcMessage tmtcMsg; - for (status = msgQueue->receiveMessage(&tmtcMsg); status == returnvalue::OK; - status = msgQueue->receiveMessage(&tmtcMsg)) { + for (status = msgQueue.receiveMessage(&tmtcMsg); status == returnvalue::OK; + status = msgQueue.receiveMessage(&tmtcMsg)) { result = handleCfdpPacket(tmtcMsg); if (result != OK) { status = result; diff --git a/src/fsfw/cfdp/handler/CfdpHandler.h b/src/fsfw/cfdp/handler/CfdpHandler.h index d7472905..147ffc70 100644 --- a/src/fsfw/cfdp/handler/CfdpHandler.h +++ b/src/fsfw/cfdp/handler/CfdpHandler.h @@ -11,13 +11,15 @@ struct FsfwHandlerParams { FsfwHandlerParams(object_id_t objectId, HasFileSystemIF& vfs, AcceptsTelemetryIF& packetDest, - StorageManagerIF& tcStore, StorageManagerIF& tmStore) - : objectId(objectId), vfs(vfs), packetDest(packetDest), tcStore(tcStore), tmStore(tmStore) {} + StorageManagerIF& tcStore, StorageManagerIF& tmStore, MessageQueueIF& msgQueue) + : objectId(objectId), vfs(vfs), packetDest(packetDest), tcStore(tcStore), tmStore(tmStore), + msgQueue(msgQueue) {} object_id_t objectId{}; HasFileSystemIF& vfs; AcceptsTelemetryIF& packetDest; StorageManagerIF& tcStore; StorageManagerIF& tmStore; + MessageQueueIF& msgQueue; }; struct CfdpHandlerCfg { @@ -54,7 +56,7 @@ class CfdpHandler : public SystemObject, public ExecutableObjectIF, public Accep ReturnValue_t performOperation(uint8_t operationCode) override; private: - MessageQueueIF* msgQueue = nullptr; + MessageQueueIF& msgQueue; cfdp::DestHandler destHandler; StorageManagerIF* tcStore = nullptr; StorageManagerIF* tmStore = nullptr; From c5b24f2516227ad84d92f34b12e386c7e0945a09 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 16 Sep 2022 18:08:20 +0200 Subject: [PATCH 196/404] fixes and tweaks --- src/fsfw/tcdistribution/definitions.h | 10 +++++----- src/fsfw/tmtcpacket/ccsds/defs.h | 4 ++-- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/fsfw/tcdistribution/definitions.h b/src/fsfw/tcdistribution/definitions.h index 01fc3085..bec1dd1b 100644 --- a/src/fsfw/tcdistribution/definitions.h +++ b/src/fsfw/tcdistribution/definitions.h @@ -17,11 +17,11 @@ static constexpr ReturnValue_t INVALID_PACKET_TYPE = MAKE_RETURN_CODE(3); static constexpr ReturnValue_t INVALID_SEC_HEADER_FIELD = MAKE_RETURN_CODE(4); static constexpr ReturnValue_t INCORRECT_PRIMARY_HEADER = MAKE_RETURN_CODE(5); -static constexpr ReturnValue_t INCOMPLETE_PACKET = MAKE_RETURN_CODE(5); -static constexpr ReturnValue_t INVALID_PUS_VERSION = MAKE_RETURN_CODE(6); -static constexpr ReturnValue_t INCORRECT_CHECKSUM = MAKE_RETURN_CODE(7); -static constexpr ReturnValue_t ILLEGAL_PACKET_SUBTYPE = MAKE_RETURN_CODE(8); -static constexpr ReturnValue_t INCORRECT_SECONDARY_HEADER = MAKE_RETURN_CODE(9); +static constexpr ReturnValue_t INCOMPLETE_PACKET = MAKE_RETURN_CODE(7); +static constexpr ReturnValue_t INVALID_PUS_VERSION = MAKE_RETURN_CODE(8); +static constexpr ReturnValue_t INCORRECT_CHECKSUM = MAKE_RETURN_CODE(9); +static constexpr ReturnValue_t ILLEGAL_PACKET_SUBTYPE = MAKE_RETURN_CODE(10); +static constexpr ReturnValue_t INCORRECT_SECONDARY_HEADER = MAKE_RETURN_CODE(11); static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::TMTC_DISTRIBUTION; //! P1: Returnvalue, P2: 0 for TM issues, 1 for TC issues diff --git a/src/fsfw/tmtcpacket/ccsds/defs.h b/src/fsfw/tmtcpacket/ccsds/defs.h index 1c7de540..da7c524d 100644 --- a/src/fsfw/tmtcpacket/ccsds/defs.h +++ b/src/fsfw/tmtcpacket/ccsds/defs.h @@ -23,11 +23,11 @@ constexpr uint16_t getSpacePacketIdFromApid(bool isTc, uint16_t apid, return ((isTc << 4) | (secondaryHeaderFlag << 3) | ((apid >> 8) & 0x07)) << 8 | (apid & 0x00ff); } -constexpr uint16_t getTcSpacePacketIdFromApid(uint16_t apid, bool secondaryHeaderFlag = true) { +constexpr uint16_t getTcSpacePacketIdFromApid(uint16_t apid, bool secondaryHeaderFlag) { return getSpacePacketIdFromApid(true, apid, secondaryHeaderFlag); } -constexpr uint16_t getTmSpacePacketIdFromApid(uint16_t apid, bool secondaryHeaderFlag = true) { +constexpr uint16_t getTmSpacePacketIdFromApid(uint16_t apid, bool secondaryHeaderFlag) { return getSpacePacketIdFromApid(false, apid, secondaryHeaderFlag); } From 808e3e0462999d7caa1b6834174140dbbeb8b01c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 16 Sep 2022 18:54:48 +0200 Subject: [PATCH 197/404] remove unused variable --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 35a7fd08..6a5db265 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1331,7 +1331,6 @@ ReturnValue_t DeviceHandlerBase::executeAction(ActionId_t actionId, MessageQueue return result; } DeviceCommandMap::iterator iter = deviceCommandMap.find(actionId); - MessageQueueId_t prevRecipient = MessageQueueIF::NO_QUEUE; if (iter == deviceCommandMap.end()) { result = COMMAND_NOT_SUPPORTED; } else if (iter->second.isExecuting) { From 6d2f44a432aaf36ab27bd47a54bb0c96479c1c81 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 27 Sep 2022 21:46:11 +0200 Subject: [PATCH 198/404] tweaks to make windows build again --- src/fsfw/osal/host/Clock.cpp | 1 + src/fsfw/osal/windows/winTaskHelpers.cpp | 1 + src/fsfw/osal/windows/winTaskHelpers.h | 4 +++- src/fsfw/timemanager/ClockCommon.cpp | 8 +++++++- src/fsfw/timemanager/TimeReaderIF.h | 6 ++++++ 5 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/fsfw/osal/host/Clock.cpp b/src/fsfw/osal/host/Clock.cpp index 29c6c1a6..dbf6529c 100644 --- a/src/fsfw/osal/host/Clock.cpp +++ b/src/fsfw/osal/host/Clock.cpp @@ -8,6 +8,7 @@ #if defined(PLATFORM_WIN) #include +#define timegm _mkgmtime #elif defined(PLATFORM_UNIX) #include #endif diff --git a/src/fsfw/osal/windows/winTaskHelpers.cpp b/src/fsfw/osal/windows/winTaskHelpers.cpp index 206ee7a7..5b4feb86 100644 --- a/src/fsfw/osal/windows/winTaskHelpers.cpp +++ b/src/fsfw/osal/windows/winTaskHelpers.cpp @@ -1,6 +1,7 @@ #include "fsfw/osal/windows/winTaskHelpers.h" #include +#include TaskPriority tasks::makeWinPriority(PriorityClass prioClass, PriorityNumber prioNumber) { return (static_cast(prioClass) << 16) | static_cast(prioNumber); diff --git a/src/fsfw/osal/windows/winTaskHelpers.h b/src/fsfw/osal/windows/winTaskHelpers.h index 87cd92ce..2d6ef9b4 100644 --- a/src/fsfw/osal/windows/winTaskHelpers.h +++ b/src/fsfw/osal/windows/winTaskHelpers.h @@ -1,10 +1,12 @@ #include #include -#include "../../tasks/TaskFactory.h" +#include "fsfw/tasks/TaskFactory.h" #ifdef _WIN32 +#include + namespace tasks { enum PriorityClass : uint16_t { diff --git a/src/fsfw/timemanager/ClockCommon.cpp b/src/fsfw/timemanager/ClockCommon.cpp index 45755f35..d0ac9004 100644 --- a/src/fsfw/timemanager/ClockCommon.cpp +++ b/src/fsfw/timemanager/ClockCommon.cpp @@ -61,10 +61,16 @@ ReturnValue_t Clock::convertTimevalToTimeOfDay(const timeval* from, TimeOfDay_t* if (result != returnvalue::OK) { return result; } - MutexGuard helper(timeMutex); // gmtime writes its output in a global buffer which is not Thread Safe // Therefore we have to use a Mutex here + MutexGuard helper(timeMutex); +#ifdef PLATFORM_WIN + time_t time; + time = from->tv_sec; + timeInfo = gmtime(&time); +#else timeInfo = gmtime(&from->tv_sec); +#endif to->year = timeInfo->tm_year + 1900; to->month = timeInfo->tm_mon + 1; to->day = timeInfo->tm_mday; diff --git a/src/fsfw/timemanager/TimeReaderIF.h b/src/fsfw/timemanager/TimeReaderIF.h index a3343e1c..ceb0a872 100644 --- a/src/fsfw/timemanager/TimeReaderIF.h +++ b/src/fsfw/timemanager/TimeReaderIF.h @@ -2,6 +2,12 @@ #define FSFW_TIMEMANAGER_TIMEREADERIF_H #include +#include "fsfw/platform.h" + +#ifdef PLATFORM_WIN +// wtf? Required for timeval! +#include +#endif #include "TimeStampIF.h" #include "fsfw/returnvalues/returnvalue.h" From 03e1a9325034bd9805bfc3a1aa65e9c188762d12 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 27 Sep 2022 23:57:58 +0200 Subject: [PATCH 199/404] last windows tweak --- src/fsfw_hal/common/gpio/gpioDefinitions.h | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/fsfw_hal/common/gpio/gpioDefinitions.h b/src/fsfw_hal/common/gpio/gpioDefinitions.h index a15ffbc0..b8e628ea 100644 --- a/src/fsfw_hal/common/gpio/gpioDefinitions.h +++ b/src/fsfw_hal/common/gpio/gpioDefinitions.h @@ -5,6 +5,13 @@ #include #include +#ifdef PLATFORM_WIN +// What is this crap? +#undef IN +#undef OUT +#undef CALLBACK +#endif + using gpioId_t = uint16_t; namespace gpio { From 2ca8d72e83d79c1ddf810509738c2107b92221cd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 28 Sep 2022 00:03:12 +0200 Subject: [PATCH 200/404] another small windows tweak --- src/fsfw/osal/common/TcpIpBase.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/fsfw/osal/common/TcpIpBase.cpp b/src/fsfw/osal/common/TcpIpBase.cpp index 7e989c36..de0a573d 100644 --- a/src/fsfw/osal/common/TcpIpBase.cpp +++ b/src/fsfw/osal/common/TcpIpBase.cpp @@ -1,5 +1,6 @@ #include "fsfw/osal/common/TcpIpBase.h" +#include "fsfw/serviceinterface.h" #include "fsfw/platform.h" #ifdef PLATFORM_UNIX From 14a48fe41de59a1a04cc7a2468748cb0c0c001d2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 28 Sep 2022 09:56:20 +0200 Subject: [PATCH 201/404] better comment --- src/fsfw_hal/common/gpio/gpioDefinitions.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/fsfw_hal/common/gpio/gpioDefinitions.h b/src/fsfw_hal/common/gpio/gpioDefinitions.h index b8e628ea..1f921a57 100644 --- a/src/fsfw_hal/common/gpio/gpioDefinitions.h +++ b/src/fsfw_hal/common/gpio/gpioDefinitions.h @@ -6,7 +6,8 @@ #include #ifdef PLATFORM_WIN -// What is this crap? +// Defined in Windows header for whatever reason, and leads to nameclash issues with +// class enums which have entries of the same name. #undef IN #undef OUT #undef CALLBACK From c47bed07606548fd2510caa45bdd9fc867732065 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 28 Sep 2022 17:04:11 +0200 Subject: [PATCH 202/404] small important bugfix for DLE parser --- src/fsfw/globalfunctions/DleParser.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/fsfw/globalfunctions/DleParser.h b/src/fsfw/globalfunctions/DleParser.h index f1a4f193..5ccd55df 100644 --- a/src/fsfw/globalfunctions/DleParser.h +++ b/src/fsfw/globalfunctions/DleParser.h @@ -44,6 +44,7 @@ class DleParser { Context(void* args) : userArgs(args) { setType(ContextType::PACKET_FOUND); } void setType(ContextType type) { + this->type = type; if (type == ContextType::PACKET_FOUND) { error.first = ErrorTypes::NONE; error.second.len = 0; From 227535c4618d90558570a8fae788ef52e7e971cc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Sep 2022 16:46:55 +0200 Subject: [PATCH 203/404] formatting and smaller stuff --- src/fsfw/cfdp/handler/CMakeLists.txt | 5 +++-- src/fsfw/cfdp/handler/CfdpHandler.cpp | 2 +- src/fsfw/cfdp/handler/CfdpHandler.h | 6 +++++- src/fsfw/osal/common/TcpIpBase.cpp | 2 +- src/fsfw/osal/windows/winTaskHelpers.cpp | 3 ++- src/fsfw/timemanager/TimeReaderIF.h | 1 + src/fsfw/tmtcservices/SpacePacketParser.h | 5 +---- unittests/mocks/cfdp/FaultHandlerMock.h | 2 +- 8 files changed, 15 insertions(+), 11 deletions(-) diff --git a/src/fsfw/cfdp/handler/CMakeLists.txt b/src/fsfw/cfdp/handler/CMakeLists.txt index 2e7dceef..d96ce91c 100644 --- a/src/fsfw/cfdp/handler/CMakeLists.txt +++ b/src/fsfw/cfdp/handler/CMakeLists.txt @@ -1,2 +1,3 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE SourceHandler.cpp DestHandler.cpp - FaultHandlerBase.cpp UserBase.cpp CfdpHandler.cpp) +target_sources( + ${LIB_FSFW_NAME} PRIVATE SourceHandler.cpp DestHandler.cpp + FaultHandlerBase.cpp UserBase.cpp CfdpHandler.cpp) diff --git a/src/fsfw/cfdp/handler/CfdpHandler.cpp b/src/fsfw/cfdp/handler/CfdpHandler.cpp index 9d20cc5e..902097b6 100644 --- a/src/fsfw/cfdp/handler/CfdpHandler.cpp +++ b/src/fsfw/cfdp/handler/CfdpHandler.cpp @@ -17,7 +17,7 @@ CfdpHandler::CfdpHandler(const FsfwHandlerParams& fsfwParams, const CfdpHandlerC cfdpCfg.userHandler, cfdpCfg.remoteCfgProvider, cfdpCfg.packetInfoList, cfdpCfg.lostSegmentsList), FsfwParams(fsfwParams.packetDest, nullptr, this, fsfwParams.tcStore, - fsfwParams.tmStore)) { + fsfwParams.tmStore)) { destHandler.setMsgQueue(msgQueue); } diff --git a/src/fsfw/cfdp/handler/CfdpHandler.h b/src/fsfw/cfdp/handler/CfdpHandler.h index 147ffc70..2de9f7dd 100644 --- a/src/fsfw/cfdp/handler/CfdpHandler.h +++ b/src/fsfw/cfdp/handler/CfdpHandler.h @@ -12,7 +12,11 @@ struct FsfwHandlerParams { FsfwHandlerParams(object_id_t objectId, HasFileSystemIF& vfs, AcceptsTelemetryIF& packetDest, StorageManagerIF& tcStore, StorageManagerIF& tmStore, MessageQueueIF& msgQueue) - : objectId(objectId), vfs(vfs), packetDest(packetDest), tcStore(tcStore), tmStore(tmStore), + : objectId(objectId), + vfs(vfs), + packetDest(packetDest), + tcStore(tcStore), + tmStore(tmStore), msgQueue(msgQueue) {} object_id_t objectId{}; HasFileSystemIF& vfs; diff --git a/src/fsfw/osal/common/TcpIpBase.cpp b/src/fsfw/osal/common/TcpIpBase.cpp index de0a573d..486a5171 100644 --- a/src/fsfw/osal/common/TcpIpBase.cpp +++ b/src/fsfw/osal/common/TcpIpBase.cpp @@ -1,7 +1,7 @@ #include "fsfw/osal/common/TcpIpBase.h" -#include "fsfw/serviceinterface.h" #include "fsfw/platform.h" +#include "fsfw/serviceinterface.h" #ifdef PLATFORM_UNIX #include diff --git a/src/fsfw/osal/windows/winTaskHelpers.cpp b/src/fsfw/osal/windows/winTaskHelpers.cpp index 5b4feb86..235dca1e 100644 --- a/src/fsfw/osal/windows/winTaskHelpers.cpp +++ b/src/fsfw/osal/windows/winTaskHelpers.cpp @@ -1,8 +1,9 @@ #include "fsfw/osal/windows/winTaskHelpers.h" -#include #include +#include + TaskPriority tasks::makeWinPriority(PriorityClass prioClass, PriorityNumber prioNumber) { return (static_cast(prioClass) << 16) | static_cast(prioNumber); } diff --git a/src/fsfw/timemanager/TimeReaderIF.h b/src/fsfw/timemanager/TimeReaderIF.h index 6bd763c7..8dce097f 100644 --- a/src/fsfw/timemanager/TimeReaderIF.h +++ b/src/fsfw/timemanager/TimeReaderIF.h @@ -2,6 +2,7 @@ #define FSFW_TIMEMANAGER_TIMEREADERIF_H #include + #include "fsfw/platform.h" #ifdef PLATFORM_WIN diff --git a/src/fsfw/tmtcservices/SpacePacketParser.h b/src/fsfw/tmtcservices/SpacePacketParser.h index 3a1b4d16..ea0a2feb 100644 --- a/src/fsfw/tmtcservices/SpacePacketParser.h +++ b/src/fsfw/tmtcservices/SpacePacketParser.h @@ -17,7 +17,6 @@ */ class SpacePacketParser { public: - struct FoundPacketInfo { size_t startIdx = 0; size_t sizeFound = 0; @@ -51,9 +50,7 @@ class SpacePacketParser { ReturnValue_t parseSpacePackets(const uint8_t** buffer, const size_t maxSize, FoundPacketInfo& packetInfo); - size_t getAmountRead() { - return amountRead; - } + size_t getAmountRead() { return amountRead; } void reset() { nextStartIdx = 0; diff --git a/unittests/mocks/cfdp/FaultHandlerMock.h b/unittests/mocks/cfdp/FaultHandlerMock.h index 1c59485c..5e094509 100644 --- a/unittests/mocks/cfdp/FaultHandlerMock.h +++ b/unittests/mocks/cfdp/FaultHandlerMock.h @@ -17,7 +17,7 @@ class FaultHandlerMock : public FaultHandlerBase { void noticeOfSuspensionCb(TransactionId& id, ConditionCode code) override; void noticeOfCancellationCb(TransactionId& id, ConditionCode code) override; - void abandonCb(TransactionId& id,ConditionCode code) override; + void abandonCb(TransactionId& id, ConditionCode code) override; void ignoreCb(TransactionId& id, ConditionCode code) override; FaultInfo& getFhInfo(FaultHandlerCode fhCode); From 78314ad9668a2e01408a0111f7f306bbb468a40f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Sep 2022 16:47:23 +0200 Subject: [PATCH 204/404] this makes a bit more sense --- src/fsfw/devicehandlers/ChildHandlerBase.cpp | 11 +++-------- src/fsfw/devicehandlers/ChildHandlerBase.h | 3 +-- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 10 ---------- src/fsfw/devicehandlers/DeviceHandlerBase.h | 3 ++- src/fsfw/pus/Service5EventReporting.cpp | 4 ++-- src/fsfw/subsystem/SubsystemBase.cpp | 13 ++++++++++--- 6 files changed, 18 insertions(+), 26 deletions(-) diff --git a/src/fsfw/devicehandlers/ChildHandlerBase.cpp b/src/fsfw/devicehandlers/ChildHandlerBase.cpp index ecd4cfc8..39a3a20f 100644 --- a/src/fsfw/devicehandlers/ChildHandlerBase.cpp +++ b/src/fsfw/devicehandlers/ChildHandlerBase.cpp @@ -3,17 +3,12 @@ #include "fsfw/subsystem/SubsystemBase.h" ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, - CookieIF* cookie, object_id_t hkDestination, - uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, - object_id_t parent, FailureIsolationBase* customFdir, - size_t cmdQueueSize) + CookieIF* cookie, object_id_t parent, + FailureIsolationBase* customFdir, size_t cmdQueueSize) : DeviceHandlerBase(setObjectId, deviceCommunication, cookie, (customFdir == nullptr ? &childHandlerFdir : customFdir), cmdQueueSize), parentId(parent), - childHandlerFdir(setObjectId) { - this->setHkDestination(hkDestination); - this->setThermalStateRequestPoolIds(thermalStatePoolId, thermalRequestPoolId); -} + childHandlerFdir(setObjectId) {} ChildHandlerBase::~ChildHandlerBase() {} diff --git a/src/fsfw/devicehandlers/ChildHandlerBase.h b/src/fsfw/devicehandlers/ChildHandlerBase.h index 19d48a24..8145c391 100644 --- a/src/fsfw/devicehandlers/ChildHandlerBase.h +++ b/src/fsfw/devicehandlers/ChildHandlerBase.h @@ -7,8 +7,7 @@ class ChildHandlerBase : public DeviceHandlerBase { public: ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, CookieIF* cookie, - object_id_t hkDestination, uint32_t thermalStatePoolId, - uint32_t thermalRequestPoolId, object_id_t parent = objects::NO_OBJECT, + object_id_t parent = objects::NO_OBJECT, FailureIsolationBase* customFdir = nullptr, size_t cmdQueueSize = 20); virtual ~ChildHandlerBase(); diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 6a5db265..db9c147b 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -132,14 +132,6 @@ ReturnValue_t DeviceHandlerBase::initialize() { 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); if (communicationInterface == nullptr) { @@ -1578,8 +1570,6 @@ MessageQueueId_t DeviceHandlerBase::getCommanderQueueId(DeviceCommandId_t replyI 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 7c93e921..52ae473b 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -104,7 +104,6 @@ class DeviceHandlerBase : public DeviceHandlerIF, 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); @@ -200,6 +199,8 @@ class DeviceHandlerBase : public DeviceHandlerIF, virtual object_id_t getObjectId() const override; /** + * This is a helper method for classes which are parent nodes in the mode tree. + * It registers the passed queue as the destination for mode and health messages. * @param parentQueueId */ virtual void setParentQueue(MessageQueueId_t parentQueueId); diff --git a/src/fsfw/pus/Service5EventReporting.cpp b/src/fsfw/pus/Service5EventReporting.cpp index c1affa6f..b4b9b03d 100644 --- a/src/fsfw/pus/Service5EventReporting.cpp +++ b/src/fsfw/pus/Service5EventReporting.cpp @@ -15,8 +15,8 @@ Service5EventReporting::Service5EventReporting(PsbParams params, size_t maxNumbe maxNumberReportsPerCycle(maxNumberReportsPerCycle) { auto mqArgs = MqArgs(getObjectId(), static_cast(this)); psbParams.name = "PUS 5 Event Reporting"; - eventQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth, - MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); + eventQueue = QueueFactory::instance()->createMessageQueue( + messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } Service5EventReporting::~Service5EventReporting() { diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp index 2ae2cb77..cfad43e9 100644 --- a/src/fsfw/subsystem/SubsystemBase.cpp +++ b/src/fsfw/subsystem/SubsystemBase.cpp @@ -1,8 +1,9 @@ #include "fsfw/subsystem/SubsystemBase.h" +#include "fsfw/FSFW.h" #include "fsfw/ipc/QueueFactory.h" #include "fsfw/objectmanager/ObjectManager.h" -#include "fsfw/serviceinterface/ServiceInterface.h" +#include "fsfw/serviceinterface.h" SubsystemBase::SubsystemBase(object_id_t setObjectId, object_id_t parent, Mode_t initialMode, uint16_t commandQueueDepth) @@ -158,7 +159,7 @@ ReturnValue_t SubsystemBase::updateChildChangedHealth(MessageQueueId_t queue, bo MessageQueueId_t SubsystemBase::getCommandQueue() const { return commandQueue->getId(); } ReturnValue_t SubsystemBase::initialize() { - MessageQueueId_t parentQueue = MessageQueueIF::NO_QUEUE; + MessageQueueId_t also = MessageQueueIF::NO_QUEUE; ReturnValue_t result = SystemObject::initialize(); if (result != returnvalue::OK) { @@ -240,8 +241,14 @@ ReturnValue_t SubsystemBase::handleModeReply(CommandMessage* message) { } ReturnValue_t SubsystemBase::checkTable(HybridIterator tableIter) { - for (; tableIter.value != NULL; ++tableIter) { + for (; tableIter.value != nullptr; ++tableIter) { if (childrenMap.find(tableIter.value->getObject()) == childrenMap.end()) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + using namespace std; + sif::warning << "SubsystemBase::checkTable: Could not find Object " << setfill('0') << hex + << "0x" << setw(8) << tableIter.value->getObject() << " in object " << setw(8) + << "0x" << getObjectId() << dec << std::endl; +#endif return TABLE_CONTAINS_INVALID_OBJECT_ID; } } From 77f7fa2ef125635d87ad9fd3f0cf79e1cd74d071 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Sep 2022 16:48:23 +0200 Subject: [PATCH 205/404] typo --- src/fsfw/subsystem/SubsystemBase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp index cfad43e9..a7d75fea 100644 --- a/src/fsfw/subsystem/SubsystemBase.cpp +++ b/src/fsfw/subsystem/SubsystemBase.cpp @@ -159,7 +159,7 @@ ReturnValue_t SubsystemBase::updateChildChangedHealth(MessageQueueId_t queue, bo MessageQueueId_t SubsystemBase::getCommandQueue() const { return commandQueue->getId(); } ReturnValue_t SubsystemBase::initialize() { - MessageQueueId_t also = MessageQueueIF::NO_QUEUE; + MessageQueueId_t parentQueue = MessageQueueIF::NO_QUEUE; ReturnValue_t result = SystemObject::initialize(); if (result != returnvalue::OK) { From b9d0ff8fb7401db087d9a68dc9918a8f88033560 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Sep 2022 17:20:18 +0200 Subject: [PATCH 206/404] DHB bug --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 6a5db265..8f23b112 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -526,16 +526,16 @@ ReturnValue_t DeviceHandlerBase::updatePeriodicReply(bool enable, DeviceCommandI if (enable) { info->active = true; if (info->countdown != nullptr) { - info->delayCycles = info->maxDelayCycles; - } else { info->countdown->resetTimer(); + } else { + info->delayCycles = info->maxDelayCycles; } } else { info->active = false; if (info->countdown != nullptr) { - info->delayCycles = 0; - } else { info->countdown->timeOut(); + } else { + info->delayCycles = 0; } } } From 7c5308429ca61b041800626bb03387ae12a5e585 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Sep 2022 19:21:24 +0200 Subject: [PATCH 207/404] this seems to work --- src/fsfw/controller/ControllerBase.cpp | 36 ++++++------------- src/fsfw/controller/ControllerBase.h | 14 ++++++-- .../controller/ExtendedControllerBase.cpp | 5 ++- src/fsfw/controller/ExtendedControllerBase.h | 2 +- src/fsfw/subsystem/CMakeLists.txt | 3 +- src/fsfw/subsystem/HasModeTreeChildrenIF.h | 13 +++++++ src/fsfw/subsystem/ModeTreeChildIF.h | 15 ++++++++ src/fsfw/subsystem/SubsystemBase.cpp | 17 +++++++++ src/fsfw/subsystem/SubsystemBase.h | 20 ++++++----- src/fsfw/subsystem/helper.cpp | 13 +++++++ src/fsfw/subsystem/helper.h | 14 ++++++++ .../subsystem/modes/ModeTreeConnectionIF.h | 13 +++++++ .../integration/controller/TestController.cpp | 4 +-- .../integration/controller/TestController.h | 2 +- 14 files changed, 127 insertions(+), 44 deletions(-) create mode 100644 src/fsfw/subsystem/HasModeTreeChildrenIF.h create mode 100644 src/fsfw/subsystem/ModeTreeChildIF.h create mode 100644 src/fsfw/subsystem/helper.cpp create mode 100644 src/fsfw/subsystem/helper.h create mode 100644 src/fsfw/subsystem/modes/ModeTreeConnectionIF.h diff --git a/src/fsfw/controller/ControllerBase.cpp b/src/fsfw/controller/ControllerBase.cpp index 80c08590..e071b3ee 100644 --- a/src/fsfw/controller/ControllerBase.cpp +++ b/src/fsfw/controller/ControllerBase.cpp @@ -4,11 +4,10 @@ #include "fsfw/ipc/QueueFactory.h" #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/subsystem/SubsystemBase.h" +#include "fsfw/subsystem/helper.h" -ControllerBase::ControllerBase(object_id_t setObjectId, object_id_t parentId, - size_t commandQueueDepth) +ControllerBase::ControllerBase(object_id_t setObjectId, size_t commandQueueDepth) : SystemObject(setObjectId), - parentId(parentId), mode(MODE_OFF), submode(SUBMODE_NONE), modeHelper(this), @@ -25,28 +24,6 @@ ReturnValue_t ControllerBase::initialize() { if (result != returnvalue::OK) { return result; } - - MessageQueueId_t parentQueue = 0; - if (parentId != objects::NO_OBJECT) { - auto* parent = ObjectManager::instance()->get(parentId); - if (parent == nullptr) { - return returnvalue::FAILED; - } - parentQueue = parent->getCommandQueue(); - - parent->registerChild(getObjectId()); - } - - result = healthHelper.initialize(parentQueue); - if (result != returnvalue::OK) { - return result; - } - - result = modeHelper.initialize(parentQueue); - if (result != returnvalue::OK) { - return result; - } - return returnvalue::OK; } @@ -120,3 +97,12 @@ void ControllerBase::setTaskIF(PeriodicTaskIF* task_) { executingTask = task_; } void ControllerBase::changeHK(Mode_t mode_, Submode_t submode_, bool enable) {} ReturnValue_t ControllerBase::initializeAfterTaskCreation() { return returnvalue::OK; } + +const HasHealthIF* ControllerBase::getOptHealthIF() const { return this; } + +const HasModesIF& ControllerBase::getModeIF() const { return *this; } + +ReturnValue_t ControllerBase::connectModeTreeParent(HasModeTreeChildrenIF& parent, + const ModeTreeChildIF& child) { + return modetree::connectModeTreeParent(parent, child, healthHelper, modeHelper); +} diff --git a/src/fsfw/controller/ControllerBase.h b/src/fsfw/controller/ControllerBase.h index 56c28585..8f9a1497 100644 --- a/src/fsfw/controller/ControllerBase.h +++ b/src/fsfw/controller/ControllerBase.h @@ -6,6 +6,9 @@ #include "fsfw/modes/HasModesIF.h" #include "fsfw/modes/ModeHelper.h" #include "fsfw/objectmanager/SystemObject.h" +#include "fsfw/subsystem/HasModeTreeChildrenIF.h" +#include "fsfw/subsystem/ModeTreeChildIF.h" +#include "fsfw/subsystem/modes/ModeTreeConnectionIF.h" #include "fsfw/tasks/ExecutableObjectIF.h" #include "fsfw/tasks/PeriodicTaskIF.h" @@ -18,13 +21,18 @@ class ControllerBase : public HasModesIF, public HasHealthIF, public ExecutableObjectIF, + public ModeTreeChildIF, + public ModeTreeConnectionIF, public SystemObject { public: static const Mode_t MODE_NORMAL = 2; - ControllerBase(object_id_t setObjectId, object_id_t parentId, size_t commandQueueDepth = 3); + ControllerBase(object_id_t setObjectId, size_t commandQueueDepth = 3); ~ControllerBase() override; + ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent, + const ModeTreeChildIF &child) override; + /** SystemObject override */ ReturnValue_t initialize() override; @@ -38,6 +46,8 @@ class ControllerBase : public HasModesIF, ReturnValue_t performOperation(uint8_t opCode) override; void setTaskIF(PeriodicTaskIF *task) override; ReturnValue_t initializeAfterTaskCreation() override; + const HasHealthIF *getOptHealthIF() const override; + const HasModesIF &getModeIF() const override; protected: /** @@ -56,8 +66,6 @@ class ControllerBase : public HasModesIF, ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, uint32_t *msToReachTheMode) override = 0; - const object_id_t parentId; - Mode_t mode; Submode_t submode; diff --git a/src/fsfw/controller/ExtendedControllerBase.cpp b/src/fsfw/controller/ExtendedControllerBase.cpp index 4d5c90c8..58db3563 100644 --- a/src/fsfw/controller/ExtendedControllerBase.cpp +++ b/src/fsfw/controller/ExtendedControllerBase.cpp @@ -1,8 +1,7 @@ #include "fsfw/controller/ExtendedControllerBase.h" -ExtendedControllerBase::ExtendedControllerBase(object_id_t objectId, object_id_t parentId, - size_t commandQueueDepth) - : ControllerBase(objectId, parentId, commandQueueDepth), +ExtendedControllerBase::ExtendedControllerBase(object_id_t objectId, size_t commandQueueDepth) + : ControllerBase(objectId, commandQueueDepth), poolManager(this, commandQueue), actionHelper(this, commandQueue) {} diff --git a/src/fsfw/controller/ExtendedControllerBase.h b/src/fsfw/controller/ExtendedControllerBase.h index b5583a88..04a79528 100644 --- a/src/fsfw/controller/ExtendedControllerBase.h +++ b/src/fsfw/controller/ExtendedControllerBase.h @@ -17,7 +17,7 @@ class ExtendedControllerBase : public ControllerBase, public HasActionsIF, public HasLocalDataPoolIF { public: - ExtendedControllerBase(object_id_t objectId, object_id_t parentId, size_t commandQueueDepth = 3); + ExtendedControllerBase(object_id_t objectId, size_t commandQueueDepth = 3); ~ExtendedControllerBase() override; /* SystemObjectIF overrides */ diff --git a/src/fsfw/subsystem/CMakeLists.txt b/src/fsfw/subsystem/CMakeLists.txt index 164c90f7..95dce8a5 100644 --- a/src/fsfw/subsystem/CMakeLists.txt +++ b/src/fsfw/subsystem/CMakeLists.txt @@ -1,3 +1,4 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE Subsystem.cpp SubsystemBase.cpp) +target_sources(${LIB_FSFW_NAME} PRIVATE Subsystem.cpp SubsystemBase.cpp + helper.cpp) add_subdirectory(modes) diff --git a/src/fsfw/subsystem/HasModeTreeChildrenIF.h b/src/fsfw/subsystem/HasModeTreeChildrenIF.h new file mode 100644 index 00000000..3458d008 --- /dev/null +++ b/src/fsfw/subsystem/HasModeTreeChildrenIF.h @@ -0,0 +1,13 @@ +#ifndef FSFW_SUBSYSTEM_HASMODETREECHILDRENIF_H_ +#define FSFW_SUBSYSTEM_HASMODETREECHILDRENIF_H_ + +#include "ModeTreeChildIF.h" + +class HasModeTreeChildrenIF { + public: + virtual ~HasModeTreeChildrenIF() = default; + virtual ReturnValue_t registerChild(const ModeTreeChildIF& child) = 0; + virtual MessageQueueId_t getCommandQueue() const = 0; +}; + +#endif // FSFW_SUBSYSTEM_HASMODETREECHILDRENIF_H_ diff --git a/src/fsfw/subsystem/ModeTreeChildIF.h b/src/fsfw/subsystem/ModeTreeChildIF.h new file mode 100644 index 00000000..6f8d8d86 --- /dev/null +++ b/src/fsfw/subsystem/ModeTreeChildIF.h @@ -0,0 +1,15 @@ +#ifndef FSFW_SUBSYSTEM_MODETREECHILDIF_H_ +#define FSFW_SUBSYSTEM_MODETREECHILDIF_H_ + +#include +#include + +class ModeTreeChildIF { + public: + virtual ~ModeTreeChildIF() = default; + virtual object_id_t getObjectId() const = 0; + virtual const HasHealthIF* getOptHealthIF() const = 0; + virtual const HasModesIF& getModeIF() const = 0; +}; + +#endif /* FSFW_SUBSYSTEM_MODETREECHILDIF_H_ */ diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp index a7d75fea..9f79011e 100644 --- a/src/fsfw/subsystem/SubsystemBase.cpp +++ b/src/fsfw/subsystem/SubsystemBase.cpp @@ -334,3 +334,20 @@ ReturnValue_t SubsystemBase::setHealth(HealthState health) { HasHealthIF::HealthState SubsystemBase::getHealth() { return healthHelper.getHealth(); } void SubsystemBase::modeChanged() {} + +ReturnValue_t SubsystemBase::registerChild(const ModeTreeChildIF& child) { + ChildInfo info; + + const HasModesIF& modeChild = child.getModeIF(); + // intentional to force an initial command during system startup + info.commandQueue = modeChild.getCommandQueue(); + info.mode = HasModesIF::MODE_UNDEFINED; + info.submode = SUBMODE_NONE; + info.healthChanged = false; + + auto resultPair = childrenMap.emplace(child.getObjectId(), info); + if (not resultPair.second) { + return COULD_NOT_INSERT_CHILD; + } + return returnvalue::OK; +} diff --git a/src/fsfw/subsystem/SubsystemBase.h b/src/fsfw/subsystem/SubsystemBase.h index 69bdfd37..063bca85 100644 --- a/src/fsfw/subsystem/SubsystemBase.h +++ b/src/fsfw/subsystem/SubsystemBase.h @@ -3,14 +3,15 @@ #include -#include "../container/HybridIterator.h" -#include "../health/HasHealthIF.h" -#include "../health/HealthHelper.h" -#include "../ipc/MessageQueueIF.h" -#include "../modes/HasModesIF.h" -#include "../objectmanager/SystemObject.h" -#include "../returnvalues/returnvalue.h" -#include "../tasks/ExecutableObjectIF.h" +#include "fsfw/container/HybridIterator.h" +#include "fsfw/health/HasHealthIF.h" +#include "fsfw/health/HealthHelper.h" +#include "fsfw/ipc/MessageQueueIF.h" +#include "fsfw/modes/HasModesIF.h" +#include "fsfw/objectmanager/SystemObject.h" +#include "fsfw/returnvalues/returnvalue.h" +#include "fsfw/subsystem/HasModeTreeChildrenIF.h" +#include "fsfw/tasks/ExecutableObjectIF.h" #include "modes/HasModeSequenceIF.h" /** @@ -27,6 +28,7 @@ class SubsystemBase : public SystemObject, public HasModesIF, public HasHealthIF, + public HasModeTreeChildrenIF, public ExecutableObjectIF { public: static const uint8_t INTERFACE_ID = CLASS_ID::SUBSYSTEM_BASE; @@ -129,6 +131,8 @@ class SubsystemBase : public SystemObject, virtual void performChildOperation() = 0; + ReturnValue_t registerChild(const ModeTreeChildIF &child) override; + virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, uint32_t *msToReachTheMode) override = 0; diff --git a/src/fsfw/subsystem/helper.cpp b/src/fsfw/subsystem/helper.cpp new file mode 100644 index 00000000..e65ef5a9 --- /dev/null +++ b/src/fsfw/subsystem/helper.cpp @@ -0,0 +1,13 @@ +#include "helper.h" + +ReturnValue_t modetree::connectModeTreeParent(HasModeTreeChildrenIF& parent, + const ModeTreeChildIF& child, + HealthHelper& healthHelper, ModeHelper& modeHelper) { + ReturnValue_t result = parent.registerChild(child); + if (result != returnvalue::OK) { + return result; + } + healthHelper.setParentQueue(parent.getCommandQueue()); + modeHelper.setParentQueue(parent.getCommandQueue()); + return returnvalue::OK; +} diff --git a/src/fsfw/subsystem/helper.h b/src/fsfw/subsystem/helper.h new file mode 100644 index 00000000..71a5563f --- /dev/null +++ b/src/fsfw/subsystem/helper.h @@ -0,0 +1,14 @@ +#ifndef FSFW_SUBSYSTEM_HELPER_H_ +#define FSFW_SUBSYSTEM_HELPER_H_ + +#include "HasModeTreeChildrenIF.h" +#include "fsfw/health/HealthHelper.h" + +namespace modetree { + +ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent, const ModeTreeChildIF& child, + HealthHelper& healthHelper, ModeHelper& modeHelper); + +} + +#endif /* FSFW_SRC_FSFW_SUBSYSTEM_HELPER_H_ */ diff --git a/src/fsfw/subsystem/modes/ModeTreeConnectionIF.h b/src/fsfw/subsystem/modes/ModeTreeConnectionIF.h new file mode 100644 index 00000000..8f25b2d4 --- /dev/null +++ b/src/fsfw/subsystem/modes/ModeTreeConnectionIF.h @@ -0,0 +1,13 @@ +#ifndef FSFW_SUBSYSTEM_MODES_MODETREECONNECTIONIF_H_ +#define FSFW_SUBSYSTEM_MODES_MODETREECONNECTIONIF_H_ + +#include "fsfw/subsystem/HasModeTreeChildrenIF.h" + +class ModeTreeConnectionIF { + public: + virtual ~ModeTreeConnectionIF() = default; + virtual ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent, + const ModeTreeChildIF &child) = 0; +}; + +#endif /* FSFW_SRC_FSFW_SUBSYSTEM_MODES_MODETREECONNECTIONIF_H_ */ diff --git a/src/fsfw_tests/integration/controller/TestController.cpp b/src/fsfw_tests/integration/controller/TestController.cpp index 2ee4d5fe..c489f336 100644 --- a/src/fsfw_tests/integration/controller/TestController.cpp +++ b/src/fsfw_tests/integration/controller/TestController.cpp @@ -4,8 +4,8 @@ #include #include -TestController::TestController(object_id_t objectId, object_id_t parentId, size_t commandQueueDepth) - : ExtendedControllerBase(objectId, parentId, commandQueueDepth) {} +TestController::TestController(object_id_t objectId, size_t commandQueueDepth) + : ExtendedControllerBase(objectId, commandQueueDepth) {} TestController::~TestController() {} diff --git a/src/fsfw_tests/integration/controller/TestController.h b/src/fsfw_tests/integration/controller/TestController.h index 9898371f..9577bedf 100644 --- a/src/fsfw_tests/integration/controller/TestController.h +++ b/src/fsfw_tests/integration/controller/TestController.h @@ -7,7 +7,7 @@ class TestController : public ExtendedControllerBase { public: - TestController(object_id_t objectId, object_id_t parentId, size_t commandQueueDepth = 10); + TestController(object_id_t objectId, size_t commandQueueDepth = 10); virtual ~TestController(); protected: From f824004897e29bf90c2b02578625ba3d51786fdf Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 29 Sep 2022 19:39:37 +0200 Subject: [PATCH 208/404] need to fix all of these TODOs --- src/fsfw/controller/ControllerBase.cpp | 5 +- src/fsfw/controller/ControllerBase.h | 3 +- src/fsfw/devicehandlers/AssemblyBase.cpp | 4 +- src/fsfw/devicehandlers/AssemblyBase.h | 2 +- src/fsfw/devicehandlers/ChildHandlerBase.cpp | 31 +++---- src/fsfw/subsystem/Subsystem.cpp | 4 +- src/fsfw/subsystem/Subsystem.h | 3 +- src/fsfw/subsystem/SubsystemBase.cpp | 84 ++++--------------- src/fsfw/subsystem/SubsystemBase.h | 25 +++--- .../subsystem/modes/ModeTreeConnectionIF.h | 3 +- .../integration/assemblies/TestAssembly.cpp | 17 ++-- 11 files changed, 67 insertions(+), 114 deletions(-) diff --git a/src/fsfw/controller/ControllerBase.cpp b/src/fsfw/controller/ControllerBase.cpp index e071b3ee..ee1dba46 100644 --- a/src/fsfw/controller/ControllerBase.cpp +++ b/src/fsfw/controller/ControllerBase.cpp @@ -102,7 +102,6 @@ const HasHealthIF* ControllerBase::getOptHealthIF() const { return this; } const HasModesIF& ControllerBase::getModeIF() const { return *this; } -ReturnValue_t ControllerBase::connectModeTreeParent(HasModeTreeChildrenIF& parent, - const ModeTreeChildIF& child) { - return modetree::connectModeTreeParent(parent, child, healthHelper, modeHelper); +ReturnValue_t ControllerBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) { + return modetree::connectModeTreeParent(parent, *this, healthHelper, modeHelper); } diff --git a/src/fsfw/controller/ControllerBase.h b/src/fsfw/controller/ControllerBase.h index 8f9a1497..6d9e2937 100644 --- a/src/fsfw/controller/ControllerBase.h +++ b/src/fsfw/controller/ControllerBase.h @@ -30,8 +30,7 @@ class ControllerBase : public HasModesIF, ControllerBase(object_id_t setObjectId, size_t commandQueueDepth = 3); ~ControllerBase() override; - ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent, - const ModeTreeChildIF &child) override; + ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override; /** SystemObject override */ ReturnValue_t initialize() override; diff --git a/src/fsfw/devicehandlers/AssemblyBase.cpp b/src/fsfw/devicehandlers/AssemblyBase.cpp index c943a4cf..14575e69 100644 --- a/src/fsfw/devicehandlers/AssemblyBase.cpp +++ b/src/fsfw/devicehandlers/AssemblyBase.cpp @@ -1,7 +1,7 @@ #include "fsfw/devicehandlers/AssemblyBase.h" -AssemblyBase::AssemblyBase(object_id_t objectId, object_id_t parentId, uint16_t commandQueueDepth) - : SubsystemBase(objectId, parentId, MODE_OFF, commandQueueDepth), +AssemblyBase::AssemblyBase(object_id_t objectId, uint16_t commandQueueDepth) + : SubsystemBase(objectId, MODE_OFF, commandQueueDepth), internalState(STATE_NONE), recoveryState(RECOVERY_IDLE), recoveringDevice(childrenMap.end()), diff --git a/src/fsfw/devicehandlers/AssemblyBase.h b/src/fsfw/devicehandlers/AssemblyBase.h index fe1c4218..5e0d826f 100644 --- a/src/fsfw/devicehandlers/AssemblyBase.h +++ b/src/fsfw/devicehandlers/AssemblyBase.h @@ -41,7 +41,7 @@ class AssemblyBase : public SubsystemBase { static const ReturnValue_t NEED_TO_CHANGE_HEALTH = MAKE_RETURN_CODE(0x05); static const ReturnValue_t NOT_ENOUGH_CHILDREN_IN_CORRECT_STATE = MAKE_RETURN_CODE(0xa1); - AssemblyBase(object_id_t objectId, object_id_t parentId, uint16_t commandQueueDepth = 8); + AssemblyBase(object_id_t objectId, uint16_t commandQueueDepth = 8); virtual ~AssemblyBase(); protected: diff --git a/src/fsfw/devicehandlers/ChildHandlerBase.cpp b/src/fsfw/devicehandlers/ChildHandlerBase.cpp index 39a3a20f..50d7b765 100644 --- a/src/fsfw/devicehandlers/ChildHandlerBase.cpp +++ b/src/fsfw/devicehandlers/ChildHandlerBase.cpp @@ -18,21 +18,22 @@ ReturnValue_t ChildHandlerBase::initialize() { return result; } - MessageQueueId_t parentQueue = 0; - - if (parentId != objects::NO_OBJECT) { - SubsystemBase* parent = ObjectManager::instance()->get(parentId); - if (parent == NULL) { - return returnvalue::FAILED; - } - parentQueue = parent->getCommandQueue(); - - parent->registerChild(getObjectId()); - } - - healthHelper.setParentQueue(parentQueue); - - modeHelper.setParentQueue(parentQueue); + // TODO: Fix this + // MessageQueueId_t parentQueue = 0; + // + // if (parentId != objects::NO_OBJECT) { + // SubsystemBase* parent = ObjectManager::instance()->get(parentId); + // if (parent == NULL) { + // return returnvalue::FAILED; + // } + // parentQueue = parent->getCommandQueue(); + // + // parent->registerChild(getObjectId()); + // } + // + // healthHelper.setParentQueue(parentQueue); + // + // modeHelper.setParentQueue(parentQueue); return returnvalue::OK; } diff --git a/src/fsfw/subsystem/Subsystem.cpp b/src/fsfw/subsystem/Subsystem.cpp index b2af5ac3..085c843a 100644 --- a/src/fsfw/subsystem/Subsystem.cpp +++ b/src/fsfw/subsystem/Subsystem.cpp @@ -9,9 +9,9 @@ #include "fsfw/serialize/SerialLinkedListAdapter.h" #include "fsfw/serialize/SerializeElement.h" -Subsystem::Subsystem(object_id_t setObjectId, object_id_t parent, uint32_t maxNumberOfSequences, +Subsystem::Subsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables) - : SubsystemBase(setObjectId, parent, 0), + : SubsystemBase(setObjectId, 0), isInTransition(false), childrenChangedHealth(false), currentTargetTable(), diff --git a/src/fsfw/subsystem/Subsystem.h b/src/fsfw/subsystem/Subsystem.h index f4e73117..20925821 100644 --- a/src/fsfw/subsystem/Subsystem.h +++ b/src/fsfw/subsystem/Subsystem.h @@ -66,8 +66,7 @@ class Subsystem : public SubsystemBase, public HasModeSequenceIF { * @param maxNumberOfSequences * @param maxNumberOfTables */ - Subsystem(object_id_t setObjectId, object_id_t parent, uint32_t maxNumberOfSequences, - uint32_t maxNumberOfTables); + Subsystem(object_id_t setObjectId, uint32_t maxNumberOfSequences, uint32_t maxNumberOfTables); virtual ~Subsystem(); ReturnValue_t addSequence(SequenceEntry sequence); diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp index 9f79011e..058bcca9 100644 --- a/src/fsfw/subsystem/SubsystemBase.cpp +++ b/src/fsfw/subsystem/SubsystemBase.cpp @@ -4,14 +4,14 @@ #include "fsfw/ipc/QueueFactory.h" #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/serviceinterface.h" +#include "fsfw/subsystem/helper.h" -SubsystemBase::SubsystemBase(object_id_t setObjectId, object_id_t parent, Mode_t initialMode, +SubsystemBase::SubsystemBase(object_id_t setObjectId, Mode_t initialMode, uint16_t commandQueueDepth) : SystemObject(setObjectId), mode(initialMode), healthHelper(this, setObjectId), - modeHelper(this), - parentId(parent) { + modeHelper(this) { auto mqArgs = MqArgs(setObjectId, static_cast(this)); commandQueue = QueueFactory::instance()->createMessageQueue( commandQueueDepth, CommandMessage::MAX_MESSAGE_SIZE, &mqArgs); @@ -19,36 +19,6 @@ SubsystemBase::SubsystemBase(object_id_t setObjectId, object_id_t parent, Mode_t SubsystemBase::~SubsystemBase() { QueueFactory::instance()->deleteMessageQueue(commandQueue); } -ReturnValue_t SubsystemBase::registerChild(object_id_t objectId) { - ChildInfo info; - - HasModesIF* child = ObjectManager::instance()->get(objectId); - // This is a rather ugly hack to have the changedHealth info for all - // children available. - HasHealthIF* healthChild = ObjectManager::instance()->get(objectId); - if (child == nullptr) { - if (healthChild == nullptr) { - return CHILD_DOESNT_HAVE_MODES; - } else { - info.commandQueue = healthChild->getCommandQueue(); - info.mode = MODE_OFF; - } - } else { - // intentional to force an initial command during system startup - info.commandQueue = child->getCommandQueue(); - info.mode = HasModesIF::MODE_UNDEFINED; - } - - info.submode = SUBMODE_NONE; - info.healthChanged = false; - - auto resultPair = childrenMap.emplace(objectId, info); - if (not resultPair.second) { - return COULD_NOT_INSERT_CHILD; - } - return returnvalue::OK; -} - ReturnValue_t SubsystemBase::checkStateAgainstTable(HybridIterator tableIter, Submode_t targetSubmode) { std::map::iterator childIter; @@ -88,7 +58,8 @@ void SubsystemBase::executeTable(HybridIterator tableIter, Submod if ((iter = childrenMap.find(object)) == childrenMap.end()) { // illegal table entry, should only happen due to misconfigured mode table #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::debug << std::hex << getObjectId() << ": invalid mode table entry" << std::endl; + sif::debug << std::hex << SystemObject::getObjectId() << ": invalid mode table entry" + << std::endl; #endif continue; } @@ -158,38 +129,7 @@ ReturnValue_t SubsystemBase::updateChildChangedHealth(MessageQueueId_t queue, bo MessageQueueId_t SubsystemBase::getCommandQueue() const { return commandQueue->getId(); } -ReturnValue_t SubsystemBase::initialize() { - MessageQueueId_t parentQueue = MessageQueueIF::NO_QUEUE; - ReturnValue_t result = SystemObject::initialize(); - - if (result != returnvalue::OK) { - return result; - } - - if (parentId != objects::NO_OBJECT) { - SubsystemBase* parent = ObjectManager::instance()->get(parentId); - if (parent == nullptr) { - return returnvalue::FAILED; - } - parentQueue = parent->getCommandQueue(); - - parent->registerChild(getObjectId()); - } - - result = healthHelper.initialize(parentQueue); - - if (result != returnvalue::OK) { - return result; - } - - result = modeHelper.initialize(parentQueue); - - if (result != returnvalue::OK) { - return result; - } - - return returnvalue::OK; -} +ReturnValue_t SubsystemBase::initialize() { return SystemObject::initialize(); } ReturnValue_t SubsystemBase::performOperation(uint8_t opCode) { childrenChangedMode = false; @@ -247,7 +187,7 @@ ReturnValue_t SubsystemBase::checkTable(HybridIterator tableIter) using namespace std; sif::warning << "SubsystemBase::checkTable: Could not find Object " << setfill('0') << hex << "0x" << setw(8) << tableIter.value->getObject() << " in object " << setw(8) - << "0x" << getObjectId() << dec << std::endl; + << "0x" << SystemObject::getObjectId() << dec << std::endl; #endif return TABLE_CONTAINS_INVALID_OBJECT_ID; } @@ -333,6 +273,12 @@ ReturnValue_t SubsystemBase::setHealth(HealthState health) { HasHealthIF::HealthState SubsystemBase::getHealth() { return healthHelper.getHealth(); } +ReturnValue_t SubsystemBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) { + return modetree::connectModeTreeParent(parent, *this, healthHelper, modeHelper); +} + +object_id_t SubsystemBase::getObjectId() const { return SystemObject::getObjectId(); } + void SubsystemBase::modeChanged() {} ReturnValue_t SubsystemBase::registerChild(const ModeTreeChildIF& child) { @@ -351,3 +297,7 @@ ReturnValue_t SubsystemBase::registerChild(const ModeTreeChildIF& child) { } return returnvalue::OK; } + +const HasHealthIF* SubsystemBase::getOptHealthIF() const { return this; } + +const HasModesIF& SubsystemBase::getModeIF() const { return *this; } diff --git a/src/fsfw/subsystem/SubsystemBase.h b/src/fsfw/subsystem/SubsystemBase.h index 063bca85..84b89a12 100644 --- a/src/fsfw/subsystem/SubsystemBase.h +++ b/src/fsfw/subsystem/SubsystemBase.h @@ -1,6 +1,8 @@ #ifndef FSFW_SUBSYSTEM_SUBSYSTEMBASE_H_ #define FSFW_SUBSYSTEM_SUBSYSTEMBASE_H_ +#include + #include #include "fsfw/container/HybridIterator.h" @@ -29,6 +31,8 @@ class SubsystemBase : public SystemObject, public HasModesIF, public HasHealthIF, public HasModeTreeChildrenIF, + public ModeTreeConnectionIF, + public ModeTreeChildIF, public ExecutableObjectIF { public: static const uint8_t INTERFACE_ID = CLASS_ID::SUBSYSTEM_BASE; @@ -38,24 +42,25 @@ class SubsystemBase : public SystemObject, static const ReturnValue_t COULD_NOT_INSERT_CHILD = MAKE_RETURN_CODE(0x04); static const ReturnValue_t TABLE_CONTAINS_INVALID_OBJECT_ID = MAKE_RETURN_CODE(0x05); - SubsystemBase(object_id_t setObjectId, object_id_t parent, Mode_t initialMode = 0, - uint16_t commandQueueDepth = 8); + SubsystemBase(object_id_t setObjectId, Mode_t initialMode = 0, uint16_t commandQueueDepth = 8); virtual ~SubsystemBase(); virtual MessageQueueId_t getCommandQueue() const override; + ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override; + /** * Function to register the child objects. * Performs a checks if the child does implement HasHealthIF and/or HasModesIF * - * Also adds them to the internal childrenMap. + * Also adds them to the internal childrenMap. * * @param objectId * @return returnvalue::OK if successful - * CHILD_DOESNT_HAVE_MODES if Child is no HasHealthIF and no HasModesIF - * COULD_NOT_INSERT_CHILD If the Child could not be added to the ChildrenMap + * CHILD_DOESNT_HAVE_MODES if Child is no HasHealthIF and no HasModesIF + * COULD_NOT_INSERT_CHILD If the Child could not be added to the ChildrenMap */ - ReturnValue_t registerChild(object_id_t objectId); + ReturnValue_t registerChild(const ModeTreeChildIF &child) override; virtual ReturnValue_t initialize() override; @@ -90,8 +95,6 @@ class SubsystemBase : public SystemObject, ModeHelper modeHelper; - const object_id_t parentId; - typedef std::map ChildrenMap; ChildrenMap childrenMap; @@ -131,8 +134,6 @@ class SubsystemBase : public SystemObject, virtual void performChildOperation() = 0; - ReturnValue_t registerChild(const ModeTreeChildIF &child) override; - virtual ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, uint32_t *msToReachTheMode) override = 0; @@ -140,6 +141,10 @@ class SubsystemBase : public SystemObject, virtual void getMode(Mode_t *mode, Submode_t *submode) override; + object_id_t getObjectId() const override; + const HasHealthIF *getOptHealthIF() const override; + const HasModesIF &getModeIF() const override; + virtual void setToExternalControl() override; virtual void announceMode(bool recursive) override; diff --git a/src/fsfw/subsystem/modes/ModeTreeConnectionIF.h b/src/fsfw/subsystem/modes/ModeTreeConnectionIF.h index 8f25b2d4..b86fc10f 100644 --- a/src/fsfw/subsystem/modes/ModeTreeConnectionIF.h +++ b/src/fsfw/subsystem/modes/ModeTreeConnectionIF.h @@ -6,8 +6,7 @@ class ModeTreeConnectionIF { public: virtual ~ModeTreeConnectionIF() = default; - virtual ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent, - const ModeTreeChildIF &child) = 0; + virtual ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) = 0; }; #endif /* FSFW_SRC_FSFW_SUBSYSTEM_MODES_MODETREECONNECTIONIF_H_ */ diff --git a/src/fsfw_tests/integration/assemblies/TestAssembly.cpp b/src/fsfw_tests/integration/assemblies/TestAssembly.cpp index 668120c6..0d72257a 100644 --- a/src/fsfw_tests/integration/assemblies/TestAssembly.cpp +++ b/src/fsfw_tests/integration/assemblies/TestAssembly.cpp @@ -142,14 +142,15 @@ ReturnValue_t TestAssembly::initialize() { handler0->setParentQueue(this->getCommandQueue()); handler1->setParentQueue(this->getCommandQueue()); - result = registerChild(deviceHandler0Id); - if (result != returnvalue::OK) { - return result; - } - result = registerChild(deviceHandler1Id); - if (result != returnvalue::OK) { - return result; - } + // TODO: Fix this + // result = registerChild(deviceHandler0Id); + // if (result != returnvalue::OK) { + // return result; + // } + // result = registerChild(deviceHandler1Id); + // if (result != returnvalue::OK) { + // return result; + // } return result; } From 10dd8552446e409e40d830e25fb0eed927995764 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 30 Sep 2022 13:30:07 +0200 Subject: [PATCH 209/404] expose child itself in interface --- src/fsfw/controller/ControllerBase.cpp | 4 ++ src/fsfw/controller/ControllerBase.h | 3 +- src/fsfw/devicehandlers/ChildHandlerBase.cpp | 23 ++-------- src/fsfw/devicehandlers/ChildHandlerBase.h | 5 ++- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 19 ++++++++- src/fsfw/devicehandlers/DeviceHandlerBase.h | 16 +++++-- .../{modes => }/ModeTreeConnectionIF.h | 1 + src/fsfw/subsystem/SubsystemBase.cpp | 8 +++- src/fsfw/subsystem/SubsystemBase.h | 4 +- .../integration/assemblies/TestAssembly.cpp | 42 +++++++------------ .../integration/assemblies/TestAssembly.h | 10 ++--- 11 files changed, 73 insertions(+), 62 deletions(-) rename src/fsfw/subsystem/{modes => }/ModeTreeConnectionIF.h (88%) diff --git a/src/fsfw/controller/ControllerBase.cpp b/src/fsfw/controller/ControllerBase.cpp index ee1dba46..7c2456af 100644 --- a/src/fsfw/controller/ControllerBase.cpp +++ b/src/fsfw/controller/ControllerBase.cpp @@ -102,6 +102,10 @@ const HasHealthIF* ControllerBase::getOptHealthIF() const { return this; } const HasModesIF& ControllerBase::getModeIF() const { return *this; } +ModeTreeChildIF& ControllerBase::getModeTreeChildIF() { + return *this; +} + ReturnValue_t ControllerBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) { return modetree::connectModeTreeParent(parent, *this, healthHelper, modeHelper); } diff --git a/src/fsfw/controller/ControllerBase.h b/src/fsfw/controller/ControllerBase.h index 6d9e2937..0250cd89 100644 --- a/src/fsfw/controller/ControllerBase.h +++ b/src/fsfw/controller/ControllerBase.h @@ -8,7 +8,7 @@ #include "fsfw/objectmanager/SystemObject.h" #include "fsfw/subsystem/HasModeTreeChildrenIF.h" #include "fsfw/subsystem/ModeTreeChildIF.h" -#include "fsfw/subsystem/modes/ModeTreeConnectionIF.h" +#include "fsfw/subsystem/ModeTreeConnectionIF.h" #include "fsfw/tasks/ExecutableObjectIF.h" #include "fsfw/tasks/PeriodicTaskIF.h" @@ -31,6 +31,7 @@ class ControllerBase : public HasModesIF, ~ControllerBase() override; ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override; + ModeTreeChildIF& getModeTreeChildIF() override; /** SystemObject override */ ReturnValue_t initialize() override; diff --git a/src/fsfw/devicehandlers/ChildHandlerBase.cpp b/src/fsfw/devicehandlers/ChildHandlerBase.cpp index 50d7b765..37af39dc 100644 --- a/src/fsfw/devicehandlers/ChildHandlerBase.cpp +++ b/src/fsfw/devicehandlers/ChildHandlerBase.cpp @@ -3,11 +3,11 @@ #include "fsfw/subsystem/SubsystemBase.h" ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, - CookieIF* cookie, object_id_t parent, + CookieIF* cookie, HasModeTreeChildrenIF& parent, FailureIsolationBase* customFdir, size_t cmdQueueSize) : DeviceHandlerBase(setObjectId, deviceCommunication, cookie, (customFdir == nullptr ? &childHandlerFdir : customFdir), cmdQueueSize), - parentId(parent), + parent(parent), childHandlerFdir(setObjectId) {} ChildHandlerBase::~ChildHandlerBase() {} @@ -18,22 +18,5 @@ ReturnValue_t ChildHandlerBase::initialize() { return result; } - // TODO: Fix this - // MessageQueueId_t parentQueue = 0; - // - // if (parentId != objects::NO_OBJECT) { - // SubsystemBase* parent = ObjectManager::instance()->get(parentId); - // if (parent == NULL) { - // return returnvalue::FAILED; - // } - // parentQueue = parent->getCommandQueue(); - // - // parent->registerChild(getObjectId()); - // } - // - // healthHelper.setParentQueue(parentQueue); - // - // modeHelper.setParentQueue(parentQueue); - - return returnvalue::OK; + return DeviceHandlerBase::connectModeTreeParent(parent); } diff --git a/src/fsfw/devicehandlers/ChildHandlerBase.h b/src/fsfw/devicehandlers/ChildHandlerBase.h index 8145c391..b9f991d6 100644 --- a/src/fsfw/devicehandlers/ChildHandlerBase.h +++ b/src/fsfw/devicehandlers/ChildHandlerBase.h @@ -1,13 +1,14 @@ #ifndef FSFW_DEVICEHANDLER_CHILDHANDLERBASE_H_ #define FSFW_DEVICEHANDLER_CHILDHANDLERBASE_H_ +#include #include "ChildHandlerFDIR.h" #include "DeviceHandlerBase.h" class ChildHandlerBase : public DeviceHandlerBase { public: ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, CookieIF* cookie, - object_id_t parent = objects::NO_OBJECT, + HasModeTreeChildrenIF& parent, FailureIsolationBase* customFdir = nullptr, size_t cmdQueueSize = 20); virtual ~ChildHandlerBase(); @@ -15,7 +16,7 @@ class ChildHandlerBase : public DeviceHandlerBase { virtual ReturnValue_t initialize(); protected: - const uint32_t parentId; + HasModeTreeChildrenIF& parent; ChildHandlerFDIR childHandlerFdir; }; diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 851fb6ec..cdfec03b 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1,5 +1,6 @@ -#include "fsfw/devicehandlers/DeviceHandlerBase.h" +#include "DeviceHandlerBase.h" +#include "fsfw/subsystem/helper.h" #include "fsfw/datapoollocal/LocalPoolVariable.h" #include "fsfw/devicehandlers/AcceptsDeviceResponsesIF.h" #include "fsfw/devicehandlers/DeviceTmReportingWrapper.h" @@ -1591,3 +1592,19 @@ void DeviceHandlerBase::disableCommandsAndReplies() { } } } + +ReturnValue_t DeviceHandlerBase::connectModeTreeParent(HasModeTreeChildrenIF &parent) { + return modetree::connectModeTreeParent(parent, *this, healthHelper, modeHelper); +} + +const HasHealthIF* DeviceHandlerBase::getOptHealthIF() const { + return this; +} + +const HasModesIF& DeviceHandlerBase::getModeIF() const { + return *this; +} + +ModeTreeChildIF& DeviceHandlerBase::getModeTreeChildIF() { + return *this; +} diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index 52ae473b..f9c776e0 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -11,6 +11,7 @@ #include "fsfw/action/HasActionsIF.h" #include "fsfw/datapool/PoolVariableIF.h" #include "fsfw/datapoollocal/HasLocalDataPoolIF.h" +#include "fsfw/subsystem/ModeTreeConnectionIF.h" #include "fsfw/datapoollocal/LocalDataPoolManager.h" #include "fsfw/health/HealthHelper.h" #include "fsfw/ipc/MessageQueueIF.h" @@ -84,6 +85,8 @@ class DeviceHandlerBase : public DeviceHandlerIF, public HasModesIF, public HasHealthIF, public HasActionsIF, + public ModeTreeChildIF, + public ModeTreeConnectionIF, public ReceivesParameterMessagesIF, public HasLocalDataPoolIF { friend void(Factory::setStaticFrameworkObjectIds)(); @@ -120,6 +123,10 @@ class DeviceHandlerBase : public DeviceHandlerIF, lp_id_t thermalStatePoolId = DeviceHandlerIF::DEFAULT_THERMAL_STATE_POOL_ID, lp_id_t thermalRequestPoolId = DeviceHandlerIF::DEFAULT_THERMAL_HEATING_REQUEST_POOL_ID, uint32_t thermalSetId = DeviceHandlerIF::DEFAULT_THERMAL_SET_ID); + + ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override; + ModeTreeChildIF& getModeTreeChildIF() override; + /** * @brief Helper function to ease device handler development. * This will instruct the transition to MODE_ON immediately @@ -165,7 +172,7 @@ class DeviceHandlerBase : public DeviceHandlerIF, * @param counter Specifies which Action to perform * @return returnvalue::OK for successful execution */ - virtual ReturnValue_t performOperation(uint8_t counter) override; + ReturnValue_t performOperation(uint8_t counter) override; /** * @brief Initializes the device handler @@ -175,14 +182,14 @@ class DeviceHandlerBase : public DeviceHandlerIF, * Calls fillCommandAndReplyMap(). * @return */ - virtual ReturnValue_t initialize() override; + ReturnValue_t initialize() override; /** * @brief Intialization steps performed after all tasks have been created. * This function will be called by the executing task. * @return */ - virtual ReturnValue_t initializeAfterTaskCreation() override; + ReturnValue_t initializeAfterTaskCreation() override; /** Destructor. */ virtual ~DeviceHandlerBase(); @@ -960,6 +967,9 @@ class DeviceHandlerBase : public DeviceHandlerIF, */ LocalDataPoolManager *getHkManagerHandle() override; + const HasHealthIF* getOptHealthIF() const override; + const HasModesIF& getModeIF() const override; + /** * Returns the delay cycle count of a reply. * A count != 0 indicates that the command is already executed. diff --git a/src/fsfw/subsystem/modes/ModeTreeConnectionIF.h b/src/fsfw/subsystem/ModeTreeConnectionIF.h similarity index 88% rename from src/fsfw/subsystem/modes/ModeTreeConnectionIF.h rename to src/fsfw/subsystem/ModeTreeConnectionIF.h index b86fc10f..78e22435 100644 --- a/src/fsfw/subsystem/modes/ModeTreeConnectionIF.h +++ b/src/fsfw/subsystem/ModeTreeConnectionIF.h @@ -7,6 +7,7 @@ class ModeTreeConnectionIF { public: virtual ~ModeTreeConnectionIF() = default; virtual ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) = 0; + virtual ModeTreeChildIF& getModeTreeChildIF() = 0; }; #endif /* FSFW_SRC_FSFW_SUBSYSTEM_MODES_MODETREECONNECTIONIF_H_ */ diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp index 058bcca9..b303e323 100644 --- a/src/fsfw/subsystem/SubsystemBase.cpp +++ b/src/fsfw/subsystem/SubsystemBase.cpp @@ -300,4 +300,10 @@ ReturnValue_t SubsystemBase::registerChild(const ModeTreeChildIF& child) { const HasHealthIF* SubsystemBase::getOptHealthIF() const { return this; } -const HasModesIF& SubsystemBase::getModeIF() const { return *this; } +const HasModesIF& SubsystemBase::getModeIF() const { + return *this; +} + +ModeTreeChildIF& SubsystemBase::getModeTreeChildIF() { + return *this; +} diff --git a/src/fsfw/subsystem/SubsystemBase.h b/src/fsfw/subsystem/SubsystemBase.h index 84b89a12..d3ee34f9 100644 --- a/src/fsfw/subsystem/SubsystemBase.h +++ b/src/fsfw/subsystem/SubsystemBase.h @@ -1,13 +1,12 @@ #ifndef FSFW_SUBSYSTEM_SUBSYSTEMBASE_H_ #define FSFW_SUBSYSTEM_SUBSYSTEMBASE_H_ -#include - #include #include "fsfw/container/HybridIterator.h" #include "fsfw/health/HasHealthIF.h" #include "fsfw/health/HealthHelper.h" +#include "fsfw/subsystem/ModeTreeConnectionIF.h" #include "fsfw/ipc/MessageQueueIF.h" #include "fsfw/modes/HasModesIF.h" #include "fsfw/objectmanager/SystemObject.h" @@ -48,6 +47,7 @@ class SubsystemBase : public SystemObject, virtual MessageQueueId_t getCommandQueue() const override; ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override; + ModeTreeChildIF& getModeTreeChildIF() override; /** * Function to register the child objects. diff --git a/src/fsfw_tests/integration/assemblies/TestAssembly.cpp b/src/fsfw_tests/integration/assemblies/TestAssembly.cpp index 0d72257a..793e591f 100644 --- a/src/fsfw_tests/integration/assemblies/TestAssembly.cpp +++ b/src/fsfw_tests/integration/assemblies/TestAssembly.cpp @@ -2,19 +2,19 @@ #include -TestAssembly::TestAssembly(object_id_t objectId, object_id_t parentId, object_id_t testDevice0, - object_id_t testDevice1) +TestAssembly::TestAssembly(object_id_t objectId, object_id_t parentId, ModeTreeChildIF& testDevice0, + ModeTreeChildIF& testDevice1) : AssemblyBase(objectId, parentId), - deviceHandler0Id(testDevice0), - deviceHandler1Id(testDevice1) { + deviceHandler0(testDevice0), + deviceHandler1(testDevice1) { ModeListEntry newModeListEntry; - newModeListEntry.setObject(testDevice0); + newModeListEntry.setObject(testDevice0.getObjectId()); newModeListEntry.setMode(MODE_OFF); newModeListEntry.setSubmode(SUBMODE_NONE); commandTable.insert(newModeListEntry); - newModeListEntry.setObject(testDevice1); + newModeListEntry.setObject(testDevice1.getObjectId()); newModeListEntry.setMode(MODE_OFF); newModeListEntry.setSubmode(SUBMODE_NONE); @@ -43,8 +43,8 @@ ReturnValue_t TestAssembly::commandChildren(Mode_t mode, Submode_t submode) { commandTable[1].setMode(MODE_OFF); commandTable[1].setSubmode(SUBMODE_NONE); // We try to prefer 0 here but we try to switch to 1 even if it might fail - if (isDeviceAvailable(deviceHandler0Id)) { - if (childrenMap[deviceHandler0Id].mode == MODE_ON) { + if (isDeviceAvailable(deviceHandler0.getObjectId())) { + if (childrenMap[deviceHandler0.getObjectId()].mode == MODE_ON) { commandTable[0].setMode(mode); commandTable[0].setSubmode(SUBMODE_NONE); } else { @@ -53,7 +53,7 @@ ReturnValue_t TestAssembly::commandChildren(Mode_t mode, Submode_t submode) { result = NEED_SECOND_STEP; } } else { - if (childrenMap[deviceHandler1Id].mode == MODE_ON) { + if (childrenMap[deviceHandler1.getObjectId()].mode == MODE_ON) { commandTable[1].setMode(mode); commandTable[1].setSubmode(SUBMODE_NONE); } else { @@ -64,7 +64,7 @@ ReturnValue_t TestAssembly::commandChildren(Mode_t mode, Submode_t submode) { } } else { // Dual Mode Normal - if (childrenMap[deviceHandler0Id].mode == MODE_ON) { + if (childrenMap[deviceHandler0.getObjectId()].mode == MODE_ON) { commandTable[0].setMode(mode); commandTable[0].setSubmode(SUBMODE_NONE); } else { @@ -72,7 +72,7 @@ ReturnValue_t TestAssembly::commandChildren(Mode_t mode, Submode_t submode) { commandTable[0].setSubmode(SUBMODE_NONE); result = NEED_SECOND_STEP; } - if (childrenMap[deviceHandler1Id].mode == MODE_ON) { + if (childrenMap[deviceHandler1.getObjectId()].mode == MODE_ON) { commandTable[1].setMode(mode); commandTable[1].setSubmode(SUBMODE_NONE); } else { @@ -89,7 +89,7 @@ ReturnValue_t TestAssembly::commandChildren(Mode_t mode, Submode_t submode) { commandTable[1].setMode(MODE_OFF); commandTable[1].setSubmode(SUBMODE_NONE); // We try to prefer 0 here but we try to switch to 1 even if it might fail - if (isDeviceAvailable(deviceHandler0Id)) { + if (isDeviceAvailable(deviceHandler0.getObjectId())) { commandTable[0].setMode(MODE_ON); commandTable[0].setSubmode(SUBMODE_NONE); } else { @@ -133,24 +133,14 @@ ReturnValue_t TestAssembly::initialize() { if (result != returnvalue::OK) { return result; } - handler0 = ObjectManager::instance()->get(deviceHandler0Id); - handler1 = ObjectManager::instance()->get(deviceHandler1Id); + auto* handler0 = ObjectManager::instance()->get(deviceHandler0.getObjectId()); + auto* handler1 = ObjectManager::instance()->get(deviceHandler1.getObjectId()); if ((handler0 == nullptr) or (handler1 == nullptr)) { return returnvalue::FAILED; } - handler0->setParentQueue(this->getCommandQueue()); - handler1->setParentQueue(this->getCommandQueue()); - - // TODO: Fix this - // result = registerChild(deviceHandler0Id); - // if (result != returnvalue::OK) { - // return result; - // } - // result = registerChild(deviceHandler1Id); - // if (result != returnvalue::OK) { - // return result; - // } + handler0->connectModeTreeParent(*this); + handler1->connectModeTreeParent(*this); return result; } diff --git a/src/fsfw_tests/integration/assemblies/TestAssembly.h b/src/fsfw_tests/integration/assemblies/TestAssembly.h index 91ffbf60..86b4fb07 100644 --- a/src/fsfw_tests/integration/assemblies/TestAssembly.h +++ b/src/fsfw_tests/integration/assemblies/TestAssembly.h @@ -7,8 +7,8 @@ class TestAssembly : public AssemblyBase { public: - TestAssembly(object_id_t objectId, object_id_t parentId, object_id_t testDevice0, - object_id_t testDevice1); + TestAssembly(object_id_t objectId, object_id_t parentId, ModeTreeChildIF& testDevice0, + ModeTreeChildIF& testDevice1); virtual ~TestAssembly(); ReturnValue_t initialize() override; @@ -41,10 +41,8 @@ class TestAssembly : public AssemblyBase { private: FixedArrayList commandTable; - object_id_t deviceHandler0Id = 0; - object_id_t deviceHandler1Id = 0; - TestDevice* handler0 = nullptr; - TestDevice* handler1 = nullptr; + ModeTreeChildIF& deviceHandler0; + ModeTreeChildIF& deviceHandler1; bool isDeviceAvailable(object_id_t object); }; From acab5f6bceef581fae64d1c16af19482ec72abcb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 30 Sep 2022 14:14:45 +0200 Subject: [PATCH 210/404] added missing health and mode helper init --- src/fsfw/controller/ControllerBase.cpp | 10 +++++++--- src/fsfw/subsystem/SubsystemBase.cpp | 14 ++++++++++++-- src/fsfw/subsystem/SubsystemBase.h | 8 ++++---- 3 files changed, 23 insertions(+), 9 deletions(-) diff --git a/src/fsfw/controller/ControllerBase.cpp b/src/fsfw/controller/ControllerBase.cpp index 7c2456af..a7ac9092 100644 --- a/src/fsfw/controller/ControllerBase.cpp +++ b/src/fsfw/controller/ControllerBase.cpp @@ -20,11 +20,15 @@ ControllerBase::ControllerBase(object_id_t setObjectId, size_t commandQueueDepth ControllerBase::~ControllerBase() { QueueFactory::instance()->deleteMessageQueue(commandQueue); } ReturnValue_t ControllerBase::initialize() { - ReturnValue_t result = SystemObject::initialize(); - if (result != returnvalue::OK) { + ReturnValue_t result = modeHelper.initialize(); + if(result != returnvalue::OK) { return result; } - return returnvalue::OK; + result = healthHelper.initialize(); + if(result != returnvalue::OK) { + return result; + } + return SystemObject::initialize(); } MessageQueueId_t ControllerBase::getCommandQueue() const { return commandQueue->getId(); } diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp index b303e323..d5400222 100644 --- a/src/fsfw/subsystem/SubsystemBase.cpp +++ b/src/fsfw/subsystem/SubsystemBase.cpp @@ -129,7 +129,17 @@ ReturnValue_t SubsystemBase::updateChildChangedHealth(MessageQueueId_t queue, bo MessageQueueId_t SubsystemBase::getCommandQueue() const { return commandQueue->getId(); } -ReturnValue_t SubsystemBase::initialize() { return SystemObject::initialize(); } +ReturnValue_t SubsystemBase::initialize() { + ReturnValue_t result = modeHelper.initialize(); + if(result != returnvalue::OK) { + return result; + } + result = healthHelper.initialize(); + if(result != returnvalue::OK) { + return result; + } + return SystemObject::initialize(); +} ReturnValue_t SubsystemBase::performOperation(uint8_t opCode) { childrenChangedMode = false; @@ -187,7 +197,7 @@ ReturnValue_t SubsystemBase::checkTable(HybridIterator tableIter) using namespace std; sif::warning << "SubsystemBase::checkTable: Could not find Object " << setfill('0') << hex << "0x" << setw(8) << tableIter.value->getObject() << " in object " << setw(8) - << "0x" << SystemObject::getObjectId() << dec << std::endl; + << setw(0) << "0x" << setw(8) << SystemObject::getObjectId() << dec << std::endl; #endif return TABLE_CONTAINS_INVALID_OBJECT_ID; } diff --git a/src/fsfw/subsystem/SubsystemBase.h b/src/fsfw/subsystem/SubsystemBase.h index d3ee34f9..32c8af0c 100644 --- a/src/fsfw/subsystem/SubsystemBase.h +++ b/src/fsfw/subsystem/SubsystemBase.h @@ -62,13 +62,13 @@ class SubsystemBase : public SystemObject, */ ReturnValue_t registerChild(const ModeTreeChildIF &child) override; - virtual ReturnValue_t initialize() override; + ReturnValue_t initialize() override; - virtual ReturnValue_t performOperation(uint8_t opCode) override; + ReturnValue_t performOperation(uint8_t opCode) override; - virtual ReturnValue_t setHealth(HealthState health) override; + ReturnValue_t setHealth(HealthState health) override; - virtual HasHealthIF::HealthState getHealth() override; + HasHealthIF::HealthState getHealth() override; protected: struct ChildInfo { From 1eceef4645b5a052dbdcd964e7840e7121c57909 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 30 Sep 2022 15:05:32 +0200 Subject: [PATCH 211/404] move retvals --- src/fsfw/container/FixedMap.h | 22 ++++++++++--------- src/fsfw/controller/ControllerBase.cpp | 8 +++---- src/fsfw/controller/ControllerBase.h | 2 +- src/fsfw/devicehandlers/ChildHandlerBase.h | 5 +++-- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 16 +++++--------- src/fsfw/devicehandlers/DeviceHandlerBase.h | 8 +++---- src/fsfw/subsystem/ModeTreeConnectionIF.h | 2 +- src/fsfw/subsystem/Subsystem.cpp | 7 ++++++ src/fsfw/subsystem/SubsystemBase.cpp | 12 ++++------ src/fsfw/subsystem/SubsystemBase.h | 4 ++-- .../integration/assemblies/TestAssembly.cpp | 6 ++--- 11 files changed, 44 insertions(+), 48 deletions(-) diff --git a/src/fsfw/container/FixedMap.h b/src/fsfw/container/FixedMap.h index def219d1..6ed6278d 100644 --- a/src/fsfw/container/FixedMap.h +++ b/src/fsfw/container/FixedMap.h @@ -7,6 +7,13 @@ #include "../returnvalues/returnvalue.h" #include "ArrayList.h" +namespace mapdefs { +static const uint8_t INTERFACE_ID = CLASS_ID::FIXED_MAP; +static const ReturnValue_t KEY_ALREADY_EXISTS = MAKE_RETURN_CODE(0x01); +static const ReturnValue_t MAP_FULL = MAKE_RETURN_CODE(0x02); +static const ReturnValue_t KEY_DOES_NOT_EXIST = MAKE_RETURN_CODE(0x03); +} // namespace mapdefs + /** * @brief Map implementation for maps with a pre-defined size. * @details @@ -24,11 +31,6 @@ class FixedMap : public SerializeIF { "derived class from SerializeIF to be serialize-able"); public: - static const uint8_t INTERFACE_ID = CLASS_ID::FIXED_MAP; - static const ReturnValue_t KEY_ALREADY_EXISTS = MAKE_RETURN_CODE(0x01); - static const ReturnValue_t MAP_FULL = MAKE_RETURN_CODE(0x02); - static const ReturnValue_t KEY_DOES_NOT_EXIST = MAKE_RETURN_CODE(0x03); - private: static const key_t EMPTY_SLOT = -1; ArrayList, uint32_t> theMap; @@ -76,10 +78,10 @@ class FixedMap : public SerializeIF { ReturnValue_t insert(key_t key, T value, Iterator* storedValue = nullptr) { if (exists(key) == returnvalue::OK) { - return KEY_ALREADY_EXISTS; + return mapdefs::KEY_ALREADY_EXISTS; } if (_size == theMap.maxSize()) { - return MAP_FULL; + return mapdefs::MAP_FULL; } theMap[_size].first = key; theMap[_size].second = value; @@ -93,7 +95,7 @@ class FixedMap : public SerializeIF { ReturnValue_t insert(std::pair pair) { return insert(pair.first, pair.second); } ReturnValue_t exists(key_t key) const { - ReturnValue_t result = KEY_DOES_NOT_EXIST; + ReturnValue_t result = mapdefs::KEY_DOES_NOT_EXIST; if (findIndex(key) < _size) { result = returnvalue::OK; } @@ -103,7 +105,7 @@ class FixedMap : public SerializeIF { ReturnValue_t erase(Iterator* iter) { uint32_t i; if ((i = findIndex((*iter).value->first)) >= _size) { - return KEY_DOES_NOT_EXIST; + return mapdefs::KEY_DOES_NOT_EXIST; } theMap[i] = theMap[_size - 1]; --_size; @@ -114,7 +116,7 @@ class FixedMap : public SerializeIF { ReturnValue_t erase(key_t key) { uint32_t i; if ((i = findIndex(key)) >= _size) { - return KEY_DOES_NOT_EXIST; + return mapdefs::KEY_DOES_NOT_EXIST; } theMap[i] = theMap[_size - 1]; --_size; diff --git a/src/fsfw/controller/ControllerBase.cpp b/src/fsfw/controller/ControllerBase.cpp index a7ac9092..98907210 100644 --- a/src/fsfw/controller/ControllerBase.cpp +++ b/src/fsfw/controller/ControllerBase.cpp @@ -21,11 +21,11 @@ ControllerBase::~ControllerBase() { QueueFactory::instance()->deleteMessageQueue ReturnValue_t ControllerBase::initialize() { ReturnValue_t result = modeHelper.initialize(); - if(result != returnvalue::OK) { + if (result != returnvalue::OK) { return result; } result = healthHelper.initialize(); - if(result != returnvalue::OK) { + if (result != returnvalue::OK) { return result; } return SystemObject::initialize(); @@ -106,9 +106,7 @@ const HasHealthIF* ControllerBase::getOptHealthIF() const { return this; } const HasModesIF& ControllerBase::getModeIF() const { return *this; } -ModeTreeChildIF& ControllerBase::getModeTreeChildIF() { - return *this; -} +ModeTreeChildIF& ControllerBase::getModeTreeChildIF() { return *this; } ReturnValue_t ControllerBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) { return modetree::connectModeTreeParent(parent, *this, healthHelper, modeHelper); diff --git a/src/fsfw/controller/ControllerBase.h b/src/fsfw/controller/ControllerBase.h index 0250cd89..da140e49 100644 --- a/src/fsfw/controller/ControllerBase.h +++ b/src/fsfw/controller/ControllerBase.h @@ -31,7 +31,7 @@ class ControllerBase : public HasModesIF, ~ControllerBase() override; ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override; - ModeTreeChildIF& getModeTreeChildIF() override; + ModeTreeChildIF &getModeTreeChildIF() override; /** SystemObject override */ ReturnValue_t initialize() override; diff --git a/src/fsfw/devicehandlers/ChildHandlerBase.h b/src/fsfw/devicehandlers/ChildHandlerBase.h index b9f991d6..c98baf3d 100644 --- a/src/fsfw/devicehandlers/ChildHandlerBase.h +++ b/src/fsfw/devicehandlers/ChildHandlerBase.h @@ -2,14 +2,15 @@ #define FSFW_DEVICEHANDLER_CHILDHANDLERBASE_H_ #include + #include "ChildHandlerFDIR.h" #include "DeviceHandlerBase.h" class ChildHandlerBase : public DeviceHandlerBase { public: ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, CookieIF* cookie, - HasModeTreeChildrenIF& parent, - FailureIsolationBase* customFdir = nullptr, size_t cmdQueueSize = 20); + HasModeTreeChildrenIF& parent, FailureIsolationBase* customFdir = nullptr, + size_t cmdQueueSize = 20); virtual ~ChildHandlerBase(); diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index cdfec03b..61329366 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1,6 +1,5 @@ #include "DeviceHandlerBase.h" -#include "fsfw/subsystem/helper.h" #include "fsfw/datapoollocal/LocalPoolVariable.h" #include "fsfw/devicehandlers/AcceptsDeviceResponsesIF.h" #include "fsfw/devicehandlers/DeviceTmReportingWrapper.h" @@ -13,6 +12,7 @@ #include "fsfw/serviceinterface/ServiceInterface.h" #include "fsfw/storagemanager/StorageManagerIF.h" #include "fsfw/subsystem/SubsystemBase.h" +#include "fsfw/subsystem/helper.h" #include "fsfw/thermal/ThermalComponentIF.h" object_id_t DeviceHandlerBase::powerSwitcherId = objects::NO_OBJECT; @@ -1593,18 +1593,12 @@ void DeviceHandlerBase::disableCommandsAndReplies() { } } -ReturnValue_t DeviceHandlerBase::connectModeTreeParent(HasModeTreeChildrenIF &parent) { +ReturnValue_t DeviceHandlerBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) { return modetree::connectModeTreeParent(parent, *this, healthHelper, modeHelper); } -const HasHealthIF* DeviceHandlerBase::getOptHealthIF() const { - return this; -} +const HasHealthIF* DeviceHandlerBase::getOptHealthIF() const { return this; } -const HasModesIF& DeviceHandlerBase::getModeIF() const { - return *this; -} +const HasModesIF& DeviceHandlerBase::getModeIF() const { return *this; } -ModeTreeChildIF& DeviceHandlerBase::getModeTreeChildIF() { - return *this; -} +ModeTreeChildIF& DeviceHandlerBase::getModeTreeChildIF() { return *this; } diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index f9c776e0..d7a896d5 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -11,7 +11,6 @@ #include "fsfw/action/HasActionsIF.h" #include "fsfw/datapool/PoolVariableIF.h" #include "fsfw/datapoollocal/HasLocalDataPoolIF.h" -#include "fsfw/subsystem/ModeTreeConnectionIF.h" #include "fsfw/datapoollocal/LocalDataPoolManager.h" #include "fsfw/health/HealthHelper.h" #include "fsfw/ipc/MessageQueueIF.h" @@ -22,6 +21,7 @@ #include "fsfw/returnvalues/returnvalue.h" #include "fsfw/serviceinterface/ServiceInterface.h" #include "fsfw/serviceinterface/serviceInterfaceDefintions.h" +#include "fsfw/subsystem/ModeTreeConnectionIF.h" #include "fsfw/tasks/ExecutableObjectIF.h" #include "fsfw/tasks/PeriodicTaskIF.h" #include "fsfw/util/dataWrapper.h" @@ -125,7 +125,7 @@ class DeviceHandlerBase : public DeviceHandlerIF, uint32_t thermalSetId = DeviceHandlerIF::DEFAULT_THERMAL_SET_ID); ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override; - ModeTreeChildIF& getModeTreeChildIF() override; + ModeTreeChildIF &getModeTreeChildIF() override; /** * @brief Helper function to ease device handler development. @@ -967,8 +967,8 @@ class DeviceHandlerBase : public DeviceHandlerIF, */ LocalDataPoolManager *getHkManagerHandle() override; - const HasHealthIF* getOptHealthIF() const override; - const HasModesIF& getModeIF() const override; + const HasHealthIF *getOptHealthIF() const override; + const HasModesIF &getModeIF() const override; /** * Returns the delay cycle count of a reply. diff --git a/src/fsfw/subsystem/ModeTreeConnectionIF.h b/src/fsfw/subsystem/ModeTreeConnectionIF.h index 78e22435..4fe54ada 100644 --- a/src/fsfw/subsystem/ModeTreeConnectionIF.h +++ b/src/fsfw/subsystem/ModeTreeConnectionIF.h @@ -6,7 +6,7 @@ class ModeTreeConnectionIF { public: virtual ~ModeTreeConnectionIF() = default; - virtual ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) = 0; + virtual ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent) = 0; virtual ModeTreeChildIF& getModeTreeChildIF() = 0; }; diff --git a/src/fsfw/subsystem/Subsystem.cpp b/src/fsfw/subsystem/Subsystem.cpp index 085c843a..ad3afb2b 100644 --- a/src/fsfw/subsystem/Subsystem.cpp +++ b/src/fsfw/subsystem/Subsystem.cpp @@ -36,6 +36,13 @@ ReturnValue_t Subsystem::checkSequence(HybridIterator iter, for (; iter.value != nullptr; ++iter) { if (!existsModeTable(iter->getTableId())) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + using namespace std; + sif::warning << "Subsystem::checkSequence: " + << "Object " << setfill('0') << hex << "0x" << setw(8) << getObjectId() + << setw(0) << ": Mode table for mode ID " + << "0x" << setw(8) << iter->getTableId() << " does not exist" << dec << endl; +#endif return TABLE_DOES_NOT_EXIST; } else { ReturnValue_t result = checkTable(getTable(iter->getTableId())); diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp index d5400222..d14e3539 100644 --- a/src/fsfw/subsystem/SubsystemBase.cpp +++ b/src/fsfw/subsystem/SubsystemBase.cpp @@ -131,11 +131,11 @@ MessageQueueId_t SubsystemBase::getCommandQueue() const { return commandQueue->g ReturnValue_t SubsystemBase::initialize() { ReturnValue_t result = modeHelper.initialize(); - if(result != returnvalue::OK) { + if (result != returnvalue::OK) { return result; } result = healthHelper.initialize(); - if(result != returnvalue::OK) { + if (result != returnvalue::OK) { return result; } return SystemObject::initialize(); @@ -310,10 +310,6 @@ ReturnValue_t SubsystemBase::registerChild(const ModeTreeChildIF& child) { const HasHealthIF* SubsystemBase::getOptHealthIF() const { return this; } -const HasModesIF& SubsystemBase::getModeIF() const { - return *this; -} +const HasModesIF& SubsystemBase::getModeIF() const { return *this; } -ModeTreeChildIF& SubsystemBase::getModeTreeChildIF() { - return *this; -} +ModeTreeChildIF& SubsystemBase::getModeTreeChildIF() { return *this; } diff --git a/src/fsfw/subsystem/SubsystemBase.h b/src/fsfw/subsystem/SubsystemBase.h index 32c8af0c..0fbf9f4a 100644 --- a/src/fsfw/subsystem/SubsystemBase.h +++ b/src/fsfw/subsystem/SubsystemBase.h @@ -6,12 +6,12 @@ #include "fsfw/container/HybridIterator.h" #include "fsfw/health/HasHealthIF.h" #include "fsfw/health/HealthHelper.h" -#include "fsfw/subsystem/ModeTreeConnectionIF.h" #include "fsfw/ipc/MessageQueueIF.h" #include "fsfw/modes/HasModesIF.h" #include "fsfw/objectmanager/SystemObject.h" #include "fsfw/returnvalues/returnvalue.h" #include "fsfw/subsystem/HasModeTreeChildrenIF.h" +#include "fsfw/subsystem/ModeTreeConnectionIF.h" #include "fsfw/tasks/ExecutableObjectIF.h" #include "modes/HasModeSequenceIF.h" @@ -47,7 +47,7 @@ class SubsystemBase : public SystemObject, virtual MessageQueueId_t getCommandQueue() const override; ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override; - ModeTreeChildIF& getModeTreeChildIF() override; + ModeTreeChildIF &getModeTreeChildIF() override; /** * Function to register the child objects. diff --git a/src/fsfw_tests/integration/assemblies/TestAssembly.cpp b/src/fsfw_tests/integration/assemblies/TestAssembly.cpp index 793e591f..761d0d3c 100644 --- a/src/fsfw_tests/integration/assemblies/TestAssembly.cpp +++ b/src/fsfw_tests/integration/assemblies/TestAssembly.cpp @@ -3,10 +3,8 @@ #include TestAssembly::TestAssembly(object_id_t objectId, object_id_t parentId, ModeTreeChildIF& testDevice0, - ModeTreeChildIF& testDevice1) - : AssemblyBase(objectId, parentId), - deviceHandler0(testDevice0), - deviceHandler1(testDevice1) { + ModeTreeChildIF& testDevice1) + : AssemblyBase(objectId, parentId), deviceHandler0(testDevice0), deviceHandler1(testDevice1) { ModeListEntry newModeListEntry; newModeListEntry.setObject(testDevice0.getObjectId()); newModeListEntry.setMode(MODE_OFF); From cfc00d02607d22ec306d0540f9f2329774df1cdd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 4 Oct 2022 18:38:20 +0200 Subject: [PATCH 212/404] try to do this in a simpler way --- src/fsfw/globalfunctions/DleParser.cpp | 304 +++++++++++++++---------- src/fsfw/globalfunctions/DleParser.h | 8 +- 2 files changed, 194 insertions(+), 118 deletions(-) diff --git a/src/fsfw/globalfunctions/DleParser.cpp b/src/fsfw/globalfunctions/DleParser.cpp index 44c75bab..240977f1 100644 --- a/src/fsfw/globalfunctions/DleParser.cpp +++ b/src/fsfw/globalfunctions/DleParser.cpp @@ -3,6 +3,8 @@ #include #include +#include +#include DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf, BufPair decodedBuf, UserHandler handler, void* args) @@ -21,143 +23,207 @@ DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPa } } -ReturnValue_t DleParser::passData(uint8_t* data, size_t len) { +ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) { if (data == nullptr or len == 0 or handler == nullptr) { return returnvalue::FAILED; } - size_t copyIntoRingBufFromHere = 0; - size_t copyAmount = len; - size_t startIdx = 0; - ReturnValue_t result = returnvalue::OK; - bool startFoundInThisPacket = false; - for (size_t idx = 0; idx < len; idx++) { - if (data[idx] == DleEncoder::STX_CHAR) { - if (not startFound and not startFoundInThisPacket) { - startIdx = idx; - copyIntoRingBufFromHere = idx; - copyAmount = len - idx; + return decodeRingBuf.writeData(data , len); +// std::string filename = std::string("/mnt/sd0/scex/transfer") + std::to_string(COUNTER++); +// std::ofstream of(filename); +// of.write(reinterpret_cast(data), len); +// size_t copyIntoRingBufFromHere = 0; +// size_t copyAmount = len; +// size_t startIdx = 0; +// ReturnValue_t result = returnvalue::OK; +// bool startFoundInThisPacket = false; +// for (size_t idx = 0; idx < len; idx++) { +// if (data[idx] == DleEncoder::STX_CHAR) { +// if (not startFound and not startFoundInThisPacket) { +// startIdx = idx; +// copyIntoRingBufFromHere = idx; +// copyAmount = len - idx; +// } else { +// // Maybe print warning, should not happen +// decodeRingBuf.clear(); +// ErrorInfo info; +// info.len = idx; +// prepareErrorContext(ErrorTypes::CONSECUTIVE_STX_CHARS, info); +// handler(ctx); +// copyIntoRingBufFromHere = idx; +// copyAmount = len - idx; +// } +// startFound = true; +// startFoundInThisPacket = true; +// } else if (data[idx] == DleEncoder::ETX_CHAR) { +// if (startFoundInThisPacket) { +// size_t readLen = 0; +// size_t decodedLen = 0; +// result = decoder.decode(data + startIdx, idx + 1 - startIdx, &readLen, decodedBuf.first, +// decodedBuf.second, &decodedLen); +// if (result == returnvalue::OK) { +// ctx.setType(ContextType::PACKET_FOUND); +// ctx.decodedPacket.first = decodedBuf.first; +// ctx.decodedPacket.second = decodedLen; +// this->handler(ctx); +// } else if (result == DleEncoder::STREAM_TOO_SHORT) { +// ErrorInfo info; +// info.res = result; +// prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); +// handler(ctx); +// } else { +// ErrorInfo info; +// info.res = result; +// prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); +// handler(ctx); +// } +// decodeRingBuf.clear(); +// if ((idx + 1) < len) { +// copyIntoRingBufFromHere = idx + 1; +// copyAmount = len - idx - 1; +// } else { +// copyAmount = 0; +// } +// } else if (startFound) { +// // ETX found but STX was found in another mini packet. Reconstruct the full packet +// // to decode it +// result = decodeRingBuf.writeData(data, idx + 1); +// if (result != returnvalue::OK) { +// ErrorInfo info; +// info.res = result; +// prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info); +// handler(ctx); +// } +// size_t fullEncodedLen = decodeRingBuf.getAvailableReadData(); +// if (fullEncodedLen > encodedBuf.second) { +// ErrorInfo info; +// info.len = fullEncodedLen; +// prepareErrorContext(ErrorTypes::ENCODED_BUF_TOO_SMALL, info); +// handler(ctx); +// decodeRingBuf.clear(); +// } else { +// size_t decodedLen = 0; +// size_t readLen = 0; +// decodeRingBuf.readData(encodedBuf.first, fullEncodedLen, true); +// result = decoder.decode(encodedBuf.first, fullEncodedLen, &readLen, decodedBuf.first, +// decodedBuf.second, &decodedLen); +// if (result == returnvalue::OK) { +// if (this->handler != nullptr) { +// ctx.setType(ContextType::PACKET_FOUND); +// ctx.decodedPacket.first = decodedBuf.first; +// ctx.decodedPacket.second = decodedLen; +// this->handler(ctx); +// } +// } else if (result == DleEncoder::STREAM_TOO_SHORT) { +// ErrorInfo info; +// info.res = result; +// prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); +// handler(ctx); +// } else { +// ErrorInfo info; +// info.res = result; +// prepareErrorContext(ErrorTypes::DECODE_ERROR, info); +// handler(ctx); +// } +// decodeRingBuf.clear(); +// startFound = false; +// startFoundInThisPacket = false; +// if ((idx + 1) < len) { +// copyIntoRingBufFromHere = idx + 1; +// copyAmount = len - idx - 1; +// } else { +// copyAmount = 0; +// } +// } +// } else { +// // End data without preceeding STX +// ErrorInfo info; +// info.len = idx + 1; +// prepareErrorContext(ErrorTypes::CONSECUTIVE_ETX_CHARS, info); +// handler(ctx); +// decodeRingBuf.clear(); +// if ((idx + 1) < len) { +// copyIntoRingBufFromHere = idx + 1; +// copyAmount = len - idx - 1; +// } else { +// copyAmount = 0; +// } +// } +// startFoundInThisPacket = false; +// startFound = false; +// } +// } +// if (copyAmount > 0) { +// result = decodeRingBuf.writeData(data + copyIntoRingBufFromHere, copyAmount); +// if (result != returnvalue::OK) { +// ErrorInfo info; +// info.res = result; +// prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info); +// handler(ctx); +// } +// } +} + + +ReturnValue_t DleParser::parseRingBuf(size_t& readSize) { + size_t availableData = decodeRingBuf.getAvailableReadData(); + if (availableData > encodedBuf.second) { + ErrorInfo info; + info.len = decodeRingBuf.getAvailableReadData(); + prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); + handler(ctx); + } + ReturnValue_t result = decodeRingBuf.readData(encodedBuf.first, availableData); + if(result != returnvalue::OK) { + ErrorInfo info; + info.res = result; + prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info); + } + bool stxFound = false; + size_t stxIdx = 0; + for (size_t vectorIdx = 0; vectorIdx < availableData; vectorIdx++) { + // handle STX char + if (encodedBuf.first[vectorIdx] == DleEncoder::STX_CHAR) { + if (not stxFound) { + stxFound = true; + stxIdx = vectorIdx; } else { - // Maybe print warning, should not happen - decodeRingBuf.clear(); - ErrorInfo info; - info.len = idx; - prepareErrorContext(ErrorTypes::CONSECUTIVE_STX_CHARS, info); - handler(ctx); - copyIntoRingBufFromHere = idx; - copyAmount = len - idx; + // might be lost packet, so we should advance the read pointer + // without skipping the STX + readSize = vectorIdx; + return POSSIBLE_PACKET_LOSS; } - startFound = true; - startFoundInThisPacket = true; - } else if (data[idx] == DleEncoder::ETX_CHAR) { - if (startFoundInThisPacket) { - size_t readLen = 0; + } + // handle ETX char + if (encodedBuf.first[vectorIdx] == DleEncoder::ETX_CHAR) { + if (stxFound) { + // This is propably a packet, so we decode it. size_t decodedLen = 0; - result = decoder.decode(data + startIdx, idx + 1 - startIdx, &readLen, decodedBuf.first, - decodedBuf.second, &decodedLen); + size_t dummy = 0; + readSize = vectorIdx; + ReturnValue_t result = decoder.decode(&encodedBuf.first[stxIdx], availableData - stxIdx, + &dummy, decodedBuf.first, decodedBuf.second, &decodedLen); if (result == returnvalue::OK) { ctx.setType(ContextType::PACKET_FOUND); ctx.decodedPacket.first = decodedBuf.first; ctx.decodedPacket.second = decodedLen; this->handler(ctx); - } else if (result == DleEncoder::STREAM_TOO_SHORT) { - ErrorInfo info; - info.res = result; - prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); - handler(ctx); + return returnvalue::OK; } else { - ErrorInfo info; - info.res = result; - prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); - handler(ctx); - } - decodeRingBuf.clear(); - if ((idx + 1) < len) { - copyIntoRingBufFromHere = idx + 1; - copyAmount = len - idx - 1; - } else { - copyAmount = 0; - } - } else if (startFound) { - // ETX found but STX was found in another mini packet. Reconstruct the full packet - // to decode it - result = decodeRingBuf.writeData(data, idx + 1); - if (result != returnvalue::OK) { - ErrorInfo info; - info.res = result; - prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info); - handler(ctx); - } - size_t fullEncodedLen = decodeRingBuf.getAvailableReadData(); - if (fullEncodedLen > encodedBuf.second) { - ErrorInfo info; - info.len = fullEncodedLen; - prepareErrorContext(ErrorTypes::ENCODED_BUF_TOO_SMALL, info); - handler(ctx); - decodeRingBuf.clear(); - } else { - size_t decodedLen = 0; - size_t readLen = 0; - decodeRingBuf.readData(encodedBuf.first, fullEncodedLen, true); - result = decoder.decode(encodedBuf.first, fullEncodedLen, &readLen, decodedBuf.first, - decodedBuf.second, &decodedLen); - if (result == returnvalue::OK) { - if (this->handler != nullptr) { - ctx.setType(ContextType::PACKET_FOUND); - ctx.decodedPacket.first = decodedBuf.first; - ctx.decodedPacket.second = decodedLen; - this->handler(ctx); - } - } else if (result == DleEncoder::STREAM_TOO_SHORT) { - ErrorInfo info; - info.res = result; - prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); - handler(ctx); - } else { - ErrorInfo info; - info.res = result; - prepareErrorContext(ErrorTypes::DECODE_ERROR, info); - handler(ctx); - } - decodeRingBuf.clear(); - startFound = false; - startFoundInThisPacket = false; - if ((idx + 1) < len) { - copyIntoRingBufFromHere = idx + 1; - copyAmount = len - idx - 1; - } else { - copyAmount = 0; - } + // invalid packet, skip. + readSize = ++vectorIdx; + return POSSIBLE_PACKET_LOSS; } } else { - // End data without preceeding STX - ErrorInfo info; - info.len = idx + 1; - prepareErrorContext(ErrorTypes::CONSECUTIVE_ETX_CHARS, info); - handler(ctx); - decodeRingBuf.clear(); - if ((idx + 1) < len) { - copyIntoRingBufFromHere = idx + 1; - copyAmount = len - idx - 1; - } else { - copyAmount = 0; - } + // might be lost packet, so we should advance the read pointer + readSize = ++vectorIdx; + return POSSIBLE_PACKET_LOSS; } - startFoundInThisPacket = false; - startFound = false; - } - } - if (copyAmount > 0) { - result = decodeRingBuf.writeData(data + copyIntoRingBufFromHere, copyAmount); - if (result != returnvalue::OK) { - ErrorInfo info; - info.res = result; - prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info); - handler(ctx); } } return returnvalue::OK; } + void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* args) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -224,6 +290,10 @@ void DleParser::prepareErrorContext(ErrorTypes err, ErrorInfo info) { ctx.error.second = info; } +ReturnValue_t DleParser::confirmBytesRead(size_t bytesRead) { + return decodeRingBuf.deleteData(bytesRead); +} + void DleParser::reset() { startFound = false; decodeRingBuf.clear(); diff --git a/src/fsfw/globalfunctions/DleParser.h b/src/fsfw/globalfunctions/DleParser.h index 5ccd55df..b31fbf83 100644 --- a/src/fsfw/globalfunctions/DleParser.h +++ b/src/fsfw/globalfunctions/DleParser.h @@ -18,6 +18,8 @@ */ class DleParser { public: + static constexpr ReturnValue_t NO_PACKET_FOUND = returnvalue::makeCode(1, 1); + static constexpr ReturnValue_t POSSIBLE_PACKET_LOSS = returnvalue::makeCode(1, 2); using BufPair = std::pair; enum class ContextType { PACKET_FOUND, ERROR }; @@ -88,7 +90,11 @@ class DleParser { * @param len * @return */ - ReturnValue_t passData(uint8_t* data, size_t len); + ReturnValue_t passData(const uint8_t* data, size_t len); + + ReturnValue_t parseRingBuf(size_t& bytesRead); + + ReturnValue_t confirmBytesRead(size_t bytesRead); /** * Example found packet handler From 49747fc8a49e85e7b3d442eabe668d34e44bad32 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 4 Oct 2022 20:51:58 +0200 Subject: [PATCH 213/404] some bugfixes --- src/fsfw/globalfunctions/DleParser.cpp | 200 +++++------------------- src/fsfw/globalfunctions/DleParser.h | 16 +- src/fsfw/pus/Service5EventReporting.cpp | 4 +- 3 files changed, 45 insertions(+), 175 deletions(-) diff --git a/src/fsfw/globalfunctions/DleParser.cpp b/src/fsfw/globalfunctions/DleParser.cpp index 240977f1..b992debd 100644 --- a/src/fsfw/globalfunctions/DleParser.cpp +++ b/src/fsfw/globalfunctions/DleParser.cpp @@ -7,176 +7,35 @@ #include DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf, - BufPair decodedBuf, UserHandler handler, void* args) + BufPair decodedBuf) : decodeRingBuf(decodeRingBuf), decoder(decoder), encodedBuf(encodedBuf), - decodedBuf(decodedBuf), - handler(handler), - ctx(args) { - if (handler == nullptr) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "DleParser::DleParser: Invalid user handler" << std::endl; -#else - sif::printError("DleParser::DleParser: Invalid user handler\n"); -#endif - } + decodedBuf(decodedBuf) { } ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) { - if (data == nullptr or len == 0 or handler == nullptr) { + if (data == nullptr or len == 0) { return returnvalue::FAILED; } - return decodeRingBuf.writeData(data , len); -// std::string filename = std::string("/mnt/sd0/scex/transfer") + std::to_string(COUNTER++); -// std::ofstream of(filename); -// of.write(reinterpret_cast(data), len); -// size_t copyIntoRingBufFromHere = 0; -// size_t copyAmount = len; -// size_t startIdx = 0; -// ReturnValue_t result = returnvalue::OK; -// bool startFoundInThisPacket = false; -// for (size_t idx = 0; idx < len; idx++) { -// if (data[idx] == DleEncoder::STX_CHAR) { -// if (not startFound and not startFoundInThisPacket) { -// startIdx = idx; -// copyIntoRingBufFromHere = idx; -// copyAmount = len - idx; -// } else { -// // Maybe print warning, should not happen -// decodeRingBuf.clear(); -// ErrorInfo info; -// info.len = idx; -// prepareErrorContext(ErrorTypes::CONSECUTIVE_STX_CHARS, info); -// handler(ctx); -// copyIntoRingBufFromHere = idx; -// copyAmount = len - idx; -// } -// startFound = true; -// startFoundInThisPacket = true; -// } else if (data[idx] == DleEncoder::ETX_CHAR) { -// if (startFoundInThisPacket) { -// size_t readLen = 0; -// size_t decodedLen = 0; -// result = decoder.decode(data + startIdx, idx + 1 - startIdx, &readLen, decodedBuf.first, -// decodedBuf.second, &decodedLen); -// if (result == returnvalue::OK) { -// ctx.setType(ContextType::PACKET_FOUND); -// ctx.decodedPacket.first = decodedBuf.first; -// ctx.decodedPacket.second = decodedLen; -// this->handler(ctx); -// } else if (result == DleEncoder::STREAM_TOO_SHORT) { -// ErrorInfo info; -// info.res = result; -// prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); -// handler(ctx); -// } else { -// ErrorInfo info; -// info.res = result; -// prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); -// handler(ctx); -// } -// decodeRingBuf.clear(); -// if ((idx + 1) < len) { -// copyIntoRingBufFromHere = idx + 1; -// copyAmount = len - idx - 1; -// } else { -// copyAmount = 0; -// } -// } else if (startFound) { -// // ETX found but STX was found in another mini packet. Reconstruct the full packet -// // to decode it -// result = decodeRingBuf.writeData(data, idx + 1); -// if (result != returnvalue::OK) { -// ErrorInfo info; -// info.res = result; -// prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info); -// handler(ctx); -// } -// size_t fullEncodedLen = decodeRingBuf.getAvailableReadData(); -// if (fullEncodedLen > encodedBuf.second) { -// ErrorInfo info; -// info.len = fullEncodedLen; -// prepareErrorContext(ErrorTypes::ENCODED_BUF_TOO_SMALL, info); -// handler(ctx); -// decodeRingBuf.clear(); -// } else { -// size_t decodedLen = 0; -// size_t readLen = 0; -// decodeRingBuf.readData(encodedBuf.first, fullEncodedLen, true); -// result = decoder.decode(encodedBuf.first, fullEncodedLen, &readLen, decodedBuf.first, -// decodedBuf.second, &decodedLen); -// if (result == returnvalue::OK) { -// if (this->handler != nullptr) { -// ctx.setType(ContextType::PACKET_FOUND); -// ctx.decodedPacket.first = decodedBuf.first; -// ctx.decodedPacket.second = decodedLen; -// this->handler(ctx); -// } -// } else if (result == DleEncoder::STREAM_TOO_SHORT) { -// ErrorInfo info; -// info.res = result; -// prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); -// handler(ctx); -// } else { -// ErrorInfo info; -// info.res = result; -// prepareErrorContext(ErrorTypes::DECODE_ERROR, info); -// handler(ctx); -// } -// decodeRingBuf.clear(); -// startFound = false; -// startFoundInThisPacket = false; -// if ((idx + 1) < len) { -// copyIntoRingBufFromHere = idx + 1; -// copyAmount = len - idx - 1; -// } else { -// copyAmount = 0; -// } -// } -// } else { -// // End data without preceeding STX -// ErrorInfo info; -// info.len = idx + 1; -// prepareErrorContext(ErrorTypes::CONSECUTIVE_ETX_CHARS, info); -// handler(ctx); -// decodeRingBuf.clear(); -// if ((idx + 1) < len) { -// copyIntoRingBufFromHere = idx + 1; -// copyAmount = len - idx - 1; -// } else { -// copyAmount = 0; -// } -// } -// startFoundInThisPacket = false; -// startFound = false; -// } -// } -// if (copyAmount > 0) { -// result = decodeRingBuf.writeData(data + copyIntoRingBufFromHere, copyAmount); -// if (result != returnvalue::OK) { -// ErrorInfo info; -// info.res = result; -// prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info); -// handler(ctx); -// } -// } + return decodeRingBuf.writeData(data, len); } - ReturnValue_t DleParser::parseRingBuf(size_t& readSize) { + ctx.setType(DleParser::ContextType::NONE); size_t availableData = decodeRingBuf.getAvailableReadData(); if (availableData > encodedBuf.second) { ErrorInfo info; info.len = decodeRingBuf.getAvailableReadData(); - prepareErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); - handler(ctx); + setErrorContext(ErrorTypes::DECODING_BUF_TOO_SMALL, info); + return returnvalue::FAILED; } ReturnValue_t result = decodeRingBuf.readData(encodedBuf.first, availableData); - if(result != returnvalue::OK) { + if (result != returnvalue::OK) { ErrorInfo info; info.res = result; - prepareErrorContext(ErrorTypes::RING_BUF_ERROR, info); + setErrorContext(ErrorTypes::RING_BUF_ERROR, info); + return result; } bool stxFound = false; size_t stxIdx = 0; @@ -190,6 +49,8 @@ ReturnValue_t DleParser::parseRingBuf(size_t& readSize) { // might be lost packet, so we should advance the read pointer // without skipping the STX readSize = vectorIdx; + ErrorInfo info; + setErrorContext(ErrorTypes::CONSECUTIVE_STX_CHARS, info); return POSSIBLE_PACKET_LOSS; } } @@ -199,31 +60,37 @@ ReturnValue_t DleParser::parseRingBuf(size_t& readSize) { // This is propably a packet, so we decode it. size_t decodedLen = 0; size_t dummy = 0; - readSize = vectorIdx; - ReturnValue_t result = decoder.decode(&encodedBuf.first[stxIdx], availableData - stxIdx, - &dummy, decodedBuf.first, decodedBuf.second, &decodedLen); + + ReturnValue_t result = + decoder.decode(&encodedBuf.first[stxIdx], availableData - stxIdx, &dummy, + decodedBuf.first, decodedBuf.second, &decodedLen); if (result == returnvalue::OK) { ctx.setType(ContextType::PACKET_FOUND); ctx.decodedPacket.first = decodedBuf.first; ctx.decodedPacket.second = decodedLen; - this->handler(ctx); + readSize = vectorIdx + 1; return returnvalue::OK; } else { // invalid packet, skip. readSize = ++vectorIdx; + ErrorInfo info; + info.res = result; + setErrorContext(ErrorTypes::DECODE_ERROR, info); return POSSIBLE_PACKET_LOSS; } } else { // might be lost packet, so we should advance the read pointer readSize = ++vectorIdx; + ErrorInfo info; + info.len = 0; + setErrorContext(ErrorTypes::CONSECUTIVE_ETX_CHARS, info); return POSSIBLE_PACKET_LOSS; } } } - return returnvalue::OK; + return NO_PACKET_FOUND; } - void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* args) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -235,8 +102,12 @@ void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* arg #endif } -void DleParser::defaultErrorHandler(ErrorTypes err, ErrorInfo ctx) { - switch (err) { +void DleParser::defaultErrorHandler() { + if(ctx.getType() != DleParser::ContextType::ERROR) { + errorPrinter("No error"); + return; + } + switch (ctx.error.first) { case (ErrorTypes::NONE): { errorPrinter("No error"); break; @@ -252,8 +123,8 @@ void DleParser::defaultErrorHandler(ErrorTypes err, ErrorInfo ctx) { case (ErrorTypes::ENCODED_BUF_TOO_SMALL): case (ErrorTypes::DECODING_BUF_TOO_SMALL): { char opt[64]; - snprintf(opt, sizeof(opt), ": Too small for packet with length %zu", ctx.len); - if (err == ErrorTypes::ENCODED_BUF_TOO_SMALL) { + snprintf(opt, sizeof(opt), ": Too small for packet with length %zu", ctx.decodedPacket.second); + if (ctx.error.first == ErrorTypes::ENCODED_BUF_TOO_SMALL) { errorPrinter("Encoded buf too small", opt); } else { errorPrinter("Decoding buf too small", opt); @@ -284,7 +155,7 @@ void DleParser::errorPrinter(const char* str, const char* opt) { #endif } -void DleParser::prepareErrorContext(ErrorTypes err, ErrorInfo info) { +void DleParser::setErrorContext(ErrorTypes err, ErrorInfo info) { ctx.setType(ContextType::ERROR); ctx.error.first = err; ctx.error.second = info; @@ -294,7 +165,10 @@ ReturnValue_t DleParser::confirmBytesRead(size_t bytesRead) { return decodeRingBuf.deleteData(bytesRead); } +const DleParser::Context& DleParser::getContext() { + return ctx; +} + void DleParser::reset() { - startFound = false; decodeRingBuf.clear(); } diff --git a/src/fsfw/globalfunctions/DleParser.h b/src/fsfw/globalfunctions/DleParser.h index b31fbf83..9802017a 100644 --- a/src/fsfw/globalfunctions/DleParser.h +++ b/src/fsfw/globalfunctions/DleParser.h @@ -22,7 +22,7 @@ class DleParser { static constexpr ReturnValue_t POSSIBLE_PACKET_LOSS = returnvalue::makeCode(1, 2); using BufPair = std::pair; - enum class ContextType { PACKET_FOUND, ERROR }; + enum class ContextType { NONE, PACKET_FOUND, ERROR }; enum class ErrorTypes { NONE, @@ -43,7 +43,7 @@ class DleParser { struct Context { public: - Context(void* args) : userArgs(args) { setType(ContextType::PACKET_FOUND); } + Context() { setType(ContextType::PACKET_FOUND); } void setType(ContextType type) { this->type = type; @@ -60,14 +60,11 @@ class DleParser { BufPair decodedPacket = {}; ErrorPair error; - void* userArgs; private: ContextType type; }; - using UserHandler = void (*)(const Context& ctx); - /** * Base class constructor * @param decodeRingBuf Ring buffer used to store multiple packets to allow detecting DLE packets @@ -81,7 +78,7 @@ class DleParser { * @param args Arbitrary user argument */ DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPair encodedBuf, - BufPair decodedBuf, UserHandler handler, void* args); + BufPair decodedBuf); /** * This function allows to pass new data into the parser. It then scans for DLE packets @@ -96,6 +93,7 @@ class DleParser { ReturnValue_t confirmBytesRead(size_t bytesRead); + const Context& getContext(); /** * Example found packet handler * function call @@ -110,11 +108,11 @@ class DleParser { * - For buffer length errors, will be set to the detected packet length which is too large * - For decode or ring buffer errors, will be set to the result returned from the failed call */ - static void defaultErrorHandler(ErrorTypes err, ErrorInfo ctx); + void defaultErrorHandler(); static void errorPrinter(const char* str, const char* opt = nullptr); - void prepareErrorContext(ErrorTypes err, ErrorInfo ctx); + void setErrorContext(ErrorTypes err, ErrorInfo ctx); /** * Resets the parser by resetting the internal states and clearing the decoding ring buffer */ @@ -125,7 +123,5 @@ class DleParser { DleEncoder& decoder; BufPair encodedBuf; BufPair decodedBuf; - UserHandler handler = nullptr; Context ctx; - bool startFound = false; }; diff --git a/src/fsfw/pus/Service5EventReporting.cpp b/src/fsfw/pus/Service5EventReporting.cpp index c1affa6f..b4b9b03d 100644 --- a/src/fsfw/pus/Service5EventReporting.cpp +++ b/src/fsfw/pus/Service5EventReporting.cpp @@ -15,8 +15,8 @@ Service5EventReporting::Service5EventReporting(PsbParams params, size_t maxNumbe maxNumberReportsPerCycle(maxNumberReportsPerCycle) { auto mqArgs = MqArgs(getObjectId(), static_cast(this)); psbParams.name = "PUS 5 Event Reporting"; - eventQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth, - MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); + eventQueue = QueueFactory::instance()->createMessageQueue( + messageQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } Service5EventReporting::~Service5EventReporting() { From d1630cdc4c3fc801934f1e394efa80e898f8031b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 4 Oct 2022 21:17:35 +0200 Subject: [PATCH 214/404] something is wrong --- src/fsfw/globalfunctions/DleParser.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/globalfunctions/DleParser.cpp b/src/fsfw/globalfunctions/DleParser.cpp index b992debd..88d1dfb2 100644 --- a/src/fsfw/globalfunctions/DleParser.cpp +++ b/src/fsfw/globalfunctions/DleParser.cpp @@ -68,7 +68,7 @@ ReturnValue_t DleParser::parseRingBuf(size_t& readSize) { ctx.setType(ContextType::PACKET_FOUND); ctx.decodedPacket.first = decodedBuf.first; ctx.decodedPacket.second = decodedLen; - readSize = vectorIdx + 1; + readSize = ++vectorIdx; return returnvalue::OK; } else { // invalid packet, skip. From 1c53b60442f5b858b00938169ef1daba41ba272d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 4 Oct 2022 23:04:28 +0200 Subject: [PATCH 215/404] small additional tweak --- src/fsfw/globalfunctions/DleParser.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/fsfw/globalfunctions/DleParser.cpp b/src/fsfw/globalfunctions/DleParser.cpp index 88d1dfb2..90adfa7a 100644 --- a/src/fsfw/globalfunctions/DleParser.cpp +++ b/src/fsfw/globalfunctions/DleParser.cpp @@ -24,6 +24,9 @@ ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) { ReturnValue_t DleParser::parseRingBuf(size_t& readSize) { ctx.setType(DleParser::ContextType::NONE); size_t availableData = decodeRingBuf.getAvailableReadData(); + if(availableData == 0) { + return NO_PACKET_FOUND; + } if (availableData > encodedBuf.second) { ErrorInfo info; info.len = decodeRingBuf.getAvailableReadData(); From 40e7b2dc31686840ea001628b7f8ebb803b995b2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Oct 2022 11:14:00 +0200 Subject: [PATCH 216/404] new uart helper module --- src/fsfw_hal/linux/uart/CMakeLists.txt | 2 +- src/fsfw_hal/linux/uart/UartComIF.cpp | 147 +----------------------- src/fsfw_hal/linux/uart/UartComIF.h | 12 +- src/fsfw_hal/linux/uart/UartCookie.h | 41 +------ src/fsfw_hal/linux/uart/helper.cpp | 150 +++++++++++++++++++++++++ src/fsfw_hal/linux/uart/helper.h | 62 ++++++++++ 6 files changed, 221 insertions(+), 193 deletions(-) create mode 100644 src/fsfw_hal/linux/uart/helper.cpp create mode 100644 src/fsfw_hal/linux/uart/helper.h diff --git a/src/fsfw_hal/linux/uart/CMakeLists.txt b/src/fsfw_hal/linux/uart/CMakeLists.txt index 9cad62a4..d8daa9ff 100644 --- a/src/fsfw_hal/linux/uart/CMakeLists.txt +++ b/src/fsfw_hal/linux/uart/CMakeLists.txt @@ -1 +1 @@ -target_sources(${LIB_FSFW_NAME} PUBLIC UartComIF.cpp UartCookie.cpp) +target_sources(${LIB_FSFW_NAME} PUBLIC UartComIF.cpp UartCookie.cpp helper.cpp) diff --git a/src/fsfw_hal/linux/uart/UartComIF.cpp b/src/fsfw_hal/linux/uart/UartComIF.cpp index 8947c562..df21da64 100644 --- a/src/fsfw_hal/linux/uart/UartComIF.cpp +++ b/src/fsfw_hal/linux/uart/UartComIF.cpp @@ -1,5 +1,4 @@ #include "UartComIF.h" - #include #include #include @@ -93,7 +92,7 @@ int UartComIF::configureUartPort(UartCookie* uartCookie) { setStopBitOptions(&options, uartCookie); setDatasizeOptions(&options, uartCookie); setFixedOptions(&options); - setUartMode(&options, *uartCookie); + uart::setMode(options, uartCookie->getUartMode()); if (uartCookie->getInputShouldBeFlushed()) { tcflush(fd, TCIFLUSH); } @@ -102,7 +101,7 @@ int UartComIF::configureUartPort(UartCookie* uartCookie) { options.c_cc[VTIME] = 0; options.c_cc[VMIN] = 0; - configureBaudrate(&options, uartCookie); + uart::setBaudrate(options, uartCookie->getBaudrate()); /* Save option settings */ if (tcsetattr(fd, TCSANOW, &options) != 0) { @@ -191,138 +190,6 @@ void UartComIF::setFixedOptions(struct termios* options) { options->c_oflag &= ~ONLCR; } -void UartComIF::configureBaudrate(struct termios* options, UartCookie* uartCookie) { - switch (uartCookie->getBaudrate()) { - case UartBaudRate::RATE_50: - cfsetispeed(options, B50); - cfsetospeed(options, B50); - break; - case UartBaudRate::RATE_75: - cfsetispeed(options, B75); - cfsetospeed(options, B75); - break; - case UartBaudRate::RATE_110: - cfsetispeed(options, B110); - cfsetospeed(options, B110); - break; - case UartBaudRate::RATE_134: - cfsetispeed(options, B134); - cfsetospeed(options, B134); - break; - case UartBaudRate::RATE_150: - cfsetispeed(options, B150); - cfsetospeed(options, B150); - break; - case UartBaudRate::RATE_200: - cfsetispeed(options, B200); - cfsetospeed(options, B200); - break; - case UartBaudRate::RATE_300: - cfsetispeed(options, B300); - cfsetospeed(options, B300); - break; - case UartBaudRate::RATE_600: - cfsetispeed(options, B600); - cfsetospeed(options, B600); - break; - case UartBaudRate::RATE_1200: - cfsetispeed(options, B1200); - cfsetospeed(options, B1200); - break; - case UartBaudRate::RATE_1800: - cfsetispeed(options, B1800); - cfsetospeed(options, B1800); - break; - case UartBaudRate::RATE_2400: - cfsetispeed(options, B2400); - cfsetospeed(options, B2400); - break; - case UartBaudRate::RATE_4800: - cfsetispeed(options, B4800); - cfsetospeed(options, B4800); - break; - case UartBaudRate::RATE_9600: - cfsetispeed(options, B9600); - cfsetospeed(options, B9600); - break; - case UartBaudRate::RATE_19200: - cfsetispeed(options, B19200); - cfsetospeed(options, B19200); - break; - case UartBaudRate::RATE_38400: - cfsetispeed(options, B38400); - cfsetospeed(options, B38400); - break; - case UartBaudRate::RATE_57600: - cfsetispeed(options, B57600); - cfsetospeed(options, B57600); - break; - case UartBaudRate::RATE_115200: - cfsetispeed(options, B115200); - cfsetospeed(options, B115200); - break; - case UartBaudRate::RATE_230400: - cfsetispeed(options, B230400); - cfsetospeed(options, B230400); - break; -#ifndef __APPLE__ - case UartBaudRate::RATE_460800: - cfsetispeed(options, B460800); - cfsetospeed(options, B460800); - break; - case UartBaudRate::RATE_500000: - cfsetispeed(options, B500000); - cfsetospeed(options, B500000); - break; - case UartBaudRate::RATE_576000: - cfsetispeed(options, B576000); - cfsetospeed(options, B576000); - break; - case UartBaudRate::RATE_921600: - cfsetispeed(options, B921600); - cfsetospeed(options, B921600); - break; - case UartBaudRate::RATE_1000000: - cfsetispeed(options, B1000000); - cfsetospeed(options, B1000000); - break; - case UartBaudRate::RATE_1152000: - cfsetispeed(options, B1152000); - cfsetospeed(options, B1152000); - break; - case UartBaudRate::RATE_1500000: - cfsetispeed(options, B1500000); - cfsetospeed(options, B1500000); - break; - case UartBaudRate::RATE_2000000: - cfsetispeed(options, B2000000); - cfsetospeed(options, B2000000); - break; - case UartBaudRate::RATE_2500000: - cfsetispeed(options, B2500000); - cfsetospeed(options, B2500000); - break; - case UartBaudRate::RATE_3000000: - cfsetispeed(options, B3000000); - cfsetospeed(options, B3000000); - break; - case UartBaudRate::RATE_3500000: - cfsetispeed(options, B3500000); - cfsetospeed(options, B3500000); - break; - case UartBaudRate::RATE_4000000: - cfsetispeed(options, B4000000); - cfsetospeed(options, B4000000); - break; -#endif // ! __APPLE__ - default: -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "UartComIF::configureBaudrate: Baudrate not supported" << std::endl; -#endif - break; - } -} - ReturnValue_t UartComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) { int fd = 0; std::string deviceFile; @@ -592,12 +459,4 @@ ReturnValue_t UartComIF::flushUartTxAndRxBuf(CookieIF* cookie) { return returnvalue::FAILED; } -void UartComIF::setUartMode(struct termios* options, UartCookie& uartCookie) { - UartModes uartMode = uartCookie.getUartMode(); - if (uartMode == UartModes::NON_CANONICAL) { - /* Disable canonical mode */ - options->c_lflag &= ~ICANON; - } else if (uartMode == UartModes::CANONICAL) { - options->c_lflag |= ICANON; - } -} + diff --git a/src/fsfw_hal/linux/uart/UartComIF.h b/src/fsfw_hal/linux/uart/UartComIF.h index 77318166..940938d9 100644 --- a/src/fsfw_hal/linux/uart/UartComIF.h +++ b/src/fsfw_hal/linux/uart/UartComIF.h @@ -1,13 +1,15 @@ #ifndef BSP_Q7S_COMIF_UARTCOMIF_H_ #define BSP_Q7S_COMIF_UARTCOMIF_H_ +#include "UartCookie.h" +#include "helper.h" + #include #include #include #include -#include "UartCookie.h" /** * @brief This is the communication interface to access serial ports on linux based operating @@ -101,14 +103,6 @@ class UartComIF : public DeviceCommunicationIF, public SystemObject { */ void setDatasizeOptions(struct termios* options, UartCookie* uartCookie); - /** - * @brief This functions adds the baudrate specified in the uartCookie to the termios options - * struct. - */ - void configureBaudrate(struct termios* options, UartCookie* uartCookie); - - void setUartMode(struct termios* options, UartCookie& uartCookie); - ReturnValue_t handleCanonicalRead(UartCookie& uartCookie, UartDeviceMapIter& iter, size_t requestLen); ReturnValue_t handleNoncanonicalRead(UartCookie& uartCookie, UartDeviceMapIter& iter, diff --git a/src/fsfw_hal/linux/uart/UartCookie.h b/src/fsfw_hal/linux/uart/UartCookie.h index cae33d58..6fa2bd1b 100644 --- a/src/fsfw_hal/linux/uart/UartCookie.h +++ b/src/fsfw_hal/linux/uart/UartCookie.h @@ -1,51 +1,14 @@ #ifndef SAM9G20_COMIF_COOKIES_UART_COOKIE_H_ #define SAM9G20_COMIF_COOKIES_UART_COOKIE_H_ +#include "helper.h" + #include #include #include -enum class Parity { NONE, EVEN, ODD }; -enum class StopBits { ONE_STOP_BIT, TWO_STOP_BITS }; - -enum class UartModes { CANONICAL, NON_CANONICAL }; - -enum class BitsPerWord { BITS_5, BITS_6, BITS_7, BITS_8 }; - -enum class UartBaudRate { - RATE_50, - RATE_75, - RATE_110, - RATE_134, - RATE_150, - RATE_200, - RATE_300, - RATE_600, - RATE_1200, - RATE_1800, - RATE_2400, - RATE_4800, - RATE_9600, - RATE_19200, - RATE_38400, - RATE_57600, - RATE_115200, - RATE_230400, - RATE_460800, - RATE_500000, - RATE_576000, - RATE_921600, - RATE_1000000, - RATE_1152000, - RATE_1500000, - RATE_2000000, - RATE_2500000, - RATE_3000000, - RATE_3500000, - RATE_4000000 -}; /** * @brief Cookie for the UartComIF. There are many options available to configure the UART driver. diff --git a/src/fsfw_hal/linux/uart/helper.cpp b/src/fsfw_hal/linux/uart/helper.cpp new file mode 100644 index 00000000..b451f457 --- /dev/null +++ b/src/fsfw_hal/linux/uart/helper.cpp @@ -0,0 +1,150 @@ +#include "helper.h" +#include "fsfw/serviceinterface.h" + +#include + +void uart::setMode(struct termios& options, UartModes mode) { + if (mode == UartModes::NON_CANONICAL) { + /* Disable canonical mode */ + options.c_lflag &= ~ICANON; + } else if (mode == UartModes::CANONICAL) { + options.c_lflag |= ICANON; + } +} + +void uart::setBaudrate(struct termios& options, UartBaudRate baud) { + switch (baud) { + case UartBaudRate::RATE_50: + cfsetispeed(&options, B50); + cfsetospeed(&options, B50); + break; + case UartBaudRate::RATE_75: + cfsetispeed(&options, B75); + cfsetospeed(&options, B75); + break; + case UartBaudRate::RATE_110: + cfsetispeed(&options, B110); + cfsetospeed(&options, B110); + break; + case UartBaudRate::RATE_134: + cfsetispeed(&options, B134); + cfsetospeed(&options, B134); + break; + case UartBaudRate::RATE_150: + cfsetispeed(&options, B150); + cfsetospeed(&options, B150); + break; + case UartBaudRate::RATE_200: + cfsetispeed(&options, B200); + cfsetospeed(&options, B200); + break; + case UartBaudRate::RATE_300: + cfsetispeed(&options, B300); + cfsetospeed(&options, B300); + break; + case UartBaudRate::RATE_600: + cfsetispeed(&options, B600); + cfsetospeed(&options, B600); + break; + case UartBaudRate::RATE_1200: + cfsetispeed(&options, B1200); + cfsetospeed(&options, B1200); + break; + case UartBaudRate::RATE_1800: + cfsetispeed(&options, B1800); + cfsetospeed(&options, B1800); + break; + case UartBaudRate::RATE_2400: + cfsetispeed(&options, B2400); + cfsetospeed(&options, B2400); + break; + case UartBaudRate::RATE_4800: + cfsetispeed(&options, B4800); + cfsetospeed(&options, B4800); + break; + case UartBaudRate::RATE_9600: + cfsetispeed(&options, B9600); + cfsetospeed(&options, B9600); + break; + case UartBaudRate::RATE_19200: + cfsetispeed(&options, B19200); + cfsetospeed(&options, B19200); + break; + case UartBaudRate::RATE_38400: + cfsetispeed(&options, B38400); + cfsetospeed(&options, B38400); + break; + case UartBaudRate::RATE_57600: + cfsetispeed(&options, B57600); + cfsetospeed(&options, B57600); + break; + case UartBaudRate::RATE_115200: + cfsetispeed(&options, B115200); + cfsetospeed(&options, B115200); + break; + case UartBaudRate::RATE_230400: + cfsetispeed(&options, B230400); + cfsetospeed(&options, B230400); + break; +#ifndef __APPLE__ + case UartBaudRate::RATE_460800: + cfsetispeed(&options, B460800); + cfsetospeed(&options, B460800); + break; + case UartBaudRate::RATE_500000: + cfsetispeed(&options, B500000); + cfsetospeed(&options, B500000); + break; + case UartBaudRate::RATE_576000: + cfsetispeed(&options, B576000); + cfsetospeed(&options, B576000); + break; + case UartBaudRate::RATE_921600: + cfsetispeed(&options, B921600); + cfsetospeed(&options, B921600); + break; + case UartBaudRate::RATE_1000000: + cfsetispeed(&options, B1000000); + cfsetospeed(&options, B1000000); + break; + case UartBaudRate::RATE_1152000: + cfsetispeed(&options, B1152000); + cfsetospeed(&options, B1152000); + break; + case UartBaudRate::RATE_1500000: + cfsetispeed(&options, B1500000); + cfsetospeed(&options, B1500000); + break; + case UartBaudRate::RATE_2000000: + cfsetispeed(&options, B2000000); + cfsetospeed(&options, B2000000); + break; + case UartBaudRate::RATE_2500000: + cfsetispeed(&options, B2500000); + cfsetospeed(&options, B2500000); + break; + case UartBaudRate::RATE_3000000: + cfsetispeed(&options, B3000000); + cfsetospeed(&options, B3000000); + break; + case UartBaudRate::RATE_3500000: + cfsetispeed(&options, B3500000); + cfsetospeed(&options, B3500000); + break; + case UartBaudRate::RATE_4000000: + cfsetispeed(&options, B4000000); + cfsetospeed(&options, B4000000); + break; +#endif // ! __APPLE__ + default: +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "UartComIF::configureBaudrate: Baudrate not supported" << std::endl; +#endif + break; + } +} + +int uart::readCountersAndErrors(int serialPort, serial_icounter_struct& icounter) { + return ioctl(serialPort, TIOCGICOUNT, &icounter); +} + diff --git a/src/fsfw_hal/linux/uart/helper.h b/src/fsfw_hal/linux/uart/helper.h new file mode 100644 index 00000000..b6a524d6 --- /dev/null +++ b/src/fsfw_hal/linux/uart/helper.h @@ -0,0 +1,62 @@ +#ifndef FSFW_HAL_LINUX_UART_HELPER_H_ +#define FSFW_HAL_LINUX_UART_HELPER_H_ + +#include +#include + +enum class Parity { NONE, EVEN, ODD }; + +enum class StopBits { ONE_STOP_BIT, TWO_STOP_BITS }; + +enum class UartModes { CANONICAL, NON_CANONICAL }; + +enum class BitsPerWord { BITS_5, BITS_6, BITS_7, BITS_8 }; + +enum class UartBaudRate { + RATE_50, + RATE_75, + RATE_110, + RATE_134, + RATE_150, + RATE_200, + RATE_300, + RATE_600, + RATE_1200, + RATE_1800, + RATE_2400, + RATE_4800, + RATE_9600, + RATE_19200, + RATE_38400, + RATE_57600, + RATE_115200, + RATE_230400, + RATE_460800, + RATE_500000, + RATE_576000, + RATE_921600, + RATE_1000000, + RATE_1152000, + RATE_1500000, + RATE_2000000, + RATE_2500000, + RATE_3000000, + RATE_3500000, + RATE_4000000 +}; + +namespace uart { + +void setMode(struct termios& options, UartModes mode); +/** + * @brief This functions adds the baudrate specified in the uartCookie to the termios options + * struct. + */ +void setBaudrate(struct termios& options, UartBaudRate baud); + +int readCountersAndErrors(int serialPort, serial_icounter_struct& icounter); + +} + + +#endif /* FSFW_HAL_LINUX_UART_HELPER_H_ */ From 692be9df8d06beb3bfc83aad77cefd727d8f7c35 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Oct 2022 16:57:47 +0200 Subject: [PATCH 217/404] DHB bugfix and addition --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 13 ++++++++++++- src/fsfw/devicehandlers/DeviceHandlerBase.h | 2 ++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 8f23b112..c641eb58 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -467,7 +467,7 @@ ReturnValue_t DeviceHandlerBase::insertInCommandMap(DeviceCommandId_t deviceComm info.expectedReplies = 0; info.isExecuting = false; info.sendReplyTo = NO_COMMANDER; - info.useAlternativeReplyId = alternativeReplyId; + info.useAlternativeReplyId = useAlternativeReply; info.alternativeReplyId = alternativeReplyId; auto resultPair = deviceCommandMap.emplace(deviceCommand, info); if (resultPair.second) { @@ -1601,3 +1601,14 @@ void DeviceHandlerBase::disableCommandsAndReplies() { } } } + +ReturnValue_t DeviceHandlerBase::finishAction(bool success, DeviceCommandId_t action, + ReturnValue_t result) { + auto commandIter = deviceCommandMap.find(action); + if (commandIter == deviceCommandMap.end()) { + return MessageQueueIF::NO_QUEUE; + } + commandIter->second.isExecuting = false; + actionHelper.finish(success, commandIter->second.sendReplyTo, action, result); + return returnvalue::OK; +} diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index 7c93e921..0efecbd5 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -399,6 +399,8 @@ class DeviceHandlerBase : public DeviceHandlerIF, */ virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) = 0; MessageQueueId_t getCommanderQueueId(DeviceCommandId_t replyId) const; + ReturnValue_t finishAction(bool success, DeviceCommandId_t action, ReturnValue_t result); + /** * Helper function to get pending command. This is useful for devices * like SPI sensors to identify the last sent command. From 221df7ece631213abc12bc786585ea5cb7069ec7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 10 Oct 2022 10:46:29 +0200 Subject: [PATCH 218/404] allow recursive mode announcements --- src/fsfw/pus/CService200ModeCommanding.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/fsfw/pus/CService200ModeCommanding.cpp b/src/fsfw/pus/CService200ModeCommanding.cpp index d28df59b..809fb02b 100644 --- a/src/fsfw/pus/CService200ModeCommanding.cpp +++ b/src/fsfw/pus/CService200ModeCommanding.cpp @@ -20,6 +20,7 @@ ReturnValue_t CService200ModeCommanding::isValidSubservice(uint8_t subservice) { case (Subservice::COMMAND_MODE_COMMAND): case (Subservice::COMMAND_MODE_READ): case (Subservice::COMMAND_MODE_ANNCOUNCE): + case (Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY): return returnvalue::OK; default: return AcceptsTelecommandsIF::INVALID_SUBSERVICE; From 1d6ccfe5ab63c268e1b62a731eb55d35eac88492 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 10 Oct 2022 11:06:58 +0200 Subject: [PATCH 219/404] Service 200: Add mode announcement support --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 16 ++--- src/fsfw/globalfunctions/DleParser.cpp | 18 +++--- src/fsfw/modes/ModeMessage.cpp | 10 ++++ src/fsfw/modes/ModeMessage.h | 60 +++++++++---------- src/fsfw/pus/CService200ModeCommanding.cpp | 25 +++++--- .../pus/servicepackets/Service200Packets.h | 6 +- src/fsfw_hal/linux/uart/UartComIF.cpp | 3 +- src/fsfw_hal/linux/uart/UartComIF.h | 5 +- src/fsfw_hal/linux/uart/UartCookie.h | 4 +- src/fsfw_hal/linux/uart/helper.cpp | 6 +- src/fsfw_hal/linux/uart/helper.h | 5 +- 11 files changed, 85 insertions(+), 73 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 69fa9399..63124dc8 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1604,12 +1604,12 @@ const HasModesIF& DeviceHandlerBase::getModeIF() const { return *this; } ModeTreeChildIF& DeviceHandlerBase::getModeTreeChildIF() { return *this; } ReturnValue_t DeviceHandlerBase::finishAction(bool success, DeviceCommandId_t action, - ReturnValue_t result) { - auto commandIter = deviceCommandMap.find(action); - if (commandIter == deviceCommandMap.end()) { - return MessageQueueIF::NO_QUEUE; - } - commandIter->second.isExecuting = false; - actionHelper.finish(success, commandIter->second.sendReplyTo, action, result); - return returnvalue::OK; + ReturnValue_t result) { + auto commandIter = deviceCommandMap.find(action); + if (commandIter == deviceCommandMap.end()) { + return MessageQueueIF::NO_QUEUE; + } + commandIter->second.isExecuting = false; + actionHelper.finish(success, commandIter->second.sendReplyTo, action, result); + return returnvalue::OK; } diff --git a/src/fsfw/globalfunctions/DleParser.cpp b/src/fsfw/globalfunctions/DleParser.cpp index 90adfa7a..cc695bab 100644 --- a/src/fsfw/globalfunctions/DleParser.cpp +++ b/src/fsfw/globalfunctions/DleParser.cpp @@ -11,8 +11,7 @@ DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPa : decodeRingBuf(decodeRingBuf), decoder(decoder), encodedBuf(encodedBuf), - decodedBuf(decodedBuf) { -} + decodedBuf(decodedBuf) {} ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) { if (data == nullptr or len == 0) { @@ -24,7 +23,7 @@ ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) { ReturnValue_t DleParser::parseRingBuf(size_t& readSize) { ctx.setType(DleParser::ContextType::NONE); size_t availableData = decodeRingBuf.getAvailableReadData(); - if(availableData == 0) { + if (availableData == 0) { return NO_PACKET_FOUND; } if (availableData > encodedBuf.second) { @@ -106,7 +105,7 @@ void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* arg } void DleParser::defaultErrorHandler() { - if(ctx.getType() != DleParser::ContextType::ERROR) { + if (ctx.getType() != DleParser::ContextType::ERROR) { errorPrinter("No error"); return; } @@ -126,7 +125,8 @@ void DleParser::defaultErrorHandler() { case (ErrorTypes::ENCODED_BUF_TOO_SMALL): case (ErrorTypes::DECODING_BUF_TOO_SMALL): { char opt[64]; - snprintf(opt, sizeof(opt), ": Too small for packet with length %zu", ctx.decodedPacket.second); + snprintf(opt, sizeof(opt), ": Too small for packet with length %zu", + ctx.decodedPacket.second); if (ctx.error.first == ErrorTypes::ENCODED_BUF_TOO_SMALL) { errorPrinter("Encoded buf too small", opt); } else { @@ -168,10 +168,6 @@ ReturnValue_t DleParser::confirmBytesRead(size_t bytesRead) { return decodeRingBuf.deleteData(bytesRead); } -const DleParser::Context& DleParser::getContext() { - return ctx; -} +const DleParser::Context& DleParser::getContext() { return ctx; } -void DleParser::reset() { - decodeRingBuf.clear(); -} +void DleParser::reset() { decodeRingBuf.clear(); } diff --git a/src/fsfw/modes/ModeMessage.cpp b/src/fsfw/modes/ModeMessage.cpp index ecc52c94..efe6d534 100644 --- a/src/fsfw/modes/ModeMessage.cpp +++ b/src/fsfw/modes/ModeMessage.cpp @@ -24,3 +24,13 @@ void ModeMessage::setCantReachMode(CommandMessage* message, ReturnValue_t reason message->setParameter(reason); message->setParameter2(0); } + +void ModeMessage::setModeAnnounceMessage(CommandMessage& message, bool recursive) { + Command_t cmd; + if (recursive) { + cmd = CMD_MODE_ANNOUNCE_RECURSIVELY; + } else { + cmd = CMD_MODE_ANNOUNCE; + } + message.setCommand(cmd); +} diff --git a/src/fsfw/modes/ModeMessage.h b/src/fsfw/modes/ModeMessage.h index 84429e84..1582ccd4 100644 --- a/src/fsfw/modes/ModeMessage.h +++ b/src/fsfw/modes/ModeMessage.h @@ -1,43 +1,42 @@ #ifndef FSFW_MODES_MODEMESSAGE_H_ #define FSFW_MODES_MODEMESSAGE_H_ -#include "../ipc/CommandMessage.h" +#include "fsfw/ipc/CommandMessage.h" typedef uint32_t Mode_t; typedef uint8_t Submode_t; class ModeMessage { - private: - ModeMessage(); - public: 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!! - 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!! - 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) - 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 - 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. + //!> 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!! + static const Command_t CMD_MODE_COMMAND = MAKE_COMMAND_ID(0x01); + //!> 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!! + static const Command_t CMD_MODE_COMMAND_FORCED = MAKE_COMMAND_ID(0xF1); + //!> Reply to a CMD_MODE_COMMAND or CMD_MODE_READ + static const Command_t REPLY_MODE_REPLY = MAKE_COMMAND_ID(0x02); + //!> Unrequested info about the current mode (used for composites to + //! inform their container of a changed mode) + static const Command_t REPLY_MODE_INFO = MAKE_COMMAND_ID(0x03); + //!> Reply in case a mode command can't be executed. Par1: returnCode, Par2: 0 + static const Command_t REPLY_CANT_REACH_MODE = MAKE_COMMAND_ID(0x04); + //!> 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 + static const Command_t REPLY_WRONG_MODE_REPLY = MAKE_COMMAND_ID(0x05); + //!> Command to read the current mode and reply with a REPLY_MODE_REPLY + static const Command_t CMD_MODE_READ = MAKE_COMMAND_ID(0x06); + //!> Command to trigger an ModeInfo Event. This command does NOT have a reply. + static const Command_t CMD_MODE_ANNOUNCE = MAKE_COMMAND_ID(0x07); + //!> Command to trigger an ModeInfo Event and to send this command to + //! every child. This command does NOT have a reply. + static const Command_t CMD_MODE_ANNOUNCE_RECURSIVELY = MAKE_COMMAND_ID(0x08); + + ModeMessage() = delete; static Mode_t getMode(const CommandMessage* message); static Submode_t getSubmode(const CommandMessage* message); @@ -45,6 +44,7 @@ class ModeMessage { static void setModeMessage(CommandMessage* message, Command_t command, Mode_t mode, Submode_t submode); + static void setModeAnnounceMessage(CommandMessage& message, bool recursive); static void setCantReachMode(CommandMessage* message, ReturnValue_t reason); static void clear(CommandMessage* message); }; diff --git a/src/fsfw/pus/CService200ModeCommanding.cpp b/src/fsfw/pus/CService200ModeCommanding.cpp index 809fb02b..f5928be7 100644 --- a/src/fsfw/pus/CService200ModeCommanding.cpp +++ b/src/fsfw/pus/CService200ModeCommanding.cpp @@ -54,15 +54,26 @@ ReturnValue_t CService200ModeCommanding::checkInterfaceAndAcquireMessageQueue( ReturnValue_t CService200ModeCommanding::prepareCommand(CommandMessage *message, uint8_t subservice, const uint8_t *tcData, size_t tcDataLen, uint32_t *state, object_id_t objectId) { - ModePacket modeCommandPacket; - ReturnValue_t result = - modeCommandPacket.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG); - if (result != returnvalue::OK) { - return result; + ReturnValue_t result = returnvalue::OK; + if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE or + subservice == Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY) { + bool recursive = true; + if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE) { + recursive = false; + } + ModeMessage::setModeAnnounceMessage(*message, recursive); + } else { + ModePacket modeCommandPacket; + ReturnValue_t result = + modeCommandPacket.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG); + if (result != returnvalue::OK) { + return result; + } + + ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(), + modeCommandPacket.getSubmode()); } - ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(), - modeCommandPacket.getSubmode()); return result; } diff --git a/src/fsfw/pus/servicepackets/Service200Packets.h b/src/fsfw/pus/servicepackets/Service200Packets.h index 701e3f09..3e1df55a 100644 --- a/src/fsfw/pus/servicepackets/Service200Packets.h +++ b/src/fsfw/pus/servicepackets/Service200Packets.h @@ -1,9 +1,9 @@ #ifndef FSFW_PUS_SERVICEPACKETS_SERVICE200PACKETS_H_ #define FSFW_PUS_SERVICEPACKETS_SERVICE200PACKETS_H_ -#include "../../modes/ModeMessage.h" -#include "../../serialize/SerialLinkedListAdapter.h" -#include "../../serialize/SerializeIF.h" +#include "fsfw/modes/ModeMessage.h" +#include "fsfw/serialize/SerialLinkedListAdapter.h" +#include "fsfw/serialize/SerializeIF.h" /** * @brief Subservice 1, 2, 3, 4, 5 diff --git a/src/fsfw_hal/linux/uart/UartComIF.cpp b/src/fsfw_hal/linux/uart/UartComIF.cpp index df21da64..0880dec6 100644 --- a/src/fsfw_hal/linux/uart/UartComIF.cpp +++ b/src/fsfw_hal/linux/uart/UartComIF.cpp @@ -1,4 +1,5 @@ #include "UartComIF.h" + #include #include #include @@ -458,5 +459,3 @@ ReturnValue_t UartComIF::flushUartTxAndRxBuf(CookieIF* cookie) { } return returnvalue::FAILED; } - - diff --git a/src/fsfw_hal/linux/uart/UartComIF.h b/src/fsfw_hal/linux/uart/UartComIF.h index 940938d9..657e0123 100644 --- a/src/fsfw_hal/linux/uart/UartComIF.h +++ b/src/fsfw_hal/linux/uart/UartComIF.h @@ -1,15 +1,14 @@ #ifndef BSP_Q7S_COMIF_UARTCOMIF_H_ #define BSP_Q7S_COMIF_UARTCOMIF_H_ -#include "UartCookie.h" -#include "helper.h" - #include #include #include #include +#include "UartCookie.h" +#include "helper.h" /** * @brief This is the communication interface to access serial ports on linux based operating diff --git a/src/fsfw_hal/linux/uart/UartCookie.h b/src/fsfw_hal/linux/uart/UartCookie.h index 6fa2bd1b..670ef72f 100644 --- a/src/fsfw_hal/linux/uart/UartCookie.h +++ b/src/fsfw_hal/linux/uart/UartCookie.h @@ -1,14 +1,12 @@ #ifndef SAM9G20_COMIF_COOKIES_UART_COOKIE_H_ #define SAM9G20_COMIF_COOKIES_UART_COOKIE_H_ -#include "helper.h" - #include #include #include - +#include "helper.h" /** * @brief Cookie for the UartComIF. There are many options available to configure the UART driver. diff --git a/src/fsfw_hal/linux/uart/helper.cpp b/src/fsfw_hal/linux/uart/helper.cpp index b451f457..df4b7aa8 100644 --- a/src/fsfw_hal/linux/uart/helper.cpp +++ b/src/fsfw_hal/linux/uart/helper.cpp @@ -1,8 +1,9 @@ #include "helper.h" -#include "fsfw/serviceinterface.h" #include +#include "fsfw/serviceinterface.h" + void uart::setMode(struct termios& options, UartModes mode) { if (mode == UartModes::NON_CANONICAL) { /* Disable canonical mode */ @@ -145,6 +146,5 @@ void uart::setBaudrate(struct termios& options, UartBaudRate baud) { } int uart::readCountersAndErrors(int serialPort, serial_icounter_struct& icounter) { - return ioctl(serialPort, TIOCGICOUNT, &icounter); + return ioctl(serialPort, TIOCGICOUNT, &icounter); } - diff --git a/src/fsfw_hal/linux/uart/helper.h b/src/fsfw_hal/linux/uart/helper.h index b6a524d6..515f815b 100644 --- a/src/fsfw_hal/linux/uart/helper.h +++ b/src/fsfw_hal/linux/uart/helper.h @@ -1,8 +1,8 @@ #ifndef FSFW_HAL_LINUX_UART_HELPER_H_ #define FSFW_HAL_LINUX_UART_HELPER_H_ -#include #include +#include enum class Parity { NONE, EVEN, ODD }; @@ -56,7 +56,6 @@ void setBaudrate(struct termios& options, UartBaudRate baud); int readCountersAndErrors(int serialPort, serial_icounter_struct& icounter); -} - +} // namespace uart #endif /* FSFW_HAL_LINUX_UART_HELPER_H_ */ From e893e73f867efe7471371fa79ff1cf01e561aa31 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 17 Oct 2022 12:19:31 +0200 Subject: [PATCH 220/404] add first CFDP events --- src/fsfw/cfdp/handler/CMakeLists.txt | 5 +++-- src/fsfw/cfdp/handler/CfdpHandler.cpp | 2 +- src/fsfw/cfdp/handler/CfdpHandler.h | 6 +++++- src/fsfw/cfdp/handler/DestHandler.cpp | 13 ++++++++++++- src/fsfw/cfdp/handler/defs.h | 12 +++++++++++- src/fsfw/events/fwSubsystemIdRanges.h | 1 + unittests/mocks/cfdp/FaultHandlerMock.h | 2 +- 7 files changed, 34 insertions(+), 7 deletions(-) diff --git a/src/fsfw/cfdp/handler/CMakeLists.txt b/src/fsfw/cfdp/handler/CMakeLists.txt index 2e7dceef..d96ce91c 100644 --- a/src/fsfw/cfdp/handler/CMakeLists.txt +++ b/src/fsfw/cfdp/handler/CMakeLists.txt @@ -1,2 +1,3 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE SourceHandler.cpp DestHandler.cpp - FaultHandlerBase.cpp UserBase.cpp CfdpHandler.cpp) +target_sources( + ${LIB_FSFW_NAME} PRIVATE SourceHandler.cpp DestHandler.cpp + FaultHandlerBase.cpp UserBase.cpp CfdpHandler.cpp) diff --git a/src/fsfw/cfdp/handler/CfdpHandler.cpp b/src/fsfw/cfdp/handler/CfdpHandler.cpp index 9d20cc5e..902097b6 100644 --- a/src/fsfw/cfdp/handler/CfdpHandler.cpp +++ b/src/fsfw/cfdp/handler/CfdpHandler.cpp @@ -17,7 +17,7 @@ CfdpHandler::CfdpHandler(const FsfwHandlerParams& fsfwParams, const CfdpHandlerC cfdpCfg.userHandler, cfdpCfg.remoteCfgProvider, cfdpCfg.packetInfoList, cfdpCfg.lostSegmentsList), FsfwParams(fsfwParams.packetDest, nullptr, this, fsfwParams.tcStore, - fsfwParams.tmStore)) { + fsfwParams.tmStore)) { destHandler.setMsgQueue(msgQueue); } diff --git a/src/fsfw/cfdp/handler/CfdpHandler.h b/src/fsfw/cfdp/handler/CfdpHandler.h index 147ffc70..2de9f7dd 100644 --- a/src/fsfw/cfdp/handler/CfdpHandler.h +++ b/src/fsfw/cfdp/handler/CfdpHandler.h @@ -12,7 +12,11 @@ struct FsfwHandlerParams { FsfwHandlerParams(object_id_t objectId, HasFileSystemIF& vfs, AcceptsTelemetryIF& packetDest, StorageManagerIF& tcStore, StorageManagerIF& tmStore, MessageQueueIF& msgQueue) - : objectId(objectId), vfs(vfs), packetDest(packetDest), tcStore(tcStore), tmStore(tmStore), + : objectId(objectId), + vfs(vfs), + packetDest(packetDest), + tcStore(tcStore), + tmStore(tmStore), msgQueue(msgQueue) {} object_id_t objectId{}; HasFileSystemIF& vfs; diff --git a/src/fsfw/cfdp/handler/DestHandler.cpp b/src/fsfw/cfdp/handler/DestHandler.cpp index 8049cc0a..13b93a36 100644 --- a/src/fsfw/cfdp/handler/DestHandler.cpp +++ b/src/fsfw/cfdp/handler/DestHandler.cpp @@ -427,22 +427,33 @@ ReturnValue_t cfdp::DestHandler::sendFinishedPdu() { ReturnValue_t result = fp.tcStore->getFreeElement(&storeId, finishedPdu.getSerializedSize(), &dataPtr); if (result != OK) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "cfdp::DestHandler:sendFinishedPdu: Getting store slot failed" << std::endl; +#endif // TODO: Error handling and event, this is a non CFDP specific error (most likely store is full) return result; } size_t serLen = 0; result = finishedPdu.serialize(dataPtr, serLen, finishedPdu.getSerializedSize()); if (result != OK) { - // TODO: Error printout, this really should not happen +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "cfdp::DestHandler::sendFinishedPdu: Serializing Finished PDU failed" + << std::endl; +#endif + // TODO: Event, non CFDP specific error return result; } TmTcMessage msg(storeId); result = fp.msgQueue->sendMessage(fp.packetDest.getReportReceptionQueue(), &msg); if (result != OK) { // TODO: Error handling and event, this is a non CFDP specific error (most likely store is full) +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "cfdp::DestHandler::sendFinishedPdu: Sending PDU failed" << std::endl; +#endif return result; } fsmRes.packetsSent++; + sif::info << "Sent finished PDU" << std::endl; return OK; } diff --git a/src/fsfw/cfdp/handler/defs.h b/src/fsfw/cfdp/handler/defs.h index 9e837a96..190fb67d 100644 --- a/src/fsfw/cfdp/handler/defs.h +++ b/src/fsfw/cfdp/handler/defs.h @@ -5,5 +5,15 @@ namespace cfdp { enum class CfdpStates { IDLE, BUSY_CLASS_1_NACKED, BUSY_CLASS_2_ACKED, SUSPENDED }; -} +static constexpr uint8_t SSID = SUBSYSTEM_ID::CFDP; + +namespace events { + +static constexpr Event STORE_ERROR = event::makeEvent(SSID, 0, severity::LOW); +static constexpr Event MSG_QUEUE_ERROR = event::makeEvent(SSID, 1, severity::LOW); +static constexpr Event SERIALIZATION_ERROR = event::makeEvent(SSID, 2, severity::LOW); + +} // namespace events + +} // namespace cfdp #endif // FSFW_CFDP_HANDLER_DEFS_H diff --git a/src/fsfw/events/fwSubsystemIdRanges.h b/src/fsfw/events/fwSubsystemIdRanges.h index d8e4ade6..574ea070 100644 --- a/src/fsfw/events/fwSubsystemIdRanges.h +++ b/src/fsfw/events/fwSubsystemIdRanges.h @@ -33,6 +33,7 @@ enum : uint8_t { PUS_SERVICE_23 = 103, MGM_LIS3MDL = 106, MGM_RM3100 = 107, + CFDP = 108, FW_SUBSYSTEM_ID_RANGE }; diff --git a/unittests/mocks/cfdp/FaultHandlerMock.h b/unittests/mocks/cfdp/FaultHandlerMock.h index 1c59485c..5e094509 100644 --- a/unittests/mocks/cfdp/FaultHandlerMock.h +++ b/unittests/mocks/cfdp/FaultHandlerMock.h @@ -17,7 +17,7 @@ class FaultHandlerMock : public FaultHandlerBase { void noticeOfSuspensionCb(TransactionId& id, ConditionCode code) override; void noticeOfCancellationCb(TransactionId& id, ConditionCode code) override; - void abandonCb(TransactionId& id,ConditionCode code) override; + void abandonCb(TransactionId& id, ConditionCode code) override; void ignoreCb(TransactionId& id, ConditionCode code) override; FaultInfo& getFhInfo(FaultHandlerCode fhCode); From 79c38b45df4647a8748c97a491467472275a6a51 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 17 Oct 2022 12:20:26 +0200 Subject: [PATCH 221/404] events for FSFW specific errors --- src/fsfw/cfdp/handler/DestHandler.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/fsfw/cfdp/handler/DestHandler.cpp b/src/fsfw/cfdp/handler/DestHandler.cpp index 13b93a36..92b4341a 100644 --- a/src/fsfw/cfdp/handler/DestHandler.cpp +++ b/src/fsfw/cfdp/handler/DestHandler.cpp @@ -430,7 +430,7 @@ ReturnValue_t cfdp::DestHandler::sendFinishedPdu() { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "cfdp::DestHandler:sendFinishedPdu: Getting store slot failed" << std::endl; #endif - // TODO: Error handling and event, this is a non CFDP specific error (most likely store is full) + fp.eventReporter->forwardEvent(events::STORE_ERROR, result, 0); return result; } size_t serLen = 0; @@ -440,7 +440,7 @@ ReturnValue_t cfdp::DestHandler::sendFinishedPdu() { sif::warning << "cfdp::DestHandler::sendFinishedPdu: Serializing Finished PDU failed" << std::endl; #endif - // TODO: Event, non CFDP specific error + fp.eventReporter->forwardEvent(events::SERIALIZATION_ERROR, result, 0); return result; } TmTcMessage msg(storeId); From 9f81926aeccf65149db02bf58d67326e7ca10c63 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 17 Oct 2022 12:22:01 +0200 Subject: [PATCH 222/404] some more basic error handling --- src/fsfw/cfdp/handler/DestHandler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/cfdp/handler/DestHandler.cpp b/src/fsfw/cfdp/handler/DestHandler.cpp index 92b4341a..2e59179b 100644 --- a/src/fsfw/cfdp/handler/DestHandler.cpp +++ b/src/fsfw/cfdp/handler/DestHandler.cpp @@ -446,10 +446,10 @@ ReturnValue_t cfdp::DestHandler::sendFinishedPdu() { TmTcMessage msg(storeId); result = fp.msgQueue->sendMessage(fp.packetDest.getReportReceptionQueue(), &msg); if (result != OK) { - // TODO: Error handling and event, this is a non CFDP specific error (most likely store is full) #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "cfdp::DestHandler::sendFinishedPdu: Sending PDU failed" << std::endl; #endif + fp.eventReporter->forwardEvent(events::MSG_QUEUE_ERROR, result, 0); return result; } fsmRes.packetsSent++; From 14a8924a83dc15e29cd4bf8c6ac495c8729e4828 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 17 Oct 2022 14:01:21 +0200 Subject: [PATCH 223/404] size check bugfix --- src/fsfw/cfdp/pdu/FinishedPduCreator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/cfdp/pdu/FinishedPduCreator.cpp b/src/fsfw/cfdp/pdu/FinishedPduCreator.cpp index 8ac22e0a..d002e8aa 100644 --- a/src/fsfw/cfdp/pdu/FinishedPduCreator.cpp +++ b/src/fsfw/cfdp/pdu/FinishedPduCreator.cpp @@ -17,7 +17,7 @@ ReturnValue_t FinishPduCreator::serialize(uint8_t **buffer, size_t *size, size_t if (result != returnvalue::OK) { return result; } - if (*size + 1 >= maxSize) { + if (*size + 1 > maxSize) { return SerializeIF::BUFFER_TOO_SHORT; } **buffer = finishInfo.getConditionCode() << 4 | finishInfo.getDeliveryCode() << 2 | From 1aa062df7fc43135b8e45115eef45d45afca5a81 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 17 Oct 2022 16:27:41 +0200 Subject: [PATCH 224/404] const specifier for AcceptsTelemetryIF --- src/fsfw/cfdp/handler/DestHandler.cpp | 2 +- src/fsfw/tmtcservices/AcceptsTelemetryIF.h | 4 ++-- src/fsfw/tmtcservices/TmTcBridge.cpp | 2 +- src/fsfw/tmtcservices/TmTcBridge.h | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/fsfw/cfdp/handler/DestHandler.cpp b/src/fsfw/cfdp/handler/DestHandler.cpp index 2e59179b..86219fb7 100644 --- a/src/fsfw/cfdp/handler/DestHandler.cpp +++ b/src/fsfw/cfdp/handler/DestHandler.cpp @@ -425,7 +425,7 @@ ReturnValue_t cfdp::DestHandler::sendFinishedPdu() { store_address_t storeId; uint8_t* dataPtr = nullptr; ReturnValue_t result = - fp.tcStore->getFreeElement(&storeId, finishedPdu.getSerializedSize(), &dataPtr); + fp.tmStore->getFreeElement(&storeId, finishedPdu.getSerializedSize(), &dataPtr); if (result != OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "cfdp::DestHandler:sendFinishedPdu: Getting store slot failed" << std::endl; diff --git a/src/fsfw/tmtcservices/AcceptsTelemetryIF.h b/src/fsfw/tmtcservices/AcceptsTelemetryIF.h index c3e3eff3..0c5621c8 100644 --- a/src/fsfw/tmtcservices/AcceptsTelemetryIF.h +++ b/src/fsfw/tmtcservices/AcceptsTelemetryIF.h @@ -21,9 +21,9 @@ class AcceptsTelemetryIF { * receiving message queue. * @return The telemetry reception message queue id. */ - virtual MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) = 0; + [[nodiscard]] virtual MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const = 0; - virtual MessageQueueId_t getReportReceptionQueue() { return getReportReceptionQueue(0); } + [[nodiscard]] virtual MessageQueueId_t getReportReceptionQueue() const { return getReportReceptionQueue(0); } }; #endif /* FSFW_TMTCSERVICES_ACCEPTSTELEMETRYIF_H_ */ diff --git a/src/fsfw/tmtcservices/TmTcBridge.cpp b/src/fsfw/tmtcservices/TmTcBridge.cpp index 0e9f0da4..f22d70d6 100644 --- a/src/fsfw/tmtcservices/TmTcBridge.cpp +++ b/src/fsfw/tmtcservices/TmTcBridge.cpp @@ -240,7 +240,7 @@ void TmTcBridge::registerCommDisconnect() { } } -MessageQueueId_t TmTcBridge::getReportReceptionQueue(uint8_t virtualChannel) { +MessageQueueId_t TmTcBridge::getReportReceptionQueue(uint8_t virtualChannel) const { return tmTcReceptionQueue->getId(); } diff --git a/src/fsfw/tmtcservices/TmTcBridge.h b/src/fsfw/tmtcservices/TmTcBridge.h index 4b90d1d5..d74c612b 100644 --- a/src/fsfw/tmtcservices/TmTcBridge.h +++ b/src/fsfw/tmtcservices/TmTcBridge.h @@ -65,7 +65,7 @@ class TmTcBridge : public AcceptsTelemetryIF, ReturnValue_t performOperation(uint8_t operationCode = 0) override; /** AcceptsTelemetryIF override */ - MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel = 0) override; + MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel = 0) const override; /** AcceptsTelecommandsIF override */ uint32_t getIdentifier() const override; From 1e43296f2b127093c8ba5e18164fb361b6df7bb3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 17 Oct 2022 17:24:46 +0200 Subject: [PATCH 225/404] missing validity check --- src/fsfw/tmtcpacket/ccsds/SpacePacketCreator.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/fsfw/tmtcpacket/ccsds/SpacePacketCreator.cpp b/src/fsfw/tmtcpacket/ccsds/SpacePacketCreator.cpp index b8607079..745d5834 100644 --- a/src/fsfw/tmtcpacket/ccsds/SpacePacketCreator.cpp +++ b/src/fsfw/tmtcpacket/ccsds/SpacePacketCreator.cpp @@ -14,6 +14,7 @@ SpacePacketCreator::SpacePacketCreator(ccsds::PacketType packetType, bool secHea : params(SpacePacketParams(PacketId(packetType, secHeaderFlag, apid), PacketSeqCtrl(seqFlags, seqCount), dataLen)) { params.version = version; + checkFieldValidity(); } uint16_t SpacePacketCreator::getPacketIdRaw() const { return params.packetId.raw(); } From 009700ce801641921fd713c7fe668e27d8a7c653 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 17 Oct 2022 17:29:10 +0200 Subject: [PATCH 226/404] remove info printout --- src/fsfw/cfdp/handler/DestHandler.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/fsfw/cfdp/handler/DestHandler.cpp b/src/fsfw/cfdp/handler/DestHandler.cpp index 86219fb7..b4d4af09 100644 --- a/src/fsfw/cfdp/handler/DestHandler.cpp +++ b/src/fsfw/cfdp/handler/DestHandler.cpp @@ -453,7 +453,6 @@ ReturnValue_t cfdp::DestHandler::sendFinishedPdu() { return result; } fsmRes.packetsSent++; - sif::info << "Sent finished PDU" << std::endl; return OK; } From 0c5c2f6c4f07959a73a083eb3c6e1f3125642477 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 20 Oct 2022 14:24:03 +0200 Subject: [PATCH 227/404] important bugfix for i2c device com IF --- src/fsfw_hal/linux/i2c/I2cComIF.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/src/fsfw_hal/linux/i2c/I2cComIF.cpp index 7c365a20..d7bcaa4c 100644 --- a/src/fsfw_hal/linux/i2c/I2cComIF.cpp +++ b/src/fsfw_hal/linux/i2c/I2cComIF.cpp @@ -216,7 +216,7 @@ ReturnValue_t I2cComIF::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, } *buffer = i2cDeviceMapIter->second.replyBuffer.data(); *size = i2cDeviceMapIter->second.replyLen; - + i2cDeviceMapIter->second.replyLen = 0; return returnvalue::OK; } From 73454c629c042de8efe7aa4fa6dcbf1e184b0961 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 20 Oct 2022 16:05:45 +0200 Subject: [PATCH 228/404] oh god --- src/fsfw/devicehandlers/DeviceHandlerBase.h | 31 +++++++++++---------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index 0efecbd5..92788ca3 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -1140,6 +1140,22 @@ class DeviceHandlerBase : public DeviceHandlerIF, */ virtual ReturnValue_t doSendReadHook(); + /** + * Send a RMAP getRead command. + * + * The size of the getRead command is #maxDeviceReplyLen. + * This is always executed, independently from the current mode. + */ + virtual void doSendRead(void); + /** + * Check the getRead reply and the contained data. + * + * If data was received scanForReply() and, if successful, handleReply() + * are called. If the current mode is @c MODE_RAW, the received packet + * is sent to the commanding object via commandQueue. + */ + virtual void doGetRead(); + private: /** * State a cookie is in. @@ -1275,21 +1291,6 @@ class DeviceHandlerBase : public DeviceHandlerIF, * - if the action was successful, the reply timout counter is initialized */ void doGetWrite(void); - /** - * Send a RMAP getRead command. - * - * The size of the getRead command is #maxDeviceReplyLen. - * This is always executed, independently from the current mode. - */ - void doSendRead(void); - /** - * Check the getRead reply and the contained data. - * - * If data was received scanForReply() and, if successful, handleReply() - * are called. If the current mode is @c MODE_RAW, the received packet - * is sent to the commanding object via commandQueue. - */ - void doGetRead(void); /** * @brief Resets replies which use a timeout to detect missed replies. From 56e8e5a8b34dee6fcf240111618109e53b77841f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 24 Oct 2022 10:39:43 +0200 Subject: [PATCH 229/404] dont know if I am going to need this --- src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h | 15 +++++++++++++++ 1 file changed, 15 insertions(+) create mode 100644 src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h diff --git a/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h b/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h new file mode 100644 index 00000000..3590339d --- /dev/null +++ b/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h @@ -0,0 +1,15 @@ +#ifndef FSFW_SRC_FSFW_TMSTORAGE_TMSTOREBACKENDSIMPLEIF_H_ +#define FSFW_SRC_FSFW_TMSTORAGE_TMSTOREBACKENDSIMPLEIF_H_ + +#include + +class TmStoreFrontendSimpleIF { +public: + virtual ~TmStoreFrontendSimpleIF() = default; +private: + virtual MessageQueueId_t getCommandQueue() const = 0; +}; + + + +#endif /* FSFW_SRC_FSFW_TMSTORAGE_TMSTOREBACKENDSIMPLEIF_H_ */ From 096af44e39c4a94b17ee051fbdf907ddb3026a00 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 24 Oct 2022 10:56:01 +0200 Subject: [PATCH 230/404] needs some fixing --- src/fsfw/tmstorage/TmStoreFrontendIF.h | 72 ++++++++++----------- src/fsfw/tmstorage/TmStorePackets.h | 90 ++++++++++++++------------ 2 files changed, 84 insertions(+), 78 deletions(-) diff --git a/src/fsfw/tmstorage/TmStoreFrontendIF.h b/src/fsfw/tmstorage/TmStoreFrontendIF.h index 56bcd7fa..1e3dd579 100644 --- a/src/fsfw/tmstorage/TmStoreFrontendIF.h +++ b/src/fsfw/tmstorage/TmStoreFrontendIF.h @@ -6,47 +6,12 @@ #include "fsfw/returnvalues/returnvalue.h" #include "tmStorageConf.h" -class TmPacketMinimal; -class SpacePacketBase; +class PusTmReader; +class SpacePacketReader; class TmStoreBackendIF; class TmStoreFrontendIF { public: - virtual TmStoreBackendIF* getBackend() const = 0; - - /** - * What do I need to implement here? - * This is propably used by PUS Service 15 so we should propably check for messages.. - * Provide base implementation? - * @param opCode - * @return - */ - virtual ReturnValue_t performOperation(uint8_t opCode) = 0; - /** - * Callback from the back-end to indicate a certain packet was received. - * front-end takes care of discarding/downloading the packet. - * @param packet Pointer to the newly received Space Packet. - * @param address Start address of the packet found - * @param isLastPacket Indicates if no more packets can be fetched. - * @return If more packets shall be fetched, returnvalue::OK must be returned. - * Any other code stops fetching packets. - */ - virtual ReturnValue_t packetRetrieved(TmPacketMinimal* packet, uint32_t address) = 0; - virtual void noMorePacketsInStore() = 0; - virtual void handleRetrievalFailed(ReturnValue_t errorCode, uint32_t parameter1 = 0, - uint32_t parameter2 = 0) = 0; - /** - * To get the queue where commands shall be sent. - * @return Id of command queue. - */ - virtual MessageQueueId_t getCommandQueue() const = 0; - virtual ReturnValue_t fetchPackets(ApidSsc start, ApidSsc end) = 0; - virtual ReturnValue_t deletePackets(ApidSsc upTo) = 0; - virtual ReturnValue_t checkPacket(SpacePacketBase* tmPacket) = 0; - virtual bool isEnabled() const = 0; - virtual void setEnabled(bool enabled) = 0; - virtual void resetDownlinkedPacketCount() = 0; - virtual ReturnValue_t setDumpTarget(object_id_t dumpTarget) = 0; static const uint8_t INTERFACE_ID = CLASS_ID::TM_STORE_FRONTEND_IF; static const ReturnValue_t BUSY = MAKE_RETURN_CODE(1); static const ReturnValue_t LAST_PACKET_FOUND = MAKE_RETURN_CODE(2); @@ -57,7 +22,38 @@ class TmStoreFrontendIF { static const ReturnValue_t ALL_DELETED = MAKE_RETURN_CODE(7); static const ReturnValue_t INVALID_DATA = MAKE_RETURN_CODE(8); static const ReturnValue_t NOT_READY = MAKE_RETURN_CODE(9); - virtual ~TmStoreFrontendIF() {} + + virtual ~TmStoreFrontendIF() = default; + + /** + * To get the queue where commands shall be sent. + * @return Id of command queue. + */ + virtual MessageQueueId_t getCommandQueue() const = 0; + + virtual TmStoreBackendIF* getBackend() const = 0; + + /** + * Callback from the back-end to indicate a certain packet was received. + * front-end takes care of discarding/downloading the packet. + * @param packet Pointer to the newly received Space Packet. + * @param address Start address of the packet found + * @param isLastPacket Indicates if no more packets can be fetched. + * @return If more packets shall be fetched, returnvalue::OK must be returned. + * Any other code stops fetching packets. + */ + virtual ReturnValue_t packetRetrieved(PusTmReader* packet, uint32_t address) = 0; + virtual void noMorePacketsInStore() = 0; + virtual void handleRetrievalFailed(ReturnValue_t errorCode, uint32_t parameter1 = 0, + uint32_t parameter2 = 0) = 0; + + virtual ReturnValue_t fetchPackets(ApidSsc start, ApidSsc end) = 0; + virtual ReturnValue_t deletePackets(ApidSsc upTo) = 0; + virtual ReturnValue_t checkPacket(SpacePacketReader* tmPacket) = 0; + virtual bool isEnabled() const = 0; + virtual void setEnabled(bool enabled) = 0; + virtual void resetDownlinkedPacketCount() = 0; + virtual ReturnValue_t setDumpTarget(object_id_t dumpTarget) = 0; }; #endif /* FSFW_TMTCSERVICES_TMSTOREFRONTENDIF_H_ */ diff --git a/src/fsfw/tmstorage/TmStorePackets.h b/src/fsfw/tmstorage/TmStorePackets.h index e519b3b7..7bead984 100644 --- a/src/fsfw/tmstorage/TmStorePackets.h +++ b/src/fsfw/tmstorage/TmStorePackets.h @@ -1,6 +1,7 @@ #ifndef FSFW_TMSTORAGE_TMSTOREPACKETS_H_ #define FSFW_TMSTORAGE_TMSTOREPACKETS_H_ +#include #include "fsfw/globalfunctions/timevalOperations.h" #include "fsfw/serialize/SerialBufferAdapter.h" #include "fsfw/serialize/SerialFixedArrayListAdapter.h" @@ -11,6 +12,8 @@ #include "fsfw/tmtcpacket/pus/tm/PusTmMinimal.h" #include "tmStorageConf.h" +#include + class ServiceSubservice : public SerialLinkedListAdapter { public: SerializeElement service; @@ -24,7 +27,7 @@ class ServiceSubservice : public SerialLinkedListAdapter { class ApidSsc : public SerializeIF { public: - ApidSsc() : apid(SpacePacketBase::LIMIT_APID), ssc(0) {} + ApidSsc() : apid(ccsds::LIMIT_APID), ssc(0) {} ApidSsc(uint16_t apid, uint16_t ssc) : apid(apid), ssc(ssc) {} uint16_t apid; uint16_t ssc; @@ -62,51 +65,57 @@ class ChangeSelectionDefinition : public SerialLinkedListAdapter { class TmPacketInformation : public SerializeIF { public: - TmPacketInformation(TmPacketMinimal* packet) { setContent(packet); } - TmPacketInformation() - : apid(SpacePacketBase::LIMIT_APID), + TmPacketInformation(PusTmReader* packet, size_t timestampLen) + : rawTimestamp(timestampLen) { setContent(packet); } + TmPacketInformation(size_t timestampLen) + : apid(ccsds::LIMIT_APID), sourceSequenceCount(0), serviceType(0), serviceSubtype(0), - subCounter(0) {} + subCounter(0), + rawTimestamp(timestampLen) {} void reset() { - apid = SpacePacketBase::LIMIT_APID; + apid = ccsds::LIMIT_APID; sourceSequenceCount = 0; serviceType = 0; serviceSubtype = 0; subCounter = 0; - memset(rawTimestamp, 0, sizeof(rawTimestamp)); + memset(rawTimestamp.data(), 0, rawTimestamp.size()); } - void setContent(TmPacketMinimal* packet) { - apid = packet->getAPID(); - sourceSequenceCount = packet->getPacketSequenceCount(); + void setContent(PusTmReader* packet) { + apid = packet->getApid(); + sourceSequenceCount = packet->getSequenceCount(); serviceType = packet->getService(); serviceSubtype = packet->getSubService(); - subCounter = packet->getPacketSubcounter(); - memset(rawTimestamp, 0, sizeof(rawTimestamp)); - const uint8_t* pField = NULL; - uint32_t size = 0; - ReturnValue_t result = packet->getPacketTimeRaw(&pField, &size); - if (result != returnvalue::OK) { - return; - } - if (*pField == CCSDSTime::P_FIELD_CDS_SHORT && size <= TimeStamperIF::MISSION_TIMESTAMP_SIZE) { + subCounter = packet->getMessageTypeCounter(); + memset(rawTimestamp.data(), 0, rawTimestamp.size()); + // TODO: Fix all of this + //const uint8_t* pField = NULL; + //uint32_t size = 0; + //auto* timeReader = packet->getTimeReader(); + //ReturnValue_t result = packet->getPacketTimeRaw(&pField, &size); + //if (result != returnvalue::OK) { + // return; + //} + //if (*pField == CCSDSTime::P_FIELD_CDS_SHORT && size <= TimeStamperIF::MISSION_TIMESTAMP_SIZE) { // Shortcut to avoid converting CDS back and forth. - memcpy(rawTimestamp, pField, size); - return; - } - timeval time = {0, 0}; - result = packet->getPacketTime(&time); - if (result != returnvalue::OK) { - return; - } - - CCSDSTime::CDS_short cdsFormat; - result = CCSDSTime::convertToCcsds(&cdsFormat, &time); - if (result != returnvalue::OK) { - return; - } - memcpy(rawTimestamp, &cdsFormat, sizeof(cdsFormat)); + // TODO: Fix + //memcpy(rawTimestamp, pField, size); +// return; +// } +// timeval time = {0, 0}; +// result = packet->getPacketTime(&time); +// if (result != returnvalue::OK) { +// return; +// } +// +// CCSDSTime::CDS_short cdsFormat; +// result = CCSDSTime::convertToCcsds(&cdsFormat, &time); +// if (result != returnvalue::OK) { +// return; +// } + // TODO: Fix + // memcpy(rawTimestamp, &cdsFormat, sizeof(cdsFormat)); } void setContent(TmPacketInformation* content) { apid = content->apid; @@ -114,9 +123,10 @@ class TmPacketInformation : public SerializeIF { serviceType = content->serviceType; serviceSubtype = content->serviceSubtype; subCounter = content->subCounter; - memcpy(rawTimestamp, content->rawTimestamp, sizeof(rawTimestamp)); + // TODO: Fix + // memcpy(rawTimestamp, content->rawTimestamp, sizeof(rawTimestamp)); } - bool isValid() const { return (apid < SpacePacketBase::LIMIT_APID) ? true : false; } + bool isValid() const { return (apid < ccsds::LIMIT_APID) ? true : false; } static void reset(TmPacketInformation* packet) { packet->reset(); } static bool isOlderThan(const TmPacketInformation* packet, const timeval* cmpTime) { @@ -216,7 +226,7 @@ class TmPacketInformation : public SerializeIF { if (result != returnvalue::OK) { return result; } - SerialBufferAdapter adapter(rawTimestamp, sizeof(rawTimestamp)); + SerialBufferAdapter adapter(rawTimestamp.data(), rawTimestamp.size()); return adapter.serialize(buffer, size, maxSize, streamEndianness); } @@ -227,7 +237,7 @@ class TmPacketInformation : public SerializeIF { size += SerializeAdapter::getSerializedSize(&serviceType); size += SerializeAdapter::getSerializedSize(&serviceSubtype); size += SerializeAdapter::getSerializedSize(&subCounter); - SerialBufferAdapter adapter(rawTimestamp, sizeof(rawTimestamp)); + SerialBufferAdapter adapter(rawTimestamp.data(), rawTimestamp.size()); size += adapter.getSerializedSize(); return size; }; @@ -253,7 +263,7 @@ class TmPacketInformation : public SerializeIF { if (result != returnvalue::OK) { return result; } - SerialBufferAdapter adapter(rawTimestamp, sizeof(rawTimestamp)); + SerialBufferAdapter adapter(rawTimestamp.data(), rawTimestamp.size()); return adapter.deSerialize(buffer, size, streamEndianness); } @@ -263,6 +273,6 @@ class TmPacketInformation : public SerializeIF { uint8_t serviceType; uint8_t serviceSubtype; uint8_t subCounter; - uint8_t rawTimestamp[TimeStamperIF::MISSION_TIMESTAMP_SIZE]; + std::vector rawTimestamp; }; #endif /* FSFW_TMSTORAGE_TMSTOREPACKETS_H_ */ From b0c5a49b504708ec9130228100d7bbd49025598d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 24 Oct 2022 14:23:43 +0200 Subject: [PATCH 231/404] iter not a member anymore, more bugfixes --- src/fsfw_hal/linux/i2c/I2cComIF.cpp | 17 +++++++++-------- src/fsfw_hal/linux/i2c/I2cComIF.h | 2 -- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/src/fsfw_hal/linux/i2c/I2cComIF.cpp index d7bcaa4c..1ad19e82 100644 --- a/src/fsfw_hal/linux/i2c/I2cComIF.cpp +++ b/src/fsfw_hal/linux/i2c/I2cComIF.cpp @@ -41,7 +41,7 @@ ReturnValue_t I2cComIF::initializeInterface(CookieIF* cookie) { i2cAddress = i2cCookie->getAddress(); - i2cDeviceMapIter = i2cDeviceMap.find(i2cAddress); + auto i2cDeviceMapIter = i2cDeviceMap.find(i2cAddress); if (i2cDeviceMapIter == i2cDeviceMap.end()) { size_t maxReplyLen = i2cCookie->getMaxReplyLen(); I2cInstance i2cInstance = {std::vector(maxReplyLen), 0}; @@ -89,7 +89,7 @@ ReturnValue_t I2cComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s } address_t i2cAddress = i2cCookie->getAddress(); - i2cDeviceMapIter = i2cDeviceMap.find(i2cAddress); + auto i2cDeviceMapIter = i2cDeviceMap.find(i2cAddress); if (i2cDeviceMapIter == i2cDeviceMap.end()) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "I2cComIF::sendMessage: i2cAddress of Cookie not " @@ -140,20 +140,19 @@ ReturnValue_t I2cComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLe #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "I2cComIF::requestReceiveMessage: Invalid I2C Cookie!" << std::endl; #endif - i2cDeviceMapIter->second.replyLen = 0; return NULLPOINTER; } address_t i2cAddress = i2cCookie->getAddress(); - i2cDeviceMapIter = i2cDeviceMap.find(i2cAddress); + auto i2cDeviceMapIter = i2cDeviceMap.find(i2cAddress); if (i2cDeviceMapIter == i2cDeviceMap.end()) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "I2cComIF::requestReceiveMessage: i2cAddress of Cookie not " << "registered in i2cDeviceMap" << std::endl; #endif - i2cDeviceMapIter->second.replyLen = 0; return returnvalue::FAILED; } + i2cDeviceMapIter->second.replyLen = 0; deviceFile = i2cCookie->getDeviceFile(); UnixFileGuard fileHelper(deviceFile, &fd, O_RDWR, "I2cComIF::requestReceiveMessage"); @@ -162,7 +161,6 @@ ReturnValue_t I2cComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLe } result = openDevice(deviceFile, i2cAddress, &fd); if (result != returnvalue::OK) { - i2cDeviceMapIter->second.replyLen = 0; return result; } @@ -183,7 +181,10 @@ ReturnValue_t I2cComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLe #else #endif #endif - i2cDeviceMapIter->second.replyLen = 0; +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::debug << "I2cComIF::requestReceiveMessage: Read " << readLen << " of " << requestLen + << " bytes" << std::endl; +#endif return returnvalue::FAILED; } @@ -206,7 +207,7 @@ ReturnValue_t I2cComIF::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, } address_t i2cAddress = i2cCookie->getAddress(); - i2cDeviceMapIter = i2cDeviceMap.find(i2cAddress); + auto i2cDeviceMapIter = i2cDeviceMap.find(i2cAddress); if (i2cDeviceMapIter == i2cDeviceMap.end()) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "I2cComIF::readReceivedMessage: i2cAddress of Cookie not " diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.h b/src/fsfw_hal/linux/i2c/I2cComIF.h index 0a15c3a4..8c44cee0 100644 --- a/src/fsfw_hal/linux/i2c/I2cComIF.h +++ b/src/fsfw_hal/linux/i2c/I2cComIF.h @@ -36,12 +36,10 @@ class I2cComIF : public DeviceCommunicationIF, public SystemObject { }; using I2cDeviceMap = std::unordered_map; - using I2cDeviceMapIter = I2cDeviceMap::iterator; /* In this map all i2c devices will be registered with their address and * the appropriate file descriptor will be stored */ I2cDeviceMap i2cDeviceMap; - I2cDeviceMapIter i2cDeviceMapIter; /** * @brief This function opens an I2C device and binds the opened file From 1f05e6b297af8a6d310394e959c4d0cf13632831 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 25 Oct 2022 11:30:44 +0200 Subject: [PATCH 232/404] fs retval --- src/fsfw/filesystem/HasFileSystemIF.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/fsfw/filesystem/HasFileSystemIF.h b/src/fsfw/filesystem/HasFileSystemIF.h index 6f7112ad..24400b1c 100644 --- a/src/fsfw/filesystem/HasFileSystemIF.h +++ b/src/fsfw/filesystem/HasFileSystemIF.h @@ -40,6 +40,7 @@ class HasFileSystemIF { //! [EXPORT] : P1: Can be file system specific error code static constexpr ReturnValue_t GENERIC_FILE_ERROR = MAKE_RETURN_CODE(0); static constexpr ReturnValue_t GENERIC_DIR_ERROR = MAKE_RETURN_CODE(1); + static constexpr ReturnValue_t FILESYSTEM_INACTIVE = MAKE_RETURN_CODE(2); static constexpr ReturnValue_t GENERIC_RENAME_ERROR = MAKE_RETURN_CODE(3); //! [EXPORT] : File system is currently busy From 819a2bfac49babfc6a78452bb83cd581bf902bc8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 25 Oct 2022 18:20:48 +0200 Subject: [PATCH 233/404] add prototype for new ToAscii CCSDSTime function --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 16 +++--- src/fsfw/globalfunctions/DleParser.cpp | 18 +++--- src/fsfw/timemanager/CCSDSTime.cpp | 5 ++ src/fsfw/timemanager/CCSDSTime.h | 2 +- src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h | 7 +-- src/fsfw/tmstorage/TmStorePackets.h | 57 ++++++++++--------- src/fsfw/tmtcservices/AcceptsTelemetryIF.h | 4 +- src/fsfw_hal/linux/uart/UartComIF.cpp | 3 +- src/fsfw_hal/linux/uart/UartComIF.h | 5 +- src/fsfw_hal/linux/uart/UartCookie.h | 4 +- src/fsfw_hal/linux/uart/helper.cpp | 6 +- src/fsfw_hal/linux/uart/helper.h | 5 +- 12 files changed, 66 insertions(+), 66 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index c641eb58..2e157efa 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1603,12 +1603,12 @@ void DeviceHandlerBase::disableCommandsAndReplies() { } ReturnValue_t DeviceHandlerBase::finishAction(bool success, DeviceCommandId_t action, - ReturnValue_t result) { - auto commandIter = deviceCommandMap.find(action); - if (commandIter == deviceCommandMap.end()) { - return MessageQueueIF::NO_QUEUE; - } - commandIter->second.isExecuting = false; - actionHelper.finish(success, commandIter->second.sendReplyTo, action, result); - return returnvalue::OK; + ReturnValue_t result) { + auto commandIter = deviceCommandMap.find(action); + if (commandIter == deviceCommandMap.end()) { + return MessageQueueIF::NO_QUEUE; + } + commandIter->second.isExecuting = false; + actionHelper.finish(success, commandIter->second.sendReplyTo, action, result); + return returnvalue::OK; } diff --git a/src/fsfw/globalfunctions/DleParser.cpp b/src/fsfw/globalfunctions/DleParser.cpp index 90adfa7a..cc695bab 100644 --- a/src/fsfw/globalfunctions/DleParser.cpp +++ b/src/fsfw/globalfunctions/DleParser.cpp @@ -11,8 +11,7 @@ DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPa : decodeRingBuf(decodeRingBuf), decoder(decoder), encodedBuf(encodedBuf), - decodedBuf(decodedBuf) { -} + decodedBuf(decodedBuf) {} ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) { if (data == nullptr or len == 0) { @@ -24,7 +23,7 @@ ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) { ReturnValue_t DleParser::parseRingBuf(size_t& readSize) { ctx.setType(DleParser::ContextType::NONE); size_t availableData = decodeRingBuf.getAvailableReadData(); - if(availableData == 0) { + if (availableData == 0) { return NO_PACKET_FOUND; } if (availableData > encodedBuf.second) { @@ -106,7 +105,7 @@ void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* arg } void DleParser::defaultErrorHandler() { - if(ctx.getType() != DleParser::ContextType::ERROR) { + if (ctx.getType() != DleParser::ContextType::ERROR) { errorPrinter("No error"); return; } @@ -126,7 +125,8 @@ void DleParser::defaultErrorHandler() { case (ErrorTypes::ENCODED_BUF_TOO_SMALL): case (ErrorTypes::DECODING_BUF_TOO_SMALL): { char opt[64]; - snprintf(opt, sizeof(opt), ": Too small for packet with length %zu", ctx.decodedPacket.second); + snprintf(opt, sizeof(opt), ": Too small for packet with length %zu", + ctx.decodedPacket.second); if (ctx.error.first == ErrorTypes::ENCODED_BUF_TOO_SMALL) { errorPrinter("Encoded buf too small", opt); } else { @@ -168,10 +168,6 @@ ReturnValue_t DleParser::confirmBytesRead(size_t bytesRead) { return decodeRingBuf.deleteData(bytesRead); } -const DleParser::Context& DleParser::getContext() { - return ctx; -} +const DleParser::Context& DleParser::getContext() { return ctx; } -void DleParser::reset() { - decodeRingBuf.clear(); -} +void DleParser::reset() { decodeRingBuf.clear(); } diff --git a/src/fsfw/timemanager/CCSDSTime.cpp b/src/fsfw/timemanager/CCSDSTime.cpp index cb0d5758..cf4dbe74 100644 --- a/src/fsfw/timemanager/CCSDSTime.cpp +++ b/src/fsfw/timemanager/CCSDSTime.cpp @@ -246,6 +246,11 @@ ReturnValue_t CCSDSTime::convertFromASCII(Clock::TimeOfDay_t* to, const uint8_t* return UNSUPPORTED_TIME_FORMAT; } +ReturnValue_t CCSDSTime::convertToASCII(int8_t* to, const Clock::TimeOfDay_t* from, + uint8_t length) { + return returnvalue::OK; +} + ReturnValue_t CCSDSTime::checkCcs(const uint8_t* time, uint8_t length) { const Ccs_mseconds* time_struct = reinterpret_cast(time); diff --git a/src/fsfw/timemanager/CCSDSTime.h b/src/fsfw/timemanager/CCSDSTime.h index 77801cec..1ba94e2c 100644 --- a/src/fsfw/timemanager/CCSDSTime.h +++ b/src/fsfw/timemanager/CCSDSTime.h @@ -207,7 +207,7 @@ class CCSDSTime { static ReturnValue_t convertFromASCII(Clock::TimeOfDay_t *to, uint8_t const *from, uint8_t length); - + static ReturnValue_t convertToASCII(int8_t *to, const Clock::TimeOfDay_t *from, uint8_t length); static uint32_t subsecondsToMicroseconds(uint16_t subseconds); private: diff --git a/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h b/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h index 3590339d..1c2596da 100644 --- a/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h +++ b/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h @@ -4,12 +4,11 @@ #include class TmStoreFrontendSimpleIF { -public: + public: virtual ~TmStoreFrontendSimpleIF() = default; -private: + + private: virtual MessageQueueId_t getCommandQueue() const = 0; }; - - #endif /* FSFW_SRC_FSFW_TMSTORAGE_TMSTOREBACKENDSIMPLEIF_H_ */ diff --git a/src/fsfw/tmstorage/TmStorePackets.h b/src/fsfw/tmstorage/TmStorePackets.h index 7bead984..22ed3c21 100644 --- a/src/fsfw/tmstorage/TmStorePackets.h +++ b/src/fsfw/tmstorage/TmStorePackets.h @@ -2,6 +2,9 @@ #define FSFW_TMSTORAGE_TMSTOREPACKETS_H_ #include + +#include + #include "fsfw/globalfunctions/timevalOperations.h" #include "fsfw/serialize/SerialBufferAdapter.h" #include "fsfw/serialize/SerialFixedArrayListAdapter.h" @@ -12,8 +15,6 @@ #include "fsfw/tmtcpacket/pus/tm/PusTmMinimal.h" #include "tmStorageConf.h" -#include - class ServiceSubservice : public SerialLinkedListAdapter { public: SerializeElement service; @@ -65,8 +66,9 @@ class ChangeSelectionDefinition : public SerialLinkedListAdapter { class TmPacketInformation : public SerializeIF { public: - TmPacketInformation(PusTmReader* packet, size_t timestampLen) - : rawTimestamp(timestampLen) { setContent(packet); } + TmPacketInformation(PusTmReader* packet, size_t timestampLen) : rawTimestamp(timestampLen) { + setContent(packet); + } TmPacketInformation(size_t timestampLen) : apid(ccsds::LIMIT_APID), sourceSequenceCount(0), @@ -90,30 +92,31 @@ class TmPacketInformation : public SerializeIF { subCounter = packet->getMessageTypeCounter(); memset(rawTimestamp.data(), 0, rawTimestamp.size()); // TODO: Fix all of this - //const uint8_t* pField = NULL; - //uint32_t size = 0; - //auto* timeReader = packet->getTimeReader(); - //ReturnValue_t result = packet->getPacketTimeRaw(&pField, &size); - //if (result != returnvalue::OK) { - // return; + // const uint8_t* pField = NULL; + // uint32_t size = 0; + // auto* timeReader = packet->getTimeReader(); + // ReturnValue_t result = packet->getPacketTimeRaw(&pField, &size); + // if (result != returnvalue::OK) { + // return; //} - //if (*pField == CCSDSTime::P_FIELD_CDS_SHORT && size <= TimeStamperIF::MISSION_TIMESTAMP_SIZE) { - // Shortcut to avoid converting CDS back and forth. - // TODO: Fix - //memcpy(rawTimestamp, pField, size); -// return; -// } -// timeval time = {0, 0}; -// result = packet->getPacketTime(&time); -// if (result != returnvalue::OK) { -// return; -// } -// -// CCSDSTime::CDS_short cdsFormat; -// result = CCSDSTime::convertToCcsds(&cdsFormat, &time); -// if (result != returnvalue::OK) { -// return; -// } + // if (*pField == CCSDSTime::P_FIELD_CDS_SHORT && size <= TimeStamperIF::MISSION_TIMESTAMP_SIZE) + // { + // Shortcut to avoid converting CDS back and forth. + // TODO: Fix + // memcpy(rawTimestamp, pField, size); + // return; + // } + // timeval time = {0, 0}; + // result = packet->getPacketTime(&time); + // if (result != returnvalue::OK) { + // return; + // } + // + // CCSDSTime::CDS_short cdsFormat; + // result = CCSDSTime::convertToCcsds(&cdsFormat, &time); + // if (result != returnvalue::OK) { + // return; + // } // TODO: Fix // memcpy(rawTimestamp, &cdsFormat, sizeof(cdsFormat)); } diff --git a/src/fsfw/tmtcservices/AcceptsTelemetryIF.h b/src/fsfw/tmtcservices/AcceptsTelemetryIF.h index 0c5621c8..5b421cf9 100644 --- a/src/fsfw/tmtcservices/AcceptsTelemetryIF.h +++ b/src/fsfw/tmtcservices/AcceptsTelemetryIF.h @@ -23,7 +23,9 @@ class AcceptsTelemetryIF { */ [[nodiscard]] virtual MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const = 0; - [[nodiscard]] virtual MessageQueueId_t getReportReceptionQueue() const { return getReportReceptionQueue(0); } + [[nodiscard]] virtual MessageQueueId_t getReportReceptionQueue() const { + return getReportReceptionQueue(0); + } }; #endif /* FSFW_TMTCSERVICES_ACCEPTSTELEMETRYIF_H_ */ diff --git a/src/fsfw_hal/linux/uart/UartComIF.cpp b/src/fsfw_hal/linux/uart/UartComIF.cpp index df21da64..0880dec6 100644 --- a/src/fsfw_hal/linux/uart/UartComIF.cpp +++ b/src/fsfw_hal/linux/uart/UartComIF.cpp @@ -1,4 +1,5 @@ #include "UartComIF.h" + #include #include #include @@ -458,5 +459,3 @@ ReturnValue_t UartComIF::flushUartTxAndRxBuf(CookieIF* cookie) { } return returnvalue::FAILED; } - - diff --git a/src/fsfw_hal/linux/uart/UartComIF.h b/src/fsfw_hal/linux/uart/UartComIF.h index 940938d9..657e0123 100644 --- a/src/fsfw_hal/linux/uart/UartComIF.h +++ b/src/fsfw_hal/linux/uart/UartComIF.h @@ -1,15 +1,14 @@ #ifndef BSP_Q7S_COMIF_UARTCOMIF_H_ #define BSP_Q7S_COMIF_UARTCOMIF_H_ -#include "UartCookie.h" -#include "helper.h" - #include #include #include #include +#include "UartCookie.h" +#include "helper.h" /** * @brief This is the communication interface to access serial ports on linux based operating diff --git a/src/fsfw_hal/linux/uart/UartCookie.h b/src/fsfw_hal/linux/uart/UartCookie.h index 6fa2bd1b..670ef72f 100644 --- a/src/fsfw_hal/linux/uart/UartCookie.h +++ b/src/fsfw_hal/linux/uart/UartCookie.h @@ -1,14 +1,12 @@ #ifndef SAM9G20_COMIF_COOKIES_UART_COOKIE_H_ #define SAM9G20_COMIF_COOKIES_UART_COOKIE_H_ -#include "helper.h" - #include #include #include - +#include "helper.h" /** * @brief Cookie for the UartComIF. There are many options available to configure the UART driver. diff --git a/src/fsfw_hal/linux/uart/helper.cpp b/src/fsfw_hal/linux/uart/helper.cpp index b451f457..df4b7aa8 100644 --- a/src/fsfw_hal/linux/uart/helper.cpp +++ b/src/fsfw_hal/linux/uart/helper.cpp @@ -1,8 +1,9 @@ #include "helper.h" -#include "fsfw/serviceinterface.h" #include +#include "fsfw/serviceinterface.h" + void uart::setMode(struct termios& options, UartModes mode) { if (mode == UartModes::NON_CANONICAL) { /* Disable canonical mode */ @@ -145,6 +146,5 @@ void uart::setBaudrate(struct termios& options, UartBaudRate baud) { } int uart::readCountersAndErrors(int serialPort, serial_icounter_struct& icounter) { - return ioctl(serialPort, TIOCGICOUNT, &icounter); + return ioctl(serialPort, TIOCGICOUNT, &icounter); } - diff --git a/src/fsfw_hal/linux/uart/helper.h b/src/fsfw_hal/linux/uart/helper.h index b6a524d6..515f815b 100644 --- a/src/fsfw_hal/linux/uart/helper.h +++ b/src/fsfw_hal/linux/uart/helper.h @@ -1,8 +1,8 @@ #ifndef FSFW_HAL_LINUX_UART_HELPER_H_ #define FSFW_HAL_LINUX_UART_HELPER_H_ -#include #include +#include enum class Parity { NONE, EVEN, ODD }; @@ -56,7 +56,6 @@ void setBaudrate(struct termios& options, UartBaudRate baud); int readCountersAndErrors(int serialPort, serial_icounter_struct& icounter); -} - +} // namespace uart #endif /* FSFW_HAL_LINUX_UART_HELPER_H_ */ From bddc7a7ca69b679c132a6640a2ad2fdd5a47ff07 Mon Sep 17 00:00:00 2001 From: Ulrich Mohr Date: Wed, 26 Oct 2022 13:22:16 +0200 Subject: [PATCH 234/404] minimal first version --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 33 ++++--- src/fsfw/devicehandlers/DeviceHandlerBase.h | 93 +++++++++++++++++-- src/fsfw/devicehandlers/DeviceHandlerIF.h | 43 --------- .../devicehandlers/MgmLIS3MDLHandler.cpp | 4 +- .../devicehandlers/MgmRM3100Handler.cpp | 6 +- .../integration/devices/TestDeviceHandler.cpp | 8 +- unittests/mocks/DeviceHandlerMock.cpp | 2 +- 7 files changed, 115 insertions(+), 74 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 8ab540a0..ae986ca3 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -22,8 +22,6 @@ DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId, object_id_t device CookieIF* comCookie, FailureIsolationBase* fdirInstance, size_t cmdQueueSize) : SystemObject(setObjectId), - mode(MODE_OFF), - submode(SUBMODE_NONE), wiretappingMode(OFF), storedRawData(StorageManagerIF::INVALID_ADDRESS), deviceCommunicationId(deviceCommunication), @@ -38,6 +36,8 @@ DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId, object_id_t device defaultFDIRUsed(fdirInstance == nullptr), switchOffWasReported(false), childTransitionDelay(5000), + mode(MODE_OFF), + submode(SUBMODE_NONE), transitionSourceMode(_MODE_POWER_DOWN), transitionSourceSubMode(SUBMODE_NONE) { commandQueue = QueueFactory::instance()->createMessageQueue( @@ -352,7 +352,6 @@ void DeviceHandlerBase::doStateMachine() { currentUptime - timeoutStart >= powerSwitcher->getSwitchDelayMs()) { triggerEvent(MODE_TRANSITION_FAILED, PowerSwitchIF::SWITCH_TIMEOUT, 0); setMode(_MODE_POWER_DOWN); - callChildStatemachine(); break; } ReturnValue_t switchState = getStateOfSwitches(); @@ -567,11 +566,23 @@ void DeviceHandlerBase::setTransition(Mode_t modeTo, Submode_t submodeTo) { void DeviceHandlerBase::setMode(Mode_t newMode, uint8_t newSubmode) { /* TODO: This will probably be done by the LocalDataPoolManager now */ // changeHK(mode, submode, false); + + /** + * handle transition from OFF to NORMAL by continuing towards normal when ON is reached + */ + if (newMode == MODE_ON and continueToNormal) { + continueToNormal = false; + mode = _MODE_TO_NORMAL; + return; + } + submode = newSubmode; mode = newMode; modeChanged(); setNormalDatapoolEntriesInvalid(); if (!isTransitionalMode()) { + //clear this flag when a non-transitional Mode is reached to be safe + continueToNormal = false; modeHelper.modeChanged(newMode, newSubmode); announceMode(false); } @@ -1056,8 +1067,7 @@ Mode_t DeviceHandlerBase::getBaseMode(Mode_t transitionMode) { return transitionMode & ~(TRANSITION_MODE_BASE_ACTION_MASK | TRANSITION_MODE_CHILD_ACTION_MASK); } -// SHOULDDO: Allow transition from OFF to NORMAL to reduce complexity in assemblies. And, by the -// way, throw away DHB and write a new one: +// SHOULDDO: throw away DHB and write a new one: // - Include power and thermal completely, but more modular :-) // - Don't use modes for state transitions, reduce FSM (Finte State Machine) complexity. // - Modularization? @@ -1069,11 +1079,10 @@ ReturnValue_t DeviceHandlerBase::checkModeCommand(Mode_t commandedMode, Submode_ if ((mode == MODE_ERROR_ON) && (commandedMode != MODE_OFF)) { return TRANS_NOT_ALLOWED; } - if ((commandedMode == MODE_NORMAL) && (mode == MODE_OFF)) { - return TRANS_NOT_ALLOWED; - } - if ((commandedMode == MODE_ON) && (mode == MODE_OFF) and (thermalSet != nullptr)) { + // Do not check thermal state for MODE_RAW + if ((mode == MODE_OFF) and ((commandedMode == MODE_ON) or (commandedMode == MODE_NORMAL)) and + (thermalSet != nullptr)) { ReturnValue_t result = thermalSet->read(); if (result == returnvalue::OK) { if ((thermalSet->heaterRequest.value != ThermalComponentIF::STATE_REQUEST_IGNORE) and @@ -1088,6 +1097,7 @@ ReturnValue_t DeviceHandlerBase::checkModeCommand(Mode_t commandedMode, Submode_ } void DeviceHandlerBase::startTransition(Mode_t commandedMode, Submode_t commandedSubmode) { + continueToNormal = false; switch (commandedMode) { case MODE_ON: handleTransitionToOnMode(commandedMode, commandedSubmode); @@ -1117,8 +1127,9 @@ void DeviceHandlerBase::startTransition(Mode_t commandedMode, Submode_t commande case MODE_NORMAL: if (mode != MODE_OFF) { setTransition(MODE_NORMAL, commandedSubmode); - } else { - replyReturnvalueToCommand(HasModesIF::TRANS_NOT_ALLOWED); + } else { // mode is off + continueToNormal = true; + handleTransitionToOnMode(MODE_NORMAL, commandedSubmode); } break; } diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index 700e960d..1fc4b1de 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -102,6 +102,54 @@ class DeviceHandlerBase : public DeviceHandlerIF, DeviceHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, CookieIF *comCookie, FailureIsolationBase *fdirInstance = nullptr, size_t cmdQueueSize = 20); + + /** + * extending the modes of DeviceHandler IF for internal state machine + */ + static constexpr uint8_t TRANSITION_MODE_CHILD_ACTION_MASK = 0x20; + static constexpr uint8_t TRANSITION_MODE_BASE_ACTION_MASK = 0x10; + //! This is a transitional state which can not be commanded. The device + //! handler performs all commands to get the device in a state ready to + //! perform commands. When this is completed, the mode changes to @c MODE_ON. + static const Mode_t _MODE_START_UP = TRANSITION_MODE_CHILD_ACTION_MASK | 5; + //! This is a transitional state which can not be commanded. + //! The device handler performs all actions and commands to get the device + //! shut down. When the device is off, the mode changes to @c MODE_OFF. + //! It is possible to set the mode to _MODE_SHUT_DOWN to use the to off + //! transition if available. + static const Mode_t _MODE_SHUT_DOWN = TRANSITION_MODE_CHILD_ACTION_MASK | 6; + //! It is possible to set the mode to _MODE_TO_ON to use the to on + //! transition if available. + static const Mode_t _MODE_TO_ON = TRANSITION_MODE_CHILD_ACTION_MASK | HasModesIF::MODE_ON; + //! It is possible to set the mode to _MODE_TO_RAW to use the to raw + //! transition if available. + static const Mode_t _MODE_TO_RAW = TRANSITION_MODE_CHILD_ACTION_MASK | MODE_RAW; + //! It is possible to set the mode to _MODE_TO_NORMAL to use the to normal + //! transition if available. + static const Mode_t _MODE_TO_NORMAL = TRANSITION_MODE_CHILD_ACTION_MASK | MODE_NORMAL; + //! This is a transitional state which can not be commanded. + //! The device is shut down and ready to be switched off. + //! After the command to set the switch off has been sent, + //! the mode changes to @c _MODE_WAIT_OFF + static const Mode_t _MODE_POWER_DOWN = TRANSITION_MODE_BASE_ACTION_MASK | 1; + //! This is a transitional state which can not be commanded. The device + //! will be switched on in this state. After the command to set the switch + //! on has been sent, the mode changes to @c _MODE_WAIT_ON. + static const Mode_t _MODE_POWER_ON = TRANSITION_MODE_BASE_ACTION_MASK | 2; + //! This is a transitional state which can not be commanded. The switch has + //! been commanded off and the handler waits for it to be off. + //! When the switch is off, the mode changes to @c MODE_OFF. + static const Mode_t _MODE_WAIT_OFF = TRANSITION_MODE_BASE_ACTION_MASK | 3; + //! This is a transitional state which can not be commanded. The switch + //! has been commanded on and the handler waits for it to be on. + //! When the switch is on, the mode changes to @c _MODE_TO_ON. + static const Mode_t _MODE_WAIT_ON = TRANSITION_MODE_BASE_ACTION_MASK | 4; + //! This is a transitional state which can not be commanded. The switch has + //! been commanded off and is off now. This state is only to do an RMAP + //! cycle once more where the doSendRead() function will set the mode to + //! MODE_OFF. The reason to do this is to get rid of stuck packets in the IO Board. + static const Mode_t _MODE_SWITCH_IS_OFF = TRANSITION_MODE_BASE_ACTION_MASK | 5; + void setHkDestination(object_id_t hkDestination); /** @@ -684,15 +732,18 @@ class DeviceHandlerBase : public DeviceHandlerIF, size_t rawPacketLen = 0; /** - * The mode the device handler is currently in. - * This should never be changed directly but only with setMode() - */ - Mode_t mode; + * Get the current mode + * + * set via setMode() + */ + Mode_t getMode(); + /** - * The submode the device handler is currently in. - * This should never be changed directly but only with setMode() - */ - Submode_t submode; + * Get the current Submode + * + * set via setMode() + */ + Submode_t getSubmode; /** This is the counter value from performOperation(). */ uint8_t pstStep = 0; @@ -873,8 +924,8 @@ class DeviceHandlerBase : public DeviceHandlerIF, * Do the transition to the main modes (MODE_ON, MODE_NORMAL and MODE_RAW). * * If the transition is complete, the mode should be set to the target mode, - * which can be deduced from the current mode which is - * [_MODE_TO_ON, _MODE_TO_NORMAL, _MODE_TO_RAW] + * which can be deduced from the current mode (which is + * [_MODE_TO_ON, _MODE_TO_NORMAL, _MODE_TO_RAW]) using getBaseMode() * * The intended target submode is already set. * The origin submode can be read in subModeFrom. @@ -1170,6 +1221,19 @@ class DeviceHandlerBase : public DeviceHandlerIF, */ uint32_t childTransitionDelay; + /** + * The mode the device handler is currently in. + * This should not be changed directly but only with setMode() + */ + Mode_t mode; + + + /** + * The submode the device handler is currently in. + * This should not be changed directly but only with setMode() + */ + Submode_t submode; + /** * @brief The mode the current transition originated from * @@ -1187,6 +1251,15 @@ class DeviceHandlerBase : public DeviceHandlerIF, */ Submode_t transitionSourceSubMode; + /** + * used to make the state machine continue from ON to NOMAL when + * a Device is commanded to NORMAL in OFF mode + * + * set in startTransition() + * evaluated in setMode() to continue to NORMAL when ON is reached + */ + bool continueToNormal; + /** * read the command queue */ diff --git a/src/fsfw/devicehandlers/DeviceHandlerIF.h b/src/fsfw/devicehandlers/DeviceHandlerIF.h index 1fc63d3b..672f2830 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerIF.h +++ b/src/fsfw/devicehandlers/DeviceHandlerIF.h @@ -24,8 +24,6 @@ class DeviceHandlerIF { static const DeviceCommandId_t RAW_COMMAND_ID = -1; static const DeviceCommandId_t NO_COMMAND_ID = -2; - static constexpr uint8_t TRANSITION_MODE_CHILD_ACTION_MASK = 0x20; - static constexpr uint8_t TRANSITION_MODE_BASE_ACTION_MASK = 0x10; using dh_heater_request_t = uint8_t; using dh_thermal_state_t = int8_t; @@ -54,47 +52,6 @@ class DeviceHandlerIF { //! device still is powered. In this mode, only a mode change to @c MODE_OFF //! can be commanded, which tries to switch off the device again. static const Mode_t MODE_ERROR_ON = 4; - //! This is a transitional state which can not be commanded. The device - //! handler performs all commands to get the device in a state ready to - //! perform commands. When this is completed, the mode changes to @c MODE_ON. - static const Mode_t _MODE_START_UP = TRANSITION_MODE_CHILD_ACTION_MASK | 5; - //! This is a transitional state which can not be commanded. - //! The device handler performs all actions and commands to get the device - //! shut down. When the device is off, the mode changes to @c MODE_OFF. - //! It is possible to set the mode to _MODE_SHUT_DOWN to use the to off - //! transition if available. - static const Mode_t _MODE_SHUT_DOWN = TRANSITION_MODE_CHILD_ACTION_MASK | 6; - //! It is possible to set the mode to _MODE_TO_ON to use the to on - //! transition if available. - static const Mode_t _MODE_TO_ON = TRANSITION_MODE_CHILD_ACTION_MASK | HasModesIF::MODE_ON; - //! It is possible to set the mode to _MODE_TO_RAW to use the to raw - //! transition if available. - static const Mode_t _MODE_TO_RAW = TRANSITION_MODE_CHILD_ACTION_MASK | MODE_RAW; - //! It is possible to set the mode to _MODE_TO_NORMAL to use the to normal - //! transition if available. - static const Mode_t _MODE_TO_NORMAL = TRANSITION_MODE_CHILD_ACTION_MASK | MODE_NORMAL; - //! This is a transitional state which can not be commanded. - //! The device is shut down and ready to be switched off. - //! After the command to set the switch off has been sent, - //! the mode changes to @c MODE_WAIT_OFF - static const Mode_t _MODE_POWER_DOWN = TRANSITION_MODE_BASE_ACTION_MASK | 1; - //! This is a transitional state which can not be commanded. The device - //! will be switched on in this state. After the command to set the switch - //! on has been sent, the mode changes to @c MODE_WAIT_ON. - static const Mode_t _MODE_POWER_ON = TRANSITION_MODE_BASE_ACTION_MASK | 2; - //! This is a transitional state which can not be commanded. The switch has - //! been commanded off and the handler waits for it to be off. - //! When the switch is off, the mode changes to @c MODE_OFF. - static const Mode_t _MODE_WAIT_OFF = TRANSITION_MODE_BASE_ACTION_MASK | 3; - //! This is a transitional state which can not be commanded. The switch - //! has been commanded on and the handler waits for it to be on. - //! When the switch is on, the mode changes to @c MODE_TO_ON. - static const Mode_t _MODE_WAIT_ON = TRANSITION_MODE_BASE_ACTION_MASK | 4; - //! This is a transitional state which can not be commanded. The switch has - //! been commanded off and is off now. This state is only to do an RMAP - //! cycle once more where the doSendRead() function will set the mode to - //! MODE_OFF. The reason to do this is to get rid of stuck packets in the IO Board. - static const Mode_t _MODE_SWITCH_IS_OFF = TRANSITION_MODE_BASE_ACTION_MASK | 5; static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::CDH; static const Event DEVICE_BUILDING_COMMAND_FAILED = MAKE_EVENT(0, severity::LOW); diff --git a/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp b/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp index 9cb16c35..1ff42890 100644 --- a/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp +++ b/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp @@ -195,7 +195,7 @@ ReturnValue_t MgmLIS3MDLHandler::scanForReply(const uint8_t *start, size_t len, #endif return DeviceHandlerIF::INVALID_DATA; } - if (mode == _MODE_START_UP) { + if (getMode() == _MODE_START_UP) { commandExecuted = true; } @@ -224,7 +224,7 @@ ReturnValue_t MgmLIS3MDLHandler::scanForReply(const uint8_t *start, size_t len, return DeviceHandlerIF::INVALID_DATA; } - if (mode == _MODE_START_UP) { + if (getMode() == _MODE_START_UP) { commandExecuted = true; } } diff --git a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp index a32153eb..707ca338 100644 --- a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp +++ b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp @@ -169,7 +169,7 @@ ReturnValue_t MgmRM3100Handler::interpretDeviceReply(DeviceCommandId_t id, const case (RM3100::CONFIGURE_CYCLE_COUNT): case (RM3100::CONFIGURE_TMRC): { // We can only check whether write was successful with read operation - if (mode == _MODE_START_UP) { + if (getMode() == _MODE_START_UP) { commandExecuted = true; } break; @@ -192,7 +192,7 @@ ReturnValue_t MgmRM3100Handler::interpretDeviceReply(DeviceCommandId_t id, const if (packet[1] == tmrcRegValue) { commandExecuted = true; // Reading TMRC was commanded. Trigger event to inform ground - if (mode != _MODE_START_UP) { + if (getMode() != _MODE_START_UP) { triggerEvent(tmrcSet, tmrcRegValue, 0); } } else { @@ -211,7 +211,7 @@ ReturnValue_t MgmRM3100Handler::interpretDeviceReply(DeviceCommandId_t id, const return DeviceHandlerIF::DEVICE_REPLY_INVALID; } // Reading TMRC was commanded. Trigger event to inform ground - if (mode != _MODE_START_UP) { + if (getMode() != _MODE_START_UP) { uint32_t eventParam1 = (cycleCountX << 16) | cycleCountY; triggerEvent(cycleCountersSet, eventParam1, cycleCountZ); } diff --git a/src/fsfw_tests/integration/devices/TestDeviceHandler.cpp b/src/fsfw_tests/integration/devices/TestDeviceHandler.cpp index fdf02a70..b6a6f465 100644 --- a/src/fsfw_tests/integration/devices/TestDeviceHandler.cpp +++ b/src/fsfw_tests/integration/devices/TestDeviceHandler.cpp @@ -65,7 +65,7 @@ ReturnValue_t TestDevice::buildNormalDeviceCommand(DeviceCommandId_t* id) { } ReturnValue_t TestDevice::buildTransitionDeviceCommand(DeviceCommandId_t* id) { - if (mode == _MODE_TO_ON) { + if (getMode() == _MODE_TO_ON) { if (fullInfoPrintout) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::info << "TestDevice" << deviceIdx @@ -80,7 +80,7 @@ ReturnValue_t TestDevice::buildTransitionDeviceCommand(DeviceCommandId_t* id) { #endif } } - if (mode == _MODE_TO_NORMAL) { + if (getMode() == _MODE_TO_NORMAL) { if (fullInfoPrintout) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::info << "TestDevice" << deviceIdx @@ -97,7 +97,7 @@ ReturnValue_t TestDevice::buildTransitionDeviceCommand(DeviceCommandId_t* id) { setMode(MODE_NORMAL); } - if (mode == _MODE_SHUT_DOWN) { + if (getMode() == _MODE_SHUT_DOWN) { if (fullInfoPrintout) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::info << "TestDevice" << deviceIdx @@ -118,7 +118,7 @@ ReturnValue_t TestDevice::buildTransitionDeviceCommand(DeviceCommandId_t* id) { } void TestDevice::doTransition(Mode_t modeFrom, Submode_t submodeFrom) { - if (mode == _MODE_TO_NORMAL) { + if (getMode() == _MODE_TO_NORMAL) { if (fullInfoPrintout) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::info << "TestDevice" << deviceIdx diff --git a/unittests/mocks/DeviceHandlerMock.cpp b/unittests/mocks/DeviceHandlerMock.cpp index ef0a23d8..3ad80048 100644 --- a/unittests/mocks/DeviceHandlerMock.cpp +++ b/unittests/mocks/DeviceHandlerMock.cpp @@ -5,7 +5,7 @@ DeviceHandlerMock::DeviceHandlerMock(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, FailureIsolationBase *fdirInstance) : DeviceHandlerBase(objectId, deviceCommunication, comCookie, fdirInstance) { - mode = MODE_ON; + setMode(MODE_ON); } DeviceHandlerMock::~DeviceHandlerMock() = default; From 60ff411721909bd6e4c34523a2248d8dca2507b1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 26 Oct 2022 17:06:24 +0200 Subject: [PATCH 235/404] improvements for HAL com IFs --- src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp | 8 +++---- src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h | 1 - src/fsfw_hal/linux/spi/SpiComIF.cpp | 24 ++++++--------------- src/fsfw_hal/linux/spi/SpiComIF.h | 6 +++--- src/fsfw_hal/linux/uart/UartComIF.cpp | 25 ++++++++-------------- src/fsfw_hal/linux/uart/UartComIF.h | 5 ++--- 6 files changed, 25 insertions(+), 44 deletions(-) diff --git a/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp b/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp index 9113a89a..701de8f0 100644 --- a/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp +++ b/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp @@ -214,7 +214,7 @@ ReturnValue_t LinuxLibgpioIF::configureRegularGpio(gpioId_t gpioId, struct gpiod } ReturnValue_t LinuxLibgpioIF::pullHigh(gpioId_t gpioId) { - gpioMapIter = gpioMap.find(gpioId); + auto gpioMapIter = gpioMap.find(gpioId); if (gpioMapIter == gpioMap.end()) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "LinuxLibgpioIF::pullHigh: Unknown GPIO ID " << gpioId << std::endl; @@ -244,7 +244,7 @@ ReturnValue_t LinuxLibgpioIF::pullHigh(gpioId_t gpioId) { } ReturnValue_t LinuxLibgpioIF::pullLow(gpioId_t gpioId) { - gpioMapIter = gpioMap.find(gpioId); + auto gpioMapIter = gpioMap.find(gpioId); if (gpioMapIter == gpioMap.end()) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "LinuxLibgpioIF::pullLow: Unknown GPIO ID " << gpioId << std::endl; @@ -295,7 +295,7 @@ ReturnValue_t LinuxLibgpioIF::driveGpio(gpioId_t gpioId, GpiodRegularBase& regul } ReturnValue_t LinuxLibgpioIF::readGpio(gpioId_t gpioId, gpio::Levels& gpioState) { - gpioMapIter = gpioMap.find(gpioId); + auto gpioMapIter = gpioMap.find(gpioId); if (gpioMapIter == gpioMap.end()) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "LinuxLibgpioIF::readGpio: Unknown GPIOD ID " << gpioId << std::endl; @@ -377,7 +377,7 @@ ReturnValue_t LinuxLibgpioIF::checkForConflictsById(gpioId_t gpioIdToCheck, gpio::GpioTypes expectedType, GpioMap& mapToAdd) { // Cross check with private map - gpioMapIter = gpioMap.find(gpioIdToCheck); + auto gpioMapIter = gpioMap.find(gpioIdToCheck); if (gpioMapIter != gpioMap.end()) { auto& gpioType = gpioMapIter->second->gpioType; bool eraseDuplicateDifferentType = false; diff --git a/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h b/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h index 337fa1a8..a625770c 100644 --- a/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +++ b/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h @@ -44,7 +44,6 @@ class LinuxLibgpioIF : public GpioIF, public SystemObject { // Holds the information and configuration of all used GPIOs GpioUnorderedMap gpioMap; - GpioUnorderedMapIter gpioMapIter; /** * @brief This functions drives line of a GPIO specified by the GPIO ID. diff --git a/src/fsfw_hal/linux/spi/SpiComIF.cpp b/src/fsfw_hal/linux/spi/SpiComIF.cpp index dc30e94b..11db7cfe 100644 --- a/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -15,18 +15,8 @@ #include "fsfw_hal/linux/spi/SpiCookie.h" #include "fsfw_hal/linux/utility.h" -SpiComIF::SpiComIF(object_id_t objectId, std::string devname, GpioIF* gpioComIF) +SpiComIF::SpiComIF(object_id_t objectId, std::string devname, GpioIF& gpioComIF) : SystemObject(objectId), gpioComIF(gpioComIF), dev(std::move(devname)) { - if (gpioComIF == nullptr) { -#if FSFW_VERBOSE_LEVEL >= 1 -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "SpiComIF::SpiComIF: GPIO communication interface invalid!" << std::endl; -#else - sif::printError("SpiComIF::SpiComIF: GPIO communication interface invalid!\n"); -#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ -#endif /* FSFW_VERBOSE_LEVEL >= 1 */ - } - csMutex = MutexFactory::instance()->createMutex(); } @@ -75,7 +65,7 @@ ReturnValue_t SpiComIF::initializeInterface(CookieIF* cookie) { /* Pull CS high in any case to be sure that device is inactive */ gpioId_t gpioId = spiCookie->getChipSelectPin(); if (gpioId != gpio::NO_GPIO) { - gpioComIF->pullHigh(gpioId); + gpioComIF.pullHigh(gpioId); } uint32_t spiSpeed = 0; @@ -215,7 +205,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const return result; } updateLinePolarity(fileDescriptor); - result = gpioComIF->pullLow(gpioId); + result = gpioComIF.pullLow(gpioId); if (result != returnvalue::OK) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -256,7 +246,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const } if (gpioId != gpio::NO_GPIO and not csLockManual) { - gpioComIF->pullHigh(gpioId); + gpioComIF.pullHigh(gpioId); result = csMutex->unlockMutex(); if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -317,7 +307,7 @@ ReturnValue_t SpiComIF::performHalfDuplexReception(SpiCookie* spiCookie) { #endif return result; } - gpioComIF->pullLow(gpioId); + gpioComIF.pullLow(gpioId); } if (read(fileDescriptor, rxBuf, readSize) != static_cast(readSize)) { @@ -332,7 +322,7 @@ ReturnValue_t SpiComIF::performHalfDuplexReception(SpiCookie* spiCookie) { } if (gpioId != gpio::NO_GPIO and not csLockManual) { - gpioComIF->pullHigh(gpioId); + gpioComIF.pullHigh(gpioId); result = csMutex->unlockMutex(); if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -397,7 +387,7 @@ ReturnValue_t SpiComIF::getReadBuffer(address_t spiAddress, uint8_t** buffer) { return returnvalue::OK; } -GpioIF* SpiComIF::getGpioInterface() { return gpioComIF; } +GpioIF& SpiComIF::getGpioInterface() { return gpioComIF; } void SpiComIF::setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed) { int retval = ioctl(spiFd, SPI_IOC_WR_MODE, reinterpret_cast(&mode)); diff --git a/src/fsfw_hal/linux/spi/SpiComIF.h b/src/fsfw_hal/linux/spi/SpiComIF.h index c3e39b11..7033ea37 100644 --- a/src/fsfw_hal/linux/spi/SpiComIF.h +++ b/src/fsfw_hal/linux/spi/SpiComIF.h @@ -31,7 +31,7 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { static constexpr ReturnValue_t HALF_DUPLEX_TRANSFER_FAILED = returnvalue::makeCode(spiRetvalId, 2); - SpiComIF(object_id_t objectId, std::string devname, GpioIF* gpioComIF); + SpiComIF(object_id_t objectId, std::string devname, GpioIF& gpioComIF); ReturnValue_t initializeInterface(CookieIF* cookie) override; ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override; @@ -57,7 +57,7 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { ReturnValue_t performRegularSendOperation(SpiCookie* spiCookie, const uint8_t* sendData, size_t sendLen); - GpioIF* getGpioInterface(); + GpioIF& getGpioInterface(); void setSpiSpeedAndMode(int spiFd, spi::SpiModes mode, uint32_t speed); void getSpiSpeedAndMode(int spiFd, spi::SpiModes& mode, uint32_t& speed) const; @@ -83,7 +83,7 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { std::vector replyBuffer; }; - GpioIF* gpioComIF = nullptr; + GpioIF& gpioComIF; std::string dev = ""; /** * Protects the chip select operations. Lock when GPIO is pulled low, unlock after it was diff --git a/src/fsfw_hal/linux/uart/UartComIF.cpp b/src/fsfw_hal/linux/uart/UartComIF.cpp index df21da64..9de3ec10 100644 --- a/src/fsfw_hal/linux/uart/UartComIF.cpp +++ b/src/fsfw_hal/linux/uart/UartComIF.cpp @@ -16,7 +16,6 @@ UartComIF::~UartComIF() {} ReturnValue_t UartComIF::initializeInterface(CookieIF* cookie) { std::string deviceFile; - UartDeviceMapIter uartDeviceMapIter; if (cookie == nullptr) { return NULLPOINTER; @@ -32,7 +31,7 @@ ReturnValue_t UartComIF::initializeInterface(CookieIF* cookie) { deviceFile = uartCookie->getDeviceFile(); - uartDeviceMapIter = uartDeviceMap.find(deviceFile); + auto uartDeviceMapIter = uartDeviceMap.find(deviceFile); if (uartDeviceMapIter == uartDeviceMap.end()) { int fileDescriptor = configureUartPort(uartCookie); if (fileDescriptor < 0) { @@ -193,7 +192,6 @@ void UartComIF::setFixedOptions(struct termios* options) { ReturnValue_t UartComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) { int fd = 0; std::string deviceFile; - UartDeviceMapIter uartDeviceMapIter; if (sendLen == 0) { return returnvalue::OK; @@ -215,7 +213,7 @@ ReturnValue_t UartComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, } deviceFile = uartCookie->getDeviceFile(); - uartDeviceMapIter = uartDeviceMap.find(deviceFile); + auto 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" @@ -241,7 +239,6 @@ ReturnValue_t UartComIF::getSendSuccess(CookieIF* cookie) { return returnvalue:: ReturnValue_t UartComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLen) { std::string deviceFile; - UartDeviceMapIter uartDeviceMapIter; UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { @@ -253,7 +250,7 @@ ReturnValue_t UartComIF::requestReceiveMessage(CookieIF* cookie, size_t requestL UartModes uartMode = uartCookie->getUartMode(); deviceFile = uartCookie->getDeviceFile(); - uartDeviceMapIter = uartDeviceMap.find(deviceFile); + auto uartDeviceMapIter = uartDeviceMap.find(deviceFile); if (uartMode == UartModes::NON_CANONICAL and requestLen == 0) { return returnvalue::OK; @@ -276,7 +273,7 @@ ReturnValue_t UartComIF::requestReceiveMessage(CookieIF* cookie, size_t requestL } } -ReturnValue_t UartComIF::handleCanonicalRead(UartCookie& uartCookie, UartDeviceMapIter& iter, +ReturnValue_t UartComIF::handleCanonicalRead(UartCookie& uartCookie, UartDeviceMap::iterator& iter, size_t requestLen) { ReturnValue_t result = returnvalue::OK; uint8_t maxReadCycles = uartCookie.getReadCycles(); @@ -334,7 +331,7 @@ ReturnValue_t UartComIF::handleCanonicalRead(UartCookie& uartCookie, UartDeviceM return result; } -ReturnValue_t UartComIF::handleNoncanonicalRead(UartCookie& uartCookie, UartDeviceMapIter& iter, +ReturnValue_t UartComIF::handleNoncanonicalRead(UartCookie& uartCookie, UartDeviceMap::iterator& iter, size_t requestLen) { int fd = iter->second.fileDescriptor; auto bufferPtr = iter->second.replyBuffer.data(); @@ -370,7 +367,6 @@ ReturnValue_t UartComIF::handleNoncanonicalRead(UartCookie& uartCookie, UartDevi ReturnValue_t UartComIF::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) { std::string deviceFile; - UartDeviceMapIter uartDeviceMapIter; UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { @@ -381,7 +377,7 @@ ReturnValue_t UartComIF::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, } deviceFile = uartCookie->getDeviceFile(); - uartDeviceMapIter = uartDeviceMap.find(deviceFile); + auto 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" @@ -401,7 +397,6 @@ ReturnValue_t UartComIF::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, ReturnValue_t UartComIF::flushUartRxBuffer(CookieIF* cookie) { std::string deviceFile; - UartDeviceMapIter uartDeviceMapIter; UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -410,7 +405,7 @@ ReturnValue_t UartComIF::flushUartRxBuffer(CookieIF* cookie) { return NULLPOINTER; } deviceFile = uartCookie->getDeviceFile(); - uartDeviceMapIter = uartDeviceMap.find(deviceFile); + auto uartDeviceMapIter = uartDeviceMap.find(deviceFile); if (uartDeviceMapIter != uartDeviceMap.end()) { int fd = uartDeviceMapIter->second.fileDescriptor; tcflush(fd, TCIFLUSH); @@ -421,7 +416,6 @@ ReturnValue_t UartComIF::flushUartRxBuffer(CookieIF* cookie) { ReturnValue_t UartComIF::flushUartTxBuffer(CookieIF* cookie) { std::string deviceFile; - UartDeviceMapIter uartDeviceMapIter; UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -430,7 +424,7 @@ ReturnValue_t UartComIF::flushUartTxBuffer(CookieIF* cookie) { return NULLPOINTER; } deviceFile = uartCookie->getDeviceFile(); - uartDeviceMapIter = uartDeviceMap.find(deviceFile); + auto uartDeviceMapIter = uartDeviceMap.find(deviceFile); if (uartDeviceMapIter != uartDeviceMap.end()) { int fd = uartDeviceMapIter->second.fileDescriptor; tcflush(fd, TCOFLUSH); @@ -441,7 +435,6 @@ ReturnValue_t UartComIF::flushUartTxBuffer(CookieIF* cookie) { ReturnValue_t UartComIF::flushUartTxAndRxBuf(CookieIF* cookie) { std::string deviceFile; - UartDeviceMapIter uartDeviceMapIter; UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -450,7 +443,7 @@ ReturnValue_t UartComIF::flushUartTxAndRxBuf(CookieIF* cookie) { return NULLPOINTER; } deviceFile = uartCookie->getDeviceFile(); - uartDeviceMapIter = uartDeviceMap.find(deviceFile); + auto uartDeviceMapIter = uartDeviceMap.find(deviceFile); if (uartDeviceMapIter != uartDeviceMap.end()) { int fd = uartDeviceMapIter->second.fileDescriptor; tcflush(fd, TCIOFLUSH); diff --git a/src/fsfw_hal/linux/uart/UartComIF.h b/src/fsfw_hal/linux/uart/UartComIF.h index 940938d9..ea264ddb 100644 --- a/src/fsfw_hal/linux/uart/UartComIF.h +++ b/src/fsfw_hal/linux/uart/UartComIF.h @@ -64,7 +64,6 @@ class UartComIF : public DeviceCommunicationIF, public SystemObject { }; using UartDeviceMap = std::unordered_map; - using UartDeviceMapIter = UartDeviceMap::iterator; /** * The uart devie map stores informations of initialized uart ports. @@ -103,9 +102,9 @@ class UartComIF : public DeviceCommunicationIF, public SystemObject { */ void setDatasizeOptions(struct termios* options, UartCookie* uartCookie); - ReturnValue_t handleCanonicalRead(UartCookie& uartCookie, UartDeviceMapIter& iter, + ReturnValue_t handleCanonicalRead(UartCookie& uartCookie, UartDeviceMap::iterator& iter, size_t requestLen); - ReturnValue_t handleNoncanonicalRead(UartCookie& uartCookie, UartDeviceMapIter& iter, + ReturnValue_t handleNoncanonicalRead(UartCookie& uartCookie, UartDeviceMap::iterator& iter, size_t requestLen); }; From 1b7e94d718ba8f526dd53324d33d0e577cbfc81b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 26 Oct 2022 18:26:48 +0200 Subject: [PATCH 236/404] this api works as well --- src/fsfw_hal/linux/uart/helper.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/fsfw_hal/linux/uart/helper.cpp b/src/fsfw_hal/linux/uart/helper.cpp index b451f457..530169f7 100644 --- a/src/fsfw_hal/linux/uart/helper.cpp +++ b/src/fsfw_hal/linux/uart/helper.cpp @@ -71,16 +71,14 @@ void uart::setBaudrate(struct termios& options, UartBaudRate baud) { cfsetospeed(&options, B19200); break; case UartBaudRate::RATE_38400: - cfsetispeed(&options, B38400); - cfsetospeed(&options, B38400); + cfsetspeed(&options, B38400); break; case UartBaudRate::RATE_57600: cfsetispeed(&options, B57600); cfsetospeed(&options, B57600); break; case UartBaudRate::RATE_115200: - cfsetispeed(&options, B115200); - cfsetospeed(&options, B115200); + cfsetspeed(&options, B115200); break; case UartBaudRate::RATE_230400: cfsetispeed(&options, B230400); From 226dc4d8b742fea832861bf70d37dc48beb15dc9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 27 Oct 2022 14:01:35 +0200 Subject: [PATCH 237/404] UIO mapper can handle symlinks now --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 16 ++++++++-------- src/fsfw/globalfunctions/DleParser.cpp | 18 +++++++----------- src/fsfw/tmtcservices/AcceptsTelemetryIF.h | 4 +++- src/fsfw_hal/linux/uart/UartComIF.cpp | 7 +++---- src/fsfw_hal/linux/uart/UartComIF.h | 5 ++--- src/fsfw_hal/linux/uart/UartCookie.h | 4 +--- src/fsfw_hal/linux/uart/helper.cpp | 6 +++--- src/fsfw_hal/linux/uart/helper.h | 5 ++--- src/fsfw_hal/linux/uio/UioMapper.cpp | 19 +++++++++++++++++-- 9 files changed, 46 insertions(+), 38 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index c641eb58..2e157efa 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1603,12 +1603,12 @@ void DeviceHandlerBase::disableCommandsAndReplies() { } ReturnValue_t DeviceHandlerBase::finishAction(bool success, DeviceCommandId_t action, - ReturnValue_t result) { - auto commandIter = deviceCommandMap.find(action); - if (commandIter == deviceCommandMap.end()) { - return MessageQueueIF::NO_QUEUE; - } - commandIter->second.isExecuting = false; - actionHelper.finish(success, commandIter->second.sendReplyTo, action, result); - return returnvalue::OK; + ReturnValue_t result) { + auto commandIter = deviceCommandMap.find(action); + if (commandIter == deviceCommandMap.end()) { + return MessageQueueIF::NO_QUEUE; + } + commandIter->second.isExecuting = false; + actionHelper.finish(success, commandIter->second.sendReplyTo, action, result); + return returnvalue::OK; } diff --git a/src/fsfw/globalfunctions/DleParser.cpp b/src/fsfw/globalfunctions/DleParser.cpp index 90adfa7a..cc695bab 100644 --- a/src/fsfw/globalfunctions/DleParser.cpp +++ b/src/fsfw/globalfunctions/DleParser.cpp @@ -11,8 +11,7 @@ DleParser::DleParser(SimpleRingBuffer& decodeRingBuf, DleEncoder& decoder, BufPa : decodeRingBuf(decodeRingBuf), decoder(decoder), encodedBuf(encodedBuf), - decodedBuf(decodedBuf) { -} + decodedBuf(decodedBuf) {} ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) { if (data == nullptr or len == 0) { @@ -24,7 +23,7 @@ ReturnValue_t DleParser::passData(const uint8_t* data, size_t len) { ReturnValue_t DleParser::parseRingBuf(size_t& readSize) { ctx.setType(DleParser::ContextType::NONE); size_t availableData = decodeRingBuf.getAvailableReadData(); - if(availableData == 0) { + if (availableData == 0) { return NO_PACKET_FOUND; } if (availableData > encodedBuf.second) { @@ -106,7 +105,7 @@ void DleParser::defaultFoundPacketHandler(uint8_t* packet, size_t len, void* arg } void DleParser::defaultErrorHandler() { - if(ctx.getType() != DleParser::ContextType::ERROR) { + if (ctx.getType() != DleParser::ContextType::ERROR) { errorPrinter("No error"); return; } @@ -126,7 +125,8 @@ void DleParser::defaultErrorHandler() { case (ErrorTypes::ENCODED_BUF_TOO_SMALL): case (ErrorTypes::DECODING_BUF_TOO_SMALL): { char opt[64]; - snprintf(opt, sizeof(opt), ": Too small for packet with length %zu", ctx.decodedPacket.second); + snprintf(opt, sizeof(opt), ": Too small for packet with length %zu", + ctx.decodedPacket.second); if (ctx.error.first == ErrorTypes::ENCODED_BUF_TOO_SMALL) { errorPrinter("Encoded buf too small", opt); } else { @@ -168,10 +168,6 @@ ReturnValue_t DleParser::confirmBytesRead(size_t bytesRead) { return decodeRingBuf.deleteData(bytesRead); } -const DleParser::Context& DleParser::getContext() { - return ctx; -} +const DleParser::Context& DleParser::getContext() { return ctx; } -void DleParser::reset() { - decodeRingBuf.clear(); -} +void DleParser::reset() { decodeRingBuf.clear(); } diff --git a/src/fsfw/tmtcservices/AcceptsTelemetryIF.h b/src/fsfw/tmtcservices/AcceptsTelemetryIF.h index 0c5621c8..5b421cf9 100644 --- a/src/fsfw/tmtcservices/AcceptsTelemetryIF.h +++ b/src/fsfw/tmtcservices/AcceptsTelemetryIF.h @@ -23,7 +23,9 @@ class AcceptsTelemetryIF { */ [[nodiscard]] virtual MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const = 0; - [[nodiscard]] virtual MessageQueueId_t getReportReceptionQueue() const { return getReportReceptionQueue(0); } + [[nodiscard]] virtual MessageQueueId_t getReportReceptionQueue() const { + return getReportReceptionQueue(0); + } }; #endif /* FSFW_TMTCSERVICES_ACCEPTSTELEMETRYIF_H_ */ diff --git a/src/fsfw_hal/linux/uart/UartComIF.cpp b/src/fsfw_hal/linux/uart/UartComIF.cpp index 9de3ec10..0b44e4a5 100644 --- a/src/fsfw_hal/linux/uart/UartComIF.cpp +++ b/src/fsfw_hal/linux/uart/UartComIF.cpp @@ -1,4 +1,5 @@ #include "UartComIF.h" + #include #include #include @@ -331,8 +332,8 @@ ReturnValue_t UartComIF::handleCanonicalRead(UartCookie& uartCookie, UartDeviceM return result; } -ReturnValue_t UartComIF::handleNoncanonicalRead(UartCookie& uartCookie, UartDeviceMap::iterator& iter, - size_t requestLen) { +ReturnValue_t UartComIF::handleNoncanonicalRead(UartCookie& uartCookie, + UartDeviceMap::iterator& iter, size_t requestLen) { int fd = iter->second.fileDescriptor; auto bufferPtr = iter->second.replyBuffer.data(); // Size check to prevent buffer overflow @@ -451,5 +452,3 @@ ReturnValue_t UartComIF::flushUartTxAndRxBuf(CookieIF* cookie) { } return returnvalue::FAILED; } - - diff --git a/src/fsfw_hal/linux/uart/UartComIF.h b/src/fsfw_hal/linux/uart/UartComIF.h index ea264ddb..4bdbd954 100644 --- a/src/fsfw_hal/linux/uart/UartComIF.h +++ b/src/fsfw_hal/linux/uart/UartComIF.h @@ -1,15 +1,14 @@ #ifndef BSP_Q7S_COMIF_UARTCOMIF_H_ #define BSP_Q7S_COMIF_UARTCOMIF_H_ -#include "UartCookie.h" -#include "helper.h" - #include #include #include #include +#include "UartCookie.h" +#include "helper.h" /** * @brief This is the communication interface to access serial ports on linux based operating diff --git a/src/fsfw_hal/linux/uart/UartCookie.h b/src/fsfw_hal/linux/uart/UartCookie.h index 6fa2bd1b..670ef72f 100644 --- a/src/fsfw_hal/linux/uart/UartCookie.h +++ b/src/fsfw_hal/linux/uart/UartCookie.h @@ -1,14 +1,12 @@ #ifndef SAM9G20_COMIF_COOKIES_UART_COOKIE_H_ #define SAM9G20_COMIF_COOKIES_UART_COOKIE_H_ -#include "helper.h" - #include #include #include - +#include "helper.h" /** * @brief Cookie for the UartComIF. There are many options available to configure the UART driver. diff --git a/src/fsfw_hal/linux/uart/helper.cpp b/src/fsfw_hal/linux/uart/helper.cpp index 530169f7..141f68e3 100644 --- a/src/fsfw_hal/linux/uart/helper.cpp +++ b/src/fsfw_hal/linux/uart/helper.cpp @@ -1,8 +1,9 @@ #include "helper.h" -#include "fsfw/serviceinterface.h" #include +#include "fsfw/serviceinterface.h" + void uart::setMode(struct termios& options, UartModes mode) { if (mode == UartModes::NON_CANONICAL) { /* Disable canonical mode */ @@ -143,6 +144,5 @@ void uart::setBaudrate(struct termios& options, UartBaudRate baud) { } int uart::readCountersAndErrors(int serialPort, serial_icounter_struct& icounter) { - return ioctl(serialPort, TIOCGICOUNT, &icounter); + return ioctl(serialPort, TIOCGICOUNT, &icounter); } - diff --git a/src/fsfw_hal/linux/uart/helper.h b/src/fsfw_hal/linux/uart/helper.h index b6a524d6..515f815b 100644 --- a/src/fsfw_hal/linux/uart/helper.h +++ b/src/fsfw_hal/linux/uart/helper.h @@ -1,8 +1,8 @@ #ifndef FSFW_HAL_LINUX_UART_HELPER_H_ #define FSFW_HAL_LINUX_UART_HELPER_H_ -#include #include +#include enum class Parity { NONE, EVEN, ODD }; @@ -56,7 +56,6 @@ void setBaudrate(struct termios& options, UartBaudRate baud); int readCountersAndErrors(int serialPort, serial_icounter_struct& icounter); -} - +} // namespace uart #endif /* FSFW_HAL_LINUX_UART_HELPER_H_ */ diff --git a/src/fsfw_hal/linux/uio/UioMapper.cpp b/src/fsfw_hal/linux/uio/UioMapper.cpp index 3d7e5987..74d0e41f 100644 --- a/src/fsfw_hal/linux/uio/UioMapper.cpp +++ b/src/fsfw_hal/linux/uio/UioMapper.cpp @@ -1,6 +1,7 @@ #include "UioMapper.h" #include +#include #include #include @@ -13,7 +14,21 @@ 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(std::string uioFile, int mapNum) : mapNum(mapNum) { + struct stat buf; + lstat(uioFile.c_str(), &buf); + if (S_ISLNK(buf.st_mode)) { + char* res = realpath(uioFile.c_str(), nullptr); + if (res) { + this->uioFile = res; + } else { + sif::error << "Could not resolve real path of UIO file " << uioFile << std::endl; + } + free(res); + } else { + this->uioFile = std::move(uioFile); + } +} UioMapper::~UioMapper() {} @@ -22,7 +37,7 @@ ReturnValue_t UioMapper::getMappedAdress(uint32_t** address, Permissions permiss int fd = open(uioFile.c_str(), O_RDWR); if (fd < 1) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PtmeAxiConfig::initialize: Invalid UIO device file" << std::endl; + sif::error << "UioMapper::getMappedAdress: Invalid UIO device file " << uioFile << std::endl; #endif return returnvalue::FAILED; } From f80566777923ae1e8f619ed2056c7637666be26e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Oct 2022 10:16:59 +0200 Subject: [PATCH 238/404] afmt --- CMakeLists.txt | 12 +++++++++--- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 2 +- src/fsfw/devicehandlers/DeviceHandlerBase.h | 16 +++++++--------- src/fsfw/devicehandlers/DeviceHandlerIF.h | 1 - .../serviceinterface/ServiceInterfaceBuffer.cpp | 1 - src/fsfw_hal/common/CMakeLists.txt | 2 +- src/fsfw_hal/common/printChar.c | 2 +- 7 files changed, 19 insertions(+), 17 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index e2572b20..cccd51a4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -345,9 +345,15 @@ if(FSFW_BUILD_TESTS) DEPENDENCIES ${FSFW_TEST_TGT}) else() setup_target_for_coverage_lcov( - NAME ${FSFW_TEST_TGT}_coverage EXECUTABLE ${FSFW_TEST_TGT} - DEPENDENCIES ${FSFW_TEST_TGT} - GENHTML_ARGS --html-epilog ${CMAKE_SOURCE_DIR}/unittests/lcov_epilog.html) + NAME + ${FSFW_TEST_TGT}_coverage + EXECUTABLE + ${FSFW_TEST_TGT} + DEPENDENCIES + ${FSFW_TEST_TGT} + GENHTML_ARGS + --html-epilog + ${CMAKE_SOURCE_DIR}/unittests/lcov_epilog.html) endif() endif() endif() diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index ae986ca3..8bcba7c4 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -581,7 +581,7 @@ void DeviceHandlerBase::setMode(Mode_t newMode, uint8_t newSubmode) { modeChanged(); setNormalDatapoolEntriesInvalid(); if (!isTransitionalMode()) { - //clear this flag when a non-transitional Mode is reached to be safe + // clear this flag when a non-transitional Mode is reached to be safe continueToNormal = false; modeHelper.modeChanged(newMode, newSubmode); announceMode(false); diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index 1fc4b1de..4498284b 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -102,10 +102,9 @@ class DeviceHandlerBase : public DeviceHandlerIF, DeviceHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, CookieIF *comCookie, FailureIsolationBase *fdirInstance = nullptr, size_t cmdQueueSize = 20); - /** * extending the modes of DeviceHandler IF for internal state machine - */ + */ static constexpr uint8_t TRANSITION_MODE_CHILD_ACTION_MASK = 0x20; static constexpr uint8_t TRANSITION_MODE_BASE_ACTION_MASK = 0x10; //! This is a transitional state which can not be commanded. The device @@ -733,16 +732,16 @@ class DeviceHandlerBase : public DeviceHandlerIF, /** * Get the current mode - * + * * set via setMode() - */ + */ Mode_t getMode(); /** * Get the current Submode - * + * * set via setMode() - */ + */ Submode_t getSubmode; /** This is the counter value from performOperation(). */ @@ -1227,7 +1226,6 @@ class DeviceHandlerBase : public DeviceHandlerIF, */ Mode_t mode; - /** * The submode the device handler is currently in. * This should not be changed directly but only with setMode() @@ -1254,10 +1252,10 @@ class DeviceHandlerBase : public DeviceHandlerIF, /** * used to make the state machine continue from ON to NOMAL when * a Device is commanded to NORMAL in OFF mode - * + * * set in startTransition() * evaluated in setMode() to continue to NORMAL when ON is reached - */ + */ bool continueToNormal; /** diff --git a/src/fsfw/devicehandlers/DeviceHandlerIF.h b/src/fsfw/devicehandlers/DeviceHandlerIF.h index 672f2830..c683b2cc 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerIF.h +++ b/src/fsfw/devicehandlers/DeviceHandlerIF.h @@ -24,7 +24,6 @@ class DeviceHandlerIF { static const DeviceCommandId_t RAW_COMMAND_ID = -1; static const DeviceCommandId_t NO_COMMAND_ID = -2; - using dh_heater_request_t = uint8_t; using dh_thermal_state_t = int8_t; diff --git a/src/fsfw/serviceinterface/ServiceInterfaceBuffer.cpp b/src/fsfw/serviceinterface/ServiceInterfaceBuffer.cpp index 23892dcc..0e73be83 100644 --- a/src/fsfw/serviceinterface/ServiceInterfaceBuffer.cpp +++ b/src/fsfw/serviceinterface/ServiceInterfaceBuffer.cpp @@ -3,7 +3,6 @@ #if FSFW_CPP_OSTREAM_ENABLED == 1 #include - #include #include "fsfw/serviceinterface/serviceInterfaceDefintions.h" diff --git a/src/fsfw_hal/common/CMakeLists.txt b/src/fsfw_hal/common/CMakeLists.txt index 1cd9c678..2f4608f8 100644 --- a/src/fsfw_hal/common/CMakeLists.txt +++ b/src/fsfw_hal/common/CMakeLists.txt @@ -1,3 +1,3 @@ add_subdirectory(gpio) -target_sources(${LIB_FSFW_NAME} PRIVATE printChar.c) \ No newline at end of file +target_sources(${LIB_FSFW_NAME} PRIVATE printChar.c) diff --git a/src/fsfw_hal/common/printChar.c b/src/fsfw_hal/common/printChar.c index 6e02c1df..24fba5c8 100644 --- a/src/fsfw_hal/common/printChar.c +++ b/src/fsfw_hal/common/printChar.c @@ -1,5 +1,5 @@ -#include #include +#include void __attribute__((weak)) printChar(const char* character, bool errStream) { if (errStream) { From 033676ad3b5136c4e2fb9d41309d2f0883f5f2de Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 28 Oct 2022 10:30:48 +0200 Subject: [PATCH 239/404] smaller fixes for DHB --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 10 ++++++++-- src/fsfw/devicehandlers/DeviceHandlerBase.h | 2 +- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 972aa4fe..7b1929cd 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -573,8 +573,6 @@ void DeviceHandlerBase::setTransition(Mode_t modeTo, Submode_t submodeTo) { } void DeviceHandlerBase::setMode(Mode_t newMode, uint8_t newSubmode) { - /* TODO: This will probably be done by the LocalDataPoolManager now */ - // changeHK(mode, submode, false); /** * handle transition from OFF to NORMAL by continuing towards normal when ON is reached @@ -1595,6 +1593,14 @@ void DeviceHandlerBase::setPowerSwitcher(PowerSwitchIF* switcher) { this->powerSwitcher = switcher; } +Mode_t DeviceHandlerBase::getMode() { + return mode; +} + +Submode_t DeviceHandlerBase::getSubmode() { + return submode; +} + void DeviceHandlerBase::disableCommandsAndReplies() { for (auto& command : deviceCommandMap) { if (command.second.isExecuting) { diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index 3f13c108..65b3ded0 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -755,7 +755,7 @@ class DeviceHandlerBase : public DeviceHandlerIF, * * set via setMode() */ - Submode_t getSubmode; + Submode_t getSubmode(); /** This is the counter value from performOperation(). */ uint8_t pstStep = 0; From e302c89f7465dee400b75a43fd562b4ef1470c24 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 2 Nov 2022 15:48:13 +0100 Subject: [PATCH 240/404] health helper dtor bugfix --- src/fsfw/health/HealthHelper.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/fsfw/health/HealthHelper.cpp b/src/fsfw/health/HealthHelper.cpp index bf1a92d2..f6077ea1 100644 --- a/src/fsfw/health/HealthHelper.cpp +++ b/src/fsfw/health/HealthHelper.cpp @@ -5,7 +5,11 @@ HealthHelper::HealthHelper(HasHealthIF* owner, object_id_t objectId) : objectId(objectId), owner(owner) {} -HealthHelper::~HealthHelper() { healthTable->removeObject(objectId); } +HealthHelper::~HealthHelper() { + if (healthTable != nullptr) { + healthTable->removeObject(objectId); + } +} ReturnValue_t HealthHelper::handleHealthCommand(CommandMessage* message) { switch (message->getCommand()) { From e1d4209fbe06581c36dd0a1b8113dfb56e673613 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 2 Nov 2022 16:07:00 +0100 Subject: [PATCH 241/404] missing fifo pop command --- src/fsfw/osal/common/TcpTmTcServer.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index dff959ba..bc2cd152 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -274,6 +274,8 @@ ReturnValue_t TcpTmTcServer::handleTmSending(socket_t connSocket, bool& tmSent) ConstStorageAccessor storeAccessor(storeId); ReturnValue_t result = tmStore->getData(storeId, storeAccessor); if (result != returnvalue::OK) { + // Invalid entry, pop FIFO + tmtcBridge->tmFifo->pop(); return result; } if (wiretappingEnabled) { From 91ebf98c2851b0470a912c1c3279cb20e10a5490 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 2 Nov 2022 19:35:22 +0100 Subject: [PATCH 242/404] add new ping with data TC --- CMakeLists.txt | 12 +++++++++--- src/fsfw/pus/Service17Test.cpp | 11 +++++++++++ src/fsfw/pus/Service17Test.h | 3 +++ 3 files changed, 23 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f31cd6bc..0c0d2bb4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -345,9 +345,15 @@ if(FSFW_BUILD_TESTS) DEPENDENCIES ${FSFW_TEST_TGT}) else() setup_target_for_coverage_lcov( - NAME ${FSFW_TEST_TGT}_coverage EXECUTABLE ${FSFW_TEST_TGT} - DEPENDENCIES ${FSFW_TEST_TGT} - GENHTML_ARGS --html-epilog ${CMAKE_SOURCE_DIR}/unittests/lcov_epilog.html) + NAME + ${FSFW_TEST_TGT}_coverage + EXECUTABLE + ${FSFW_TEST_TGT} + DEPENDENCIES + ${FSFW_TEST_TGT} + GENHTML_ARGS + --html-epilog + ${CMAKE_SOURCE_DIR}/unittests/lcov_epilog.html) endif() endif() endif() diff --git a/src/fsfw/pus/Service17Test.cpp b/src/fsfw/pus/Service17Test.cpp index bea2eeb8..35fc7c9d 100644 --- a/src/fsfw/pus/Service17Test.cpp +++ b/src/fsfw/pus/Service17Test.cpp @@ -1,5 +1,7 @@ #include "fsfw/pus/Service17Test.h" +#include + #include "fsfw/FSFW.h" #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/objectmanager/SystemObject.h" @@ -31,6 +33,15 @@ ReturnValue_t Service17Test::handleRequest(uint8_t subservice) { } return tmHelper.storeAndSendTmPacket(); } + case Subservice::PING_WITH_DATA: { + SerializeElement receivedDataLen = currentPacket.getUserDataLen(); + ReturnValue_t result = + tmHelper.prepareTmPacket(Subservice::PING_WITH_DATA_REPORT_WITH_SIZE, receivedDataLen); + if (result != returnvalue::OK) { + return result; + } + return tmHelper.storeAndSendTmPacket(); + } default: return AcceptsTelecommandsIF::INVALID_SUBSERVICE; } diff --git a/src/fsfw/pus/Service17Test.h b/src/fsfw/pus/Service17Test.h index f2ec6e4f..d3b6a6dc 100644 --- a/src/fsfw/pus/Service17Test.h +++ b/src/fsfw/pus/Service17Test.h @@ -32,6 +32,9 @@ class Service17Test : public PusServiceBase { CONNECTION_TEST_REPORT = 2, //! [EXPORT] : [COMMAND] Trigger test reply and test event EVENT_TRIGGER_TEST = 128, + PING_WITH_DATA = 129, + //! [EXPORT] : [COMMAND] Report which reports the sent user data size + PING_WITH_DATA_REPORT_WITH_SIZE = 130 }; explicit Service17Test(PsbParams params); From 6efa482eb09939aebe1c9c4427ae621e3ffa7e81 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 3 Nov 2022 10:10:36 +0100 Subject: [PATCH 243/404] use uniform uart api --- src/fsfw_hal/linux/uart/helper.cpp | 72 ++++++++++-------------------- 1 file changed, 24 insertions(+), 48 deletions(-) diff --git a/src/fsfw_hal/linux/uart/helper.cpp b/src/fsfw_hal/linux/uart/helper.cpp index 141f68e3..f9569cd3 100644 --- a/src/fsfw_hal/linux/uart/helper.cpp +++ b/src/fsfw_hal/linux/uart/helper.cpp @@ -16,123 +16,99 @@ void uart::setMode(struct termios& options, UartModes mode) { void uart::setBaudrate(struct termios& options, UartBaudRate baud) { switch (baud) { case UartBaudRate::RATE_50: - cfsetispeed(&options, B50); - cfsetospeed(&options, B50); + cfsetspeed(&options, B50); break; case UartBaudRate::RATE_75: - cfsetispeed(&options, B75); - cfsetospeed(&options, B75); + cfsetspeed(&options, B75); break; case UartBaudRate::RATE_110: - cfsetispeed(&options, B110); - cfsetospeed(&options, B110); + cfsetspeed(&options, B110); break; case UartBaudRate::RATE_134: - cfsetispeed(&options, B134); - cfsetospeed(&options, B134); + cfsetspeed(&options, B134); break; case UartBaudRate::RATE_150: - cfsetispeed(&options, B150); - cfsetospeed(&options, B150); + cfsetspeed(&options, B150); break; case UartBaudRate::RATE_200: cfsetispeed(&options, B200); cfsetospeed(&options, B200); break; case UartBaudRate::RATE_300: - cfsetispeed(&options, B300); - cfsetospeed(&options, B300); + cfsetspeed(&options, B300); break; case UartBaudRate::RATE_600: - cfsetispeed(&options, B600); - cfsetospeed(&options, B600); + cfsetspeed(&options, B600); break; case UartBaudRate::RATE_1200: - cfsetispeed(&options, B1200); - cfsetospeed(&options, B1200); + cfsetspeed(&options, B1200); break; case UartBaudRate::RATE_1800: - cfsetispeed(&options, B1800); - cfsetospeed(&options, B1800); + cfsetspeed(&options, B1800); break; case UartBaudRate::RATE_2400: - cfsetispeed(&options, B2400); - cfsetospeed(&options, B2400); + cfsetspeed(&options, B2400); break; case UartBaudRate::RATE_4800: - cfsetispeed(&options, B4800); - cfsetospeed(&options, B4800); + cfsetspeed(&options, B4800); break; case UartBaudRate::RATE_9600: cfsetispeed(&options, B9600); cfsetospeed(&options, B9600); break; case UartBaudRate::RATE_19200: - cfsetispeed(&options, B19200); - cfsetospeed(&options, B19200); + cfsetspeed(&options, B19200); break; case UartBaudRate::RATE_38400: cfsetspeed(&options, B38400); break; case UartBaudRate::RATE_57600: - cfsetispeed(&options, B57600); - cfsetospeed(&options, B57600); + cfsetspeed(&options, B57600); break; case UartBaudRate::RATE_115200: cfsetspeed(&options, B115200); break; case UartBaudRate::RATE_230400: - cfsetispeed(&options, B230400); - cfsetospeed(&options, B230400); + cfsetspeed(&options, B230400); break; #ifndef __APPLE__ case UartBaudRate::RATE_460800: - cfsetispeed(&options, B460800); - cfsetospeed(&options, B460800); + cfsetspeed(&options, B460800); break; case UartBaudRate::RATE_500000: - cfsetispeed(&options, B500000); - cfsetospeed(&options, B500000); + cfsetspeed(&options, B500000); break; case UartBaudRate::RATE_576000: cfsetispeed(&options, B576000); cfsetospeed(&options, B576000); break; case UartBaudRate::RATE_921600: - cfsetispeed(&options, B921600); - cfsetospeed(&options, B921600); + cfsetspeed(&options, B921600); break; case UartBaudRate::RATE_1000000: - cfsetispeed(&options, B1000000); - cfsetospeed(&options, B1000000); + cfsetspeed(&options, B1000000); break; case UartBaudRate::RATE_1152000: - cfsetispeed(&options, B1152000); - cfsetospeed(&options, B1152000); + cfsetspeed(&options, B1152000); break; case UartBaudRate::RATE_1500000: - cfsetispeed(&options, B1500000); - cfsetospeed(&options, B1500000); + cfsetspeed(&options, B1500000); break; case UartBaudRate::RATE_2000000: - cfsetispeed(&options, B2000000); - cfsetospeed(&options, B2000000); + cfsetspeed(&options, B2000000); break; case UartBaudRate::RATE_2500000: - cfsetispeed(&options, B2500000); - cfsetospeed(&options, B2500000); + cfsetspeed(&options, B2500000); break; case UartBaudRate::RATE_3000000: cfsetispeed(&options, B3000000); cfsetospeed(&options, B3000000); break; case UartBaudRate::RATE_3500000: - cfsetispeed(&options, B3500000); - cfsetospeed(&options, B3500000); + cfsetspeed(&options, B3500000); break; case UartBaudRate::RATE_4000000: - cfsetispeed(&options, B4000000); - cfsetospeed(&options, B4000000); + cfsetspeed(&options, B4000000); break; #endif // ! __APPLE__ default: From 00f1c5bbe924f987e83362cbff92d4c83b3e332c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 3 Nov 2022 10:12:05 +0100 Subject: [PATCH 244/404] missing replacements --- src/fsfw_hal/linux/uart/helper.cpp | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/src/fsfw_hal/linux/uart/helper.cpp b/src/fsfw_hal/linux/uart/helper.cpp index f9569cd3..c32fed31 100644 --- a/src/fsfw_hal/linux/uart/helper.cpp +++ b/src/fsfw_hal/linux/uart/helper.cpp @@ -31,8 +31,7 @@ void uart::setBaudrate(struct termios& options, UartBaudRate baud) { cfsetspeed(&options, B150); break; case UartBaudRate::RATE_200: - cfsetispeed(&options, B200); - cfsetospeed(&options, B200); + cfsetspeed(&options, B200); break; case UartBaudRate::RATE_300: cfsetspeed(&options, B300); @@ -53,8 +52,7 @@ void uart::setBaudrate(struct termios& options, UartBaudRate baud) { cfsetspeed(&options, B4800); break; case UartBaudRate::RATE_9600: - cfsetispeed(&options, B9600); - cfsetospeed(&options, B9600); + cfsetspeed(&options, B9600); break; case UartBaudRate::RATE_19200: cfsetspeed(&options, B19200); @@ -79,8 +77,7 @@ void uart::setBaudrate(struct termios& options, UartBaudRate baud) { cfsetspeed(&options, B500000); break; case UartBaudRate::RATE_576000: - cfsetispeed(&options, B576000); - cfsetospeed(&options, B576000); + cfsetspeed(&options, B576000); break; case UartBaudRate::RATE_921600: cfsetspeed(&options, B921600); @@ -101,8 +98,7 @@ void uart::setBaudrate(struct termios& options, UartBaudRate baud) { cfsetspeed(&options, B2500000); break; case UartBaudRate::RATE_3000000: - cfsetispeed(&options, B3000000); - cfsetospeed(&options, B3000000); + cfsetspeed(&options, B3000000); break; case UartBaudRate::RATE_3500000: cfsetspeed(&options, B3500000); From e5b5c7d2533cd25dbe53d33be540babf15a3c70b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 3 Nov 2022 10:33:52 +0100 Subject: [PATCH 245/404] higher default MQ depth for events, printout tweak --- src/fsfw/pus/Service5EventReporting.h | 2 +- src/fsfw/tmtcservices/PusServiceBase.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fsfw/pus/Service5EventReporting.h b/src/fsfw/pus/Service5EventReporting.h index 1f4e5a3a..0c738226 100644 --- a/src/fsfw/pus/Service5EventReporting.h +++ b/src/fsfw/pus/Service5EventReporting.h @@ -42,7 +42,7 @@ class Service5EventReporting : public PusServiceBase { public: Service5EventReporting(PsbParams params, size_t maxNumberReportsPerCycle = 10, - uint32_t messageQueueDepth = 10); + uint32_t messageQueueDepth = 20); ~Service5EventReporting() override; /*** diff --git a/src/fsfw/tmtcservices/PusServiceBase.cpp b/src/fsfw/tmtcservices/PusServiceBase.cpp index 9598e536..8e0c5c4f 100644 --- a/src/fsfw/tmtcservices/PusServiceBase.cpp +++ b/src/fsfw/tmtcservices/PusServiceBase.cpp @@ -27,8 +27,8 @@ ReturnValue_t PusServiceBase::performOperation(uint8_t opCode) { ReturnValue_t result = performService(); if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PusService " << psbParams.serviceId << ": performService returned with " - << static_cast(result) << std::endl; + sif::error << "PusService " << static_cast(psbParams.serviceId) << + ": performService returned with " << static_cast(result) << std::endl; #endif return returnvalue::FAILED; } From 672fca5169b017387e58e2ff864913d932c59aa1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Nov 2022 11:08:23 +0100 Subject: [PATCH 246/404] extend uart helper a bit --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 9 +--- src/fsfw/tmtcservices/PusServiceBase.cpp | 4 +- src/fsfw_hal/linux/uart/UartComIF.cpp | 19 +------- src/fsfw_hal/linux/uart/UartComIF.h | 11 ----- src/fsfw_hal/linux/uart/helper.cpp | 44 +++++++++++++++++++ src/fsfw_hal/linux/uart/helper.h | 10 +++++ 6 files changed, 59 insertions(+), 38 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 7bded0de..559e4cfd 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -566,7 +566,6 @@ void DeviceHandlerBase::setTransition(Mode_t modeTo, Submode_t submodeTo) { } void DeviceHandlerBase::setMode(Mode_t newMode, uint8_t newSubmode) { - /** * handle transition from OFF to NORMAL by continuing towards normal when ON is reached */ @@ -1584,13 +1583,9 @@ void DeviceHandlerBase::setPowerSwitcher(PowerSwitchIF* switcher) { this->powerSwitcher = switcher; } -Mode_t DeviceHandlerBase::getMode() { - return mode; -} +Mode_t DeviceHandlerBase::getMode() { return mode; } -Submode_t DeviceHandlerBase::getSubmode() { - return submode; -} +Submode_t DeviceHandlerBase::getSubmode() { return submode; } void DeviceHandlerBase::disableCommandsAndReplies() { for (auto& command : deviceCommandMap) { diff --git a/src/fsfw/tmtcservices/PusServiceBase.cpp b/src/fsfw/tmtcservices/PusServiceBase.cpp index 8e0c5c4f..fbabbd70 100644 --- a/src/fsfw/tmtcservices/PusServiceBase.cpp +++ b/src/fsfw/tmtcservices/PusServiceBase.cpp @@ -27,8 +27,8 @@ ReturnValue_t PusServiceBase::performOperation(uint8_t opCode) { ReturnValue_t result = performService(); if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "PusService " << static_cast(psbParams.serviceId) << - ": performService returned with " << static_cast(result) << std::endl; + sif::error << "PusService " << static_cast(psbParams.serviceId) + << ": performService returned with " << static_cast(result) << std::endl; #endif return returnvalue::FAILED; } diff --git a/src/fsfw_hal/linux/uart/UartComIF.cpp b/src/fsfw_hal/linux/uart/UartComIF.cpp index 0b44e4a5..a65adb33 100644 --- a/src/fsfw_hal/linux/uart/UartComIF.cpp +++ b/src/fsfw_hal/linux/uart/UartComIF.cpp @@ -88,7 +88,7 @@ int UartComIF::configureUartPort(UartCookie* uartCookie) { return fd; } - setParityOptions(&options, uartCookie); + uart::setParity(options, uartCookie->getParity()); setStopBitOptions(&options, uartCookie); setDatasizeOptions(&options, uartCookie); setFixedOptions(&options); @@ -114,23 +114,6 @@ int UartComIF::configureUartPort(UartCookie* uartCookie) { return fd; } -void UartComIF::setParityOptions(struct termios* options, UartCookie* uartCookie) { - /* Clear parity bit */ - options->c_cflag &= ~PARENB; - switch (uartCookie->getParity()) { - case Parity::EVEN: - options->c_cflag |= PARENB; - options->c_cflag &= ~PARODD; - break; - case Parity::ODD: - options->c_cflag |= PARENB; - options->c_cflag |= PARODD; - break; - default: - break; - } -} - void UartComIF::setStopBitOptions(struct termios* options, UartCookie* uartCookie) { /* Clear stop field. Sets stop bit to one bit */ options->c_cflag &= ~CSTOPB; diff --git a/src/fsfw_hal/linux/uart/UartComIF.h b/src/fsfw_hal/linux/uart/UartComIF.h index 4bdbd954..fc92921f 100644 --- a/src/fsfw_hal/linux/uart/UartComIF.h +++ b/src/fsfw_hal/linux/uart/UartComIF.h @@ -78,17 +78,6 @@ class UartComIF : public DeviceCommunicationIF, public SystemObject { */ int configureUartPort(UartCookie* uartCookie); - /** - * @brief This function adds the parity settings to the termios options struct. - * - * @param options Pointer to termios options struct which will be modified to enable or disable - * parity checking. - * @param uartCookie Pointer to uart cookie containing the information about the desired - * parity settings. - * - */ - void setParityOptions(struct termios* options, UartCookie* uartCookie); - void setStopBitOptions(struct termios* options, UartCookie* uartCookie); /** diff --git a/src/fsfw_hal/linux/uart/helper.cpp b/src/fsfw_hal/linux/uart/helper.cpp index c32fed31..f38dd3d3 100644 --- a/src/fsfw_hal/linux/uart/helper.cpp +++ b/src/fsfw_hal/linux/uart/helper.cpp @@ -115,6 +115,50 @@ void uart::setBaudrate(struct termios& options, UartBaudRate baud) { } } +void uart::setBitsPerWord(struct termios& options, BitsPerWord bits) { + options.c_cflag &= ~CSIZE; // Clear all the size bits + if (bits == BitsPerWord::BITS_5) { + options.c_cflag |= CS5; + } else if (bits == BitsPerWord::BITS_6) { + options.c_cflag |= CS6; + } else if (bits == BitsPerWord::BITS_7) { + options.c_cflag |= CS7; + } else if (bits == BitsPerWord::BITS_8) { + options.c_cflag |= CS8; + } +} + +void uart::enableRead(struct termios& options) { options.c_cflag |= CREAD; } + +void uart::ignoreCtrlLines(struct termios& options) { options.c_cflag |= CLOCAL; } + +void uart::setParity(struct termios& options, Parity parity) { + /* Clear parity bit */ + options.c_cflag &= ~PARENB; + switch (parity) { + case Parity::EVEN: + options.c_cflag |= PARENB; + options.c_cflag &= ~PARODD; + break; + case Parity::ODD: + options.c_cflag |= PARENB; + options.c_cflag |= PARODD; + break; + default: + break; + } +} + int uart::readCountersAndErrors(int serialPort, serial_icounter_struct& icounter) { return ioctl(serialPort, TIOCGICOUNT, &icounter); } + +void uart::setStopbits(struct termios& options, StopBits bits) { + if (bits == StopBits::TWO_STOP_BITS) { + // Use two stop bits + options.c_cflag |= CSTOPB; + } else { + // Clear stop field, only one stop bit used in communication + options.c_cflag &= ~CSTOPB; + } +} diff --git a/src/fsfw_hal/linux/uart/helper.h b/src/fsfw_hal/linux/uart/helper.h index 515f815b..7f067d00 100644 --- a/src/fsfw_hal/linux/uart/helper.h +++ b/src/fsfw_hal/linux/uart/helper.h @@ -54,6 +54,16 @@ void setMode(struct termios& options, UartModes mode); */ void setBaudrate(struct termios& options, UartBaudRate baud); +void setStopbits(struct termios& options, StopBits bits); + +void setBitsPerWord(struct termios& options, BitsPerWord bits); + +void enableRead(struct termios& options); + +void setParity(struct termios& options, Parity parity); + +void ignoreCtrlLines(struct termios& options); + int readCountersAndErrors(int serialPort, serial_icounter_struct& icounter); } // namespace uart From 0e8f5ddd26d586dd40e69f52aef1a63c0d5a9da6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 8 Nov 2022 11:07:56 +0100 Subject: [PATCH 247/404] added missing const specifier --- src/fsfw/tmtcpacket/ccsds/SpacePacketReader.cpp | 2 +- src/fsfw/tmtcpacket/ccsds/SpacePacketReader.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.cpp b/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.cpp index 605bbd21..262653c9 100644 --- a/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.cpp +++ b/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.cpp @@ -21,7 +21,7 @@ SpacePacketReader::~SpacePacketReader() = default; inline uint16_t SpacePacketReader::getPacketIdRaw() const { return ccsds::getPacketId(*spHeader); } -const uint8_t* SpacePacketReader::getPacketData() { return packetDataField; } +const uint8_t* SpacePacketReader::getPacketData() const { return packetDataField; } ReturnValue_t SpacePacketReader::setData(uint8_t* data, size_t maxSize_, void* args) { return setInternalFields(data, maxSize_); diff --git a/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.h b/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.h index ff22510c..61939217 100644 --- a/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.h +++ b/src/fsfw/tmtcpacket/ccsds/SpacePacketReader.h @@ -71,7 +71,7 @@ class SpacePacketReader : public SpacePacketIF, // Helper methods: [[nodiscard]] ReturnValue_t checkSize() const; - const uint8_t* getPacketData(); + const uint8_t* getPacketData() const; ReturnValue_t setReadOnlyData(const uint8_t* data, size_t maxSize); From 530a261e142ae4ab2c7726303e4d571d03200e47 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 10 Nov 2022 15:12:18 +0100 Subject: [PATCH 248/404] dedicated header file for container retvals --- src/fsfw/container/ArrayList.h | 6 +++--- src/fsfw/container/FixedMap.h | 19 ++++++------------- src/fsfw/container/definitions.h | 15 +++++++++++++++ 3 files changed, 24 insertions(+), 16 deletions(-) create mode 100644 src/fsfw/container/definitions.h diff --git a/src/fsfw/container/ArrayList.h b/src/fsfw/container/ArrayList.h index f5f74cf1..12bcda99 100644 --- a/src/fsfw/container/ArrayList.h +++ b/src/fsfw/container/ArrayList.h @@ -5,6 +5,8 @@ #include "../serialize/SerializeAdapter.h" #include "../serialize/SerializeIF.h" +#include "definitions.h" + /** * @brief A List that stores its values in an array. * @details @@ -19,8 +21,6 @@ class ArrayList { friend class SerialArrayListAdapter; public: - static const uint8_t INTERFACE_ID = CLASS_ID::ARRAY_LIST; - static const ReturnValue_t FULL = MAKE_RETURN_CODE(0x01); /** * This is the allocating constructor. @@ -187,7 +187,7 @@ class ArrayList { */ ReturnValue_t insert(T entry) { if (size >= maxSize_) { - return FULL; + return containers::LIST_FULL; } entries[size] = entry; ++size; diff --git a/src/fsfw/container/FixedMap.h b/src/fsfw/container/FixedMap.h index 6ed6278d..553d3cac 100644 --- a/src/fsfw/container/FixedMap.h +++ b/src/fsfw/container/FixedMap.h @@ -4,16 +4,9 @@ #include #include -#include "../returnvalues/returnvalue.h" +#include "definitions.h" #include "ArrayList.h" -namespace mapdefs { -static const uint8_t INTERFACE_ID = CLASS_ID::FIXED_MAP; -static const ReturnValue_t KEY_ALREADY_EXISTS = MAKE_RETURN_CODE(0x01); -static const ReturnValue_t MAP_FULL = MAKE_RETURN_CODE(0x02); -static const ReturnValue_t KEY_DOES_NOT_EXIST = MAKE_RETURN_CODE(0x03); -} // namespace mapdefs - /** * @brief Map implementation for maps with a pre-defined size. * @details @@ -78,10 +71,10 @@ class FixedMap : public SerializeIF { ReturnValue_t insert(key_t key, T value, Iterator* storedValue = nullptr) { if (exists(key) == returnvalue::OK) { - return mapdefs::KEY_ALREADY_EXISTS; + return containers::KEY_ALREADY_EXISTS; } if (_size == theMap.maxSize()) { - return mapdefs::MAP_FULL; + return containers::MAP_FULL; } theMap[_size].first = key; theMap[_size].second = value; @@ -95,7 +88,7 @@ class FixedMap : public SerializeIF { ReturnValue_t insert(std::pair pair) { return insert(pair.first, pair.second); } ReturnValue_t exists(key_t key) const { - ReturnValue_t result = mapdefs::KEY_DOES_NOT_EXIST; + ReturnValue_t result = containers::KEY_DOES_NOT_EXIST; if (findIndex(key) < _size) { result = returnvalue::OK; } @@ -105,7 +98,7 @@ class FixedMap : public SerializeIF { ReturnValue_t erase(Iterator* iter) { uint32_t i; if ((i = findIndex((*iter).value->first)) >= _size) { - return mapdefs::KEY_DOES_NOT_EXIST; + return containers::KEY_DOES_NOT_EXIST; } theMap[i] = theMap[_size - 1]; --_size; @@ -116,7 +109,7 @@ class FixedMap : public SerializeIF { ReturnValue_t erase(key_t key) { uint32_t i; if ((i = findIndex(key)) >= _size) { - return mapdefs::KEY_DOES_NOT_EXIST; + return containers::KEY_DOES_NOT_EXIST; } theMap[i] = theMap[_size - 1]; --_size; diff --git a/src/fsfw/container/definitions.h b/src/fsfw/container/definitions.h new file mode 100644 index 00000000..23620176 --- /dev/null +++ b/src/fsfw/container/definitions.h @@ -0,0 +1,15 @@ +#ifndef FSFW_CONTAINER_DEFINITIONS_H_ +#define FSFW_CONTAINER_DEFINITIONS_H_ + +#include "fsfw/retval.h" + +using namespace returnvalue; +namespace containers { +static const ReturnValue_t KEY_ALREADY_EXISTS = makeCode(CLASS_ID::FIXED_MAP, 0x01); +static const ReturnValue_t MAP_FULL = makeCode(CLASS_ID::FIXED_MAP, 0x02); +static const ReturnValue_t KEY_DOES_NOT_EXIST = makeCode(CLASS_ID::FIXED_MAP, 0x03); + +static const ReturnValue_t LIST_FULL = makeCode(CLASS_ID::ARRAY_LIST, 0x01); +} // namespace containers + +#endif /* FSFW_CONTAINER_DEFINITIONS_H_ */ From 177c39dd53198a1b05e2f40fc3c5e88e7f7c2e0b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 10 Nov 2022 15:38:34 +0100 Subject: [PATCH 249/404] helper interface implementations --- src/fsfw/container/ArrayList.h | 2 -- src/fsfw/container/FixedMap.h | 2 +- src/fsfw/power/PowerSwitcherComponent.cpp | 18 +++++++++++++++--- src/fsfw/power/PowerSwitcherComponent.h | 11 +++++++++++ 4 files changed, 27 insertions(+), 6 deletions(-) diff --git a/src/fsfw/container/ArrayList.h b/src/fsfw/container/ArrayList.h index 12bcda99..92834558 100644 --- a/src/fsfw/container/ArrayList.h +++ b/src/fsfw/container/ArrayList.h @@ -4,7 +4,6 @@ #include "../returnvalues/returnvalue.h" #include "../serialize/SerializeAdapter.h" #include "../serialize/SerializeIF.h" - #include "definitions.h" /** @@ -21,7 +20,6 @@ class ArrayList { friend class SerialArrayListAdapter; public: - /** * This is the allocating constructor. * It allocates an array of the specified size. diff --git a/src/fsfw/container/FixedMap.h b/src/fsfw/container/FixedMap.h index 553d3cac..ce90d814 100644 --- a/src/fsfw/container/FixedMap.h +++ b/src/fsfw/container/FixedMap.h @@ -4,8 +4,8 @@ #include #include -#include "definitions.h" #include "ArrayList.h" +#include "definitions.h" /** * @brief Map implementation for maps with a pre-defined size. diff --git a/src/fsfw/power/PowerSwitcherComponent.cpp b/src/fsfw/power/PowerSwitcherComponent.cpp index b6b67a83..b07a7296 100644 --- a/src/fsfw/power/PowerSwitcherComponent.cpp +++ b/src/fsfw/power/PowerSwitcherComponent.cpp @@ -3,7 +3,7 @@ #include #include -PowerSwitcherComponent::PowerSwitcherComponent(object_id_t objectId, PowerSwitchIF *pwrSwitcher, +PowerSwitcherComponent::PowerSwitcherComponent(object_id_t objectId, PowerSwitchIF* pwrSwitcher, power::Switch_t pwrSwitch) : SystemObject(objectId), switcher(pwrSwitcher, pwrSwitch), @@ -54,7 +54,7 @@ ReturnValue_t PowerSwitcherComponent::initialize() { MessageQueueId_t PowerSwitcherComponent::getCommandQueue() const { return queue->getId(); } -void PowerSwitcherComponent::getMode(Mode_t *mode, Submode_t *submode) { +void PowerSwitcherComponent::getMode(Mode_t* mode, Submode_t* submode) { *mode = this->mode; *submode = this->submode; } @@ -65,7 +65,7 @@ ReturnValue_t PowerSwitcherComponent::setHealth(HealthState health) { } ReturnValue_t PowerSwitcherComponent::checkModeCommand(Mode_t mode, Submode_t submode, - uint32_t *msToReachTheMode) { + uint32_t* msToReachTheMode) { *msToReachTheMode = 5000; if (mode != MODE_ON and mode != MODE_OFF) { return TRANS_NOT_ALLOWED; @@ -105,3 +105,15 @@ void PowerSwitcherComponent::setMode(Mode_t newMode, Submode_t newSubmode) { } HasHealthIF::HealthState PowerSwitcherComponent::getHealth() { return healthHelper.getHealth(); } + +const HasHealthIF* PowerSwitcherComponent::getOptHealthIF() const { return this; } + +const HasModesIF& PowerSwitcherComponent::getModeIF() const { return *this; } + +ReturnValue_t PowerSwitcherComponent::connectModeTreeParent(HasModeTreeChildrenIF& parent) { + return parent.registerChild(*this); +} + +object_id_t PowerSwitcherComponent::getObjectId() const { return SystemObject::getObjectId(); } + +ModeTreeChildIF& PowerSwitcherComponent::getModeTreeChildIF() { return *this; } diff --git a/src/fsfw/power/PowerSwitcherComponent.h b/src/fsfw/power/PowerSwitcherComponent.h index 01689bef..38dddc37 100644 --- a/src/fsfw/power/PowerSwitcherComponent.h +++ b/src/fsfw/power/PowerSwitcherComponent.h @@ -8,6 +8,8 @@ #include #include #include +#include +#include #include class PowerSwitchIF; @@ -24,12 +26,17 @@ class PowerSwitchIF; */ class PowerSwitcherComponent : public SystemObject, public ExecutableObjectIF, + public ModeTreeChildIF, + public ModeTreeConnectionIF, public HasModesIF, public HasHealthIF { public: PowerSwitcherComponent(object_id_t objectId, PowerSwitchIF *pwrSwitcher, power::Switch_t pwrSwitch); + ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override; + ModeTreeChildIF &getModeTreeChildIF() override; + private: MessageQueueIF *queue = nullptr; PowerSwitcher switcher; @@ -56,6 +63,10 @@ class PowerSwitcherComponent : public SystemObject, ReturnValue_t setHealth(HealthState health) override; HasHealthIF::HealthState getHealth() override; + + object_id_t getObjectId() const override; + const HasHealthIF *getOptHealthIF() const override; + const HasModesIF &getModeIF() const override; }; #endif /* _FSFW_POWER_POWERSWITCHERCOMPONENT_H_ */ From 194b3e100a036b1fef22566693549b327d48c6a5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 10 Nov 2022 15:53:18 +0100 Subject: [PATCH 250/404] fix compiler error for fixed array list copy ctor --- src/fsfw/container/FixedArrayList.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/fsfw/container/FixedArrayList.h b/src/fsfw/container/FixedArrayList.h index fc8be393..78184e35 100644 --- a/src/fsfw/container/FixedArrayList.h +++ b/src/fsfw/container/FixedArrayList.h @@ -20,7 +20,9 @@ class FixedArrayList : public ArrayList { FixedArrayList() : ArrayList(data, MAX_SIZE) {} FixedArrayList(const FixedArrayList& other) : ArrayList(data, MAX_SIZE) { - memcpy(this->data, other.data, sizeof(this->data)); + for (size_t idx = 0; idx < sizeof(data); idx++) { + data[idx] = other.data[idx]; + } this->entries = data; this->size = other.size; } From 6ca1a5c7969198ff01391fa4fbd4fa0a80552543 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 10 Nov 2022 16:11:05 +0100 Subject: [PATCH 251/404] fix unittests --- src/fsfw/container/definitions.h | 9 ++++----- src/fsfw_hal/linux/uio/UioMapper.cpp | 2 ++ unittests/container/TestArrayList.cpp | 2 +- unittests/container/TestFixedArrayList.cpp | 2 +- unittests/container/TestFixedMap.cpp | 18 ++++++++++-------- unittests/mocks/AcceptsTmMock.cpp | 2 +- unittests/mocks/AcceptsTmMock.h | 2 +- 7 files changed, 20 insertions(+), 17 deletions(-) diff --git a/src/fsfw/container/definitions.h b/src/fsfw/container/definitions.h index 23620176..b50ea45d 100644 --- a/src/fsfw/container/definitions.h +++ b/src/fsfw/container/definitions.h @@ -3,13 +3,12 @@ #include "fsfw/retval.h" -using namespace returnvalue; namespace containers { -static const ReturnValue_t KEY_ALREADY_EXISTS = makeCode(CLASS_ID::FIXED_MAP, 0x01); -static const ReturnValue_t MAP_FULL = makeCode(CLASS_ID::FIXED_MAP, 0x02); -static const ReturnValue_t KEY_DOES_NOT_EXIST = makeCode(CLASS_ID::FIXED_MAP, 0x03); +static const ReturnValue_t KEY_ALREADY_EXISTS = returnvalue::makeCode(CLASS_ID::FIXED_MAP, 0x01); +static const ReturnValue_t MAP_FULL = returnvalue::makeCode(CLASS_ID::FIXED_MAP, 0x02); +static const ReturnValue_t KEY_DOES_NOT_EXIST = returnvalue::makeCode(CLASS_ID::FIXED_MAP, 0x03); -static const ReturnValue_t LIST_FULL = makeCode(CLASS_ID::ARRAY_LIST, 0x01); +static const ReturnValue_t LIST_FULL = returnvalue::makeCode(CLASS_ID::ARRAY_LIST, 0x01); } // namespace containers #endif /* FSFW_CONTAINER_DEFINITIONS_H_ */ diff --git a/src/fsfw_hal/linux/uio/UioMapper.cpp b/src/fsfw_hal/linux/uio/UioMapper.cpp index 74d0e41f..e34c209a 100644 --- a/src/fsfw_hal/linux/uio/UioMapper.cpp +++ b/src/fsfw_hal/linux/uio/UioMapper.cpp @@ -22,7 +22,9 @@ UioMapper::UioMapper(std::string uioFile, int mapNum) : mapNum(mapNum) { if (res) { this->uioFile = res; } else { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "Could not resolve real path of UIO file " << uioFile << std::endl; +#endif } free(res); } else { diff --git a/unittests/container/TestArrayList.cpp b/unittests/container/TestArrayList.cpp index 69b08faa..0c327685 100644 --- a/unittests/container/TestArrayList.cpp +++ b/unittests/container/TestArrayList.cpp @@ -43,7 +43,7 @@ TEST_CASE("Array List", "[containers]") { for (auto i = 0; i < 20; i++) { REQUIRE(list.insert(i) == static_cast(returnvalue::OK)); } - REQUIRE(list.insert(20) == static_cast(ArrayList::FULL)); + REQUIRE(list.insert(20) == static_cast(containers::LIST_FULL)); ArrayList::Iterator it = list.begin(); REQUIRE((*it) == 0); it++; diff --git a/unittests/container/TestFixedArrayList.cpp b/unittests/container/TestFixedArrayList.cpp index 876d6792..f3131c64 100644 --- a/unittests/container/TestFixedArrayList.cpp +++ b/unittests/container/TestFixedArrayList.cpp @@ -31,7 +31,7 @@ TEST_CASE("FixedArrayList Tests", "[containers]") { for (auto i = 1; i < 260; i++) { REQUIRE(list.insert(i) == static_cast(returnvalue::OK)); } - REQUIRE(list.insert(260) == static_cast(ArrayList::FULL)); + REQUIRE(list.insert(260) == static_cast(containers::LIST_FULL)); list.clear(); REQUIRE(list.size == 0); } diff --git a/unittests/container/TestFixedMap.cpp b/unittests/container/TestFixedMap.cpp index 8bfe9fed..231edb4f 100644 --- a/unittests/container/TestFixedMap.cpp +++ b/unittests/container/TestFixedMap.cpp @@ -7,6 +7,8 @@ template class FixedMap; +using namespace returnvalue; + TEST_CASE("FixedMap Tests", "[containers]") { INFO("FixedMap Tests"); @@ -24,9 +26,9 @@ TEST_CASE("FixedMap Tests", "[containers]") { REQUIRE(map.find(i)->second == i + 1); REQUIRE(not map.empty()); } - REQUIRE(map.insert(0, 0) == static_cast(FixedMap::KEY_ALREADY_EXISTS)); - REQUIRE(map.insert(31, 0) == static_cast(FixedMap::MAP_FULL)); - REQUIRE(map.exists(31) == static_cast(FixedMap::KEY_DOES_NOT_EXIST)); + REQUIRE(map.insert(0, 0) == static_cast(containers::KEY_ALREADY_EXISTS)); + REQUIRE(map.insert(31, 0) == static_cast(containers::MAP_FULL)); + REQUIRE(map.exists(31) == static_cast(containers::KEY_DOES_NOT_EXIST)); REQUIRE(map.size() == 30); REQUIRE(map.full()); { @@ -35,14 +37,14 @@ TEST_CASE("FixedMap Tests", "[containers]") { REQUIRE(*ptr == 6); REQUIRE(*(map.findValue(6)) == 7); REQUIRE(map.find(31, &ptr) == - static_cast(FixedMap::KEY_DOES_NOT_EXIST)); + static_cast(containers::KEY_DOES_NOT_EXIST)); } REQUIRE(map.getSerializedSize() == (sizeof(uint32_t) + 30 * (sizeof(uint32_t) + sizeof(uint16_t)))); REQUIRE(map.erase(2) == static_cast(returnvalue::OK)); - REQUIRE(map.erase(31) == static_cast(FixedMap::KEY_DOES_NOT_EXIST)); - REQUIRE(map.exists(2) == static_cast(FixedMap::KEY_DOES_NOT_EXIST)); + REQUIRE(map.erase(31) == static_cast(containers::KEY_DOES_NOT_EXIST)); + REQUIRE(map.exists(2) == static_cast(containers::KEY_DOES_NOT_EXIST)); REQUIRE(map.size() == 29); for (auto element : map) { @@ -80,7 +82,7 @@ TEST_CASE("FixedMap Tests", "[containers]") { REQUIRE(map.find(37)->second == 38); REQUIRE(map.size() == 2); REQUIRE(map.insert(37, 24, nullptr) == - static_cast(FixedMap::KEY_ALREADY_EXISTS)); + static_cast(containers::KEY_ALREADY_EXISTS)); REQUIRE(map.find(37)->second != 24); REQUIRE(map.size() == 2); }; @@ -137,7 +139,7 @@ TEST_CASE("FixedMap Tests", "[containers]") { FixedMap::Iterator it; std::pair pair = std::make_pair(44, 43); it = FixedMap::Iterator(&pair); - REQUIRE(map.erase(&it) == static_cast(FixedMap::KEY_DOES_NOT_EXIST)); + REQUIRE(map.erase(&it) == static_cast(containers::KEY_DOES_NOT_EXIST)); REQUIRE(map.find(45) == map.end()); size_t toLargeMap = 100; const uint8_t* ptr = reinterpret_cast(&toLargeMap); diff --git a/unittests/mocks/AcceptsTmMock.cpp b/unittests/mocks/AcceptsTmMock.cpp index 7b997047..2f718e61 100644 --- a/unittests/mocks/AcceptsTmMock.cpp +++ b/unittests/mocks/AcceptsTmMock.cpp @@ -6,7 +6,7 @@ AcceptsTmMock::AcceptsTmMock(object_id_t registeredId, MessageQueueId_t queueToR AcceptsTmMock::AcceptsTmMock(MessageQueueId_t queueToReturn) : SystemObject(objects::NO_OBJECT, false), returnedQueue(queueToReturn) {} -MessageQueueId_t AcceptsTmMock::getReportReceptionQueue(uint8_t virtualChannel) { +MessageQueueId_t AcceptsTmMock::getReportReceptionQueue(uint8_t virtualChannel) const { return returnedQueue; } diff --git a/unittests/mocks/AcceptsTmMock.h b/unittests/mocks/AcceptsTmMock.h index d6cc7f85..b12e1094 100644 --- a/unittests/mocks/AcceptsTmMock.h +++ b/unittests/mocks/AcceptsTmMock.h @@ -9,7 +9,7 @@ class AcceptsTmMock : public SystemObject, public AcceptsTelemetryIF { AcceptsTmMock(object_id_t registeredId, MessageQueueId_t queueToReturn); explicit AcceptsTmMock(MessageQueueId_t queueToReturn); - MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) override; + MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel) const override; const char* getName() const override; MessageQueueId_t returnedQueue; From 2a203ae13d0b872189c0a79147c92312a44c08b1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 10 Nov 2022 16:15:28 +0100 Subject: [PATCH 252/404] this not crash --- src/fsfw/container/FixedArrayList.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fsfw/container/FixedArrayList.h b/src/fsfw/container/FixedArrayList.h index 78184e35..6ae7da41 100644 --- a/src/fsfw/container/FixedArrayList.h +++ b/src/fsfw/container/FixedArrayList.h @@ -20,11 +20,11 @@ class FixedArrayList : public ArrayList { FixedArrayList() : ArrayList(data, MAX_SIZE) {} FixedArrayList(const FixedArrayList& other) : ArrayList(data, MAX_SIZE) { - for (size_t idx = 0; idx < sizeof(data); idx++) { - data[idx] = other.data[idx]; - } this->entries = data; this->size = other.size; + for (size_t idx = 0; idx < this->size; idx++) { + data[idx] = other.data[idx]; + } } FixedArrayList& operator=(FixedArrayList other) { From 39946bff58db7c5ac9016ca3156abb059560d9cb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 10 Nov 2022 17:31:11 +0100 Subject: [PATCH 253/404] some renaming and tweaks for linux serial driver --- CMakeLists.txt | 1 + src/fsfw_hal/linux/CMakeLists.txt | 11 +++-- src/fsfw_hal/linux/serial/CMakeLists.txt | 2 + .../UartComIF.cpp => serial/SerialComIF.cpp} | 40 +++++++++---------- .../UartComIF.h => serial/SerialComIF.h} | 11 +++-- .../SerialCookie.cpp} | 3 +- .../UartCookie.h => serial/SerialCookie.h} | 3 +- .../linux/{uart => serial}/helper.cpp | 3 +- src/fsfw_hal/linux/{uart => serial}/helper.h | 0 src/fsfw_hal/linux/uart/CMakeLists.txt | 1 - unittests/container/TestFixedMap.cpp | 6 +-- 11 files changed, 40 insertions(+), 41 deletions(-) create mode 100644 src/fsfw_hal/linux/serial/CMakeLists.txt rename src/fsfw_hal/linux/{uart/UartComIF.cpp => serial/SerialComIF.cpp} (89%) rename src/fsfw_hal/linux/{uart/UartComIF.h => serial/SerialComIF.h} (93%) rename src/fsfw_hal/linux/{uart/UartCookie.cpp => serial/SerialCookie.cpp} (97%) rename src/fsfw_hal/linux/{uart/UartCookie.h => serial/SerialCookie.h} (98%) rename src/fsfw_hal/linux/{uart => serial}/helper.cpp (99%) rename src/fsfw_hal/linux/{uart => serial}/helper.h (100%) delete mode 100644 src/fsfw_hal/linux/uart/CMakeLists.txt diff --git a/CMakeLists.txt b/CMakeLists.txt index 0c0d2bb4..9025537a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -122,6 +122,7 @@ if(UNIX) option(FSFW_HAL_LINUX_ADD_PERIPHERAL_DRIVERS "Add Linux peripheral drivers" OFF) option(FSFW_HAL_LINUX_ADD_LIBGPIOD "Attempt to add Linux GPIOD drivers" OFF) + option(FSFW_HAL_LINUX_ADD_SERIAL_DRIVERS "Add serial drivers" ON) endif() # Optional sources diff --git a/src/fsfw_hal/linux/CMakeLists.txt b/src/fsfw_hal/linux/CMakeLists.txt index ffa5f5ee..d22d7ff1 100644 --- a/src/fsfw_hal/linux/CMakeLists.txt +++ b/src/fsfw_hal/linux/CMakeLists.txt @@ -5,11 +5,14 @@ endif() target_sources(${LIB_FSFW_NAME} PRIVATE UnixFileGuard.cpp CommandExecutor.cpp utility.cpp) +if(FSFW_HAL_LINUX_ADD_LIBGPIOD) + add_subdirectory(gpio) +endif() +if(FSFW_HAL_LINUX_ADD_SERIAL_DRIVERS) + add_subdirectory(serial) +endif() + if(FSFW_HAL_LINUX_ADD_PERIPHERAL_DRIVERS) - if(FSFW_HAL_LINUX_ADD_LIBGPIOD) - add_subdirectory(gpio) - endif() - add_subdirectory(uart) # Adding those does not really make sense on Apple systems which are generally # host systems. It won't even compile as the headers are missing if(NOT APPLE) diff --git a/src/fsfw_hal/linux/serial/CMakeLists.txt b/src/fsfw_hal/linux/serial/CMakeLists.txt new file mode 100644 index 00000000..07d20d29 --- /dev/null +++ b/src/fsfw_hal/linux/serial/CMakeLists.txt @@ -0,0 +1,2 @@ +target_sources(${LIB_FSFW_NAME} PUBLIC SerialComIF.cpp SerialCookie.cpp + helper.cpp) diff --git a/src/fsfw_hal/linux/uart/UartComIF.cpp b/src/fsfw_hal/linux/serial/SerialComIF.cpp similarity index 89% rename from src/fsfw_hal/linux/uart/UartComIF.cpp rename to src/fsfw_hal/linux/serial/SerialComIF.cpp index a65adb33..0fd293a0 100644 --- a/src/fsfw_hal/linux/uart/UartComIF.cpp +++ b/src/fsfw_hal/linux/serial/SerialComIF.cpp @@ -1,7 +1,6 @@ -#include "UartComIF.h" - #include #include +#include #include #include @@ -11,11 +10,11 @@ #include "fsfw/serviceinterface.h" #include "fsfw_hal/linux/utility.h" -UartComIF::UartComIF(object_id_t objectId) : SystemObject(objectId) {} +SerialComIF::SerialComIF(object_id_t objectId) : SystemObject(objectId) {} -UartComIF::~UartComIF() {} +SerialComIF::~SerialComIF() {} -ReturnValue_t UartComIF::initializeInterface(CookieIF* cookie) { +ReturnValue_t SerialComIF::initializeInterface(CookieIF* cookie) { std::string deviceFile; if (cookie == nullptr) { @@ -59,7 +58,7 @@ ReturnValue_t UartComIF::initializeInterface(CookieIF* cookie) { return returnvalue::OK; } -int UartComIF::configureUartPort(UartCookie* uartCookie) { +int SerialComIF::configureUartPort(UartCookie* uartCookie) { struct termios options = {}; std::string deviceFile = uartCookie->getDeviceFile(); @@ -114,7 +113,7 @@ int UartComIF::configureUartPort(UartCookie* uartCookie) { return fd; } -void UartComIF::setStopBitOptions(struct termios* options, UartCookie* uartCookie) { +void SerialComIF::setStopBitOptions(struct termios* options, UartCookie* uartCookie) { /* Clear stop field. Sets stop bit to one bit */ options->c_cflag &= ~CSTOPB; switch (uartCookie->getStopBits()) { @@ -126,7 +125,7 @@ void UartComIF::setStopBitOptions(struct termios* options, UartCookie* uartCooki } } -void UartComIF::setDatasizeOptions(struct termios* options, UartCookie* uartCookie) { +void SerialComIF::setDatasizeOptions(struct termios* options, UartCookie* uartCookie) { /* Clear size bits */ options->c_cflag &= ~CSIZE; switch (uartCookie->getBitsPerWord()) { @@ -150,7 +149,7 @@ void UartComIF::setDatasizeOptions(struct termios* options, UartCookie* uartCook } } -void UartComIF::setFixedOptions(struct termios* options) { +void SerialComIF::setFixedOptions(struct termios* options) { /* Disable RTS/CTS hardware flow control */ options->c_cflag &= ~CRTSCTS; /* Turn on READ & ignore ctrl lines (CLOCAL = 1) */ @@ -173,7 +172,7 @@ void UartComIF::setFixedOptions(struct termios* options) { options->c_oflag &= ~ONLCR; } -ReturnValue_t UartComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) { +ReturnValue_t SerialComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) { int fd = 0; std::string deviceFile; @@ -219,9 +218,9 @@ ReturnValue_t UartComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, return returnvalue::OK; } -ReturnValue_t UartComIF::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; } +ReturnValue_t SerialComIF::getSendSuccess(CookieIF* cookie) { return returnvalue::OK; } -ReturnValue_t UartComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLen) { +ReturnValue_t SerialComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLen) { std::string deviceFile; UartCookie* uartCookie = dynamic_cast(cookie); @@ -257,8 +256,8 @@ ReturnValue_t UartComIF::requestReceiveMessage(CookieIF* cookie, size_t requestL } } -ReturnValue_t UartComIF::handleCanonicalRead(UartCookie& uartCookie, UartDeviceMap::iterator& iter, - size_t requestLen) { +ReturnValue_t SerialComIF::handleCanonicalRead(UartCookie& uartCookie, + UartDeviceMap::iterator& iter, size_t requestLen) { ReturnValue_t result = returnvalue::OK; uint8_t maxReadCycles = uartCookie.getReadCycles(); uint8_t currentReadCycles = 0; @@ -315,8 +314,9 @@ ReturnValue_t UartComIF::handleCanonicalRead(UartCookie& uartCookie, UartDeviceM return result; } -ReturnValue_t UartComIF::handleNoncanonicalRead(UartCookie& uartCookie, - UartDeviceMap::iterator& iter, size_t requestLen) { +ReturnValue_t SerialComIF::handleNoncanonicalRead(UartCookie& uartCookie, + UartDeviceMap::iterator& iter, + size_t requestLen) { int fd = iter->second.fileDescriptor; auto bufferPtr = iter->second.replyBuffer.data(); // Size check to prevent buffer overflow @@ -349,7 +349,7 @@ ReturnValue_t UartComIF::handleNoncanonicalRead(UartCookie& uartCookie, return returnvalue::OK; } -ReturnValue_t UartComIF::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) { +ReturnValue_t SerialComIF::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, size_t* size) { std::string deviceFile; UartCookie* uartCookie = dynamic_cast(cookie); @@ -379,7 +379,7 @@ ReturnValue_t UartComIF::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, return returnvalue::OK; } -ReturnValue_t UartComIF::flushUartRxBuffer(CookieIF* cookie) { +ReturnValue_t SerialComIF::flushUartRxBuffer(CookieIF* cookie) { std::string deviceFile; UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { @@ -398,7 +398,7 @@ ReturnValue_t UartComIF::flushUartRxBuffer(CookieIF* cookie) { return returnvalue::FAILED; } -ReturnValue_t UartComIF::flushUartTxBuffer(CookieIF* cookie) { +ReturnValue_t SerialComIF::flushUartTxBuffer(CookieIF* cookie) { std::string deviceFile; UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { @@ -417,7 +417,7 @@ ReturnValue_t UartComIF::flushUartTxBuffer(CookieIF* cookie) { return returnvalue::FAILED; } -ReturnValue_t UartComIF::flushUartTxAndRxBuf(CookieIF* cookie) { +ReturnValue_t SerialComIF::flushUartTxAndRxBuf(CookieIF* cookie) { std::string deviceFile; UartCookie* uartCookie = dynamic_cast(cookie); if (uartCookie == nullptr) { diff --git a/src/fsfw_hal/linux/uart/UartComIF.h b/src/fsfw_hal/linux/serial/SerialComIF.h similarity index 93% rename from src/fsfw_hal/linux/uart/UartComIF.h rename to src/fsfw_hal/linux/serial/SerialComIF.h index fc92921f..4953884a 100644 --- a/src/fsfw_hal/linux/uart/UartComIF.h +++ b/src/fsfw_hal/linux/serial/SerialComIF.h @@ -3,13 +3,12 @@ #include #include +#include +#include #include #include -#include "UartCookie.h" -#include "helper.h" - /** * @brief This is the communication interface to access serial ports on linux based operating * systems. @@ -19,7 +18,7 @@ * * @author J. Meier */ -class UartComIF : public DeviceCommunicationIF, public SystemObject { +class SerialComIF : public DeviceCommunicationIF, public SystemObject { public: static constexpr uint8_t uartRetvalId = CLASS_ID::HAL_UART; @@ -27,9 +26,9 @@ class UartComIF : public DeviceCommunicationIF, public SystemObject { static constexpr ReturnValue_t UART_READ_SIZE_MISSMATCH = returnvalue::makeCode(uartRetvalId, 2); static constexpr ReturnValue_t UART_RX_BUFFER_TOO_SMALL = returnvalue::makeCode(uartRetvalId, 3); - UartComIF(object_id_t objectId); + SerialComIF(object_id_t objectId); - virtual ~UartComIF(); + virtual ~SerialComIF(); ReturnValue_t initializeInterface(CookieIF* cookie) override; ReturnValue_t sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) override; diff --git a/src/fsfw_hal/linux/uart/UartCookie.cpp b/src/fsfw_hal/linux/serial/SerialCookie.cpp similarity index 97% rename from src/fsfw_hal/linux/uart/UartCookie.cpp rename to src/fsfw_hal/linux/serial/SerialCookie.cpp index 31dbc903..733d84a5 100644 --- a/src/fsfw_hal/linux/uart/UartCookie.cpp +++ b/src/fsfw_hal/linux/serial/SerialCookie.cpp @@ -1,6 +1,5 @@ -#include "UartCookie.h" - #include +#include UartCookie::UartCookie(object_id_t handlerId, std::string deviceFile, UartBaudRate baudrate, size_t maxReplyLen, UartModes uartMode) diff --git a/src/fsfw_hal/linux/uart/UartCookie.h b/src/fsfw_hal/linux/serial/SerialCookie.h similarity index 98% rename from src/fsfw_hal/linux/uart/UartCookie.h rename to src/fsfw_hal/linux/serial/SerialCookie.h index 670ef72f..8d0f5fb4 100644 --- a/src/fsfw_hal/linux/uart/UartCookie.h +++ b/src/fsfw_hal/linux/serial/SerialCookie.h @@ -3,11 +3,10 @@ #include #include +#include #include -#include "helper.h" - /** * @brief Cookie for the UartComIF. There are many options available to configure the UART driver. * The constructor only requests for common options like the baudrate. Other options can diff --git a/src/fsfw_hal/linux/uart/helper.cpp b/src/fsfw_hal/linux/serial/helper.cpp similarity index 99% rename from src/fsfw_hal/linux/uart/helper.cpp rename to src/fsfw_hal/linux/serial/helper.cpp index f38dd3d3..ba975f2f 100644 --- a/src/fsfw_hal/linux/uart/helper.cpp +++ b/src/fsfw_hal/linux/serial/helper.cpp @@ -1,5 +1,4 @@ -#include "helper.h" - +#include #include #include "fsfw/serviceinterface.h" diff --git a/src/fsfw_hal/linux/uart/helper.h b/src/fsfw_hal/linux/serial/helper.h similarity index 100% rename from src/fsfw_hal/linux/uart/helper.h rename to src/fsfw_hal/linux/serial/helper.h diff --git a/src/fsfw_hal/linux/uart/CMakeLists.txt b/src/fsfw_hal/linux/uart/CMakeLists.txt deleted file mode 100644 index d8daa9ff..00000000 --- a/src/fsfw_hal/linux/uart/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -target_sources(${LIB_FSFW_NAME} PUBLIC UartComIF.cpp UartCookie.cpp helper.cpp) diff --git a/unittests/container/TestFixedMap.cpp b/unittests/container/TestFixedMap.cpp index 231edb4f..83ff975d 100644 --- a/unittests/container/TestFixedMap.cpp +++ b/unittests/container/TestFixedMap.cpp @@ -36,8 +36,7 @@ TEST_CASE("FixedMap Tests", "[containers]") { REQUIRE(map.find(5, &ptr) == static_cast(returnvalue::OK)); REQUIRE(*ptr == 6); REQUIRE(*(map.findValue(6)) == 7); - REQUIRE(map.find(31, &ptr) == - static_cast(containers::KEY_DOES_NOT_EXIST)); + REQUIRE(map.find(31, &ptr) == static_cast(containers::KEY_DOES_NOT_EXIST)); } REQUIRE(map.getSerializedSize() == @@ -81,8 +80,7 @@ TEST_CASE("FixedMap Tests", "[containers]") { REQUIRE(map.insert(37, 38, nullptr) == static_cast(returnvalue::OK)); REQUIRE(map.find(37)->second == 38); REQUIRE(map.size() == 2); - REQUIRE(map.insert(37, 24, nullptr) == - static_cast(containers::KEY_ALREADY_EXISTS)); + REQUIRE(map.insert(37, 24, nullptr) == static_cast(containers::KEY_ALREADY_EXISTS)); REQUIRE(map.find(37)->second != 24); REQUIRE(map.size() == 2); }; From 0303c1a8853f423dc9ff2ba12e9e56351aa00961 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 11 Nov 2022 11:31:22 +0100 Subject: [PATCH 254/404] remove file --- src/fsfw_hal/linux/uart/helper.cpp | 150 ----------------------------- 1 file changed, 150 deletions(-) delete mode 100644 src/fsfw_hal/linux/uart/helper.cpp diff --git a/src/fsfw_hal/linux/uart/helper.cpp b/src/fsfw_hal/linux/uart/helper.cpp deleted file mode 100644 index df4b7aa8..00000000 --- a/src/fsfw_hal/linux/uart/helper.cpp +++ /dev/null @@ -1,150 +0,0 @@ -#include "helper.h" - -#include - -#include "fsfw/serviceinterface.h" - -void uart::setMode(struct termios& options, UartModes mode) { - if (mode == UartModes::NON_CANONICAL) { - /* Disable canonical mode */ - options.c_lflag &= ~ICANON; - } else if (mode == UartModes::CANONICAL) { - options.c_lflag |= ICANON; - } -} - -void uart::setBaudrate(struct termios& options, UartBaudRate baud) { - switch (baud) { - case UartBaudRate::RATE_50: - cfsetispeed(&options, B50); - cfsetospeed(&options, B50); - break; - case UartBaudRate::RATE_75: - cfsetispeed(&options, B75); - cfsetospeed(&options, B75); - break; - case UartBaudRate::RATE_110: - cfsetispeed(&options, B110); - cfsetospeed(&options, B110); - break; - case UartBaudRate::RATE_134: - cfsetispeed(&options, B134); - cfsetospeed(&options, B134); - break; - case UartBaudRate::RATE_150: - cfsetispeed(&options, B150); - cfsetospeed(&options, B150); - break; - case UartBaudRate::RATE_200: - cfsetispeed(&options, B200); - cfsetospeed(&options, B200); - break; - case UartBaudRate::RATE_300: - cfsetispeed(&options, B300); - cfsetospeed(&options, B300); - break; - case UartBaudRate::RATE_600: - cfsetispeed(&options, B600); - cfsetospeed(&options, B600); - break; - case UartBaudRate::RATE_1200: - cfsetispeed(&options, B1200); - cfsetospeed(&options, B1200); - break; - case UartBaudRate::RATE_1800: - cfsetispeed(&options, B1800); - cfsetospeed(&options, B1800); - break; - case UartBaudRate::RATE_2400: - cfsetispeed(&options, B2400); - cfsetospeed(&options, B2400); - break; - case UartBaudRate::RATE_4800: - cfsetispeed(&options, B4800); - cfsetospeed(&options, B4800); - break; - case UartBaudRate::RATE_9600: - cfsetispeed(&options, B9600); - cfsetospeed(&options, B9600); - break; - case UartBaudRate::RATE_19200: - cfsetispeed(&options, B19200); - cfsetospeed(&options, B19200); - break; - case UartBaudRate::RATE_38400: - cfsetispeed(&options, B38400); - cfsetospeed(&options, B38400); - break; - case UartBaudRate::RATE_57600: - cfsetispeed(&options, B57600); - cfsetospeed(&options, B57600); - break; - case UartBaudRate::RATE_115200: - cfsetispeed(&options, B115200); - cfsetospeed(&options, B115200); - break; - case UartBaudRate::RATE_230400: - cfsetispeed(&options, B230400); - cfsetospeed(&options, B230400); - break; -#ifndef __APPLE__ - case UartBaudRate::RATE_460800: - cfsetispeed(&options, B460800); - cfsetospeed(&options, B460800); - break; - case UartBaudRate::RATE_500000: - cfsetispeed(&options, B500000); - cfsetospeed(&options, B500000); - break; - case UartBaudRate::RATE_576000: - cfsetispeed(&options, B576000); - cfsetospeed(&options, B576000); - break; - case UartBaudRate::RATE_921600: - cfsetispeed(&options, B921600); - cfsetospeed(&options, B921600); - break; - case UartBaudRate::RATE_1000000: - cfsetispeed(&options, B1000000); - cfsetospeed(&options, B1000000); - break; - case UartBaudRate::RATE_1152000: - cfsetispeed(&options, B1152000); - cfsetospeed(&options, B1152000); - break; - case UartBaudRate::RATE_1500000: - cfsetispeed(&options, B1500000); - cfsetospeed(&options, B1500000); - break; - case UartBaudRate::RATE_2000000: - cfsetispeed(&options, B2000000); - cfsetospeed(&options, B2000000); - break; - case UartBaudRate::RATE_2500000: - cfsetispeed(&options, B2500000); - cfsetospeed(&options, B2500000); - break; - case UartBaudRate::RATE_3000000: - cfsetispeed(&options, B3000000); - cfsetospeed(&options, B3000000); - break; - case UartBaudRate::RATE_3500000: - cfsetispeed(&options, B3500000); - cfsetospeed(&options, B3500000); - break; - case UartBaudRate::RATE_4000000: - cfsetispeed(&options, B4000000); - cfsetospeed(&options, B4000000); - break; -#endif // ! __APPLE__ - default: -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "UartComIF::configureBaudrate: Baudrate not supported" << std::endl; -#endif - break; - } -} - -int uart::readCountersAndErrors(int serialPort, serial_icounter_struct& icounter) { - return ioctl(serialPort, TIOCGICOUNT, &icounter); -} From 61fd5d1b62fe3149b193c6c4175cb9b57cbe67f1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 11 Nov 2022 14:12:42 +0100 Subject: [PATCH 255/404] impl function to generate ASCII timestamp sec accuracy --- src/fsfw/timemanager/CCSDSTime.cpp | 15 +++++++++++++-- src/fsfw/timemanager/CCSDSTime.h | 2 +- 2 files changed, 14 insertions(+), 3 deletions(-) diff --git a/src/fsfw/timemanager/CCSDSTime.cpp b/src/fsfw/timemanager/CCSDSTime.cpp index cf4dbe74..535b18a2 100644 --- a/src/fsfw/timemanager/CCSDSTime.cpp +++ b/src/fsfw/timemanager/CCSDSTime.cpp @@ -246,8 +246,19 @@ ReturnValue_t CCSDSTime::convertFromASCII(Clock::TimeOfDay_t* to, const uint8_t* return UNSUPPORTED_TIME_FORMAT; } -ReturnValue_t CCSDSTime::convertToASCII(int8_t* to, const Clock::TimeOfDay_t* from, - uint8_t length) { +ReturnValue_t CCSDSTime::convertToAsciiWithSecs(int8_t* to, const Clock::TimeOfDay_t* from, + uint8_t length) { + if(from == nullptr or to == nullptr) { + return returnvalue::FAILED; + } + int count = snprintf(reinterpret_cast(to), length, + "%4" SCNu32 "-%2" SCNu32 "-%2" SCNu32 + "T%" + "2" SCNu32 ":%2" SCNu32 ":%2" SCNu32, + from->year, from->month, from->day, from->hour, from->minute, from->second); + if(count >= length) { + return returnvalue::FAILED; + } return returnvalue::OK; } diff --git a/src/fsfw/timemanager/CCSDSTime.h b/src/fsfw/timemanager/CCSDSTime.h index 1ba94e2c..0d4b6e61 100644 --- a/src/fsfw/timemanager/CCSDSTime.h +++ b/src/fsfw/timemanager/CCSDSTime.h @@ -207,7 +207,7 @@ class CCSDSTime { static ReturnValue_t convertFromASCII(Clock::TimeOfDay_t *to, uint8_t const *from, uint8_t length); - static ReturnValue_t convertToASCII(int8_t *to, const Clock::TimeOfDay_t *from, uint8_t length); + static ReturnValue_t convertToAsciiWithSecs(int8_t *to, const Clock::TimeOfDay_t *from, uint8_t length); static uint32_t subsecondsToMicroseconds(uint16_t subseconds); private: From 2b6a33e718f896300ca08345a9211c2ae974d014 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 11 Nov 2022 14:13:46 +0100 Subject: [PATCH 256/404] afmt --- src/fsfw/timemanager/CCSDSTime.cpp | 12 +++++------- src/fsfw/timemanager/CCSDSTime.h | 3 ++- src/fsfw_hal/linux/serial/SerialCookie.cpp | 2 +- src/fsfw_hal/linux/serial/SerialCookie.h | 2 +- 4 files changed, 9 insertions(+), 10 deletions(-) diff --git a/src/fsfw/timemanager/CCSDSTime.cpp b/src/fsfw/timemanager/CCSDSTime.cpp index 535b18a2..7ca1f7f1 100644 --- a/src/fsfw/timemanager/CCSDSTime.cpp +++ b/src/fsfw/timemanager/CCSDSTime.cpp @@ -247,16 +247,14 @@ ReturnValue_t CCSDSTime::convertFromASCII(Clock::TimeOfDay_t* to, const uint8_t* } ReturnValue_t CCSDSTime::convertToAsciiWithSecs(int8_t* to, const Clock::TimeOfDay_t* from, - uint8_t length) { - if(from == nullptr or to == nullptr) { + uint8_t length) { + if (from == nullptr or to == nullptr) { return returnvalue::FAILED; } int count = snprintf(reinterpret_cast(to), length, - "%4" SCNu32 "-%2" SCNu32 "-%2" SCNu32 - "T%" - "2" SCNu32 ":%2" SCNu32 ":%2" SCNu32, - from->year, from->month, from->day, from->hour, from->minute, from->second); - if(count >= length) { + "%4" SCNu32 "-%2" SCNu32 "-%2" SCNu32 "T%2" SCNu32 ":%2" SCNu32 ":%2" SCNu32, + from->year, from->month, from->day, from->hour, from->minute, from->second); + if (count >= length) { return returnvalue::FAILED; } return returnvalue::OK; diff --git a/src/fsfw/timemanager/CCSDSTime.h b/src/fsfw/timemanager/CCSDSTime.h index 0d4b6e61..03a8ea8f 100644 --- a/src/fsfw/timemanager/CCSDSTime.h +++ b/src/fsfw/timemanager/CCSDSTime.h @@ -207,7 +207,8 @@ class CCSDSTime { static ReturnValue_t convertFromASCII(Clock::TimeOfDay_t *to, uint8_t const *from, uint8_t length); - static ReturnValue_t convertToAsciiWithSecs(int8_t *to, const Clock::TimeOfDay_t *from, uint8_t length); + static ReturnValue_t convertToAsciiWithSecs(int8_t *to, const Clock::TimeOfDay_t *from, + uint8_t length); static uint32_t subsecondsToMicroseconds(uint16_t subseconds); private: diff --git a/src/fsfw_hal/linux/serial/SerialCookie.cpp b/src/fsfw_hal/linux/serial/SerialCookie.cpp index e85d339d..1b7e9b51 100644 --- a/src/fsfw_hal/linux/serial/SerialCookie.cpp +++ b/src/fsfw_hal/linux/serial/SerialCookie.cpp @@ -3,7 +3,7 @@ #include SerialCookie::SerialCookie(object_id_t handlerId, std::string deviceFile, UartBaudRate baudrate, - size_t maxReplyLen, UartModes uartMode) + size_t maxReplyLen, UartModes uartMode) : handlerId(handlerId), deviceFile(deviceFile), uartMode(uartMode), diff --git a/src/fsfw_hal/linux/serial/SerialCookie.h b/src/fsfw_hal/linux/serial/SerialCookie.h index 3916e4ca..7a9c0eca 100644 --- a/src/fsfw_hal/linux/serial/SerialCookie.h +++ b/src/fsfw_hal/linux/serial/SerialCookie.h @@ -30,7 +30,7 @@ class SerialCookie : public CookieIF { * One stop bit */ SerialCookie(object_id_t handlerId, std::string deviceFile, UartBaudRate baudrate, - size_t maxReplyLen, UartModes uartMode = UartModes::NON_CANONICAL); + size_t maxReplyLen, UartModes uartMode = UartModes::NON_CANONICAL); virtual ~SerialCookie(); From 1f58ba1f9b26ae974f671501e62f267af4e6bc88 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 09:58:15 +0100 Subject: [PATCH 257/404] update changelog --- CHANGELOG.md | 8 -------- 1 file changed, 8 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4e8f3154..1dcc6aa2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -24,14 +24,6 @@ and this project adheres to [Semantic Versioning](http://semver.org/). ## Added -- DHB TM handler `handleDeviceTM` renamed to `handleDeviceTm` and now takes - `util::DataWrapper` as the data input argument. This allows more flexibility in the possible - types of telemetry. - PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/669 -- Add `util::DataWrapper` class inside the `util` module. This is a tagged union which allows - to specify raw data either as a classic C-style raw pointer and size or as a `SerializeIF` - pointer. - PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/668 - Add new `UnsignedByteField` class PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/660 From 686dc9723453bd0dd7e36057d0d2d21fb1fcfb1b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 10:52:47 +0100 Subject: [PATCH 258/404] this is better / more correct --- src/fsfw/tmtcservices/TmTcBridge.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/fsfw/tmtcservices/TmTcBridge.cpp b/src/fsfw/tmtcservices/TmTcBridge.cpp index f89cac79..9ff58766 100644 --- a/src/fsfw/tmtcservices/TmTcBridge.cpp +++ b/src/fsfw/tmtcservices/TmTcBridge.cpp @@ -183,7 +183,7 @@ ReturnValue_t TmTcBridge::storeDownlinkData(TmTcMessage* message) { "TmTcBridge::storeDownlinkData: TM downlink max. number " "of stored packet IDs reached!\n"); #endif - warningSwitch = true; + warningSwitch = false; } if (overwriteOld) { tmFifo->retrieve(&storeId); @@ -226,6 +226,7 @@ ReturnValue_t TmTcBridge::handleStoredTm() { packetSentCounter++; if (tmFifo->empty()) { + warningSwitch = true; tmStored = false; } tmStore->deleteData(storeId); From 5e5eb82830187feb9e73e1898a00c7be54ae830a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 10:53:14 +0100 Subject: [PATCH 259/404] make warning switch protected --- src/fsfw/tmtcservices/TmTcBridge.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/fsfw/tmtcservices/TmTcBridge.h b/src/fsfw/tmtcservices/TmTcBridge.h index f3f28ef6..e5c60bc6 100644 --- a/src/fsfw/tmtcservices/TmTcBridge.h +++ b/src/fsfw/tmtcservices/TmTcBridge.h @@ -72,8 +72,6 @@ class TmTcBridge : public AcceptsTelemetryIF, MessageQueueId_t getRequestQueue() const override; const char* getName() const override; - bool warningSwitch = true; - protected: const char* name = ""; @@ -93,6 +91,7 @@ class TmTcBridge : public AcceptsTelemetryIF, //! by default, so telemetry will be handled immediately. bool communicationLinkUp = true; bool tmStored = false; + bool warningSwitch = true; bool overwriteOld = true; uint8_t packetSentCounter = 0; From 99927b8e956b10ea843a9a91c997cdd379790bcd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 10:53:38 +0100 Subject: [PATCH 260/404] afmt --- src/fsfw/storagemanager/StorageManagerIF.h | 3 ++- src/fsfw_hal/linux/serial/SerialCookie.cpp | 2 +- src/fsfw_hal/linux/serial/SerialCookie.h | 2 +- unittests/hal/CMakeLists.txt | 4 +--- 4 files changed, 5 insertions(+), 6 deletions(-) diff --git a/src/fsfw/storagemanager/StorageManagerIF.h b/src/fsfw/storagemanager/StorageManagerIF.h index f9fb33f5..e48e19db 100644 --- a/src/fsfw/storagemanager/StorageManagerIF.h +++ b/src/fsfw/storagemanager/StorageManagerIF.h @@ -181,7 +181,8 @@ class StorageManagerIF { * @return Returns @returnvalue::OK if data was added. * @returnvalue::FAILED if data could not be added, storageId is unchanged then. */ - virtual ReturnValue_t getFreeElement(store_address_t* storageId, size_t size, uint8_t** dataPtr) = 0; + virtual ReturnValue_t getFreeElement(store_address_t* storageId, size_t size, + uint8_t** dataPtr) = 0; [[nodiscard]] virtual bool hasDataAtId(store_address_t storeId) const = 0; diff --git a/src/fsfw_hal/linux/serial/SerialCookie.cpp b/src/fsfw_hal/linux/serial/SerialCookie.cpp index e85d339d..1b7e9b51 100644 --- a/src/fsfw_hal/linux/serial/SerialCookie.cpp +++ b/src/fsfw_hal/linux/serial/SerialCookie.cpp @@ -3,7 +3,7 @@ #include SerialCookie::SerialCookie(object_id_t handlerId, std::string deviceFile, UartBaudRate baudrate, - size_t maxReplyLen, UartModes uartMode) + size_t maxReplyLen, UartModes uartMode) : handlerId(handlerId), deviceFile(deviceFile), uartMode(uartMode), diff --git a/src/fsfw_hal/linux/serial/SerialCookie.h b/src/fsfw_hal/linux/serial/SerialCookie.h index 3916e4ca..7a9c0eca 100644 --- a/src/fsfw_hal/linux/serial/SerialCookie.h +++ b/src/fsfw_hal/linux/serial/SerialCookie.h @@ -30,7 +30,7 @@ class SerialCookie : public CookieIF { * One stop bit */ SerialCookie(object_id_t handlerId, std::string deviceFile, UartBaudRate baudrate, - size_t maxReplyLen, UartModes uartMode = UartModes::NON_CANONICAL); + size_t maxReplyLen, UartModes uartMode = UartModes::NON_CANONICAL); virtual ~SerialCookie(); diff --git a/unittests/hal/CMakeLists.txt b/unittests/hal/CMakeLists.txt index 2a6c012b..76aabd51 100644 --- a/unittests/hal/CMakeLists.txt +++ b/unittests/hal/CMakeLists.txt @@ -1,7 +1,5 @@ target_sources(${FSFW_TEST_TGT} PRIVATE testHostFilesystem.cpp testFsMock.cpp) if(UNIX) - target_sources(${FSFW_TEST_TGT} PRIVATE - testCommandExecutor.cpp - ) + target_sources(${FSFW_TEST_TGT} PRIVATE testCommandExecutor.cpp) endif() From be4a87535d5f2f88dabdad9a9d7fcae4fd55fe20 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 10:54:11 +0100 Subject: [PATCH 261/404] remove data wrapper --- src/fsfw/util/dataWrapper.h | 72 ------------------------------------- 1 file changed, 72 deletions(-) delete mode 100644 src/fsfw/util/dataWrapper.h diff --git a/src/fsfw/util/dataWrapper.h b/src/fsfw/util/dataWrapper.h deleted file mode 100644 index f2620556..00000000 --- a/src/fsfw/util/dataWrapper.h +++ /dev/null @@ -1,72 +0,0 @@ -#ifndef FSFW_UTIL_DATAWRAPPER_H -#define FSFW_UTIL_DATAWRAPPER_H - -#include -#include -#include - -#include "fsfw/serialize.h" - -namespace util { - -using BufPair = std::pair; - -struct RawData { - RawData() = default; - const uint8_t* data = nullptr; - size_t len = 0; -}; - -enum DataTypes { NONE, RAW, SERIALIZABLE }; - -union DataUnion { - RawData raw{}; - SerializeIF* serializable; -}; - -struct DataWrapper { - DataWrapper() = default; - - DataWrapper(const uint8_t* data, size_t size) : type(DataTypes::RAW) { setRawData({data, size}); } - - explicit DataWrapper(BufPair raw) : type(DataTypes::RAW) { setRawData(raw); } - - explicit DataWrapper(SerializeIF& serializable) : type(DataTypes::SERIALIZABLE) { - setSerializable(serializable); - } - - DataTypes type = DataTypes::NONE; - DataUnion dataUnion; - - [[nodiscard]] size_t getLength() const { - if (type == DataTypes::RAW) { - return dataUnion.raw.len; - } else if (type == DataTypes::SERIALIZABLE and dataUnion.serializable != nullptr) { - return dataUnion.serializable->getSerializedSize(); - } - return 0; - } - - [[nodiscard]] bool isNull() const { - if ((type == DataTypes::NONE) or (type == DataTypes::RAW and dataUnion.raw.data == nullptr) or - (type == DataTypes::SERIALIZABLE and dataUnion.serializable == nullptr)) { - return true; - } - return false; - } - - void setRawData(BufPair bufPair) { - type = DataTypes::RAW; - dataUnion.raw.data = bufPair.first; - dataUnion.raw.len = bufPair.second; - } - - void setSerializable(SerializeIF& serializable) { - type = DataTypes::SERIALIZABLE; - dataUnion.serializable = &serializable; - } -}; - -} // namespace util - -#endif // FSFW_UTIL_DATAWRAPPER_H From c5f91926c95a8db0c7921176aa3f62cc2bebef47 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Nov 2022 10:54:47 +0100 Subject: [PATCH 262/404] remove includes --- src/fsfw/devicehandlers/DeviceHandlerBase.h | 1 - src/fsfw/devicehandlers/DeviceTmReportingWrapper.h | 1 - 2 files changed, 2 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index ea7b2eee..b06815d1 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -24,7 +24,6 @@ #include "fsfw/subsystem/ModeTreeConnectionIF.h" #include "fsfw/tasks/ExecutableObjectIF.h" #include "fsfw/tasks/PeriodicTaskIF.h" -#include "fsfw/util/dataWrapper.h" namespace Factory { void setStaticFrameworkObjectIds(); diff --git a/src/fsfw/devicehandlers/DeviceTmReportingWrapper.h b/src/fsfw/devicehandlers/DeviceTmReportingWrapper.h index 53f4b283..344cef01 100644 --- a/src/fsfw/devicehandlers/DeviceTmReportingWrapper.h +++ b/src/fsfw/devicehandlers/DeviceTmReportingWrapper.h @@ -4,7 +4,6 @@ #include "fsfw/action/HasActionsIF.h" #include "fsfw/objectmanager/SystemObjectIF.h" #include "fsfw/serialize/SerializeIF.h" -#include "fsfw/util/dataWrapper.h" class DeviceTmReportingWrapper : public SerializeIF { public: From 160ff799ace61e24708dcf1fdeaf5fafdf23a4ca Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 17 Nov 2022 15:09:08 +0100 Subject: [PATCH 263/404] small fix to allow teardown handling --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 559e4cfd..67cd6a8b 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -361,6 +361,8 @@ void DeviceHandlerBase::doStateMachine() { if ((switchState == PowerSwitchIF::SWITCH_ON) || (switchState == NO_SWITCH)) { // NOTE: TransitionSourceMode and -SubMode are set by handleCommandedModeTransition childTransitionFailure = CHILD_TIMEOUT; + transitionSourceMode = _MODE_SHUT_DOWN; + transitionSourceSubMode = SUBMODE_NONE; setMode(_MODE_START_UP); callChildStatemachine(); } From 46a1c2bacea142994666b2201acf0246ba0fd0b4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Nov 2022 08:35:10 +0100 Subject: [PATCH 264/404] remove data wrapper --- unittests/util/testDataWrapper.cpp | 55 ------------------------------ 1 file changed, 55 deletions(-) delete mode 100644 unittests/util/testDataWrapper.cpp diff --git a/unittests/util/testDataWrapper.cpp b/unittests/util/testDataWrapper.cpp deleted file mode 100644 index ef381a62..00000000 --- a/unittests/util/testDataWrapper.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include -#include - -#include "fsfw/util/dataWrapper.h" -#include "mocks/SimpleSerializable.h" - -TEST_CASE("Data Wrapper", "[util]") { - util::DataWrapper wrapper; - SECTION("State") { - REQUIRE(wrapper.isNull()); - REQUIRE(wrapper.type == util::DataTypes::NONE); - } - - SECTION("Set Raw Data") { - util::DataWrapper* instance = &wrapper; - bool deleteInst = false; - REQUIRE(wrapper.isNull()); - std::array data = {1, 2, 3, 4}; - SECTION("Setter") { wrapper.setRawData({data.data(), data.size()}); } - SECTION("Direct Construction Pair") { - instance = new util::DataWrapper(util::BufPair(data.data(), data.size())); - deleteInst = true; - } - SECTION("Direct Construction Single Args") { - instance = new util::DataWrapper(data.data(), data.size()); - deleteInst = true; - } - REQUIRE(not instance->isNull()); - REQUIRE(instance->type == util::DataTypes::RAW); - REQUIRE(instance->dataUnion.raw.data == data.data()); - REQUIRE(instance->dataUnion.raw.len == data.size()); - if (deleteInst) { - delete instance; - } - } - - SECTION("Simple Serializable") { - util::DataWrapper* instance = &wrapper; - bool deleteInst = false; - REQUIRE(instance->isNull()); - SimpleSerializable serializable; - SECTION("Setter") { wrapper.setSerializable(serializable); } - SECTION("Direct Construction") { - instance = new util::DataWrapper(serializable); - deleteInst = true; - } - - REQUIRE(not instance->isNull()); - REQUIRE(instance->type == util::DataTypes::SERIALIZABLE); - REQUIRE(instance->dataUnion.serializable == &serializable); - if (deleteInst) { - delete instance; - } - } -} \ No newline at end of file From 8eb869e073d99e257c6cc62dbf26b4d99422e9b8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 28 Nov 2022 11:21:36 +0100 Subject: [PATCH 265/404] run black --- docs/conf.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/docs/conf.py b/docs/conf.py index a4232026..dad85e61 100644 --- a/docs/conf.py +++ b/docs/conf.py @@ -51,7 +51,10 @@ exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"] html_theme = "alabaster" html_theme_options = { - "extra_nav_links": {"Impressum" : "https://www.uni-stuttgart.de/impressum", "Datenschutz": "https://info.irs.uni-stuttgart.de/datenschutz/datenschutzWebmit.html"} + "extra_nav_links": { + "Impressum": "https://www.uni-stuttgart.de/impressum", + "Datenschutz": "https://info.irs.uni-stuttgart.de/datenschutz/datenschutzWebmit.html", + } } From 5b0ea91222a6b8efb2f4562cfecbcb735dfeedd5 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 29 Nov 2022 23:24:29 +0100 Subject: [PATCH 266/404] statically assert MAX_SIZE > 0 --- src/fsfw/container/FixedArrayList.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/fsfw/container/FixedArrayList.h b/src/fsfw/container/FixedArrayList.h index 97ade7e8..dde8eb16 100644 --- a/src/fsfw/container/FixedArrayList.h +++ b/src/fsfw/container/FixedArrayList.h @@ -12,6 +12,7 @@ template class FixedArrayList : public ArrayList { static_assert(MAX_SIZE <= std::numeric_limits::max(), "count_t is not large enough to hold MAX_SIZE"); + static_assert(MAX_SIZE > 0, "MAX_SIZE is 0"); private: T data[MAX_SIZE]; From 05cad893a2b713827cf4cdc9afe49675f18afcc7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 6 Dec 2022 10:05:23 +0100 Subject: [PATCH 267/404] introduce error counter to avoid spam --- src/fsfw_hal/linux/i2c/I2cComIF.cpp | 11 +++++++---- src/fsfw_hal/linux/i2c/I2cCookie.h | 2 ++ 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/src/fsfw_hal/linux/i2c/I2cComIF.cpp index 1ad19e82..08151c03 100644 --- a/src/fsfw_hal/linux/i2c/I2cComIF.cpp +++ b/src/fsfw_hal/linux/i2c/I2cComIF.cpp @@ -109,14 +109,17 @@ ReturnValue_t I2cComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s } if (write(fd, sendData, sendLen) != static_cast(sendLen)) { + i2cCookie->errorCounter++; + if (i2cCookie->errorCounter < 3) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "I2cComIF::sendMessage: Failed to send data to I2C " - "device with error code " - << errno << ". Error description: " << strerror(errno) << std::endl; + sif::error << "I2cComIF::sendMessage: Failed to send data to I2C " + "device with error code " + << errno << ". Error description: " << strerror(errno) << std::endl; #endif + } return returnvalue::FAILED; } - + i2cCookie->errorCounter = 0; #if FSFW_HAL_I2C_WIRETAPPING == 1 sif::info << "Sent I2C data to bus " << deviceFile << ":" << std::endl; arrayprinter::print(sendData, sendLen); diff --git a/src/fsfw_hal/linux/i2c/I2cCookie.h b/src/fsfw_hal/linux/i2c/I2cCookie.h index 84a1873c..8be71205 100644 --- a/src/fsfw_hal/linux/i2c/I2cCookie.h +++ b/src/fsfw_hal/linux/i2c/I2cCookie.h @@ -27,6 +27,8 @@ class I2cCookie : public CookieIF { size_t getMaxReplyLen() const; std::string getDeviceFile() const; + uint8_t errorCounter = 0; + private: address_t i2cAddress = 0; size_t maxReplyLen = 0; From 2aa4af69742d932f09ec2a1d3d29b648295035d1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 14 Dec 2022 13:50:57 +0100 Subject: [PATCH 268/404] make function public --- src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h b/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h index 1c2596da..2b23b281 100644 --- a/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h +++ b/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h @@ -7,8 +7,8 @@ class TmStoreFrontendSimpleIF { public: virtual ~TmStoreFrontendSimpleIF() = default; - private: virtual MessageQueueId_t getCommandQueue() const = 0; + private: }; #endif /* FSFW_SRC_FSFW_TMSTORAGE_TMSTOREBACKENDSIMPLEIF_H_ */ From 5bb66c9723892409ac6cbdc6de9d9250f7969213 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 10 Jan 2023 11:55:54 +0100 Subject: [PATCH 269/404] remove duplicate printout --- src/fsfw_hal/linux/i2c/I2cComIF.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/src/fsfw_hal/linux/i2c/I2cComIF.cpp index 08151c03..66c2bb51 100644 --- a/src/fsfw_hal/linux/i2c/I2cComIF.cpp +++ b/src/fsfw_hal/linux/i2c/I2cComIF.cpp @@ -183,10 +183,6 @@ ReturnValue_t I2cComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLe } #else #endif -#endif -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::debug << "I2cComIF::requestReceiveMessage: Read " << readLen << " of " << requestLen - << " bytes" << std::endl; #endif return returnvalue::FAILED; } From bd189518b6f91f25db1845e2657101e6bf46eec0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 20 Jan 2023 11:10:05 +0100 Subject: [PATCH 270/404] small tweak, gain factory was always them same --- src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp index 707ca338..aa7c22d4 100644 --- a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp +++ b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp @@ -329,8 +329,8 @@ ReturnValue_t MgmRM3100Handler::handleDataReadout(const uint8_t *packet) { // Now scale to physical value in microtesla float fieldStrengthX = fieldStrengthRawX * scaleFactorX; - float fieldStrengthY = fieldStrengthRawY * scaleFactorX; - float fieldStrengthZ = fieldStrengthRawZ * scaleFactorX; + float fieldStrengthY = fieldStrengthRawY * scaleFactorY; + float fieldStrengthZ = fieldStrengthRawZ * scaleFactorZ; if (periodicPrintout) { if (debugDivider.checkAndIncrement()) { From 049e3b431da51ac2069c2d48c5715bb12f3234bc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 23 Jan 2023 11:31:00 +0100 Subject: [PATCH 271/404] small tweak for printout --- src/fsfw/subsystem/SubsystemBase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp index d14e3539..7f215f60 100644 --- a/src/fsfw/subsystem/SubsystemBase.cpp +++ b/src/fsfw/subsystem/SubsystemBase.cpp @@ -195,7 +195,7 @@ ReturnValue_t SubsystemBase::checkTable(HybridIterator tableIter) if (childrenMap.find(tableIter.value->getObject()) == childrenMap.end()) { #if FSFW_CPP_OSTREAM_ENABLED == 1 using namespace std; - sif::warning << "SubsystemBase::checkTable: Could not find Object " << setfill('0') << hex + sif::warning << "SubsystemBase::checkTable: Could not find object " << setfill('0') << hex << "0x" << setw(8) << tableIter.value->getObject() << " in object " << setw(8) << setw(0) << "0x" << setw(8) << SystemObject::getObjectId() << dec << std::endl; #endif From da124953351d6fc0910f2f14e92dac607c09b827 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 27 Jan 2023 15:08:24 +0100 Subject: [PATCH 272/404] connect mode tree parent: make health helper optional --- src/fsfw/controller/ControllerBase.cpp | 2 +- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 2 +- src/fsfw/pus/Service11TelecommandScheduling.tpp | 6 +++--- src/fsfw/subsystem/SubsystemBase.cpp | 2 +- src/fsfw/subsystem/helper.cpp | 6 ++++-- src/fsfw/subsystem/helper.h | 2 +- 6 files changed, 11 insertions(+), 9 deletions(-) diff --git a/src/fsfw/controller/ControllerBase.cpp b/src/fsfw/controller/ControllerBase.cpp index 98907210..c41c4121 100644 --- a/src/fsfw/controller/ControllerBase.cpp +++ b/src/fsfw/controller/ControllerBase.cpp @@ -109,5 +109,5 @@ const HasModesIF& ControllerBase::getModeIF() const { return *this; } ModeTreeChildIF& ControllerBase::getModeTreeChildIF() { return *this; } ReturnValue_t ControllerBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) { - return modetree::connectModeTreeParent(parent, *this, healthHelper, modeHelper); + return modetree::connectModeTreeParent(parent, *this, &healthHelper, modeHelper); } diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 67cd6a8b..d6e39edc 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1608,7 +1608,7 @@ void DeviceHandlerBase::disableCommandsAndReplies() { } ReturnValue_t DeviceHandlerBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) { - return modetree::connectModeTreeParent(parent, *this, healthHelper, modeHelper); + return modetree::connectModeTreeParent(parent, *this, &healthHelper, modeHelper); } const HasHealthIF* DeviceHandlerBase::getOptHealthIF() const { return this; } diff --git a/src/fsfw/pus/Service11TelecommandScheduling.tpp b/src/fsfw/pus/Service11TelecommandScheduling.tpp index 962d84ac..2ad11277 100644 --- a/src/fsfw/pus/Service11TelecommandScheduling.tpp +++ b/src/fsfw/pus/Service11TelecommandScheduling.tpp @@ -2,12 +2,12 @@ #include +#include "fsfw/globalfunctions/CRC.h" #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/serialize/SerializeAdapter.h" #include "fsfw/serviceinterface.h" -#include "fsfw/tmtcservices/AcceptsTelecommandsIF.h" #include "fsfw/tmtcpacket/pus/tc/PusTcIF.h" -#include "fsfw/globalfunctions/CRC.h" +#include "fsfw/tmtcservices/AcceptsTelecommandsIF.h" static constexpr auto DEF_END = SerializeIF::Endianness::BIG; @@ -180,7 +180,7 @@ inline ReturnValue_t Service11TelecommandScheduling::doInsertActivi if (CRC::crc16ccitt(data, size) != 0) { return CONTAINED_TC_CRC_MISSMATCH; } - + // store currentPacket and receive the store address store_address_t addr{}; if (tcStore->addData(&addr, data, size) != returnvalue::OK || diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp index 7f215f60..5cfe59a3 100644 --- a/src/fsfw/subsystem/SubsystemBase.cpp +++ b/src/fsfw/subsystem/SubsystemBase.cpp @@ -284,7 +284,7 @@ ReturnValue_t SubsystemBase::setHealth(HealthState health) { HasHealthIF::HealthState SubsystemBase::getHealth() { return healthHelper.getHealth(); } ReturnValue_t SubsystemBase::connectModeTreeParent(HasModeTreeChildrenIF& parent) { - return modetree::connectModeTreeParent(parent, *this, healthHelper, modeHelper); + return modetree::connectModeTreeParent(parent, *this, &healthHelper, modeHelper); } object_id_t SubsystemBase::getObjectId() const { return SystemObject::getObjectId(); } diff --git a/src/fsfw/subsystem/helper.cpp b/src/fsfw/subsystem/helper.cpp index e65ef5a9..7b29825a 100644 --- a/src/fsfw/subsystem/helper.cpp +++ b/src/fsfw/subsystem/helper.cpp @@ -2,12 +2,14 @@ ReturnValue_t modetree::connectModeTreeParent(HasModeTreeChildrenIF& parent, const ModeTreeChildIF& child, - HealthHelper& healthHelper, ModeHelper& modeHelper) { + HealthHelper* healthHelper, ModeHelper& modeHelper) { ReturnValue_t result = parent.registerChild(child); if (result != returnvalue::OK) { return result; } - healthHelper.setParentQueue(parent.getCommandQueue()); + if (healthHelper != nullptr) { + healthHelper->setParentQueue(parent.getCommandQueue()); + } modeHelper.setParentQueue(parent.getCommandQueue()); return returnvalue::OK; } diff --git a/src/fsfw/subsystem/helper.h b/src/fsfw/subsystem/helper.h index 71a5563f..7a394e49 100644 --- a/src/fsfw/subsystem/helper.h +++ b/src/fsfw/subsystem/helper.h @@ -7,7 +7,7 @@ namespace modetree { ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF& parent, const ModeTreeChildIF& child, - HealthHelper& healthHelper, ModeHelper& modeHelper); + HealthHelper* healthHelper, ModeHelper& modeHelper); } From 226818886fe7094ff6f17a02fe5728b75ce48969 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 28 Jan 2023 14:31:32 +0100 Subject: [PATCH 273/404] improve srv20 error messages --- src/fsfw/pus/Service20ParameterManagement.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fsfw/pus/Service20ParameterManagement.cpp b/src/fsfw/pus/Service20ParameterManagement.cpp index e12d47bc..87bd5a13 100644 --- a/src/fsfw/pus/Service20ParameterManagement.cpp +++ b/src/fsfw/pus/Service20ParameterManagement.cpp @@ -69,14 +69,14 @@ ReturnValue_t Service20ParameterManagement::checkInterfaceAndAcquireMessageQueue #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "Service20ParameterManagement::checkInterfaceAndAcquire" << "MessageQueue: Can't access object" << std::endl; - sif::error << "Object ID: " << std::hex << objectId << std::dec << std::endl; - sif::error << "Make sure it implements ReceivesParameterMessagesIF!" << std::endl; + sif::error << "Object ID: 0x" << std::hex << *objectId << std::dec << std::endl; + sif::error << "Make sure it implements ReceivesParameterMessagesIF" << std::endl; #else sif::printError( "Service20ParameterManagement::checkInterfaceAndAcquire" "MessageQueue: Can't access object\n"); sif::printError("Object ID: 0x%08x\n", *objectId); - sif::printError("Make sure it implements ReceivesParameterMessagesIF!\n"); + sif::printError("Make sure it implements ReceivesParameterMessagesIF\n"); #endif return CommandingServiceBase::INVALID_OBJECT; From c64b9b3e71766d3f3d0a54613935b2de9baf5ef0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 30 Jan 2023 14:05:39 +0100 Subject: [PATCH 274/404] allow using SO_REUSEADDR and SO_REUSEPORT on TCP server --- src/fsfw/osal/common/TcpTmTcServer.cpp | 15 ++++++++++++--- src/fsfw/osal/common/TcpTmTcServer.h | 26 ++++++++++++++++++++------ 2 files changed, 32 insertions(+), 9 deletions(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.cpp b/src/fsfw/osal/common/TcpTmTcServer.cpp index bc2cd152..42417f84 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.cpp +++ b/src/fsfw/osal/common/TcpTmTcServer.cpp @@ -26,12 +26,12 @@ const std::string TcpTmTcServer::DEFAULT_SERVER_PORT = tcpip::DEFAULT_SERVER_PORT; TcpTmTcServer::TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge, - size_t receptionBufferSize, size_t ringBufferSize, - std::string customTcpServerPort, ReceptionModes receptionMode) + TcpTmTcServer::TcpConfig cfg, size_t receptionBufferSize, + size_t ringBufferSize, ReceptionModes receptionMode) : SystemObject(objectId), tmtcBridgeId(tmtcTcpBridge), receptionMode(receptionMode), - tcpConfig(std::move(customTcpServerPort)), + tcpConfig(cfg), receptionBuffer(receptionBufferSize), ringBuffer(ringBufferSize, true) {} @@ -91,6 +91,15 @@ ReturnValue_t TcpTmTcServer::initialize() { return returnvalue::FAILED; } + if (tcpConfig.reuseAddr) { + unsigned int enable = 1; + setsockopt(listenerTcpSocket, SOL_SOCKET, SO_REUSEADDR, &enable, sizeof(enable)); + } + if (tcpConfig.reusePort) { + unsigned int enable = 1; + setsockopt(listenerTcpSocket, SOL_SOCKET, SO_REUSEPORT, &enable, sizeof(enable)); + } + // Bind to the address found by getaddrinfo retval = bind(listenerTcpSocket, addrResult->ai_addr, static_cast(addrResult->ai_addrlen)); if (retval == SOCKET_ERROR) { diff --git a/src/fsfw/osal/common/TcpTmTcServer.h b/src/fsfw/osal/common/TcpTmTcServer.h index 0e2182a5..8668d845 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.h +++ b/src/fsfw/osal/common/TcpTmTcServer.h @@ -41,11 +41,11 @@ class SpacePacketParser; */ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableObjectIF { public: - enum class ReceptionModes { SPACE_PACKETS }; - struct TcpConfig { public: - explicit TcpConfig(std::string tcpPort) : tcpPort(std::move(tcpPort)) {} + TcpConfig(bool reuseAddr, bool reusePort) : reuseAddr(reuseAddr), reusePort(reusePort) {} + TcpConfig(std::string tcpPort, bool reuseAddr, bool reusePort) + : tcpPort(std::move(tcpPort)), reuseAddr(reuseAddr), reusePort(reusePort) {} /** * Passed to the recv call @@ -63,8 +63,23 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb */ int tcpTmFlags = 0; - const std::string tcpPort; + /** + * Sets the SO_REUSEADDR option on the socket. See + * https://man7.org/linux/man-pages/man7/socket.7.html for more details. This option is + * especially useful in a debugging and development environment where an OBSW image might be + * re-flashed oftentimes and where all incoming telecommands are received on a dedicated TCP + * port. + */ + bool reuseAddr = false; + /** + * Sets the SO_REUSEPORT option on the socket. See + * https://man7.org/linux/man-pages/man7/socket.7.html for more details. + */ + bool reusePort = false; + + std::string tcpPort = DEFAULT_SERVER_PORT; }; + enum class ReceptionModes { SPACE_PACKETS }; static const std::string DEFAULT_SERVER_PORT; @@ -80,10 +95,9 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb * size will be the Ethernet MTU size * @param customTcpServerPort The user can specify another port than the default (7301) here. */ - TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge, + TcpTmTcServer(object_id_t objectId, object_id_t tmtcTcpBridge, TcpTmTcServer::TcpConfig cfg, size_t receptionBufferSize = RING_BUFFER_SIZE, size_t ringBufferSize = RING_BUFFER_SIZE, - std::string customTcpServerPort = DEFAULT_SERVER_PORT, ReceptionModes receptionMode = ReceptionModes::SPACE_PACKETS); ~TcpTmTcServer() override; From 7766b24a1d55df2a3f0d702e627b38c1118c5e8d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 30 Jan 2023 14:24:28 +0100 Subject: [PATCH 275/404] re-order fields in TcpConfig --- src/fsfw/osal/common/TcpTmTcServer.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/fsfw/osal/common/TcpTmTcServer.h b/src/fsfw/osal/common/TcpTmTcServer.h index 8668d845..3d182827 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.h +++ b/src/fsfw/osal/common/TcpTmTcServer.h @@ -63,6 +63,8 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb */ int tcpTmFlags = 0; + std::string tcpPort = DEFAULT_SERVER_PORT; + /** * Sets the SO_REUSEADDR option on the socket. See * https://man7.org/linux/man-pages/man7/socket.7.html for more details. This option is @@ -77,7 +79,6 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb */ bool reusePort = false; - std::string tcpPort = DEFAULT_SERVER_PORT; }; enum class ReceptionModes { SPACE_PACKETS }; From 5c35b8e3cd3b2cb0c6e3eaed53fa24a454ac6365 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 1 Feb 2023 17:07:38 +0100 Subject: [PATCH 276/404] proper announce all impl --- src/fsfw/health/HealthTable.h | 2 + src/fsfw/osal/common/TcpTmTcServer.h | 1 - src/fsfw/pus/CService201HealthCommanding.cpp | 44 ++++++++++++++++---- src/fsfw/pus/CService201HealthCommanding.h | 7 +++- 4 files changed, 43 insertions(+), 11 deletions(-) diff --git a/src/fsfw/health/HealthTable.h b/src/fsfw/health/HealthTable.h index 076228c1..9b0722df 100644 --- a/src/fsfw/health/HealthTable.h +++ b/src/fsfw/health/HealthTable.h @@ -8,6 +8,8 @@ #include "HealthTableIF.h" class HealthTable : public HealthTableIF, public SystemObject { + friend class CService201HealthCommanding; + public: explicit HealthTable(object_id_t objectid); ~HealthTable() override; diff --git a/src/fsfw/osal/common/TcpTmTcServer.h b/src/fsfw/osal/common/TcpTmTcServer.h index 3d182827..009a1680 100644 --- a/src/fsfw/osal/common/TcpTmTcServer.h +++ b/src/fsfw/osal/common/TcpTmTcServer.h @@ -78,7 +78,6 @@ class TcpTmTcServer : public SystemObject, public TcpIpBase, public ExecutableOb * https://man7.org/linux/man-pages/man7/socket.7.html for more details. */ bool reusePort = false; - }; enum class ReceptionModes { SPACE_PACKETS }; diff --git a/src/fsfw/pus/CService201HealthCommanding.cpp b/src/fsfw/pus/CService201HealthCommanding.cpp index bf21c5bd..af261caf 100644 --- a/src/fsfw/pus/CService201HealthCommanding.cpp +++ b/src/fsfw/pus/CService201HealthCommanding.cpp @@ -1,5 +1,7 @@ #include "fsfw/pus/CService201HealthCommanding.h" +#include + #include "fsfw/health/HasHealthIF.h" #include "fsfw/health/HealthMessage.h" #include "fsfw/objectmanager/ObjectManager.h" @@ -7,11 +9,12 @@ #include "fsfw/serviceinterface/ServiceInterface.h" CService201HealthCommanding::CService201HealthCommanding(object_id_t objectId, uint16_t apid, - uint8_t serviceId, + uint8_t serviceId, HealthTable &table, uint8_t numParallelCommands, uint16_t commandTimeoutSeconds) : CommandingServiceBase(objectId, apid, "PUS 201 Health MGMT", serviceId, numParallelCommands, - commandTimeoutSeconds) {} + commandTimeoutSeconds), + healthTable(table) {} ReturnValue_t CService201HealthCommanding::isValidSubservice(uint8_t subservice) { switch (subservice) { @@ -32,12 +35,16 @@ ReturnValue_t CService201HealthCommanding::getMessageQueueAndObject(uint8_t subs size_t tcDataLen, MessageQueueId_t *id, object_id_t *objectId) { - if (tcDataLen < sizeof(object_id_t)) { - return CommandingServiceBase::INVALID_TC; - } - SerializeAdapter::deSerialize(objectId, &tcData, &tcDataLen, SerializeIF::Endianness::BIG); + if (subservice == Subservice::COMMAND_SET_HEALTH and + subservice == Subservice::COMMAND_ANNOUNCE_HEALTH) { + if (tcDataLen < sizeof(object_id_t)) { + return CommandingServiceBase::INVALID_TC; + } + SerializeAdapter::deSerialize(objectId, &tcData, &tcDataLen, SerializeIF::Endianness::BIG); - return checkInterfaceAndAcquireMessageQueue(id, objectId); + return checkInterfaceAndAcquireMessageQueue(id, objectId); + } + return returnvalue::OK; } ReturnValue_t CService201HealthCommanding::checkInterfaceAndAcquireMessageQueue( @@ -72,8 +79,15 @@ ReturnValue_t CService201HealthCommanding::prepareCommand(CommandMessage *messag break; } case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL): { - HealthMessage::setHealthMessage(message, HealthMessage::HEALTH_ANNOUNCE_ALL); - break; + ReturnValue_t result = iterateHealthTable(true); + while (true) { + ReturnValue_t result = iterateHealthTable(false); + if (result != returnvalue::OK) { + break; + } + } + + return returnvalue::OK; } default: { // Should never happen, subservice was already checked @@ -104,3 +118,15 @@ ReturnValue_t CService201HealthCommanding::handleReply(const CommandMessage *rep HealthSetReply healthSetReply(health, oldHealth); return sendTmPacket(Subservice::REPLY_HEALTH_SET, healthSetReply); } + +ReturnValue_t CService201HealthCommanding::iterateHealthTable(bool reset) { + std::pair pair; + + ReturnValue_t result = healthTable.iterate(&pair, reset); + if (result != returnvalue::OK) { + return result; + } else { + EventManagerIF::triggerEvent(pair.first, HasHealthIF::HEALTH_INFO, pair.second, pair.second); + return returnvalue::OK; + } +} diff --git a/src/fsfw/pus/CService201HealthCommanding.h b/src/fsfw/pus/CService201HealthCommanding.h index 71b7caa0..8cef5599 100644 --- a/src/fsfw/pus/CService201HealthCommanding.h +++ b/src/fsfw/pus/CService201HealthCommanding.h @@ -1,6 +1,8 @@ #ifndef FSFW_PUS_CSERVICE201HEALTHCOMMANDING_H_ #define FSFW_PUS_CSERVICE201HEALTHCOMMANDING_H_ +#include + #include "fsfw/tmtcservices/CommandingServiceBase.h" /** @@ -20,7 +22,8 @@ class CService201HealthCommanding : public CommandingServiceBase { public: CService201HealthCommanding(object_id_t objectId, uint16_t apid, uint8_t serviceId, - uint8_t numParallelCommands = 4, uint16_t commandTimeoutSeconds = 60); + HealthTable &table, uint8_t numParallelCommands = 4, + uint16_t commandTimeoutSeconds = 60); ~CService201HealthCommanding() override = default; protected: @@ -38,6 +41,8 @@ class CService201HealthCommanding : public CommandingServiceBase { bool *isStep) override; private: + HealthTable &healthTable; + ReturnValue_t iterateHealthTable(bool reset); static ReturnValue_t checkInterfaceAndAcquireMessageQueue(MessageQueueId_t *MessageQueueToSet, const object_id_t *objectId); From 1cacceddad5a918b078dbd6f493d4c3f7f59646e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 1 Feb 2023 17:33:24 +0100 Subject: [PATCH 277/404] beatufil --- src/fsfw/health/HealthTable.h | 2 +- src/fsfw/pus/CMakeLists.txt | 2 +- ...nding.cpp => CServiceHealthCommanding.cpp} | 88 ++++++++++++------- ...ommanding.h => CServiceHealthCommanding.h} | 28 ++++-- 4 files changed, 79 insertions(+), 41 deletions(-) rename src/fsfw/pus/{CService201HealthCommanding.cpp => CServiceHealthCommanding.cpp} (56%) rename src/fsfw/pus/{CService201HealthCommanding.h => CServiceHealthCommanding.h} (75%) diff --git a/src/fsfw/health/HealthTable.h b/src/fsfw/health/HealthTable.h index 9b0722df..7b42dfba 100644 --- a/src/fsfw/health/HealthTable.h +++ b/src/fsfw/health/HealthTable.h @@ -8,7 +8,7 @@ #include "HealthTableIF.h" class HealthTable : public HealthTableIF, public SystemObject { - friend class CService201HealthCommanding; + friend class CServiceHealthCommanding; public: explicit HealthTable(object_id_t objectid); diff --git a/src/fsfw/pus/CMakeLists.txt b/src/fsfw/pus/CMakeLists.txt index 35b35bea..7c5b340c 100644 --- a/src/fsfw/pus/CMakeLists.txt +++ b/src/fsfw/pus/CMakeLists.txt @@ -9,4 +9,4 @@ target_sources( Service17Test.cpp Service20ParameterManagement.cpp CService200ModeCommanding.cpp - CService201HealthCommanding.cpp) + CServiceHealthCommanding.cpp) diff --git a/src/fsfw/pus/CService201HealthCommanding.cpp b/src/fsfw/pus/CServiceHealthCommanding.cpp similarity index 56% rename from src/fsfw/pus/CService201HealthCommanding.cpp rename to src/fsfw/pus/CServiceHealthCommanding.cpp index af261caf..9a2af7bc 100644 --- a/src/fsfw/pus/CService201HealthCommanding.cpp +++ b/src/fsfw/pus/CServiceHealthCommanding.cpp @@ -1,6 +1,5 @@ -#include "fsfw/pus/CService201HealthCommanding.h" - #include +#include #include "fsfw/health/HasHealthIF.h" #include "fsfw/health/HealthMessage.h" @@ -8,15 +7,13 @@ #include "fsfw/pus/servicepackets/Service201Packets.h" #include "fsfw/serviceinterface/ServiceInterface.h" -CService201HealthCommanding::CService201HealthCommanding(object_id_t objectId, uint16_t apid, - uint8_t serviceId, HealthTable &table, - uint8_t numParallelCommands, - uint16_t commandTimeoutSeconds) - : CommandingServiceBase(objectId, apid, "PUS 201 Health MGMT", serviceId, numParallelCommands, - commandTimeoutSeconds), - healthTable(table) {} +CServiceHealthCommanding::CServiceHealthCommanding(HealthServiceCfg args) + : CommandingServiceBase(args.objectId, args.apid, "PUS 201 Health MGMT", args.service, + args.numParallelCommands, args.commandTimeoutSeconds), + healthTable(args.table), + maxNumHealthInfoPerCycle(args.maxNumHealthInfoPerCycle) {} -ReturnValue_t CService201HealthCommanding::isValidSubservice(uint8_t subservice) { +ReturnValue_t CServiceHealthCommanding::isValidSubservice(uint8_t subservice) { switch (subservice) { case (Subservice::COMMAND_SET_HEALTH): case (Subservice::COMMAND_ANNOUNCE_HEALTH): @@ -30,24 +27,31 @@ ReturnValue_t CService201HealthCommanding::isValidSubservice(uint8_t subservice) } } -ReturnValue_t CService201HealthCommanding::getMessageQueueAndObject(uint8_t subservice, - const uint8_t *tcData, - size_t tcDataLen, - MessageQueueId_t *id, - object_id_t *objectId) { - if (subservice == Subservice::COMMAND_SET_HEALTH and - subservice == Subservice::COMMAND_ANNOUNCE_HEALTH) { - if (tcDataLen < sizeof(object_id_t)) { - return CommandingServiceBase::INVALID_TC; - } - SerializeAdapter::deSerialize(objectId, &tcData, &tcDataLen, SerializeIF::Endianness::BIG); +ReturnValue_t CServiceHealthCommanding::getMessageQueueAndObject(uint8_t subservice, + const uint8_t *tcData, + size_t tcDataLen, + MessageQueueId_t *id, + object_id_t *objectId) { + switch (subservice) { + case (Subservice::COMMAND_SET_HEALTH): + case (Subservice::COMMAND_ANNOUNCE_HEALTH): { + if (tcDataLen < sizeof(object_id_t)) { + return CommandingServiceBase::INVALID_TC; + } + SerializeAdapter::deSerialize(objectId, &tcData, &tcDataLen, SerializeIF::Endianness::BIG); - return checkInterfaceAndAcquireMessageQueue(id, objectId); + return checkInterfaceAndAcquireMessageQueue(id, objectId); + } + case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL): { + return returnvalue::OK; + } + default: { + return returnvalue::FAILED; + } } - return returnvalue::OK; } -ReturnValue_t CService201HealthCommanding::checkInterfaceAndAcquireMessageQueue( +ReturnValue_t CServiceHealthCommanding::checkInterfaceAndAcquireMessageQueue( MessageQueueId_t *messageQueueToSet, const object_id_t *objectId) { auto *destination = ObjectManager::instance()->get(*objectId); if (destination == nullptr) { @@ -58,10 +62,9 @@ ReturnValue_t CService201HealthCommanding::checkInterfaceAndAcquireMessageQueue( return returnvalue::OK; } -ReturnValue_t CService201HealthCommanding::prepareCommand(CommandMessage *message, - uint8_t subservice, const uint8_t *tcData, - size_t tcDataLen, uint32_t *state, - object_id_t objectId) { +ReturnValue_t CServiceHealthCommanding::prepareCommand(CommandMessage *message, uint8_t subservice, + const uint8_t *tcData, size_t tcDataLen, + uint32_t *state, object_id_t objectId) { ReturnValue_t result = returnvalue::OK; switch (subservice) { case (Subservice::COMMAND_SET_HEALTH): { @@ -80,6 +83,11 @@ ReturnValue_t CService201HealthCommanding::prepareCommand(CommandMessage *messag } case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL): { ReturnValue_t result = iterateHealthTable(true); + if (result == returnvalue::OK) { + reportAllHealth = true; + return EXECUTION_COMPLETE; + } + return result; while (true) { ReturnValue_t result = iterateHealthTable(false); if (result != returnvalue::OK) { @@ -97,10 +105,10 @@ ReturnValue_t CService201HealthCommanding::prepareCommand(CommandMessage *messag return result; } -ReturnValue_t CService201HealthCommanding::handleReply(const CommandMessage *reply, - Command_t previousCommand, uint32_t *state, - CommandMessage *optionalNextCommand, - object_id_t objectId, bool *isStep) { +ReturnValue_t CServiceHealthCommanding::handleReply(const CommandMessage *reply, + Command_t previousCommand, uint32_t *state, + CommandMessage *optionalNextCommand, + object_id_t objectId, bool *isStep) { Command_t replyId = reply->getCommand(); if (replyId == HealthMessage::REPLY_HEALTH_SET) { return EXECUTION_COMPLETE; @@ -110,8 +118,20 @@ ReturnValue_t CService201HealthCommanding::handleReply(const CommandMessage *rep return CommandingServiceBase::INVALID_REPLY; } +void CServiceHealthCommanding::doPeriodicOperation() { + if (reportAllHealth) { + for (uint8_t i = 0; i < maxNumHealthInfoPerCycle; i++) { + ReturnValue_t result = iterateHealthTable(true); + if (result != returnvalue::OK) { + reportAllHealth = false; + break; + } + } + } +} + // Not used for now, health state already reported by event -[[maybe_unused]] ReturnValue_t CService201HealthCommanding::prepareHealthSetReply( +[[maybe_unused]] ReturnValue_t CServiceHealthCommanding::prepareHealthSetReply( const CommandMessage *reply) { auto health = static_cast(HealthMessage::getHealth(reply)); auto oldHealth = static_cast(HealthMessage::getOldHealth(reply)); @@ -119,7 +139,7 @@ ReturnValue_t CService201HealthCommanding::handleReply(const CommandMessage *rep return sendTmPacket(Subservice::REPLY_HEALTH_SET, healthSetReply); } -ReturnValue_t CService201HealthCommanding::iterateHealthTable(bool reset) { +ReturnValue_t CServiceHealthCommanding::iterateHealthTable(bool reset) { std::pair pair; ReturnValue_t result = healthTable.iterate(&pair, reset); diff --git a/src/fsfw/pus/CService201HealthCommanding.h b/src/fsfw/pus/CServiceHealthCommanding.h similarity index 75% rename from src/fsfw/pus/CService201HealthCommanding.h rename to src/fsfw/pus/CServiceHealthCommanding.h index 8cef5599..18f6c140 100644 --- a/src/fsfw/pus/CService201HealthCommanding.h +++ b/src/fsfw/pus/CServiceHealthCommanding.h @@ -5,6 +5,22 @@ #include "fsfw/tmtcservices/CommandingServiceBase.h" +struct HealthServiceCfg { + HealthServiceCfg(object_id_t objectId, uint16_t apid, HealthTable &healthTable, + uint16_t maxNumHealthInfoPerCycle) + : objectId(objectId), + apid(apid), + table(healthTable), + maxNumHealthInfoPerCycle(maxNumHealthInfoPerCycle) {} + object_id_t objectId; + uint16_t apid; + HealthTable &table; + uint16_t maxNumHealthInfoPerCycle; + uint8_t service = 201; + uint8_t numParallelCommands = 4; + uint16_t commandTimeoutSeconds = 60; +}; + /** * @brief Custom PUS service to set health of all objects * implementing hasHealthIF. @@ -19,12 +35,10 @@ * child class like this service * */ -class CService201HealthCommanding : public CommandingServiceBase { +class CServiceHealthCommanding : public CommandingServiceBase { public: - CService201HealthCommanding(object_id_t objectId, uint16_t apid, uint8_t serviceId, - HealthTable &table, uint8_t numParallelCommands = 4, - uint16_t commandTimeoutSeconds = 60); - ~CService201HealthCommanding() override = default; + CServiceHealthCommanding(HealthServiceCfg args); + ~CServiceHealthCommanding() override = default; protected: /* CSB abstract function implementations */ @@ -40,8 +54,12 @@ class CService201HealthCommanding : public CommandingServiceBase { CommandMessage *optionalNextCommand, object_id_t objectId, bool *isStep) override; + void doPeriodicOperation() override; + private: HealthTable &healthTable; + uint16_t maxNumHealthInfoPerCycle = 0; + bool reportAllHealth = false; ReturnValue_t iterateHealthTable(bool reset); static ReturnValue_t checkInterfaceAndAcquireMessageQueue(MessageQueueId_t *MessageQueueToSet, const object_id_t *objectId); From 64787f85ca4ce796003240dfbebeb07f6e2b0816 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 1 Feb 2023 17:41:47 +0100 Subject: [PATCH 278/404] update changelog --- CHANGELOG.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index bff68d84..0e28c5bc 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -25,6 +25,8 @@ and this project adheres to [Semantic Versioning](http://semver.org/). - `TcpTmTcServer.cpp`: The server was actually not able to handle CCSDS packets which were clumped together. This has been fixed now. PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/673 +- `CServiceHealthCommanding`: Add announce all health info implementation + PR: https://egit.irs.uni-stuttgart.de/eive/fsfw/pulls/122 ## Added @@ -37,6 +39,10 @@ and this project adheres to [Semantic Versioning](http://semver.org/). ## Changes +- `CService201HealthCommanding` renamed to `CServiceHealthCommanding`, + service ID customizable now. `CServiceHealthCommanding` expects configuration struct + `HealthServiceCfg` now + PR: https://egit.irs.uni-stuttgart.de/eive/fsfw/pulls/122 - `AcceptsTelemetryIF`: `getReportReceptionQueue` is const now PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/712 - Moved some container returnvalues to dedicated header and namespace From 7ee83e4e5d49e9ef99974d9e8286dadf75205c2c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 1 Feb 2023 18:10:07 +0100 Subject: [PATCH 279/404] service 9 update - fix time info event - add time dump subservice --- CHANGELOG.md | 2 ++ src/fsfw/pus/Service9TimeManagement.cpp | 22 ++++++++++++++++------ src/fsfw/pus/Service9TimeManagement.h | 11 +++++++---- 3 files changed, 25 insertions(+), 10 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0e28c5bc..93e2cd21 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/). ## Fixes + - TC Scheduler Service 11: Add size and CRC check for contained TC. - Only delete health table entry in `HealthHelper` destructor if health table was set. @@ -30,6 +31,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/). ## Added +- `Service9TimeManagement`: Add `DUMP_TIME` (129) subservice. - `DleParser` helper class to parse DLE encoded packets from a byte stream. PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/711 - `UioMapper` is able to resolve symlinks now. diff --git a/src/fsfw/pus/Service9TimeManagement.cpp b/src/fsfw/pus/Service9TimeManagement.cpp index d19cb518..fb32f60e 100644 --- a/src/fsfw/pus/Service9TimeManagement.cpp +++ b/src/fsfw/pus/Service9TimeManagement.cpp @@ -1,5 +1,7 @@ #include "fsfw/pus/Service9TimeManagement.h" +#include + #include "fsfw/events/EventManagerIF.h" #include "fsfw/pus/servicepackets/Service9Packets.h" #include "fsfw/serviceinterface/ServiceInterface.h" @@ -15,9 +17,17 @@ ReturnValue_t Service9TimeManagement::performService() { return returnvalue::OK; ReturnValue_t Service9TimeManagement::handleRequest(uint8_t subservice) { switch (subservice) { - case SUBSERVICE::SET_TIME: { + case Subservice::SET_TIME: { return setTime(); } + case Subservice::DUMP_TIME: { + timeval newTime; + Clock::getClock_timeval(&newTime); + uint32_t subsecondMs = + static_cast(std::floor(static_cast(newTime.tv_usec) / 1000.0)); + triggerEvent(CLOCK_DUMP, newTime.tv_sec, subsecondMs); + return returnvalue::OK; + } default: return AcceptsTelecommandsIF::INVALID_SUBSERVICE; } @@ -33,14 +43,14 @@ ReturnValue_t Service9TimeManagement::setTime() { return result; } - uint32_t formerUptime; - Clock::getUptime(&formerUptime); + timeval time; + Clock::getClock_timeval(&time); result = Clock::setClock(&timeToSet); if (result == returnvalue::OK) { - uint32_t newUptime; - Clock::getUptime(&newUptime); - triggerEvent(CLOCK_SET, newUptime, formerUptime); + timeval newTime; + Clock::getClock_timeval(&newTime); + triggerEvent(CLOCK_SET, time.tv_sec, newTime.tv_sec); return returnvalue::OK; } else { triggerEvent(CLOCK_SET_FAILURE, result, 0); diff --git a/src/fsfw/pus/Service9TimeManagement.h b/src/fsfw/pus/Service9TimeManagement.h index 97284d32..ba789da0 100644 --- a/src/fsfw/pus/Service9TimeManagement.h +++ b/src/fsfw/pus/Service9TimeManagement.h @@ -6,10 +6,12 @@ class Service9TimeManagement : public PusServiceBase { public: static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_9; - //!< Clock has been set. P1: New Uptime. P2: Old Uptime + //!< Clock has been set. P1: old timeval seconds. P2: new timeval seconds. static constexpr Event CLOCK_SET = MAKE_EVENT(0, severity::INFO); + //!< Clock dump event. P1: timeval seconds P2: timeval milliseconds. + static constexpr Event CLOCK_DUMP = MAKE_EVENT(1, severity::INFO); //!< Clock could not be set. P1: Returncode. - static constexpr Event CLOCK_SET_FAILURE = MAKE_EVENT(1, severity::LOW); + static constexpr Event CLOCK_SET_FAILURE = MAKE_EVENT(2, severity::LOW); static constexpr uint8_t CLASS_ID = CLASS_ID::PUS_SERVICE_9; @@ -30,8 +32,9 @@ class Service9TimeManagement : public PusServiceBase { virtual ReturnValue_t setTime(); private: - enum SUBSERVICE { - SET_TIME = 128 //!< [EXPORT] : [COMMAND] Time command in ASCII, CUC or CDS format + enum Subservice { + SET_TIME = 128, //!< [EXPORT] : [COMMAND] Time command in ASCII, CUC or CDS format + DUMP_TIME = 129, }; }; From d9d253d3bbca2707dc271783b9b76a1379522c1f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 1 Feb 2023 19:58:27 +0100 Subject: [PATCH 280/404] small but important bugfix for health service --- src/fsfw/pus/CServiceHealthCommanding.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/pus/CServiceHealthCommanding.cpp b/src/fsfw/pus/CServiceHealthCommanding.cpp index 9a2af7bc..57b704fd 100644 --- a/src/fsfw/pus/CServiceHealthCommanding.cpp +++ b/src/fsfw/pus/CServiceHealthCommanding.cpp @@ -121,7 +121,7 @@ ReturnValue_t CServiceHealthCommanding::handleReply(const CommandMessage *reply, void CServiceHealthCommanding::doPeriodicOperation() { if (reportAllHealth) { for (uint8_t i = 0; i < maxNumHealthInfoPerCycle; i++) { - ReturnValue_t result = iterateHealthTable(true); + ReturnValue_t result = iterateHealthTable(false); if (result != returnvalue::OK) { reportAllHealth = false; break; From 2339712373870880e75b57012a7497b714759ee3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 1 Feb 2023 20:46:48 +0100 Subject: [PATCH 281/404] set sequence flags for PUS TMTC to unsegmented --- src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp | 1 + src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp | 1 + 2 files changed, 2 insertions(+) diff --git a/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp b/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp index 2c818c7b..82dd1c41 100644 --- a/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp +++ b/src/fsfw/tmtcpacket/pus/tc/PusTcCreator.cpp @@ -100,5 +100,6 @@ ReturnValue_t PusTcCreator::setSerializableUserData(const SerializeIF &serializa void PusTcCreator::setup() { spCreator.setPacketType(ccsds::PacketType::TC); spCreator.setSecHeaderFlag(); + spCreator.setSeqFlags(ccsds::SequenceFlags::UNSEGMENTED); updateSpLengthField(); } diff --git a/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp b/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp index d95a18ea..c9b3290a 100644 --- a/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp +++ b/src/fsfw/tmtcpacket/pus/tm/PusTmCreator.cpp @@ -119,6 +119,7 @@ void PusTmCreator::setup() { updateSpLengthField(); spCreator.setPacketType(ccsds::PacketType::TM); spCreator.setSecHeaderFlag(); + spCreator.setSeqFlags(ccsds::SequenceFlags::UNSEGMENTED); } void PusTmCreator::setMessageTypeCounter(uint16_t messageTypeCounter) { From f2461cd7e9df449e9ea4a842ca820d5002ef6494 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 2 Feb 2023 16:22:29 +0100 Subject: [PATCH 282/404] helper method for commanding mode --- src/fsfw/modes/ModeMessage.cpp | 4 ++++ src/fsfw/modes/ModeMessage.h | 5 +++-- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/fsfw/modes/ModeMessage.cpp b/src/fsfw/modes/ModeMessage.cpp index efe6d534..0d7eaeea 100644 --- a/src/fsfw/modes/ModeMessage.cpp +++ b/src/fsfw/modes/ModeMessage.cpp @@ -34,3 +34,7 @@ void ModeMessage::setModeAnnounceMessage(CommandMessage& message, bool recursive } message.setCommand(cmd); } + +void ModeMessage::setCmdModeModeMessage(CommandMessage& message, Mode_t mode, Submode_t submode) { + setModeMessage(&message, CMD_MODE_COMMAND, mode, submode); +} diff --git a/src/fsfw/modes/ModeMessage.h b/src/fsfw/modes/ModeMessage.h index 1582ccd4..89203f6e 100644 --- a/src/fsfw/modes/ModeMessage.h +++ b/src/fsfw/modes/ModeMessage.h @@ -38,12 +38,13 @@ class ModeMessage { ModeMessage() = delete; + static void setModeMessage(CommandMessage* message, Command_t command, Mode_t mode, + Submode_t submode); static Mode_t getMode(const CommandMessage* message); static Submode_t getSubmode(const CommandMessage* message); static ReturnValue_t getCantReachModeReason(const CommandMessage* message); - static void setModeMessage(CommandMessage* message, Command_t command, Mode_t mode, - Submode_t submode); + static void setCmdModeModeMessage(CommandMessage& message, Mode_t mode, Submode_t submode); static void setModeAnnounceMessage(CommandMessage& message, bool recursive); static void setCantReachMode(CommandMessage* message, ReturnValue_t reason); static void clear(CommandMessage* message); From d17ec02cf04e8d33359a612abdfdcbfbbc7d5bfb Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 2 Feb 2023 18:26:08 +0100 Subject: [PATCH 283/404] fixed Z value calculation --- src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp index aa7c22d4..4becd420 100644 --- a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp +++ b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp @@ -325,7 +325,7 @@ ReturnValue_t MgmRM3100Handler::handleDataReadout(const uint8_t *packet) { // trickery here to calculate the raw values first int32_t fieldStrengthRawX = ((packet[1] << 24) | (packet[2] << 16) | (packet[3] << 8)) >> 8; int32_t fieldStrengthRawY = ((packet[4] << 24) | (packet[5] << 16) | (packet[6] << 8)) >> 8; - int32_t fieldStrengthRawZ = ((packet[7] << 24) | (packet[8] << 16) | (packet[3] << 8)) >> 8; + int32_t fieldStrengthRawZ = ((packet[7] << 24) | (packet[8] << 16) | (packet[9] << 8)) >> 8; // Now scale to physical value in microtesla float fieldStrengthX = fieldStrengthRawX * scaleFactorX; From acfc1cbf219702d646499aa1e9c366f2285f73fe Mon Sep 17 00:00:00 2001 From: meggert Date: Thu, 2 Feb 2023 18:31:40 +0100 Subject: [PATCH 284/404] bump changelog --- CHANGELOG.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index b0a0055d..753a267a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,11 @@ and this project adheres to [Semantic Versioning](http://semver.org/). # [unreleased] +## Fixes + +- Bugfix for RM3100 MGM sensors. Z value was previously calculated + with bytes of the X value. + # [v6.0.0] ## Fixes From 3250bbf269b3326683222bb87ce7faecae63ad97 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 2 Feb 2023 18:34:24 +0100 Subject: [PATCH 285/404] changelog update --- CHANGELOG.md | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 753a267a..a64d7863 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,15 +8,12 @@ and this project adheres to [Semantic Versioning](http://semver.org/). # [unreleased] -## Fixes - -- Bugfix for RM3100 MGM sensors. Z value was previously calculated - with bytes of the X value. - # [v6.0.0] ## Fixes +- Bugfix for RM3100 MGM sensors. Z value was previously calculated + with bytes of the X value. - PUS TMTC creator module: Sequence flags were set to continuation segment (0b00) instead of the correct unsegmented flags (0b11) as specified in the standard. - `Service9TimeManagement`: Fix the time dump at the `SET_TIME` subservice: Include clock timeval From e11eabdbcf42bf1822bb05f2a2392b05a4f6889c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 3 Feb 2023 15:58:26 +0100 Subject: [PATCH 286/404] bugfix in setNormalDataPoolEntriesInvalid Do not forget to call read and write to actually update the validity state --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 7 ++++++- unittests/testcfg/devices/logicalAddresses.h | 2 +- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index d6e39edc..555683c4 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1,5 +1,7 @@ #include "DeviceHandlerBase.h" +#include + #include "fsfw/datapoollocal/LocalPoolVariable.h" #include "fsfw/devicehandlers/AcceptsDeviceResponsesIF.h" #include "fsfw/devicehandlers/DeviceTmReportingWrapper.h" @@ -1524,7 +1526,10 @@ DeviceCommandId_t DeviceHandlerBase::getPendingCommand() const { void DeviceHandlerBase::setNormalDatapoolEntriesInvalid() { for (const auto& reply : deviceReplyMap) { if (reply.second.dataSet != nullptr) { - reply.second.dataSet->setValidity(false, true); + PoolReadGuard pg(reply.second.dataSet); + if (pg.getReadResult() == returnvalue::OK) { + reply.second.dataSet->setValidity(false, true); + } } } } diff --git a/unittests/testcfg/devices/logicalAddresses.h b/unittests/testcfg/devices/logicalAddresses.h index 788c124f..a7e34cce 100644 --- a/unittests/testcfg/devices/logicalAddresses.h +++ b/unittests/testcfg/devices/logicalAddresses.h @@ -7,7 +7,7 @@ namespace addresses { /* Logical addresses have uint32_t datatype */ -enum logicalAddresses : address_t {}; +enum LogicAddress : address_t {}; } // namespace addresses #endif /* CONFIG_DEVICES_LOGICALADDRESSES_H_ */ From 06dca7608ae50d80f648450e21186e489c75fc8d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 12:06:34 +0100 Subject: [PATCH 287/404] time stamper empty ctor --- src/fsfw/timemanager/CdsShortTimeStamper.cpp | 2 ++ src/fsfw/timemanager/CdsShortTimeStamper.h | 1 + src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h | 1 + 3 files changed, 4 insertions(+) diff --git a/src/fsfw/timemanager/CdsShortTimeStamper.cpp b/src/fsfw/timemanager/CdsShortTimeStamper.cpp index 8fb33f12..aa259029 100644 --- a/src/fsfw/timemanager/CdsShortTimeStamper.cpp +++ b/src/fsfw/timemanager/CdsShortTimeStamper.cpp @@ -4,6 +4,8 @@ #include "fsfw/timemanager/Clock.h" +CdsShortTimeStamper::CdsShortTimeStamper() : SystemObject(0, false) {} + CdsShortTimeStamper::CdsShortTimeStamper(object_id_t objectId) : SystemObject(objectId) {} ReturnValue_t CdsShortTimeStamper::serialize(uint8_t **buffer, size_t *size, size_t maxSize, diff --git a/src/fsfw/timemanager/CdsShortTimeStamper.h b/src/fsfw/timemanager/CdsShortTimeStamper.h index 244d54b6..a16a07b6 100644 --- a/src/fsfw/timemanager/CdsShortTimeStamper.h +++ b/src/fsfw/timemanager/CdsShortTimeStamper.h @@ -18,6 +18,7 @@ class CdsShortTimeStamper : public TimeWriterIF, public TimeReaderIF, public SystemObject { public: static constexpr size_t TIMESTAMP_LEN = 7; + CdsShortTimeStamper(); /** * @brief Default constructor which also registers the time stamper as a * system object so it can be found with the #objectManager. diff --git a/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h b/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h index 2b23b281..5089f37d 100644 --- a/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h +++ b/src/fsfw/tmstorage/TmStoreFrontendSimpleIF.h @@ -8,6 +8,7 @@ class TmStoreFrontendSimpleIF { virtual ~TmStoreFrontendSimpleIF() = default; virtual MessageQueueId_t getCommandQueue() const = 0; + private: }; From 37e850c5a7115cedc1385904e2116dd9b25d85ee Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 12:31:19 +0100 Subject: [PATCH 288/404] use upstream changelog --- CHANGELOG.md | 22 ++++++---------------- 1 file changed, 6 insertions(+), 16 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9f0c08f3..265e3129 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,9 @@ and this project adheres to [Semantic Versioning](http://semver.org/). ## Fixes +- `Service9TimeManagement`: Fix the time dump at the `SET_TIME` subservice: Include clock timeval + seconds instead of uptime. + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/726 - HAL MGM3100 Handler: Use axis specific gain/scaling factors. Previously, only the X scaling factor was used. PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/724 @@ -19,9 +22,6 @@ and this project adheres to [Semantic Versioning](http://semver.org/). to false correctly because the `read` and `write` calls were missing. - PUS TMTC creator module: Sequence flags were set to continuation segment (0b00) instead of the correct unsegmented flags (0b11) as specified in the standard. -- `Service9TimeManagement`: Fix the time dump at the `SET_TIME` subservice: Include clock timeval - seconds instead of uptime. - PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/726 - TC Scheduler Service 11: Add size and CRC check for contained TC. - Only delete health table entry in `HealthHelper` destructor if health table was set. @@ -35,11 +35,11 @@ and this project adheres to [Semantic Versioning](http://semver.org/). - `TcpTmTcServer.cpp`: The server was actually not able to handle CCSDS packets which were clumped together. This has been fixed now. PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/673 -- `CServiceHealthCommanding`: Add announce all health info implementation - PR: https://egit.irs.uni-stuttgart.de/eive/fsfw/pulls/122 ## Added +- `CServiceHealthCommanding`: Add announce all health info implementation + PR: https://egit.irs.uni-stuttgart.de/eive/fsfw/pulls/122 - `Service9TimeManagement`: Add `DUMP_TIME` (129) subservice. - `TcpTmTcServer`: Allow setting the `SO_REUSEADDR` and `SO_REUSEPORT` option on the TCP server. CTOR prototype has changed and expects an explicit @@ -255,6 +255,7 @@ https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/593 - https://gitlab.kitware.com/cmake/cmake/-/issues/21696 Easiest solution for now: Keep this option OFF by default. PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/616 +- Linux HAL: Add wiretapping option for I2C. Enabled with `FSFW_HAL_I2C_WIRETAPPING` defined to 1 - Dedicated Version class and constant `fsfw::FSFW_VERSION` containing version information inside `fsfw/version.h` PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/559 @@ -269,17 +270,6 @@ https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/593 PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/590 - `Subsystem`: New API to add table and sequence entries -## HAL - -- SPI: Cache the SPI device in the communication interface. Architecturally, this makes a - lot more sense because each ComIF should be responsible for one SPI bus. -- SPI: Move the empty transfer to update the line polarity to separate function. This means - it is not automatically called when calling the setter function for SPI speed and mode. - The user should call this function after locking the CS mutex if multiple SPI devices with - differing speeds and modes are attached to one bus. -- SPI: Getter functions for SPI speed and mode. -- I2C: Add wiretapping option for I2C. Enabled with `FSFW_HAL_I2C_WIRETAPPING` defined to 1. - ## Fixed - TCP TMTC Server: `MutexGuard` was not created properly in From 69c94645df81c2fc9d538a0f19d2c6552e1c73a7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Feb 2023 12:36:01 +0100 Subject: [PATCH 289/404] add back HAL section --- CHANGELOG.md | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 265e3129..b7d7845d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -122,6 +122,16 @@ and this project adheres to [Semantic Versioning](http://semver.org/). implementation without an extra component PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/682 +## HAL + +- SPI: Cache the SPI device in the communication interface. Architecturally, this makes a + lot more sense because each ComIF should be responsible for one SPI bus. +- SPI: Move the empty transfer to update the line polarity to separate function. This means + it is not automatically called when calling the setter function for SPI speed and mode. + The user should call this function after locking the CS mutex if multiple SPI devices with + differing speeds and modes are attached to one bus. +- SPI: Getter functions for SPI speed and mode. + # [v5.0.0] 25.07.2022 ## Changes From ec12ab5daaa391232e610567fbbbf849594e52e2 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 01:20:28 +0100 Subject: [PATCH 290/404] mode service fixes --- src/fsfw/modes/ModeMessage.cpp | 2 + src/fsfw/modes/ModeMessage.h | 1 + src/fsfw/pus/CService200ModeCommanding.cpp | 53 +++++++++++++--------- 3 files changed, 35 insertions(+), 21 deletions(-) diff --git a/src/fsfw/modes/ModeMessage.cpp b/src/fsfw/modes/ModeMessage.cpp index 0d7eaeea..36cbb971 100644 --- a/src/fsfw/modes/ModeMessage.cpp +++ b/src/fsfw/modes/ModeMessage.cpp @@ -38,3 +38,5 @@ void ModeMessage::setModeAnnounceMessage(CommandMessage& message, bool recursive void ModeMessage::setCmdModeModeMessage(CommandMessage& message, Mode_t mode, Submode_t submode) { setModeMessage(&message, CMD_MODE_COMMAND, mode, submode); } + +void ModeMessage::setModeReadMessage(CommandMessage& message) { message.setCommand(CMD_MODE_READ); } diff --git a/src/fsfw/modes/ModeMessage.h b/src/fsfw/modes/ModeMessage.h index 89203f6e..4e75d156 100644 --- a/src/fsfw/modes/ModeMessage.h +++ b/src/fsfw/modes/ModeMessage.h @@ -46,6 +46,7 @@ class ModeMessage { static void setCmdModeModeMessage(CommandMessage& message, Mode_t mode, Submode_t submode); static void setModeAnnounceMessage(CommandMessage& message, bool recursive); + static void setModeReadMessage(CommandMessage& message); static void setCantReachMode(CommandMessage* message, ReturnValue_t reason); static void clear(CommandMessage* message); }; diff --git a/src/fsfw/pus/CService200ModeCommanding.cpp b/src/fsfw/pus/CService200ModeCommanding.cpp index f5928be7..814c1c92 100644 --- a/src/fsfw/pus/CService200ModeCommanding.cpp +++ b/src/fsfw/pus/CService200ModeCommanding.cpp @@ -54,27 +54,36 @@ ReturnValue_t CService200ModeCommanding::checkInterfaceAndAcquireMessageQueue( ReturnValue_t CService200ModeCommanding::prepareCommand(CommandMessage *message, uint8_t subservice, const uint8_t *tcData, size_t tcDataLen, uint32_t *state, object_id_t objectId) { - ReturnValue_t result = returnvalue::OK; - if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE or - subservice == Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY) { - bool recursive = true; - if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE) { - recursive = false; - } - ModeMessage::setModeAnnounceMessage(*message, recursive); - } else { - ModePacket modeCommandPacket; - ReturnValue_t result = - modeCommandPacket.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG); - if (result != returnvalue::OK) { - return result; - } + switch (subservice) { + case (Subservice::COMMAND_MODE_COMMAND): { + ModePacket modeCommandPacket; + ReturnValue_t result = + modeCommandPacket.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG); + if (result != returnvalue::OK) { + return result; + } - ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND, modeCommandPacket.getMode(), - modeCommandPacket.getSubmode()); + ModeMessage::setModeMessage(message, ModeMessage::CMD_MODE_COMMAND, + modeCommandPacket.getMode(), modeCommandPacket.getSubmode()); + return returnvalue::OK; + } + case (Subservice::COMMAND_MODE_ANNCOUNCE): + case (Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY): { + bool recursive = true; + if (subservice == Subservice::COMMAND_MODE_ANNCOUNCE) { + recursive = false; + } + ModeMessage::setModeAnnounceMessage(*message, recursive); + return EXECUTION_COMPLETE; + } + case (Subservice::COMMAND_MODE_READ): { + ModeMessage::setModeReadMessage(*message); + return returnvalue::OK; + } + default: { + return CommandingServiceBase::INVALID_SUBSERVICE; + } } - - return result; } ReturnValue_t CService200ModeCommanding::handleReply(const CommandMessage *reply, @@ -85,8 +94,10 @@ ReturnValue_t CService200ModeCommanding::handleReply(const CommandMessage *reply ReturnValue_t result = returnvalue::FAILED; switch (replyId) { case (ModeMessage::REPLY_MODE_REPLY): { - result = prepareModeReply(reply, objectId); - break; + if (previousCommand != ModeMessage::CMD_MODE_COMMAND) { + return prepareModeReply(reply, objectId); + } + return returnvalue::OK; } case (ModeMessage::REPLY_WRONG_MODE_REPLY): { result = prepareWrongModeReply(reply, objectId); From 84dc7ac0ce4f16d93a6a5a5fcd453b2fde206dba Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 01:31:32 +0100 Subject: [PATCH 291/404] bump changelog --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index b7d7845d..f6e47418 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,9 @@ and this project adheres to [Semantic Versioning](http://semver.org/). ## Fixes +- `CService200ModeManagement`: Various bugfixes which lead to now execution complete being generated + on mode announcements, duplicate mode reply generated on announce commands, and the mode read + subservice not working properly. - `Service9TimeManagement`: Fix the time dump at the `SET_TIME` subservice: Include clock timeval seconds instead of uptime. PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/726 From 84bbef016712096147e8cf3f2cec87f317d9e7e7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 09:27:14 +0100 Subject: [PATCH 292/404] make it consistent --- src/fsfw/modes/ModeMessage.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/fsfw/modes/ModeMessage.h b/src/fsfw/modes/ModeMessage.h index ad44c30b..9a3c2cc0 100644 --- a/src/fsfw/modes/ModeMessage.h +++ b/src/fsfw/modes/ModeMessage.h @@ -38,12 +38,12 @@ class ModeMessage { ModeMessage() = delete; - static void setModeMessage(CommandMessage* message, Command_t command, Mode_t mode, - Submode_t submode); static Mode_t getMode(const CommandMessage* message); static Submode_t getSubmode(const CommandMessage* message); static ReturnValue_t getCantReachModeReason(const CommandMessage* message); + static void setModeMessage(CommandMessage* message, Command_t command, Mode_t mode, + Submode_t submode); static void setCmdModeMessage(CommandMessage& message, Mode_t mode, Submode_t submode); static void setModeAnnounceMessage(CommandMessage& message, bool recursive); static void setModeReadMessage(CommandMessage& message); From 6f05d6b7b0ee0e6bc86986ec55491d42cde1cf93 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Feb 2023 20:38:32 +0100 Subject: [PATCH 293/404] possiible leak fixes --- src/fsfw/tmtcservices/TmTcBridge.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/fsfw/tmtcservices/TmTcBridge.cpp b/src/fsfw/tmtcservices/TmTcBridge.cpp index 9ff58766..ba851a85 100644 --- a/src/fsfw/tmtcservices/TmTcBridge.cpp +++ b/src/fsfw/tmtcservices/TmTcBridge.cpp @@ -145,13 +145,17 @@ ReturnValue_t TmTcBridge::handleTmQueue() { #endif /* FSFW_VERBOSE_LEVEL >= 3 */ if (communicationLinkUp == false or packetSentCounter >= sentPacketsPerCycle) { - storeDownlinkData(&message); + ReturnValue_t result = storeDownlinkData(&message); + if (result != returnvalue::OK) { + tmStore->deleteData(message.getStorageId()); + } continue; } result = tmStore->getData(message.getStorageId(), &data, &size); if (result != returnvalue::OK) { status = result; + tmStore->deleteData(message.getStorageId()); continue; } @@ -159,9 +163,9 @@ ReturnValue_t TmTcBridge::handleTmQueue() { if (result != returnvalue::OK) { status = result; } else { - tmStore->deleteData(message.getStorageId()); packetSentCounter++; } + tmStore->deleteData(message.getStorageId()); } return status; } From b27998585912a7a2cfa0b08dae4260f0dcd8cdd3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 9 Feb 2023 16:42:38 +0100 Subject: [PATCH 294/404] refactor DHB: Bugfix for thermal module --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 14 ++++++-------- src/fsfw/devicehandlers/DeviceHandlerBase.h | 8 ++++---- src/fsfw/devicehandlers/DeviceHandlerIF.h | 6 ++++++ 3 files changed, 16 insertions(+), 12 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index b2505344..acb9e1d2 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -58,12 +58,7 @@ void DeviceHandlerBase::setHkDestination(object_id_t hkDestination) { this->hkDestination = hkDestination; } -void DeviceHandlerBase::setThermalStateRequestPoolIds(lp_id_t thermalStatePoolId, - lp_id_t heaterRequestPoolId, - uint32_t thermalSetId) { - thermalSet = - new DeviceHandlerThermalSet(this, thermalSetId, thermalStatePoolId, heaterRequestPoolId); -} +void DeviceHandlerBase::setUpThermalModule(ThermalStateCfg cfg) { this->thermalStateCfg = cfg; } DeviceHandlerBase::~DeviceHandlerBase() { if (comCookie != nullptr) { @@ -1494,9 +1489,12 @@ ReturnValue_t DeviceHandlerBase::initializeAfterTaskCreation() { } this->poolManager.initializeAfterTaskCreation(); - if (setStartupImmediately) { - startTransition(MODE_ON, getInitialSubmode()); + if (thermalStateCfg.has_value()) { + ThermalStateCfg& val = thermalStateCfg.value(); + thermalSet = new DeviceHandlerThermalSet(this, val.thermalSetId, val.thermalStatePoolId, + val.thermalRequestPoolId); } + startTransition(MODE_ON, getInitialSubmode()); return returnvalue::OK; } diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index b06815d1..b96506f5 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -2,6 +2,7 @@ #define FSFW_DEVICEHANDLERS_DEVICEHANDLERBASE_H_ #include +#include #include "DeviceCommunicationIF.h" #include "DeviceHandlerFailureIsolation.h" @@ -166,10 +167,7 @@ class DeviceHandlerBase : public DeviceHandlerIF, * @param thermalStatePoolId * @param thermalRequestPoolId */ - void setThermalStateRequestPoolIds( - lp_id_t thermalStatePoolId = DeviceHandlerIF::DEFAULT_THERMAL_STATE_POOL_ID, - lp_id_t thermalRequestPoolId = DeviceHandlerIF::DEFAULT_THERMAL_HEATING_REQUEST_POOL_ID, - uint32_t thermalSetId = DeviceHandlerIF::DEFAULT_THERMAL_SET_ID); + void setUpThermalModule(ThermalStateCfg cfg); ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override; ModeTreeChildIF &getModeTreeChildIF() override; @@ -931,6 +929,8 @@ class DeviceHandlerBase : public DeviceHandlerIF, //! Object which may be the root cause of an identified fault. static object_id_t defaultFdirParentId; + std::optional thermalStateCfg; + /** * @brief Send a reply to a received device handler command. * diff --git a/src/fsfw/devicehandlers/DeviceHandlerIF.h b/src/fsfw/devicehandlers/DeviceHandlerIF.h index 474c3a75..5e1fdd2f 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerIF.h +++ b/src/fsfw/devicehandlers/DeviceHandlerIF.h @@ -136,4 +136,10 @@ class DeviceHandlerIF { virtual MessageQueueId_t getCommandQueue() const = 0; }; +struct ThermalStateCfg { + lp_id_t thermalStatePoolId = DeviceHandlerIF::DEFAULT_THERMAL_STATE_POOL_ID; + lp_id_t thermalRequestPoolId = DeviceHandlerIF::DEFAULT_THERMAL_HEATING_REQUEST_POOL_ID; + uint32_t thermalSetId = DeviceHandlerIF::DEFAULT_THERMAL_SET_ID; +}; + #endif /* FSFW_DEVICEHANDLERS_DEVICEHANDLERIF_H_ */ From 1841f929442d7945fd0c9401de4accc72031003d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 9 Feb 2023 17:17:42 +0100 Subject: [PATCH 295/404] important bugfix for thermal set --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 19 ++++++++-------- .../devicehandlers/DeviceHandlerThermalSet.h | 22 +++++++------------ 2 files changed, 18 insertions(+), 23 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index acb9e1d2..b725b186 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -1472,11 +1472,11 @@ void DeviceHandlerBase::performOperationHook() {} ReturnValue_t DeviceHandlerBase::initializeLocalDataPool(localpool::DataPool& localDataPoolMap, LocalDataPoolManager& poolManager) { - if (thermalSet != nullptr) { - localDataPoolMap.emplace(thermalSet->thermalStatePoolId, - new PoolEntry); - localDataPoolMap.emplace(thermalSet->heaterRequestPoolId, - new PoolEntry); + if (thermalStateCfg.has_value()) { + localDataPoolMap.emplace(thermalStateCfg.value().thermalStatePoolId, + new PoolEntry()); + localDataPoolMap.emplace(thermalStateCfg.value().thermalRequestPoolId, + new PoolEntry()); } return returnvalue::OK; } @@ -1490,11 +1490,12 @@ ReturnValue_t DeviceHandlerBase::initializeAfterTaskCreation() { this->poolManager.initializeAfterTaskCreation(); if (thermalStateCfg.has_value()) { - ThermalStateCfg& val = thermalStateCfg.value(); - thermalSet = new DeviceHandlerThermalSet(this, val.thermalSetId, val.thermalStatePoolId, - val.thermalRequestPoolId); + ThermalStateCfg& cfg = thermalStateCfg.value(); + thermalSet = new DeviceHandlerThermalSet(this, cfg); + } + if (setStartupImmediately) { + startTransition(MODE_ON, getInitialSubmode()); } - startTransition(MODE_ON, getInitialSubmode()); return returnvalue::OK; } diff --git a/src/fsfw/devicehandlers/DeviceHandlerThermalSet.h b/src/fsfw/devicehandlers/DeviceHandlerThermalSet.h index 944d7c0f..49ebd5f4 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerThermalSet.h +++ b/src/fsfw/devicehandlers/DeviceHandlerThermalSet.h @@ -7,27 +7,21 @@ class DeviceHandlerThermalSet : public StaticLocalDataSet<2> { public: - DeviceHandlerThermalSet( - HasLocalDataPoolIF* hkOwner, uint32_t setId = DeviceHandlerIF::DEFAULT_THERMAL_SET_ID, - lp_id_t thermalStateId = DeviceHandlerIF::DEFAULT_THERMAL_STATE_POOL_ID, - lp_id_t heaterRequestId = DeviceHandlerIF::DEFAULT_THERMAL_HEATING_REQUEST_POOL_ID) - : DeviceHandlerThermalSet(hkOwner->getObjectId(), setId, thermalStateId, heaterRequestId) {} + DeviceHandlerThermalSet(HasLocalDataPoolIF* hkOwner, ThermalStateCfg cfg) + : DeviceHandlerThermalSet(hkOwner->getObjectId(), cfg) {} - DeviceHandlerThermalSet( - object_id_t deviceHandler, uint32_t setId = DeviceHandlerIF::DEFAULT_THERMAL_SET_ID, - lp_id_t thermalStateId = DeviceHandlerIF::DEFAULT_THERMAL_STATE_POOL_ID, - lp_id_t thermalStateRequestId = DeviceHandlerIF::DEFAULT_THERMAL_HEATING_REQUEST_POOL_ID) - : StaticLocalDataSet(sid_t(deviceHandler, setId)), - thermalStatePoolId(thermalStateId), - heaterRequestPoolId(thermalStateRequestId) {} + DeviceHandlerThermalSet(object_id_t deviceHandler, ThermalStateCfg cfg) + : StaticLocalDataSet(sid_t(deviceHandler, cfg.thermalSetId)), + thermalStatePoolId(cfg.thermalStatePoolId), + heaterRequestPoolId(cfg.thermalRequestPoolId) {} const lp_id_t thermalStatePoolId; const lp_id_t heaterRequestPoolId; lp_var_t thermalState = - lp_var_t(thermalStatePoolId, sid.objectId, this); + lp_var_t(sid.objectId, thermalStatePoolId, this); lp_var_t heaterRequest = - lp_var_t(heaterRequestPoolId, sid.objectId, this); + lp_var_t(sid.objectId, heaterRequestPoolId, this); }; #endif /* FSFW_DEVICEHANDLERS_DEVICEHANDLERTHERMALSET_H_ */ From f4d188c36f0f08c94d678048a979261d373e7642 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 9 Feb 2023 18:12:43 +0100 Subject: [PATCH 296/404] that time margin check is possible broken --- src/fsfw/pus/Service11TelecommandScheduling.tpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/pus/Service11TelecommandScheduling.tpp b/src/fsfw/pus/Service11TelecommandScheduling.tpp index 2ad11277..d0728780 100644 --- a/src/fsfw/pus/Service11TelecommandScheduling.tpp +++ b/src/fsfw/pus/Service11TelecommandScheduling.tpp @@ -160,7 +160,7 @@ inline ReturnValue_t Service11TelecommandScheduling::doInsertActivi // (See requirement for Time margin) timeval tNow = {}; Clock::getClock_timeval(&tNow); - if (timestamp - tNow.tv_sec <= RELEASE_TIME_MARGIN_SECONDS) { + if (timestamp >= tNow.tv_sec + RELEASE_TIME_MARGIN_SECONDS) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "Service11TelecommandScheduling::doInsertActivity: Release time too close to " "current time" From 820a7f059c05d4b2f2012536bfe9482c1625961d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 9 Feb 2023 18:13:58 +0100 Subject: [PATCH 297/404] logic error --- src/fsfw/pus/Service11TelecommandScheduling.tpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/pus/Service11TelecommandScheduling.tpp b/src/fsfw/pus/Service11TelecommandScheduling.tpp index d0728780..2a20e822 100644 --- a/src/fsfw/pus/Service11TelecommandScheduling.tpp +++ b/src/fsfw/pus/Service11TelecommandScheduling.tpp @@ -160,7 +160,7 @@ inline ReturnValue_t Service11TelecommandScheduling::doInsertActivi // (See requirement for Time margin) timeval tNow = {}; Clock::getClock_timeval(&tNow); - if (timestamp >= tNow.tv_sec + RELEASE_TIME_MARGIN_SECONDS) { + if (timestamp < tNow.tv_sec + RELEASE_TIME_MARGIN_SECONDS) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "Service11TelecommandScheduling::doInsertActivity: Release time too close to " "current time" From d93486a340907fb682e916ec702e1be4eb05e15f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 9 Feb 2023 18:12:43 +0100 Subject: [PATCH 298/404] that time margin check is possible broken --- src/fsfw/pus/Service11TelecommandScheduling.tpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/pus/Service11TelecommandScheduling.tpp b/src/fsfw/pus/Service11TelecommandScheduling.tpp index 540f6c68..d507e354 100644 --- a/src/fsfw/pus/Service11TelecommandScheduling.tpp +++ b/src/fsfw/pus/Service11TelecommandScheduling.tpp @@ -160,7 +160,7 @@ inline ReturnValue_t Service11TelecommandScheduling::doInsertActivi // (See requirement for Time margin) timeval tNow = {}; Clock::getClock_timeval(&tNow); - if (timestamp - tNow.tv_sec <= RELEASE_TIME_MARGIN_SECONDS) { + if (timestamp >= tNow.tv_sec + RELEASE_TIME_MARGIN_SECONDS) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "Service11TelecommandScheduling::doInsertActivity: Release time too close to " "current time" From b9b076aa4c08639e5a2dfb8caf0ec8cb764b048a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 9 Feb 2023 18:13:58 +0100 Subject: [PATCH 299/404] logic error --- src/fsfw/pus/Service11TelecommandScheduling.tpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/pus/Service11TelecommandScheduling.tpp b/src/fsfw/pus/Service11TelecommandScheduling.tpp index d507e354..090d0fd6 100644 --- a/src/fsfw/pus/Service11TelecommandScheduling.tpp +++ b/src/fsfw/pus/Service11TelecommandScheduling.tpp @@ -160,7 +160,7 @@ inline ReturnValue_t Service11TelecommandScheduling::doInsertActivi // (See requirement for Time margin) timeval tNow = {}; Clock::getClock_timeval(&tNow); - if (timestamp >= tNow.tv_sec + RELEASE_TIME_MARGIN_SECONDS) { + if (timestamp < tNow.tv_sec + RELEASE_TIME_MARGIN_SECONDS) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "Service11TelecommandScheduling::doInsertActivity: Release time too close to " "current time" From 341a66c2651b884667191b51c8246dad898d0f70 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 9 Feb 2023 18:22:00 +0100 Subject: [PATCH 300/404] changelog --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index b7788808..7b2dcfc3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,9 @@ and this project adheres to [Semantic Versioning](http://semver.org/). ## Fixes +- Bugfix in `Service11TelecommandScheduling` which allowed commands + time tagged in the past to be inserted. + PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/738 - `CService200ModeManagement`: Various bugfixes which lead to now execution complete being generated on mode announcements, duplicate mode reply generated on announce commands, and the mode read subservice not working properly. From 14a92b3d89e37d50ccd46b250826cac293185d68 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 9 Feb 2023 18:28:16 +0100 Subject: [PATCH 301/404] typo --- src/fsfw/pus/CService200ModeCommanding.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/pus/CService200ModeCommanding.cpp b/src/fsfw/pus/CService200ModeCommanding.cpp index 77488585..0dbdedfe 100644 --- a/src/fsfw/pus/CService200ModeCommanding.cpp +++ b/src/fsfw/pus/CService200ModeCommanding.cpp @@ -19,7 +19,7 @@ ReturnValue_t CService200ModeCommanding::isValidSubservice(uint8_t subservice) { switch (subservice) { case (Subservice::COMMAND_MODE_COMMAND): case (Subservice::COMMAND_MODE_READ): - case (Subservice::COMMAND_MODE_ANNCOUNCE): + case (Subservice::COMMAND_MODE_ANNOUNCE): case (Subservice::COMMAND_MODE_ANNOUNCE_RECURSIVELY): return returnvalue::OK; default: From d302ba71858edfa15834ff8b28d6cce2c2cbbb84 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 10 Feb 2023 13:51:16 +0100 Subject: [PATCH 302/404] afmt --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 2 +- src/fsfw/devicehandlers/DeviceHandlerBase.h | 4 +--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index b725b186..55eb1b19 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -58,7 +58,7 @@ void DeviceHandlerBase::setHkDestination(object_id_t hkDestination) { this->hkDestination = hkDestination; } -void DeviceHandlerBase::setUpThermalModule(ThermalStateCfg cfg) { this->thermalStateCfg = cfg; } +void DeviceHandlerBase::enableThermalModule(ThermalStateCfg cfg) { this->thermalStateCfg = cfg; } DeviceHandlerBase::~DeviceHandlerBase() { if (comCookie != nullptr) { diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index b96506f5..5e05c3a8 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -164,10 +164,8 @@ class DeviceHandlerBase : public DeviceHandlerIF, * The device handler will then take care of creating local pool entries * for the device thermal state and device heating request. * Custom local pool IDs can be assigned as well. - * @param thermalStatePoolId - * @param thermalRequestPoolId */ - void setUpThermalModule(ThermalStateCfg cfg); + void enableThermalModule(ThermalStateCfg cfg); ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override; ModeTreeChildIF &getModeTreeChildIF() override; From 7fae6cbd6db588d69fc00198e4b2a9d8a7c12f59 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 12 Feb 2023 20:06:32 +0100 Subject: [PATCH 303/404] added missing CS unlock --- src/fsfw_hal/linux/spi/SpiComIF.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/fsfw_hal/linux/spi/SpiComIF.cpp b/src/fsfw_hal/linux/spi/SpiComIF.cpp index 11db7cfe..2f84275e 100644 --- a/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -214,6 +214,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const sif::printWarning("SpiComIF::sendMessage: Pulling low CS pin failed"); #endif #endif + csMutex->unlockMutex(); return result; } } else { From 8b4f73a97b15f27e314932538d468707c57f965a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 12 Feb 2023 21:07:11 +0100 Subject: [PATCH 304/404] better error msg --- src/fsfw_hal/linux/spi/SpiComIF.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/fsfw_hal/linux/spi/SpiComIF.cpp b/src/fsfw_hal/linux/spi/SpiComIF.cpp index 2f84275e..059025c8 100644 --- a/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -195,9 +195,13 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const if (result != returnvalue::OK) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "SpiComIF::sendMessage: Failed to lock mutex with code " - << "0x" << std::hex << std::setfill('0') << std::setw(4) << result << std::dec - << std::endl; + if (result == MutexIF::MUTEX_TIMEOUT) { + sif::error << "SpiComIF::sendMessage: Lock timeout" << std::endl; + } else { + sif::error << "SpiComIF::sendMessage: Failed to lock mutex with code " + << "0x" << std::hex << std::setfill('0') << std::setw(4) << result << std::dec + << std::endl; + } #else sif::printError("SpiComIF::sendMessage: Failed to lock mutex with code %d\n", result); #endif From dac2d210b597adfaf45bd5ae6a4c027599927601 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 13 Feb 2023 00:36:54 +0100 Subject: [PATCH 305/404] updates for thermal module in DHB --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 20 +++++++++---------- 1 file changed, 9 insertions(+), 11 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 55eb1b19..ec627142 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -222,12 +222,11 @@ ReturnValue_t DeviceHandlerBase::initialize() { fillCommandAndReplyMap(); if (thermalSet != nullptr) { + PoolReadGuard pg(thermalSet); // Set temperature target state to NON_OP. - result = thermalSet->read(); - if (result == returnvalue::OK) { + if (pg.getReadResult() == returnvalue::OK) { thermalSet->heaterRequest.value = ThermalComponentIF::STATE_REQUEST_NON_OPERATIONAL; thermalSet->heaterRequest.setValid(true); - thermalSet->commit(); } } @@ -589,12 +588,12 @@ void DeviceHandlerBase::setMode(Mode_t newMode, uint8_t newSubmode) { Clock::getUptime(&timeoutStart); if (mode == MODE_OFF and thermalSet != nullptr) { - ReturnValue_t result = thermalSet->read(); - if (result == returnvalue::OK) { + PoolReadGuard pg(thermalSet); + if (pg.getReadResult() == returnvalue::OK) { if (thermalSet->heaterRequest.value != ThermalComponentIF::STATE_REQUEST_IGNORE) { thermalSet->heaterRequest.value = ThermalComponentIF::STATE_REQUEST_NON_OPERATIONAL; } - thermalSet->heaterRequest.commit(PoolVariableIF::VALID); + thermalSet->heaterRequest.setValid(true); } } /* TODO: This will probably be done by the LocalDataPoolManager now */ @@ -1083,8 +1082,8 @@ ReturnValue_t DeviceHandlerBase::checkModeCommand(Mode_t commandedMode, Submode_ // Do not check thermal state for MODE_RAW if ((mode == MODE_OFF) and ((commandedMode == MODE_ON) or (commandedMode == MODE_NORMAL)) and (thermalSet != nullptr)) { - ReturnValue_t result = thermalSet->read(); - if (result == returnvalue::OK) { + PoolReadGuard pg(thermalSet); + if (pg.getReadResult() == returnvalue::OK) { if ((thermalSet->heaterRequest.value != ThermalComponentIF::STATE_REQUEST_IGNORE) and (not ThermalComponentIF::isOperational(thermalSet->thermalState.value))) { triggerEvent(ThermalComponentIF::TEMP_NOT_IN_OP_RANGE, thermalSet->thermalState.value); @@ -1145,11 +1144,10 @@ void DeviceHandlerBase::handleTransitionToOnMode(Mode_t commandedMode, Submode_t childTransitionDelay = getTransitionDelayMs(_MODE_START_UP, MODE_ON); triggerEvent(CHANGING_MODE, commandedMode, commandedSubmode); if (thermalSet != nullptr) { - ReturnValue_t result = thermalSet->read(); - if (result == returnvalue::OK) { + PoolReadGuard pg(thermalSet); + if (pg.getReadResult() == returnvalue::OK) { if (thermalSet->heaterRequest != ThermalComponentIF::STATE_REQUEST_IGNORE) { thermalSet->heaterRequest = ThermalComponentIF::STATE_REQUEST_OPERATIONAL; - thermalSet->commit(); } } } From f0b8457ba2d9a34a42b10314c3cdccfd46ebf168 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 13 Feb 2023 13:51:52 +0100 Subject: [PATCH 306/404] bugfix for SPI - Set transfer length to 0 for failed transfers --- src/fsfw_hal/linux/spi/SpiComIF.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/fsfw_hal/linux/spi/SpiComIF.cpp b/src/fsfw_hal/linux/spi/SpiComIF.cpp index 059025c8..ea25e837 100644 --- a/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -179,12 +179,11 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const uint32_t spiSpeed = 0; spiCookie->getSpiParameters(spiMode, spiSpeed, nullptr); setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed); - spiCookie->assignWriteBuffer(sendData); - spiCookie->setTransferSize(sendLen); bool fullDuplex = spiCookie->isFullDuplex(); gpioId_t gpioId = spiCookie->getChipSelectPin(); bool csLockManual = spiCookie->getCsLockManual(); + spiCookie->setTransferSize(0); MutexIF::TimeoutType csType; dur_millis_t csTimeout = 0; @@ -225,11 +224,15 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const updateLinePolarity(fileDescriptor); } + spiCookie->assignWriteBuffer(sendData); + spiCookie->setTransferSize(sendLen); + /* Execute transfer */ if (fullDuplex) { /* Initiate a full duplex SPI transfer. */ retval = ioctl(fileDescriptor, SPI_IOC_MESSAGE(1), spiCookie->getTransferStructHandle()); if (retval < 0) { + spiCookie->setTransferSize(0); utility::handleIoctlError("SpiComIF::sendMessage: ioctl error."); result = FULL_DUPLEX_TRANSFER_FAILED; } @@ -239,6 +242,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const } else { /* We write with a blocking half-duplex transfer here */ if (write(fileDescriptor, sendData, sendLen) != static_cast(sendLen)) { + spiCookie->setTransferSize(0); #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "SpiComIF::sendMessage: Half-Duplex write operation failed!" << std::endl; From d256ede8c1d8e7a746d3a56d45313d2b863e0b28 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Feb 2023 10:33:22 +0100 Subject: [PATCH 307/404] fix cppcheck lint --- src/fsfw/osal/common/TcpIpBase.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/fsfw/osal/common/TcpIpBase.cpp b/src/fsfw/osal/common/TcpIpBase.cpp index 486a5171..3e760f0e 100644 --- a/src/fsfw/osal/common/TcpIpBase.cpp +++ b/src/fsfw/osal/common/TcpIpBase.cpp @@ -41,6 +41,7 @@ int TcpIpBase::closeSocket(socket_t socket) { #elif defined(PLATFORM_UNIX) return close(socket); #endif + return -1; } int TcpIpBase::getLastSocketError() { @@ -49,4 +50,5 @@ int TcpIpBase::getLastSocketError() { #elif defined(PLATFORM_UNIX) return errno; #endif + return 0; } From 9de6c4b3aa20ee63c28051d486be8a12df147f22 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Feb 2023 14:33:02 +0100 Subject: [PATCH 308/404] printout corrections --- src/fsfw/version.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/fsfw/version.cpp b/src/fsfw/version.cpp index 050187a9..27d44c03 100644 --- a/src/fsfw/version.cpp +++ b/src/fsfw/version.cpp @@ -1,6 +1,7 @@ #include "version.h" #include +#include #include "fsfw/FSFWVersion.h" @@ -20,7 +21,7 @@ fsfw::Version::Version(int major, int minor, int revision, const char* addInfo) void fsfw::Version::getVersion(char* str, size_t maxLen) const { size_t len = snprintf(str, maxLen, "%d.%d.%d", major, minor, revision); - if (addInfo != nullptr) { + if (addInfo != nullptr and std::strcmp(addInfo, "") != 0) { snprintf(str + len, maxLen - len, "-%s", addInfo); } } @@ -30,7 +31,7 @@ namespace fsfw { #if FSFW_CPP_OSTREAM_ENABLED == 1 std::ostream& operator<<(std::ostream& os, const Version& v) { os << v.major << "." << v.minor << "." << v.revision; - if (v.addInfo != nullptr) { + if (v.addInfo != nullptr and std::strcmp(v.addInfo, "") != 0) { os << "-" << v.addInfo; } return os; From fe41d73895270cbe4d80ebfbc41ff9f0b8b02126 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 16 Feb 2023 17:57:03 +0100 Subject: [PATCH 309/404] remove obsolete mode --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 5 +---- src/fsfw/devicehandlers/DeviceHandlerBase.h | 5 ----- 2 files changed, 1 insertion(+), 9 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index ec627142..3dbacc95 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -376,7 +376,7 @@ void DeviceHandlerBase::doStateMachine() { } ReturnValue_t switchState = getStateOfSwitches(); if ((switchState == PowerSwitchIF::SWITCH_OFF) || (switchState == NO_SWITCH)) { - setMode(_MODE_SWITCH_IS_OFF); + setMode(MODE_OFF, SUBMODE_NONE); } } break; case MODE_OFF: @@ -389,9 +389,6 @@ void DeviceHandlerBase::doStateMachine() { case MODE_NORMAL: case MODE_ERROR_ON: break; - case _MODE_SWITCH_IS_OFF: - setMode(MODE_OFF, SUBMODE_NONE); - break; default: triggerEvent(OBJECT_IN_INVALID_MODE, mode, submode); setMode(_MODE_POWER_DOWN, 0); diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index 5e05c3a8..57e9d982 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -150,11 +150,6 @@ class DeviceHandlerBase : public DeviceHandlerIF, //! has been commanded on and the handler waits for it to be on. //! When the switch is on, the mode changes to @c _MODE_TO_ON. static const Mode_t _MODE_WAIT_ON = TRANSITION_MODE_BASE_ACTION_MASK | 4; - //! This is a transitional state which can not be commanded. The switch has - //! been commanded off and is off now. This state is only to do an RMAP - //! cycle once more where the doSendRead() function will set the mode to - //! MODE_OFF. The reason to do this is to get rid of stuck packets in the IO Board. - static const Mode_t _MODE_SWITCH_IS_OFF = TRANSITION_MODE_BASE_ACTION_MASK | 5; void setHkDestination(object_id_t hkDestination); From a6d707a7db589136ac2bd917cd8b3a3e2c16a0e4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 17 Feb 2023 02:04:17 +0100 Subject: [PATCH 310/404] SubsystemBase: Add function to update child modes --- src/fsfw/subsystem/SubsystemBase.cpp | 14 ++++++++++++++ src/fsfw/subsystem/SubsystemBase.h | 1 + 2 files changed, 15 insertions(+) diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp index 5cfe59a3..66858c39 100644 --- a/src/fsfw/subsystem/SubsystemBase.cpp +++ b/src/fsfw/subsystem/SubsystemBase.cpp @@ -117,6 +117,20 @@ ReturnValue_t SubsystemBase::updateChildMode(MessageQueueId_t queue, Mode_t mode return CHILD_NOT_FOUND; } +ReturnValue_t SubsystemBase::updateChildModeByObjId(object_id_t objectId, Mode_t mode, + Submode_t submode) { + std::map::iterator iter; + + for (iter = childrenMap.begin(); iter != childrenMap.end(); iter++) { + if (iter->first == objectId) { + iter->second.mode = mode; + iter->second.submode = submode; + return returnvalue::OK; + } + } + return CHILD_NOT_FOUND; +} + ReturnValue_t SubsystemBase::updateChildChangedHealth(MessageQueueId_t queue, bool changedHealth) { for (auto iter = childrenMap.begin(); iter != childrenMap.end(); iter++) { if (iter->second.commandQueue == queue) { diff --git a/src/fsfw/subsystem/SubsystemBase.h b/src/fsfw/subsystem/SubsystemBase.h index 0fbf9f4a..c3886d61 100644 --- a/src/fsfw/subsystem/SubsystemBase.h +++ b/src/fsfw/subsystem/SubsystemBase.h @@ -115,6 +115,7 @@ class SubsystemBase : public SystemObject, Submode_t targetSubmode = SUBMODE_NONE); ReturnValue_t updateChildMode(MessageQueueId_t queue, Mode_t mode, Submode_t submode); + ReturnValue_t updateChildModeByObjId(object_id_t objectId, Mode_t mode, Submode_t submode); ReturnValue_t updateChildChangedHealth(MessageQueueId_t queue, bool changedHealth = true); From be015b4c669995dc55bc316b006699be8542d941 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 17 Feb 2023 12:07:50 +0100 Subject: [PATCH 311/404] service 11: cast to fix warning --- src/fsfw/pus/Service11TelecommandScheduling.tpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/pus/Service11TelecommandScheduling.tpp b/src/fsfw/pus/Service11TelecommandScheduling.tpp index 2a20e822..8352f85d 100644 --- a/src/fsfw/pus/Service11TelecommandScheduling.tpp +++ b/src/fsfw/pus/Service11TelecommandScheduling.tpp @@ -160,7 +160,7 @@ inline ReturnValue_t Service11TelecommandScheduling::doInsertActivi // (See requirement for Time margin) timeval tNow = {}; Clock::getClock_timeval(&tNow); - if (timestamp < tNow.tv_sec + RELEASE_TIME_MARGIN_SECONDS) { + if (timestamp < static_cast(tNow.tv_sec + RELEASE_TIME_MARGIN_SECONDS)) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "Service11TelecommandScheduling::doInsertActivity: Release time too close to " "current time" From e3c968096b3a34b29bb8896afe0d1d698f834c6f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 18 Feb 2023 13:07:38 +0100 Subject: [PATCH 312/404] i2c small improvements --- src/fsfw_hal/linux/i2c/I2cComIF.cpp | 10 +++++----- src/fsfw_hal/linux/i2c/I2cComIF.h | 2 +- src/fsfw_hal/linux/i2c/I2cCookie.cpp | 4 ++-- src/fsfw_hal/linux/i2c/I2cCookie.h | 2 +- 4 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/src/fsfw_hal/linux/i2c/I2cComIF.cpp index 66c2bb51..d66e91de 100644 --- a/src/fsfw_hal/linux/i2c/I2cComIF.cpp +++ b/src/fsfw_hal/linux/i2c/I2cComIF.cpp @@ -103,7 +103,7 @@ ReturnValue_t I2cComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s if (fileHelper.getOpenResult() != returnvalue::OK) { return fileHelper.getOpenResult(); } - result = openDevice(deviceFile, i2cAddress, &fd); + result = openI2cSlave(deviceFile, i2cAddress, fd); if (result != returnvalue::OK) { return result; } @@ -162,7 +162,7 @@ ReturnValue_t I2cComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLe if (fileHelper.getOpenResult() != returnvalue::OK) { return fileHelper.getOpenResult(); } - result = openDevice(deviceFile, i2cAddress, &fd); + result = openI2cSlave(deviceFile, i2cAddress, fd); if (result != returnvalue::OK) { return result; } @@ -220,9 +220,9 @@ ReturnValue_t I2cComIF::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, return returnvalue::OK; } -ReturnValue_t I2cComIF::openDevice(std::string deviceFile, address_t i2cAddress, - int* fileDescriptor) { - if (ioctl(*fileDescriptor, I2C_SLAVE, i2cAddress) < 0) { +ReturnValue_t I2cComIF::openI2cSlave(const std::string& deviceFile, address_t i2cAddress, + int fileDescriptor) { + if (ioctl(fileDescriptor, I2C_SLAVE, i2cAddress) < 0) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "I2cComIF: Specifying target device failed with error code " << errno << "." diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.h b/src/fsfw_hal/linux/i2c/I2cComIF.h index 8c44cee0..4bcc8cfa 100644 --- a/src/fsfw_hal/linux/i2c/I2cComIF.h +++ b/src/fsfw_hal/linux/i2c/I2cComIF.h @@ -49,7 +49,7 @@ class I2cComIF : public DeviceCommunicationIF, public SystemObject { * @param fileDescriptor Pointer to device descriptor. * @return returnvalue::OK if successful, otherwise returnvalue::FAILED. */ - ReturnValue_t openDevice(std::string deviceFile, address_t i2cAddress, int *fileDescriptor); + ReturnValue_t openI2cSlave(const std::string& deviceFile, address_t i2cAddress, int fileDescriptor); }; #endif /* LINUX_I2C_I2COMIF_H_ */ diff --git a/src/fsfw_hal/linux/i2c/I2cCookie.cpp b/src/fsfw_hal/linux/i2c/I2cCookie.cpp index 0186be60..f41527a3 100644 --- a/src/fsfw_hal/linux/i2c/I2cCookie.cpp +++ b/src/fsfw_hal/linux/i2c/I2cCookie.cpp @@ -1,12 +1,12 @@ #include "fsfw_hal/linux/i2c/I2cCookie.h" I2cCookie::I2cCookie(address_t i2cAddress_, size_t maxReplyLen_, std::string deviceFile_) - : i2cAddress(i2cAddress_), maxReplyLen(maxReplyLen_), deviceFile(deviceFile_) {} + : i2cAddress(i2cAddress_), maxReplyLen(maxReplyLen_), deviceFile(std::move(deviceFile_)) {} address_t I2cCookie::getAddress() const { return i2cAddress; } size_t I2cCookie::getMaxReplyLen() const { return maxReplyLen; } -std::string I2cCookie::getDeviceFile() const { return deviceFile; } +const std::string& I2cCookie::getDeviceFile() const { return deviceFile; } I2cCookie::~I2cCookie() {} diff --git a/src/fsfw_hal/linux/i2c/I2cCookie.h b/src/fsfw_hal/linux/i2c/I2cCookie.h index 8be71205..e54bcd55 100644 --- a/src/fsfw_hal/linux/i2c/I2cCookie.h +++ b/src/fsfw_hal/linux/i2c/I2cCookie.h @@ -25,7 +25,7 @@ class I2cCookie : public CookieIF { address_t getAddress() const; size_t getMaxReplyLen() const; - std::string getDeviceFile() const; + const std::string& getDeviceFile() const; uint8_t errorCounter = 0; From fcd84b59aee192fb0eb3a3550f036af952a68fc1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 18 Feb 2023 13:37:39 +0100 Subject: [PATCH 313/404] how the hell does this break anything --- src/fsfw_hal/linux/i2c/I2cComIF.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/src/fsfw_hal/linux/i2c/I2cComIF.cpp index d66e91de..10e493e5 100644 --- a/src/fsfw_hal/linux/i2c/I2cComIF.cpp +++ b/src/fsfw_hal/linux/i2c/I2cComIF.cpp @@ -66,8 +66,7 @@ ReturnValue_t I2cComIF::initializeInterface(CookieIF* cookie) { ReturnValue_t I2cComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, size_t sendLen) { ReturnValue_t result; - int fd; - std::string deviceFile; + int fd = 0; if (sendData == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -98,7 +97,7 @@ ReturnValue_t I2cComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s return returnvalue::FAILED; } - deviceFile = i2cCookie->getDeviceFile(); + const auto& deviceFile = i2cCookie->getDeviceFile(); UnixFileGuard fileHelper(deviceFile, &fd, O_RDWR, "I2cComIF::sendMessage"); if (fileHelper.getOpenResult() != returnvalue::OK) { return fileHelper.getOpenResult(); @@ -132,7 +131,6 @@ ReturnValue_t I2cComIF::getSendSuccess(CookieIF* cookie) { return returnvalue::O ReturnValue_t I2cComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLen) { ReturnValue_t result; int fd; - std::string deviceFile; if (requestLen == 0) { return returnvalue::OK; @@ -157,7 +155,7 @@ ReturnValue_t I2cComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLe } i2cDeviceMapIter->second.replyLen = 0; - deviceFile = i2cCookie->getDeviceFile(); + auto& deviceFile = i2cCookie->getDeviceFile(); UnixFileGuard fileHelper(deviceFile, &fd, O_RDWR, "I2cComIF::requestReceiveMessage"); if (fileHelper.getOpenResult() != returnvalue::OK) { return fileHelper.getOpenResult(); From 3568bdbecf74733970211d7ff713ad030b332a98 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 18 Feb 2023 13:45:49 +0100 Subject: [PATCH 314/404] lets see if this fixes the issue --- src/fsfw_hal/linux/UnixFileGuard.cpp | 15 ++++----------- src/fsfw_hal/linux/UnixFileGuard.h | 12 ++++++++++-- src/fsfw_hal/linux/i2c/I2cComIF.cpp | 16 +++++++++------- src/fsfw_hal/linux/i2c/I2cComIF.h | 3 ++- src/fsfw_hal/linux/spi/SpiComIF.cpp | 6 +++--- 5 files changed, 28 insertions(+), 24 deletions(-) diff --git a/src/fsfw_hal/linux/UnixFileGuard.cpp b/src/fsfw_hal/linux/UnixFileGuard.cpp index 3e916ba2..fb8493ca 100644 --- a/src/fsfw_hal/linux/UnixFileGuard.cpp +++ b/src/fsfw_hal/linux/UnixFileGuard.cpp @@ -6,14 +6,11 @@ #include "fsfw/FSFW.h" #include "fsfw/serviceinterface.h" -UnixFileGuard::UnixFileGuard(const std::string& device, int* fileDescriptor, int flags, +UnixFileGuard::UnixFileGuard(const std::string& device, int& fileDescriptor, int flags, std::string diagnosticPrefix) : fileDescriptor(fileDescriptor) { - if (fileDescriptor == nullptr) { - return; - } - *fileDescriptor = open(device.c_str(), flags); - if (*fileDescriptor < 0) { + fileDescriptor = open(device.c_str(), flags); + if (fileDescriptor < 0) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << diagnosticPrefix << ": Opening device failed with error code " << errno << ": " @@ -27,10 +24,6 @@ UnixFileGuard::UnixFileGuard(const std::string& device, int* fileDescriptor, int } } -UnixFileGuard::~UnixFileGuard() { - if (fileDescriptor != nullptr) { - close(*fileDescriptor); - } -} +UnixFileGuard::~UnixFileGuard() { close(fileDescriptor); } ReturnValue_t UnixFileGuard::getOpenResult() const { return openStatus; } diff --git a/src/fsfw_hal/linux/UnixFileGuard.h b/src/fsfw_hal/linux/UnixFileGuard.h index eec85233..19da5f4a 100644 --- a/src/fsfw_hal/linux/UnixFileGuard.h +++ b/src/fsfw_hal/linux/UnixFileGuard.h @@ -15,7 +15,15 @@ class UnixFileGuard { static constexpr ReturnValue_t OPEN_FILE_FAILED = 1; - UnixFileGuard(const std::string& device, int* fileDescriptor, int flags, + /** + * Open a device and assign the given file descriptor variable + * @param device [in] Device name. + * @param fileDescriptor [in/out] Will be assigned by file guard and re-used to + * close the guard. + * @param flags + * @param diagnosticPrefix + */ + UnixFileGuard(const std::string& device, int& fileDescriptor, int flags, std::string diagnosticPrefix = ""); virtual ~UnixFileGuard(); @@ -23,7 +31,7 @@ class UnixFileGuard { ReturnValue_t getOpenResult() const; private: - int* fileDescriptor = nullptr; + int fileDescriptor = 0; ReturnValue_t openStatus = returnvalue::OK; }; diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/src/fsfw_hal/linux/i2c/I2cComIF.cpp index 10e493e5..0a88edc7 100644 --- a/src/fsfw_hal/linux/i2c/I2cComIF.cpp +++ b/src/fsfw_hal/linux/i2c/I2cComIF.cpp @@ -98,16 +98,17 @@ ReturnValue_t I2cComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s } const auto& deviceFile = i2cCookie->getDeviceFile(); - UnixFileGuard fileHelper(deviceFile, &fd, O_RDWR, "I2cComIF::sendMessage"); + UnixFileGuard fileHelper(deviceFile, fd, O_RDWR, "I2cComIF::sendMessage"); if (fileHelper.getOpenResult() != returnvalue::OK) { return fileHelper.getOpenResult(); } - result = openI2cSlave(deviceFile, i2cAddress, fd); + int slaveFd = 0; + result = openI2cSlave(deviceFile, i2cAddress, slaveFd); if (result != returnvalue::OK) { return result; } - if (write(fd, sendData, sendLen) != static_cast(sendLen)) { + if (write(slaveFd, sendData, sendLen) != static_cast(sendLen)) { i2cCookie->errorCounter++; if (i2cCookie->errorCounter < 3) { #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -130,7 +131,7 @@ ReturnValue_t I2cComIF::getSendSuccess(CookieIF* cookie) { return returnvalue::O ReturnValue_t I2cComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLen) { ReturnValue_t result; - int fd; + int fd = 0; if (requestLen == 0) { return returnvalue::OK; @@ -156,11 +157,12 @@ ReturnValue_t I2cComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLe i2cDeviceMapIter->second.replyLen = 0; auto& deviceFile = i2cCookie->getDeviceFile(); - UnixFileGuard fileHelper(deviceFile, &fd, O_RDWR, "I2cComIF::requestReceiveMessage"); + UnixFileGuard fileHelper(deviceFile, fd, O_RDWR, "I2cComIF::requestReceiveMessage"); if (fileHelper.getOpenResult() != returnvalue::OK) { return fileHelper.getOpenResult(); } - result = openI2cSlave(deviceFile, i2cAddress, fd); + int slaveFd = 0; + result = openI2cSlave(deviceFile, i2cAddress, slaveFd); if (result != returnvalue::OK) { return result; } @@ -219,7 +221,7 @@ ReturnValue_t I2cComIF::readReceivedMessage(CookieIF* cookie, uint8_t** buffer, } ReturnValue_t I2cComIF::openI2cSlave(const std::string& deviceFile, address_t i2cAddress, - int fileDescriptor) { + int& fileDescriptor) { if (ioctl(fileDescriptor, I2C_SLAVE, i2cAddress) < 0) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.h b/src/fsfw_hal/linux/i2c/I2cComIF.h index 4bcc8cfa..983ce734 100644 --- a/src/fsfw_hal/linux/i2c/I2cComIF.h +++ b/src/fsfw_hal/linux/i2c/I2cComIF.h @@ -49,7 +49,8 @@ class I2cComIF : public DeviceCommunicationIF, public SystemObject { * @param fileDescriptor Pointer to device descriptor. * @return returnvalue::OK if successful, otherwise returnvalue::FAILED. */ - ReturnValue_t openI2cSlave(const std::string& deviceFile, address_t i2cAddress, int fileDescriptor); + ReturnValue_t openI2cSlave(const std::string &deviceFile, address_t i2cAddress, + int &fileDescriptor); }; #endif /* LINUX_I2C_I2COMIF_H_ */ diff --git a/src/fsfw_hal/linux/spi/SpiComIF.cpp b/src/fsfw_hal/linux/spi/SpiComIF.cpp index ea25e837..d285b120 100644 --- a/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -75,7 +75,7 @@ ReturnValue_t SpiComIF::initializeInterface(CookieIF* cookie) { spiCookie->getSpiParameters(spiMode, spiSpeed, ¶ms); int fileDescriptor = 0; - UnixFileGuard fileHelper(dev, &fileDescriptor, O_RDWR, "SpiComIF::initializeInterface"); + UnixFileGuard fileHelper(dev, fileDescriptor, O_RDWR, "SpiComIF::initializeInterface"); if (fileHelper.getOpenResult() != returnvalue::OK) { return fileHelper.getOpenResult(); } @@ -171,7 +171,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const int retval = 0; /* Prepare transfer */ int fileDescriptor = 0; - UnixFileGuard fileHelper(dev, &fileDescriptor, O_RDWR, "SpiComIF::sendMessage"); + UnixFileGuard fileHelper(dev, fileDescriptor, O_RDWR, "SpiComIF::sendMessage"); if (fileHelper.getOpenResult() != returnvalue::OK) { return OPENING_FILE_FAILED; } @@ -285,7 +285,7 @@ ReturnValue_t SpiComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLe ReturnValue_t SpiComIF::performHalfDuplexReception(SpiCookie* spiCookie) { ReturnValue_t result = returnvalue::OK; int fileDescriptor = 0; - UnixFileGuard fileHelper(dev, &fileDescriptor, O_RDWR, "SpiComIF::requestReceiveMessage"); + UnixFileGuard fileHelper(dev, fileDescriptor, O_RDWR, "SpiComIF::requestReceiveMessage"); if (fileHelper.getOpenResult() != returnvalue::OK) { return OPENING_FILE_FAILED; } From 5f9eb01d943d8eed610d395aa3f0c6017ea1c58f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 18 Feb 2023 13:46:51 +0100 Subject: [PATCH 315/404] better naming --- src/fsfw_hal/linux/UnixFileGuard.cpp | 4 ++-- src/fsfw_hal/linux/UnixFileGuard.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fsfw_hal/linux/UnixFileGuard.cpp b/src/fsfw_hal/linux/UnixFileGuard.cpp index fb8493ca..0a44e445 100644 --- a/src/fsfw_hal/linux/UnixFileGuard.cpp +++ b/src/fsfw_hal/linux/UnixFileGuard.cpp @@ -8,7 +8,7 @@ UnixFileGuard::UnixFileGuard(const std::string& device, int& fileDescriptor, int flags, std::string diagnosticPrefix) - : fileDescriptor(fileDescriptor) { + : cachedFd(fileDescriptor) { fileDescriptor = open(device.c_str(), flags); if (fileDescriptor < 0) { #if FSFW_VERBOSE_LEVEL >= 1 @@ -24,6 +24,6 @@ UnixFileGuard::UnixFileGuard(const std::string& device, int& fileDescriptor, int } } -UnixFileGuard::~UnixFileGuard() { close(fileDescriptor); } +UnixFileGuard::~UnixFileGuard() { close(cachedFd); } ReturnValue_t UnixFileGuard::getOpenResult() const { return openStatus; } diff --git a/src/fsfw_hal/linux/UnixFileGuard.h b/src/fsfw_hal/linux/UnixFileGuard.h index 19da5f4a..25386aa3 100644 --- a/src/fsfw_hal/linux/UnixFileGuard.h +++ b/src/fsfw_hal/linux/UnixFileGuard.h @@ -31,7 +31,7 @@ class UnixFileGuard { ReturnValue_t getOpenResult() const; private: - int fileDescriptor = 0; + int cachedFd = 0; ReturnValue_t openStatus = returnvalue::OK; }; From 2d6622b8b86f9e77eb9e350698cfb10d863f32f7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 18 Feb 2023 13:51:24 +0100 Subject: [PATCH 316/404] wtf is this interface --- src/fsfw_hal/linux/i2c/I2cComIF.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/src/fsfw_hal/linux/i2c/I2cComIF.cpp index 0a88edc7..b1d54701 100644 --- a/src/fsfw_hal/linux/i2c/I2cComIF.cpp +++ b/src/fsfw_hal/linux/i2c/I2cComIF.cpp @@ -102,13 +102,12 @@ ReturnValue_t I2cComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s if (fileHelper.getOpenResult() != returnvalue::OK) { return fileHelper.getOpenResult(); } - int slaveFd = 0; - result = openI2cSlave(deviceFile, i2cAddress, slaveFd); + result = openI2cSlave(deviceFile, i2cAddress, fd); if (result != returnvalue::OK) { return result; } - if (write(slaveFd, sendData, sendLen) != static_cast(sendLen)) { + if (write(fd, sendData, sendLen) != static_cast(sendLen)) { i2cCookie->errorCounter++; if (i2cCookie->errorCounter < 3) { #if FSFW_CPP_OSTREAM_ENABLED == 1 @@ -161,8 +160,7 @@ ReturnValue_t I2cComIF::requestReceiveMessage(CookieIF* cookie, size_t requestLe if (fileHelper.getOpenResult() != returnvalue::OK) { return fileHelper.getOpenResult(); } - int slaveFd = 0; - result = openI2cSlave(deviceFile, i2cAddress, slaveFd); + result = openI2cSlave(deviceFile, i2cAddress, fd); if (result != returnvalue::OK) { return result; } From c8469ca6473f64676e007e2e2f1c733fe6252053 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 18 Feb 2023 14:03:19 +0100 Subject: [PATCH 317/404] a lot of bug potential here --- src/fsfw_hal/linux/UnixFileGuard.cpp | 4 ++-- src/fsfw_hal/linux/UnixFileGuard.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fsfw_hal/linux/UnixFileGuard.cpp b/src/fsfw_hal/linux/UnixFileGuard.cpp index 0a44e445..3178f39d 100644 --- a/src/fsfw_hal/linux/UnixFileGuard.cpp +++ b/src/fsfw_hal/linux/UnixFileGuard.cpp @@ -8,7 +8,7 @@ UnixFileGuard::UnixFileGuard(const std::string& device, int& fileDescriptor, int flags, std::string diagnosticPrefix) - : cachedFd(fileDescriptor) { + : fdRef(fileDescriptor) { fileDescriptor = open(device.c_str(), flags); if (fileDescriptor < 0) { #if FSFW_VERBOSE_LEVEL >= 1 @@ -24,6 +24,6 @@ UnixFileGuard::UnixFileGuard(const std::string& device, int& fileDescriptor, int } } -UnixFileGuard::~UnixFileGuard() { close(cachedFd); } +UnixFileGuard::~UnixFileGuard() { close(fdRef); } ReturnValue_t UnixFileGuard::getOpenResult() const { return openStatus; } diff --git a/src/fsfw_hal/linux/UnixFileGuard.h b/src/fsfw_hal/linux/UnixFileGuard.h index 25386aa3..884e551c 100644 --- a/src/fsfw_hal/linux/UnixFileGuard.h +++ b/src/fsfw_hal/linux/UnixFileGuard.h @@ -31,7 +31,7 @@ class UnixFileGuard { ReturnValue_t getOpenResult() const; private: - int cachedFd = 0; + int& fdRef; ReturnValue_t openStatus = returnvalue::OK; }; From c32798522283d89028b7c1ecd3bd33b8391e1a39 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 20 Feb 2023 15:02:00 +0100 Subject: [PATCH 318/404] printout tweak --- src/fsfw/osal/common/UdpTmTcBridge.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/fsfw/osal/common/UdpTmTcBridge.cpp b/src/fsfw/osal/common/UdpTmTcBridge.cpp index c0848ceb..ee4c806b 100644 --- a/src/fsfw/osal/common/UdpTmTcBridge.cpp +++ b/src/fsfw/osal/common/UdpTmTcBridge.cpp @@ -126,10 +126,7 @@ ReturnValue_t UdpTmTcBridge::sendTm(const uint8_t *data, size_t dataLen) { tcpip::handleError(tcpip::Protocol::UDP, tcpip::ErrorSources::SENDTO_CALL); } #if FSFW_CPP_OSTREAM_ENABLED == 1 && FSFW_UDP_SEND_WIRETAPPING_ENABLED == 1 - sif::debug << "TmTcUdpBridge::sendTm: " << bytesSent - << " bytes were" - " sent." - << std::endl; + sif::debug << "TmTcUdpBridge::sendTm: " << bytesSent << " bytes were sent" << std::endl; #endif return returnvalue::OK; } From 2a0c244468e40c4d036a2c4d5eeec3af03a2f064 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 20 Feb 2023 16:10:21 +0100 Subject: [PATCH 319/404] add pus 15 store ID --- src/fsfw/objectmanager/frameworkObjects.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/fsfw/objectmanager/frameworkObjects.h b/src/fsfw/objectmanager/frameworkObjects.h index 9487147d..bf153cae 100644 --- a/src/fsfw/objectmanager/frameworkObjects.h +++ b/src/fsfw/objectmanager/frameworkObjects.h @@ -15,6 +15,7 @@ enum framework_objects : object_id_t { PUS_SERVICE_8_FUNCTION_MGMT = 0x53000008, PUS_SERVICE_9_TIME_MGMT = 0x53000009, PUS_SERVICE_11_TC_SCHEDULER = 0x53000011, + PUS_SERVICE_15_TM_STORAGE = 0x53000015, PUS_SERVICE_17_TEST = 0x53000017, PUS_SERVICE_20_PARAMETERS = 0x53000020, PUS_SERVICE_200_MODE_MGMT = 0x53000200, From 2efff4d2c5ef82b5b62567ab1bb0ee53aeed6a5a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 21 Feb 2023 02:59:13 +0100 Subject: [PATCH 320/404] missing include --- src/fsfw_hal/linux/spi/ManualCsLockGuard.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/fsfw_hal/linux/spi/ManualCsLockGuard.h b/src/fsfw_hal/linux/spi/ManualCsLockGuard.h index feb6dd83..1f0997b0 100644 --- a/src/fsfw_hal/linux/spi/ManualCsLockGuard.h +++ b/src/fsfw_hal/linux/spi/ManualCsLockGuard.h @@ -1,5 +1,6 @@ #pragma once +#include #include "fsfw/ipc/MutexIF.h" #include "fsfw/returnvalues/returnvalue.h" #include "fsfw_hal/common/gpio/GpioIF.h" From bd208038dd85a94dce8c763397ad5ac7eae76402 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 22 Feb 2023 15:45:39 +0100 Subject: [PATCH 321/404] printout fixes --- src/fsfw/tasks/FixedTimeslotTaskBase.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/fsfw/tasks/FixedTimeslotTaskBase.cpp b/src/fsfw/tasks/FixedTimeslotTaskBase.cpp index 3327deae..0e1e3c5d 100644 --- a/src/fsfw/tasks/FixedTimeslotTaskBase.cpp +++ b/src/fsfw/tasks/FixedTimeslotTaskBase.cpp @@ -15,10 +15,10 @@ ReturnValue_t FixedTimeslotTaskBase::addSlot(object_id_t execId, ExecutableObjec uint32_t slotTimeMs, int8_t executionStep) { if (execObj == nullptr) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "Component 0x" << std::hex << std::setw(8) << std::setfill('0') << execObj + sif::error << "Component 0x" << std::hex << std::setw(8) << std::setfill('0') << execId << std::setfill(' ') << " not found, not adding it to PST" << std::dec << std::endl; #else - sif::printError("Component 0x%08x not found, not adding it to PST\n"); + sif::printError("Component 0x%08x not found, not adding it to PST\n", execId); #endif return returnvalue::FAILED; } From abcf1b29b2002e05b8a3974a9bc27f69531b8668 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 24 Feb 2023 14:50:36 +0100 Subject: [PATCH 322/404] execution complete --- src/fsfw/pus/CServiceHealthCommanding.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/pus/CServiceHealthCommanding.cpp b/src/fsfw/pus/CServiceHealthCommanding.cpp index 57b704fd..49284b73 100644 --- a/src/fsfw/pus/CServiceHealthCommanding.cpp +++ b/src/fsfw/pus/CServiceHealthCommanding.cpp @@ -79,7 +79,7 @@ ReturnValue_t CServiceHealthCommanding::prepareCommand(CommandMessage *message, } case (Subservice::COMMAND_ANNOUNCE_HEALTH): { HealthMessage::setHealthMessage(message, HealthMessage::HEALTH_ANNOUNCE); - break; + return CommandingServiceBase::EXECUTION_COMPLETE; } case (Subservice::COMMAND_ANNOUNCE_HEALTH_ALL): { ReturnValue_t result = iterateHealthTable(true); From f0415a97b1b610e821f44726e75674a2317de135 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 24 Feb 2023 15:49:05 +0100 Subject: [PATCH 323/404] some fixes and improvements --- src/fsfw/cfdp/handler/DestHandler.cpp | 104 ++++++++++++++------- src/fsfw/cfdp/handler/DestHandler.h | 4 +- src/fsfw/cfdp/handler/defs.h | 3 + src/fsfw_hal/linux/spi/ManualCsLockGuard.h | 1 + 4 files changed, 75 insertions(+), 37 deletions(-) diff --git a/src/fsfw/cfdp/handler/DestHandler.cpp b/src/fsfw/cfdp/handler/DestHandler.cpp index b4d4af09..1b1187b1 100644 --- a/src/fsfw/cfdp/handler/DestHandler.cpp +++ b/src/fsfw/cfdp/handler/DestHandler.cpp @@ -178,16 +178,25 @@ ReturnValue_t cfdp::DestHandler::handleFileDataPdu(const cfdp::PacketInfo& info) dp.user.fileSegmentRecvdIndication(segParams); } result = dp.user.vfs.writeToFile(fileOpParams, fileData); - if (offset.value() + fileSegmentLen > tp.progress) { - tp.progress = offset.value() + fileSegmentLen; - } if (result != returnvalue::OK) { // TODO: Proper Error handling #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "File write error" << std::endl; + sif::error << "cfdp::DestHandler: VFS file write error with code 0x" << std::hex << std::setw(2) + << result << std::endl; #endif + tp.vfsErrorCount++; + if (tp.vfsErrorCount < 3) { + // TODO: Provide execution step as parameter + fp.eventReporter->forwardEvent(events::FILESTORE_ERROR, static_cast(fsmRes.step), + result); + } + return result; } else { tp.deliveryStatus = FileDeliveryStatus::RETAINED_IN_FILESTORE; + tp.vfsErrorCount = 0; + } + if (offset.value() + fileSegmentLen > tp.progress) { + tp.progress = offset.value() + fileSegmentLen; } return result; } @@ -271,35 +280,62 @@ ReturnValue_t cfdp::DestHandler::startTransaction(MetadataPduReader& reader, Met return OK; } ReturnValue_t result = OK; - fsmRes.step = TransactionStep::TRANSACTION_START; - if (reader.getTransmissionMode() == TransmissionMode::UNACKNOWLEDGED) { - fsmRes.state = CfdpStates::BUSY_CLASS_1_NACKED; - } else if (reader.getTransmissionMode() == TransmissionMode::ACKNOWLEDGED) { - fsmRes.state = CfdpStates::BUSY_CLASS_2_ACKED; - } - tp.checksumType = info.getChecksumType(); - tp.closureRequested = info.isClosureRequested(); size_t sourceNameSize = 0; const uint8_t* sourceNamePtr = info.getSourceFileName().getValue(&sourceNameSize); - if (sourceNameSize > tp.sourceName.size()) { - // TODO: Warning, event etc. + if (sourceNameSize + 1 > tp.sourceName.size()) { + fp.eventReporter->forwardEvent(events::FILENAME_TOO_LARGE_ERROR, 0, 0); +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "cfdp::DestHandler: source filename too large" << std::endl; +#endif return FAILED; } std::memcpy(tp.sourceName.data(), sourceNamePtr, sourceNameSize); tp.sourceName[sourceNameSize] = '\0'; size_t destNameSize = 0; const uint8_t* destNamePtr = info.getDestFileName().getValue(&destNameSize); - if (destNameSize > tp.destName.size()) { - // TODO: Warning, event etc. + if (destNameSize + 1 > tp.destName.size()) { + fp.eventReporter->forwardEvent(events::FILENAME_TOO_LARGE_ERROR, 0, 0); +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "cfdp::DestHandler: dest filename too large" << std::endl; +#endif return FAILED; } std::memcpy(tp.destName.data(), destNamePtr, destNameSize); tp.destName[destNameSize] = '\0'; - reader.fillConfig(tp.pduConf); - tp.pduConf.direction = Direction::TOWARDS_SENDER; - tp.transactionId.entityId = tp.pduConf.sourceId; - tp.transactionId.seqNum = tp.pduConf.seqNum; - if (not dp.remoteCfgTable.getRemoteCfg(tp.pduConf.sourceId, &tp.remoteCfg)) { + + // If both dest name size and source name size are 0, we are dealing with a metadata only PDU, + // so there is no need to create a file or truncate an existing file + if (destNameSize > 0 and sourceNameSize > 0) { + FilesystemParams fparams(tp.destName.data()); + if (dp.user.vfs.fileExists(fparams)) { + result = dp.user.vfs.truncateFile(fparams); + if (result != returnvalue::OK) { + // TODO: Provide execution step as parameter + fp.eventReporter->forwardEvent(events::FILESTORE_ERROR, static_cast(fsmRes.step), + result); +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "cfdp::DestHandler: file truncation error with code " << result + << std::endl; +#endif + return FAILED; + // TODO: Relevant for filestore rejection error? + } + } else { + result = dp.user.vfs.createFile(fparams); + if (result != OK) { + fp.eventReporter->forwardEvent(events::FILESTORE_ERROR, static_cast(fsmRes.step), + result); +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "cfdp::DestHandler: file creation error with code " << result << std::endl; +#endif + return FAILED; + // TODO: Relevant for filestore rejection error? + } + } + } + EntityId sourceId; + reader.getSourceId(sourceId); + if (not dp.remoteCfgTable.getRemoteCfg(sourceId, &tp.remoteCfg)) { // TODO: Warning, event etc. #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "cfdp::DestHandler" << __func__ @@ -308,22 +344,18 @@ ReturnValue_t cfdp::DestHandler::startTransaction(MetadataPduReader& reader, Met #endif return FAILED; } - // If both dest name size and source name size are 0, we are dealing with a metadata only PDU, - // so there is no need to create a file or truncate an existing file - if (destNameSize > 0 and sourceNameSize > 0) { - FilesystemParams fparams(tp.destName.data()); - // TODO: Filesystem errors? - if (dp.user.vfs.fileExists(fparams)) { - dp.user.vfs.truncateFile(fparams); - } else { - result = dp.user.vfs.createFile(fparams); - if (result != OK) { - // TODO: Handle FS error. This is probably a case for the filestore rejection mechanism of - // CFDP. - // In any case, it does not really make sense to continue here - } - } + fsmRes.step = TransactionStep::TRANSACTION_START; + if (reader.getTransmissionMode() == TransmissionMode::UNACKNOWLEDGED) { + fsmRes.state = CfdpStates::BUSY_CLASS_1_NACKED; + } else if (reader.getTransmissionMode() == TransmissionMode::ACKNOWLEDGED) { + fsmRes.state = CfdpStates::BUSY_CLASS_2_ACKED; } + tp.checksumType = info.getChecksumType(); + tp.closureRequested = info.isClosureRequested(); + reader.fillConfig(tp.pduConf); + tp.pduConf.direction = Direction::TOWARDS_SENDER; + tp.transactionId.entityId = tp.pduConf.sourceId; + tp.transactionId.seqNum = tp.pduConf.seqNum; fsmRes.step = TransactionStep::RECEIVING_FILE_DATA_PDUS; MetadataRecvdParams params(tp.transactionId, tp.pduConf.sourceId); params.fileSize = tp.fileSize.getSize(); diff --git a/src/fsfw/cfdp/handler/DestHandler.h b/src/fsfw/cfdp/handler/DestHandler.h index 5cef88d4..357493df 100644 --- a/src/fsfw/cfdp/handler/DestHandler.h +++ b/src/fsfw/cfdp/handler/DestHandler.h @@ -84,7 +84,7 @@ enum class CallStatus { DONE, CALL_AFTER_DELAY, CALL_AGAIN }; class DestHandler { public: - enum class TransactionStep { + enum class TransactionStep : uint8_t { IDLE = 0, TRANSACTION_START = 1, RECEIVING_FILE_DATA_PDUS = 2, @@ -157,11 +157,13 @@ class DestHandler { progress = 0; remoteCfg = nullptr; closureRequested = false; + vfsErrorCount = 0; checksumType = ChecksumType::NULL_CHECKSUM; } ChecksumType checksumType = ChecksumType::NULL_CHECKSUM; bool closureRequested = false; + uint16_t vfsErrorCount = 0; std::vector sourceName; std::vector destName; cfdp::FileSize fileSize; diff --git a/src/fsfw/cfdp/handler/defs.h b/src/fsfw/cfdp/handler/defs.h index 190fb67d..5f17ca2d 100644 --- a/src/fsfw/cfdp/handler/defs.h +++ b/src/fsfw/cfdp/handler/defs.h @@ -12,6 +12,9 @@ namespace events { static constexpr Event STORE_ERROR = event::makeEvent(SSID, 0, severity::LOW); static constexpr Event MSG_QUEUE_ERROR = event::makeEvent(SSID, 1, severity::LOW); static constexpr Event SERIALIZATION_ERROR = event::makeEvent(SSID, 2, severity::LOW); +static constexpr Event FILESTORE_ERROR = event::makeEvent(SSID, 3, severity::LOW); +//! [EXPORT] : [COMMENT] P1: Transaction step ID, P2: 0 for source file name, 1 for dest file name +static constexpr Event FILENAME_TOO_LARGE_ERROR = event::makeEvent(SSID, 4, severity::LOW); } // namespace events diff --git a/src/fsfw_hal/linux/spi/ManualCsLockGuard.h b/src/fsfw_hal/linux/spi/ManualCsLockGuard.h index 1f0997b0..8456f9f1 100644 --- a/src/fsfw_hal/linux/spi/ManualCsLockGuard.h +++ b/src/fsfw_hal/linux/spi/ManualCsLockGuard.h @@ -1,6 +1,7 @@ #pragma once #include + #include "fsfw/ipc/MutexIF.h" #include "fsfw/returnvalues/returnvalue.h" #include "fsfw_hal/common/gpio/GpioIF.h" From 6eba84566d2efea8cb7a9278e70055f8a8211606 Mon Sep 17 00:00:00 2001 From: meggert Date: Fri, 24 Feb 2023 16:39:49 +0100 Subject: [PATCH 324/404] use timeval instead of uptime --- src/fsfw/timemanager/Stopwatch.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/fsfw/timemanager/Stopwatch.cpp b/src/fsfw/timemanager/Stopwatch.cpp index 05d5c0e8..4bc3a460 100644 --- a/src/fsfw/timemanager/Stopwatch.cpp +++ b/src/fsfw/timemanager/Stopwatch.cpp @@ -9,7 +9,7 @@ Stopwatch::Stopwatch(bool displayOnDestruction, StopwatchDisplayMode displayMode) : displayOnDestruction(displayOnDestruction), displayMode(displayMode) { // Measures start time on initialization. - Clock::getUptime(&startTime); + Clock::getClock_timeval(&startTime); } void Stopwatch::start() { Clock::getUptime(&startTime); } @@ -63,6 +63,6 @@ StopwatchDisplayMode Stopwatch::getDisplayMode() const { return displayMode; } void Stopwatch::stopInternal() { timeval endTime; - Clock::getUptime(&endTime); + Clock::getClock_timeval(&endTime); elapsedTime = endTime - startTime; } From 893b43472872cdc90b4d602670c8fa9750ca7407 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 24 Feb 2023 16:49:23 +0100 Subject: [PATCH 325/404] allow dest handler to handle folder destinations --- src/fsfw/cfdp/handler/DestHandler.cpp | 64 ++++++++++++++++++--------- src/fsfw/cfdp/handler/DestHandler.h | 2 + src/fsfw/filesystem/HasFileSystemIF.h | 6 +++ src/fsfw_hal/host/HostFilesystem.cpp | 15 +++++++ src/fsfw_hal/host/HostFilesystem.h | 3 ++ unittests/mocks/FilesystemMock.cpp | 7 +++ unittests/mocks/FilesystemMock.h | 4 ++ 7 files changed, 81 insertions(+), 20 deletions(-) diff --git a/src/fsfw/cfdp/handler/DestHandler.cpp b/src/fsfw/cfdp/handler/DestHandler.cpp index 1b1187b1..4ffa797f 100644 --- a/src/fsfw/cfdp/handler/DestHandler.cpp +++ b/src/fsfw/cfdp/handler/DestHandler.cpp @@ -281,12 +281,10 @@ ReturnValue_t cfdp::DestHandler::startTransaction(MetadataPduReader& reader, Met } ReturnValue_t result = OK; size_t sourceNameSize = 0; + const uint8_t* sourceNamePtr = info.getSourceFileName().getValue(&sourceNameSize); if (sourceNameSize + 1 > tp.sourceName.size()) { - fp.eventReporter->forwardEvent(events::FILENAME_TOO_LARGE_ERROR, 0, 0); -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "cfdp::DestHandler: source filename too large" << std::endl; -#endif + fileErrorHandler(events::FILENAME_TOO_LARGE_ERROR, 0, "source filename too large"); return FAILED; } std::memcpy(tp.sourceName.data(), sourceNamePtr, sourceNameSize); @@ -294,10 +292,7 @@ ReturnValue_t cfdp::DestHandler::startTransaction(MetadataPduReader& reader, Met size_t destNameSize = 0; const uint8_t* destNamePtr = info.getDestFileName().getValue(&destNameSize); if (destNameSize + 1 > tp.destName.size()) { - fp.eventReporter->forwardEvent(events::FILENAME_TOO_LARGE_ERROR, 0, 0); -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "cfdp::DestHandler: dest filename too large" << std::endl; -#endif + fileErrorHandler(events::FILENAME_TOO_LARGE_ERROR, 0, "dest filename too large"); return FAILED; } std::memcpy(tp.destName.data(), destNamePtr, destNameSize); @@ -307,27 +302,25 @@ ReturnValue_t cfdp::DestHandler::startTransaction(MetadataPduReader& reader, Met // so there is no need to create a file or truncate an existing file if (destNameSize > 0 and sourceNameSize > 0) { FilesystemParams fparams(tp.destName.data()); + // handling to allow only specifying target directory. Example: + // Source path /test/hello.txt, dest path /tmp -> dest path /tmp/hello.txt + if (dp.user.vfs.isDirectory(tp.destName.data())) { + result = tryBuildingAbsoluteDestName(destNameSize); + if (result != OK) { + return result; + } + } if (dp.user.vfs.fileExists(fparams)) { result = dp.user.vfs.truncateFile(fparams); if (result != returnvalue::OK) { - // TODO: Provide execution step as parameter - fp.eventReporter->forwardEvent(events::FILESTORE_ERROR, static_cast(fsmRes.step), - result); -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "cfdp::DestHandler: file truncation error with code " << result - << std::endl; -#endif + fileErrorHandler(events::FILESTORE_ERROR, result, "file truncation error"); return FAILED; // TODO: Relevant for filestore rejection error? } } else { result = dp.user.vfs.createFile(fparams); if (result != OK) { - fp.eventReporter->forwardEvent(events::FILESTORE_ERROR, static_cast(fsmRes.step), - result); -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "cfdp::DestHandler: file creation error with code " << result << std::endl; -#endif + fileErrorHandler(events::FILESTORE_ERROR, result, "file creation error"); return FAILED; // TODO: Relevant for filestore rejection error? } @@ -394,6 +387,37 @@ ReturnValue_t cfdp::DestHandler::handleTransferCompletion() { return OK; } +ReturnValue_t cfdp::DestHandler::tryBuildingAbsoluteDestName(size_t destNameSize) { + char baseNameBuf[tp.destName.size()]{}; + FilesystemParams fparamsSrc(tp.sourceName.data()); + size_t baseNameLen = 0; + ReturnValue_t result = + dp.user.vfs.getBaseFilename(fparamsSrc, baseNameBuf, sizeof(baseNameBuf), baseNameLen); + if (result != returnvalue::OK or baseNameLen == 0) { + fileErrorHandler(events::FILENAME_TOO_LARGE_ERROR, 0, "error retrieving source base name"); + return FAILED; + } + // Destination name + slash + base name + null termination + if (destNameSize + 1 + baseNameLen + 1 > tp.destName.size()) { + fileErrorHandler(events::FILENAME_TOO_LARGE_ERROR, 0, + "dest filename too large after adding source base name"); + return FAILED; + } + tp.destName[destNameSize++] = '/'; + std::memcpy(tp.destName.data() + destNameSize, baseNameBuf, baseNameLen); + destNameSize += baseNameLen; + tp.destName[destNameSize++] = '\0'; + return OK; +} + +void cfdp::DestHandler::fileErrorHandler(Event event, ReturnValue_t result, const char* info) { + fp.eventReporter->forwardEvent(events::FILENAME_TOO_LARGE_ERROR, + static_cast(fsmRes.step), result); +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "cfdp::DestHandler: " << info << std::endl; +#endif +} + void cfdp::DestHandler::finish() { tp.reset(); dp.packetListRef.clear(); diff --git a/src/fsfw/cfdp/handler/DestHandler.h b/src/fsfw/cfdp/handler/DestHandler.h index 357493df..9057b3f5 100644 --- a/src/fsfw/cfdp/handler/DestHandler.h +++ b/src/fsfw/cfdp/handler/DestHandler.h @@ -191,9 +191,11 @@ class DestHandler { ReturnValue_t handleMetadataParseError(ReturnValue_t result, const uint8_t* rawData, size_t maxSize); ReturnValue_t handleTransferCompletion(); + ReturnValue_t tryBuildingAbsoluteDestName(size_t destNameSize); ReturnValue_t sendFinishedPdu(); ReturnValue_t noticeOfCompletion(); ReturnValue_t checksumVerification(); + void fileErrorHandler(Event event, ReturnValue_t result, const char* info); const FsmResult& updateFsmRes(uint8_t errors); void checkAndHandleError(ReturnValue_t result, uint8_t& errorIdx); void finish(); diff --git a/src/fsfw/filesystem/HasFileSystemIF.h b/src/fsfw/filesystem/HasFileSystemIF.h index 24400b1c..a507938e 100644 --- a/src/fsfw/filesystem/HasFileSystemIF.h +++ b/src/fsfw/filesystem/HasFileSystemIF.h @@ -74,6 +74,12 @@ class HasFileSystemIF { return MessageQueueIF::NO_QUEUE; } + // Get the base filename without the full directory path + virtual ReturnValue_t getBaseFilename(FilesystemParams params, char* nameBuf, size_t maxLen, + size_t& baseNameLen) = 0; + + virtual bool isDirectory(const char* path) = 0; + virtual bool fileExists(FilesystemParams params) = 0; /** diff --git a/src/fsfw_hal/host/HostFilesystem.cpp b/src/fsfw_hal/host/HostFilesystem.cpp index fe593f27..d430d0f0 100644 --- a/src/fsfw_hal/host/HostFilesystem.cpp +++ b/src/fsfw_hal/host/HostFilesystem.cpp @@ -160,3 +160,18 @@ ReturnValue_t HostFilesystem::truncateFile(FilesystemParams params) { ofstream of(path); return returnvalue::OK; } + +bool HostFilesystem::isDirectory(const char *path) { return filesystem::is_directory(path); } + +ReturnValue_t HostFilesystem::getBaseFilename(FilesystemParams params, char *nameBuf, size_t maxLen, + size_t &baseNameLen) { + std::string path(params.path); + std::string baseName = path.substr(path.find_last_of("/\\") + 1); + if (baseName.size() + 1 > maxLen) { + return returnvalue::FAILED; + } + std::memcpy(nameBuf, baseName.c_str(), baseName.size()); + nameBuf[baseName.size()] = '\0'; + baseNameLen = baseName.size(); + return returnvalue::OK; +} diff --git a/src/fsfw_hal/host/HostFilesystem.h b/src/fsfw_hal/host/HostFilesystem.h index 7b865e2d..da217aec 100644 --- a/src/fsfw_hal/host/HostFilesystem.h +++ b/src/fsfw_hal/host/HostFilesystem.h @@ -9,6 +9,9 @@ class HostFilesystem : public HasFileSystemIF { public: HostFilesystem(); + ReturnValue_t getBaseFilename(FilesystemParams params, char *nameBuf, size_t maxLen, + size_t &baseNameLen) override; + bool isDirectory(const char *path) override; bool fileExists(FilesystemParams params) override; ReturnValue_t truncateFile(FilesystemParams params) override; ReturnValue_t writeToFile(FileOpParams params, const uint8_t *data) override; diff --git a/unittests/mocks/FilesystemMock.cpp b/unittests/mocks/FilesystemMock.cpp index bf0c3bf6..24850227 100644 --- a/unittests/mocks/FilesystemMock.cpp +++ b/unittests/mocks/FilesystemMock.cpp @@ -138,3 +138,10 @@ ReturnValue_t FilesystemMock::truncateFile(FilesystemParams params) { truncateCalledOnFile = params.path; return returnvalue::OK; } + +ReturnValue_t FilesystemMock::getBaseFilename(FilesystemParams params, char *nameBuf, size_t maxLen, + size_t &baseNameLen) { + return returnvalue::OK; +} + +bool FilesystemMock::isDirectory(const char *path) { return false; } diff --git a/unittests/mocks/FilesystemMock.h b/unittests/mocks/FilesystemMock.h index 74221d70..2ddbefc3 100644 --- a/unittests/mocks/FilesystemMock.h +++ b/unittests/mocks/FilesystemMock.h @@ -56,6 +56,10 @@ class FilesystemMock : public HasFileSystemIF { std::string truncateCalledOnFile; ReturnValue_t feedFile(const std::string &filename, std::ifstream &file); + ReturnValue_t getBaseFilename(FilesystemParams params, char *nameBuf, size_t maxLen, + size_t &baseNameLen) override; + + bool isDirectory(const char *path) override; bool fileExists(FilesystemParams params) override; ReturnValue_t truncateFile(FilesystemParams params) override; From cf735143fe4b79db2fc7faba2b1cd239474e2cfc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 26 Feb 2023 14:54:35 +0100 Subject: [PATCH 326/404] update for SPI/gyro dev handler code --- src/fsfw_hal/devicehandlers/CMakeLists.txt | 2 + .../devicehandlers/GyroL3GD20Handler.cpp | 72 +++++++++---------- .../devicehandlers/GyroL3GD20Handler.h | 23 +++--- .../devicedefinitions/CMakeLists.txt | 1 + .../devicedefinitions/gyroL3gHelpers.cpp | 14 ++++ ...roL3GD20Definitions.h => gyroL3gHelpers.h} | 30 ++++---- src/fsfw_hal/linux/spi/SpiComIF.h | 2 - 7 files changed, 81 insertions(+), 63 deletions(-) create mode 100644 src/fsfw_hal/devicehandlers/devicedefinitions/CMakeLists.txt create mode 100644 src/fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.cpp rename src/fsfw_hal/devicehandlers/devicedefinitions/{GyroL3GD20Definitions.h => gyroL3gHelpers.h} (87%) diff --git a/src/fsfw_hal/devicehandlers/CMakeLists.txt b/src/fsfw_hal/devicehandlers/CMakeLists.txt index 17139416..e5999ad5 100644 --- a/src/fsfw_hal/devicehandlers/CMakeLists.txt +++ b/src/fsfw_hal/devicehandlers/CMakeLists.txt @@ -1,3 +1,5 @@ target_sources( ${LIB_FSFW_NAME} PRIVATE GyroL3GD20Handler.cpp MgmRM3100Handler.cpp MgmLIS3MDLHandler.cpp) + +add_subdirectory(devicedefinitions) diff --git a/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp b/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp index 46ca17b9..c71f34ce 100644 --- a/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp +++ b/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp @@ -46,17 +46,17 @@ ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t return NOTHING_TO_SEND; } case (InternalState::CONFIGURE): { - *id = L3GD20H::CONFIGURE_CTRL_REGS; + *id = l3gd20h::CONFIGURE_CTRL_REGS; uint8_t command[5]; - command[0] = L3GD20H::CTRL_REG_1_VAL; - command[1] = L3GD20H::CTRL_REG_2_VAL; - command[2] = L3GD20H::CTRL_REG_3_VAL; - command[3] = L3GD20H::CTRL_REG_4_VAL; - command[4] = L3GD20H::CTRL_REG_5_VAL; + command[0] = l3gd20h::CTRL_REG_1_VAL; + command[1] = l3gd20h::CTRL_REG_2_VAL; + command[2] = l3gd20h::CTRL_REG_3_VAL; + command[3] = l3gd20h::CTRL_REG_4_VAL; + command[4] = l3gd20h::CTRL_REG_5_VAL; return buildCommandFromCommand(*id, command, 5); } case (InternalState::CHECK_REGS): { - *id = L3GD20H::READ_REGS; + *id = l3gd20h::READ_REGS; return buildCommandFromCommand(*id, nullptr, 0); } default: @@ -76,7 +76,7 @@ ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t } ReturnValue_t GyroHandlerL3GD20H::buildNormalDeviceCommand(DeviceCommandId_t *id) { - *id = L3GD20H::READ_REGS; + *id = l3gd20h::READ_REGS; return buildCommandFromCommand(*id, nullptr, 0); } @@ -84,15 +84,15 @@ ReturnValue_t GyroHandlerL3GD20H::buildCommandFromCommand(DeviceCommandId_t devi const uint8_t *commandData, size_t commandDataLen) { switch (deviceCommand) { - case (L3GD20H::READ_REGS): { - commandBuffer[0] = L3GD20H::READ_START | L3GD20H::AUTO_INCREMENT_MASK | L3GD20H::READ_MASK; - std::memset(commandBuffer + 1, 0, L3GD20H::READ_LEN); + case (l3gd20h::READ_REGS): { + commandBuffer[0] = l3gd20h::READ_START | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK; + std::memset(commandBuffer + 1, 0, l3gd20h::READ_LEN); rawPacket = commandBuffer; - rawPacketLen = L3GD20H::READ_LEN + 1; + rawPacketLen = l3gd20h::READ_LEN + 1; break; } - case (L3GD20H::CONFIGURE_CTRL_REGS): { - commandBuffer[0] = L3GD20H::CTRL_REG_1 | L3GD20H::AUTO_INCREMENT_MASK; + case (l3gd20h::CONFIGURE_CTRL_REGS): { + commandBuffer[0] = l3gd20h::CTRL_REG_1 | l3gd20h::AUTO_INCREMENT_MASK; if (commandData == nullptr or commandDataLen != 5) { return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; } @@ -103,15 +103,15 @@ ReturnValue_t GyroHandlerL3GD20H::buildCommandFromCommand(DeviceCommandId_t devi ctrlReg4Value = commandData[3]; ctrlReg5Value = commandData[4]; - bool fsH = ctrlReg4Value & L3GD20H::SET_FS_1; - bool fsL = ctrlReg4Value & L3GD20H::SET_FS_0; + bool fsH = ctrlReg4Value & l3gd20h::SET_FS_1; + bool fsL = ctrlReg4Value & l3gd20h::SET_FS_0; if (not fsH and not fsL) { - sensitivity = L3GD20H::SENSITIVITY_00; + sensitivity = l3gd20h::SENSITIVITY_00; } else if (not fsH and fsL) { - sensitivity = L3GD20H::SENSITIVITY_01; + sensitivity = l3gd20h::SENSITIVITY_01; } else { - sensitivity = L3GD20H::SENSITIVITY_11; + sensitivity = l3gd20h::SENSITIVITY_11; } commandBuffer[1] = ctrlReg1Value; @@ -124,8 +124,8 @@ ReturnValue_t GyroHandlerL3GD20H::buildCommandFromCommand(DeviceCommandId_t devi rawPacketLen = 6; break; } - case (L3GD20H::READ_CTRL_REGS): { - commandBuffer[0] = L3GD20H::READ_START | L3GD20H::AUTO_INCREMENT_MASK | L3GD20H::READ_MASK; + case (l3gd20h::READ_CTRL_REGS): { + commandBuffer[0] = l3gd20h::READ_START | l3gd20h::AUTO_INCREMENT_MASK | l3gd20h::READ_MASK; std::memset(commandBuffer + 1, 0, 5); rawPacket = commandBuffer; @@ -151,11 +151,11 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { ReturnValue_t result = returnvalue::OK; switch (id) { - case (L3GD20H::CONFIGURE_CTRL_REGS): { + case (l3gd20h::CONFIGURE_CTRL_REGS): { commandExecuted = true; break; } - case (L3GD20H::READ_CTRL_REGS): { + case (l3gd20h::READ_CTRL_REGS): { if (packet[1] == ctrlReg1Value and packet[2] == ctrlReg2Value and packet[3] == ctrlReg3Value and packet[4] == ctrlReg4Value and packet[5] == ctrlReg5Value) { @@ -167,7 +167,7 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, } break; } - case (L3GD20H::READ_REGS): { + case (l3gd20h::READ_REGS): { if (packet[1] != ctrlReg1Value and packet[2] != ctrlReg2Value and packet[3] != ctrlReg3Value and packet[4] != ctrlReg4Value and packet[5] != ctrlReg5Value) { @@ -178,16 +178,16 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, } } - statusReg = packet[L3GD20H::STATUS_IDX]; + statusReg = packet[l3gd20h::STATUS_IDX]; - int16_t angVelocXRaw = packet[L3GD20H::OUT_X_H] << 8 | packet[L3GD20H::OUT_X_L]; - int16_t angVelocYRaw = packet[L3GD20H::OUT_Y_H] << 8 | packet[L3GD20H::OUT_Y_L]; - int16_t angVelocZRaw = packet[L3GD20H::OUT_Z_H] << 8 | packet[L3GD20H::OUT_Z_L]; + int16_t angVelocXRaw = packet[l3gd20h::OUT_X_H] << 8 | packet[l3gd20h::OUT_X_L]; + int16_t angVelocYRaw = packet[l3gd20h::OUT_Y_H] << 8 | packet[l3gd20h::OUT_Y_L]; + int16_t angVelocZRaw = packet[l3gd20h::OUT_Z_H] << 8 | packet[l3gd20h::OUT_Z_L]; float angVelocX = angVelocXRaw * sensitivity; float angVelocY = angVelocYRaw * sensitivity; float angVelocZ = angVelocZRaw * sensitivity; - int8_t temperaturOffset = (-1) * packet[L3GD20H::TEMPERATURE_IDX]; + int8_t temperaturOffset = (-1) * packet[l3gd20h::TEMPERATURE_IDX]; float temperature = 25.0 + temperaturOffset; if (periodicPrintout) { if (debugDivider.checkAndIncrement()) { @@ -248,19 +248,19 @@ void GyroHandlerL3GD20H::setToGoToNormalMode(bool enable) { this->goNormalModeIm ReturnValue_t GyroHandlerL3GD20H::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, new PoolEntry({0.0})); - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, new PoolEntry({0.0})); - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, new PoolEntry({0.0})); - localDataPoolMap.emplace(L3GD20H::TEMPERATURE, new PoolEntry({0.0})); + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_X, new PoolEntry({0.0})); + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Y, new PoolEntry({0.0})); + localDataPoolMap.emplace(l3gd20h::ANG_VELOC_Z, new PoolEntry({0.0})); + localDataPoolMap.emplace(l3gd20h::TEMPERATURE, new PoolEntry({0.0})); poolManager.subscribeForRegularPeriodicPacket( subdp::RegularHkPeriodicParams(dataset.getSid(), false, 10.0)); return returnvalue::OK; } void GyroHandlerL3GD20H::fillCommandAndReplyMap() { - insertInCommandAndReplyMap(L3GD20H::READ_REGS, 1, &dataset); - insertInCommandAndReplyMap(L3GD20H::CONFIGURE_CTRL_REGS, 1); - insertInCommandAndReplyMap(L3GD20H::READ_CTRL_REGS, 1); + insertInCommandAndReplyMap(l3gd20h::READ_REGS, 1, &dataset); + insertInCommandAndReplyMap(l3gd20h::CONFIGURE_CTRL_REGS, 1); + insertInCommandAndReplyMap(l3gd20h::READ_CTRL_REGS, 1); } void GyroHandlerL3GD20H::modeChanged() { internalState = InternalState::NONE; } diff --git a/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h b/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h index 7c1ebdac..9897dc00 100644 --- a/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h +++ b/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h @@ -3,8 +3,7 @@ #include #include - -#include "devicedefinitions/GyroL3GD20Definitions.h" +#include /** * @brief Device Handler for the L3GD20H gyroscope sensor @@ -59,9 +58,9 @@ 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; + float absLimitX = l3gd20h::RANGE_DPS_00; + float absLimitY = l3gd20h::RANGE_DPS_00; + float absLimitZ = l3gd20h::RANGE_DPS_00; enum class InternalState { NONE, CONFIGURE, CHECK_REGS, NORMAL }; InternalState internalState = InternalState::NONE; @@ -70,16 +69,16 @@ class GyroHandlerL3GD20H : public DeviceHandlerBase { uint8_t statusReg = 0; bool goNormalModeImmediately = false; - uint8_t ctrlReg1Value = L3GD20H::CTRL_REG_1_VAL; - uint8_t ctrlReg2Value = L3GD20H::CTRL_REG_2_VAL; - uint8_t ctrlReg3Value = L3GD20H::CTRL_REG_3_VAL; - uint8_t ctrlReg4Value = L3GD20H::CTRL_REG_4_VAL; - uint8_t ctrlReg5Value = L3GD20H::CTRL_REG_5_VAL; + uint8_t ctrlReg1Value = l3gd20h::CTRL_REG_1_VAL; + uint8_t ctrlReg2Value = l3gd20h::CTRL_REG_2_VAL; + uint8_t ctrlReg3Value = l3gd20h::CTRL_REG_3_VAL; + uint8_t ctrlReg4Value = l3gd20h::CTRL_REG_4_VAL; + uint8_t ctrlReg5Value = l3gd20h::CTRL_REG_5_VAL; - uint8_t commandBuffer[L3GD20H::READ_LEN + 1]; + uint8_t commandBuffer[l3gd20h::READ_LEN + 1]; // Set default value - float sensitivity = L3GD20H::SENSITIVITY_00; + float sensitivity = l3gd20h::SENSITIVITY_00; bool periodicPrintout = false; PeriodicOperationDivider debugDivider = PeriodicOperationDivider(3); diff --git a/src/fsfw_hal/devicehandlers/devicedefinitions/CMakeLists.txt b/src/fsfw_hal/devicehandlers/devicedefinitions/CMakeLists.txt new file mode 100644 index 00000000..e77862c1 --- /dev/null +++ b/src/fsfw_hal/devicehandlers/devicedefinitions/CMakeLists.txt @@ -0,0 +1 @@ +target_sources(${LIB_FSFW_NAME} PRIVATE gyroL3gHelpers.cpp) diff --git a/src/fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.cpp b/src/fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.cpp new file mode 100644 index 00000000..13f00b10 --- /dev/null +++ b/src/fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.cpp @@ -0,0 +1,14 @@ +#include + +float l3gd20h::ctrlReg4ToSensitivity(uint8_t reg) { + bool fsH = reg & l3gd20h::SET_FS_1; + bool fsL = reg & l3gd20h::SET_FS_0; + + if (not fsH and not fsL) { + return l3gd20h::SENSITIVITY_00; + } else if (not fsH and fsL) { + return l3gd20h::SENSITIVITY_01; + } else { + return l3gd20h::SENSITIVITY_11; + } +} diff --git a/src/fsfw_hal/devicehandlers/devicedefinitions/GyroL3GD20Definitions.h b/src/fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h similarity index 87% rename from src/fsfw_hal/devicehandlers/devicedefinitions/GyroL3GD20Definitions.h rename to src/fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h index 2a85ff3e..4aef6811 100644 --- a/src/fsfw_hal/devicehandlers/devicedefinitions/GyroL3GD20Definitions.h +++ b/src/fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h @@ -6,7 +6,9 @@ #include -namespace L3GD20H { +namespace l3gd20h { + +float ctrlReg4ToSensitivity(uint8_t reg); /* Actual size is 15 but we round up a bit */ static constexpr size_t MAX_BUFFER_SIZE = 16; @@ -103,31 +105,33 @@ static constexpr DeviceCommandId_t READ_REGS = 0; static constexpr DeviceCommandId_t CONFIGURE_CTRL_REGS = 1; static constexpr DeviceCommandId_t READ_CTRL_REGS = 2; +static constexpr DeviceCommandId_t REQUEST = 0x70; +static constexpr DeviceCommandId_t REPLY = 0x77; + static constexpr uint32_t GYRO_DATASET_ID = READ_REGS; enum GyroPoolIds : lp_id_t { ANG_VELOC_X, ANG_VELOC_Y, ANG_VELOC_Z, TEMPERATURE }; -} // namespace L3GD20H +} // namespace l3gd20h class GyroPrimaryDataset : public StaticLocalDataSet<5> { public: /** Constructor for data users like controllers */ GyroPrimaryDataset(object_id_t mgmId) - : StaticLocalDataSet(sid_t(mgmId, L3GD20H::GYRO_DATASET_ID)) { + : StaticLocalDataSet(sid_t(mgmId, l3gd20h::GYRO_DATASET_ID)) { setAllVariablesReadOnly(); } - - /* Angular velocities in degrees per second (DPS) */ - lp_var_t angVelocX = lp_var_t(sid.objectId, L3GD20H::ANG_VELOC_X, this); - lp_var_t angVelocY = lp_var_t(sid.objectId, L3GD20H::ANG_VELOC_Y, this); - lp_var_t angVelocZ = lp_var_t(sid.objectId, L3GD20H::ANG_VELOC_Z, this); - lp_var_t temperature = lp_var_t(sid.objectId, L3GD20H::TEMPERATURE, this); - - private: - friend class GyroHandlerL3GD20H; /** Constructor for the data creator */ GyroPrimaryDataset(HasLocalDataPoolIF* hkOwner) - : StaticLocalDataSet(hkOwner, L3GD20H::GYRO_DATASET_ID) {} + : StaticLocalDataSet(hkOwner, l3gd20h::GYRO_DATASET_ID) {} + + /* Angular velocities in degrees per second (DPS) */ + lp_var_t angVelocX = lp_var_t(sid.objectId, l3gd20h::ANG_VELOC_X, this); + lp_var_t angVelocY = lp_var_t(sid.objectId, l3gd20h::ANG_VELOC_Y, this); + lp_var_t angVelocZ = lp_var_t(sid.objectId, l3gd20h::ANG_VELOC_Z, this); + lp_var_t temperature = lp_var_t(sid.objectId, l3gd20h::TEMPERATURE, this); + + private: }; #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_GYROL3GD20DEFINITIONS_H_ */ diff --git a/src/fsfw_hal/linux/spi/SpiComIF.h b/src/fsfw_hal/linux/spi/SpiComIF.h index 7033ea37..14a3355a 100644 --- a/src/fsfw_hal/linux/spi/SpiComIF.h +++ b/src/fsfw_hal/linux/spi/SpiComIF.h @@ -90,8 +90,6 @@ class SpiComIF : public DeviceCommunicationIF, public SystemObject { * pulled high */ MutexIF* csMutex = nullptr; - // MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; - // uint32_t timeoutMs = DEFAULT_MUTEX_TIMEOUT; spi_ioc_transfer clockUpdateTransfer = {}; using SpiDeviceMap = std::unordered_map; From 511d07c0c78de7b1850e341dfcf8be7589f3c523 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 26 Feb 2023 21:26:24 +0100 Subject: [PATCH 327/404] refactored MGM device code --- .../devicehandlers/MgmLIS3MDLHandler.cpp | 190 +++++++----------- .../devicehandlers/MgmLIS3MDLHandler.h | 38 +--- .../devicehandlers/MgmRM3100Handler.cpp | 74 +++---- .../devicehandlers/MgmRM3100Handler.h | 21 +- .../devicedefinitions/CMakeLists.txt | 2 +- .../devicedefinitions/mgmLis3Helpers.cpp | 52 +++++ ...{MgmLIS3HandlerDefs.h => mgmLis3Helpers.h} | 34 +++- ...RM3100HandlerDefs.h => mgmRm3100Helpers.h} | 4 +- 8 files changed, 210 insertions(+), 205 deletions(-) create mode 100644 src/fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.cpp rename src/fsfw_hal/devicehandlers/devicedefinitions/{MgmLIS3HandlerDefs.h => mgmLis3Helpers.h} (86%) rename src/fsfw_hal/devicehandlers/devicedefinitions/{MgmRM3100HandlerDefs.h => mgmRm3100Helpers.h} (98%) diff --git a/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp b/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp index 82027bfd..a66745e1 100644 --- a/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp +++ b/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp @@ -10,11 +10,11 @@ MgmLIS3MDLHandler::MgmLIS3MDLHandler(object_id_t objectId, object_id_t deviceCom dataset(this), transitionDelay(transitionDelay) { // Set to default values right away - registers[0] = MGMLIS3MDL::CTRL_REG1_DEFAULT; - registers[1] = MGMLIS3MDL::CTRL_REG2_DEFAULT; - registers[2] = MGMLIS3MDL::CTRL_REG3_DEFAULT; - registers[3] = MGMLIS3MDL::CTRL_REG4_DEFAULT; - registers[4] = MGMLIS3MDL::CTRL_REG5_DEFAULT; + registers[0] = mgmLis3::CTRL_REG1_DEFAULT; + registers[1] = mgmLis3::CTRL_REG2_DEFAULT; + registers[2] = mgmLis3::CTRL_REG3_DEFAULT; + registers[3] = mgmLis3::CTRL_REG4_DEFAULT; + registers[4] = mgmLis3::CTRL_REG5_DEFAULT; } MgmLIS3MDLHandler::~MgmLIS3MDLHandler() {} @@ -63,15 +63,15 @@ ReturnValue_t MgmLIS3MDLHandler::buildTransitionDeviceCommand(DeviceCommandId_t return DeviceHandlerBase::NOTHING_TO_SEND; } case (InternalState::STATE_FIRST_CONTACT): { - *id = MGMLIS3MDL::IDENTIFY_DEVICE; + *id = mgmLis3::IDENTIFY_DEVICE; break; } case (InternalState::STATE_SETUP): { - *id = MGMLIS3MDL::SETUP_MGM; + *id = mgmLis3::SETUP_MGM; break; } case (InternalState::STATE_CHECK_REGISTERS): { - *id = MGMLIS3MDL::READ_CONFIG_AND_DATA; + *id = mgmLis3::READ_CONFIG_AND_DATA; break; } default: { @@ -88,28 +88,12 @@ ReturnValue_t MgmLIS3MDLHandler::buildTransitionDeviceCommand(DeviceCommandId_t return buildCommandFromCommand(*id, NULL, 0); } -uint8_t MgmLIS3MDLHandler::readCommand(uint8_t command, bool continuousCom) { - command |= (1 << MGMLIS3MDL::RW_BIT); - if (continuousCom == true) { - command |= (1 << MGMLIS3MDL::MS_BIT); - } - return command; -} - -uint8_t MgmLIS3MDLHandler::writeCommand(uint8_t command, bool continuousCom) { - command &= ~(1 << MGMLIS3MDL::RW_BIT); - if (continuousCom == true) { - command |= (1 << MGMLIS3MDL::MS_BIT); - } - return command; -} - void MgmLIS3MDLHandler::setupMgm() { - registers[0] = MGMLIS3MDL::CTRL_REG1_DEFAULT; - registers[1] = MGMLIS3MDL::CTRL_REG2_DEFAULT; - registers[2] = MGMLIS3MDL::CTRL_REG3_DEFAULT; - registers[3] = MGMLIS3MDL::CTRL_REG4_DEFAULT; - registers[4] = MGMLIS3MDL::CTRL_REG5_DEFAULT; + registers[0] = mgmLis3::CTRL_REG1_DEFAULT; + registers[1] = mgmLis3::CTRL_REG2_DEFAULT; + registers[2] = mgmLis3::CTRL_REG3_DEFAULT; + registers[3] = mgmLis3::CTRL_REG4_DEFAULT; + registers[4] = mgmLis3::CTRL_REG5_DEFAULT; prepareCtrlRegisterWrite(); } @@ -117,11 +101,11 @@ void MgmLIS3MDLHandler::setupMgm() { ReturnValue_t MgmLIS3MDLHandler::buildNormalDeviceCommand(DeviceCommandId_t *id) { // Data/config register will be read in an alternating manner. if (communicationStep == CommunicationStep::DATA) { - *id = MGMLIS3MDL::READ_CONFIG_AND_DATA; + *id = mgmLis3::READ_CONFIG_AND_DATA; communicationStep = CommunicationStep::TEMPERATURE; return buildCommandFromCommand(*id, NULL, 0); } else { - *id = MGMLIS3MDL::READ_TEMPERATURE; + *id = mgmLis3::READ_TEMPERATURE; communicationStep = CommunicationStep::DATA; return buildCommandFromCommand(*id, NULL, 0); } @@ -131,33 +115,33 @@ ReturnValue_t MgmLIS3MDLHandler::buildCommandFromCommand(DeviceCommandId_t devic const uint8_t *commandData, size_t commandDataLen) { switch (deviceCommand) { - case (MGMLIS3MDL::READ_CONFIG_AND_DATA): { + case (mgmLis3::READ_CONFIG_AND_DATA): { std::memset(commandBuffer, 0, sizeof(commandBuffer)); - commandBuffer[0] = readCommand(MGMLIS3MDL::CTRL_REG1, true); + commandBuffer[0] = mgmLis3::readCommand(mgmLis3::CTRL_REG1, true); rawPacket = commandBuffer; - rawPacketLen = MGMLIS3MDL::NR_OF_DATA_AND_CFG_REGISTERS + 1; + rawPacketLen = mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS + 1; return returnvalue::OK; } - case (MGMLIS3MDL::READ_TEMPERATURE): { + case (mgmLis3::READ_TEMPERATURE): { std::memset(commandBuffer, 0, 3); - commandBuffer[0] = readCommand(MGMLIS3MDL::TEMP_LOWBYTE, true); + commandBuffer[0] = mgmLis3::readCommand(mgmLis3::TEMP_LOWBYTE, true); rawPacket = commandBuffer; rawPacketLen = 3; return returnvalue::OK; } - case (MGMLIS3MDL::IDENTIFY_DEVICE): { + case (mgmLis3::IDENTIFY_DEVICE): { return identifyDevice(); } - case (MGMLIS3MDL::TEMP_SENSOR_ENABLE): { + case (mgmLis3::TEMP_SENSOR_ENABLE): { return enableTemperatureSensor(commandData, commandDataLen); } - case (MGMLIS3MDL::SETUP_MGM): { + case (mgmLis3::SETUP_MGM): { setupMgm(); return returnvalue::OK; } - case (MGMLIS3MDL::ACCURACY_OP_MODE_SET): { + case (mgmLis3::ACCURACY_OP_MODE_SET): { return setOperatingMode(commandData, commandDataLen); } default: @@ -168,7 +152,7 @@ ReturnValue_t MgmLIS3MDLHandler::buildCommandFromCommand(DeviceCommandId_t devic ReturnValue_t MgmLIS3MDLHandler::identifyDevice() { uint32_t size = 2; - commandBuffer[0] = readCommand(MGMLIS3MDL::IDENTIFY_DEVICE_REG_ADDR); + commandBuffer[0] = mgmLis3::readCommand(mgmLis3::IDENTIFY_DEVICE_REG_ADDR); commandBuffer[1] = 0x00; rawPacket = commandBuffer; @@ -180,9 +164,9 @@ ReturnValue_t MgmLIS3MDLHandler::identifyDevice() { ReturnValue_t MgmLIS3MDLHandler::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { *foundLen = len; - if (len == MGMLIS3MDL::NR_OF_DATA_AND_CFG_REGISTERS + 1) { + if (len == mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS + 1) { *foundLen = len; - *foundId = MGMLIS3MDL::READ_CONFIG_AND_DATA; + *foundId = mgmLis3::READ_CONFIG_AND_DATA; // Check validity by checking config registers if (start[1] != registers[0] or start[2] != registers[1] or start[3] != registers[2] or start[4] != registers[3] or start[5] != registers[4]) { @@ -199,17 +183,17 @@ ReturnValue_t MgmLIS3MDLHandler::scanForReply(const uint8_t *start, size_t len, commandExecuted = true; } - } else if (len == MGMLIS3MDL::TEMPERATURE_REPLY_LEN) { + } else if (len == mgmLis3::TEMPERATURE_REPLY_LEN) { *foundLen = len; - *foundId = MGMLIS3MDL::READ_TEMPERATURE; - } else if (len == MGMLIS3MDL::SETUP_REPLY_LEN) { + *foundId = mgmLis3::READ_TEMPERATURE; + } else if (len == mgmLis3::SETUP_REPLY_LEN) { *foundLen = len; - *foundId = MGMLIS3MDL::SETUP_MGM; + *foundId = mgmLis3::SETUP_MGM; } else if (len == SINGLE_COMMAND_ANSWER_LEN) { *foundLen = len; *foundId = getPendingCommand(); - if (*foundId == MGMLIS3MDL::IDENTIFY_DEVICE) { - if (start[1] != MGMLIS3MDL::DEVICE_ID) { + if (*foundId == mgmLis3::IDENTIFY_DEVICE) { + if (start[1] != mgmLis3::DEVICE_ID) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "MGMHandlerLIS3MDL::scanForReply: " @@ -241,30 +225,31 @@ ReturnValue_t MgmLIS3MDLHandler::scanForReply(const uint8_t *start, size_t len, } ReturnValue_t MgmLIS3MDLHandler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { switch (id) { - case MGMLIS3MDL::IDENTIFY_DEVICE: { + case mgmLis3::IDENTIFY_DEVICE: { break; } - case MGMLIS3MDL::SETUP_MGM: { + case mgmLis3::SETUP_MGM: { break; } - case MGMLIS3MDL::READ_CONFIG_AND_DATA: { + case mgmLis3::READ_CONFIG_AND_DATA: { + using namespace mgmLis3; // TODO: Store configuration in new local datasets. float sensitivityFactor = getSensitivityFactor(getSensitivity(registers[2])); int16_t mgmMeasurementRawX = - packet[MGMLIS3MDL::X_HIGHBYTE_IDX] << 8 | packet[MGMLIS3MDL::X_LOWBYTE_IDX]; + packet[mgmLis3::X_HIGHBYTE_IDX] << 8 | packet[mgmLis3::X_LOWBYTE_IDX]; int16_t mgmMeasurementRawY = - packet[MGMLIS3MDL::Y_HIGHBYTE_IDX] << 8 | packet[MGMLIS3MDL::Y_LOWBYTE_IDX]; + packet[mgmLis3::Y_HIGHBYTE_IDX] << 8 | packet[mgmLis3::Y_LOWBYTE_IDX]; int16_t mgmMeasurementRawZ = - packet[MGMLIS3MDL::Z_HIGHBYTE_IDX] << 8 | packet[MGMLIS3MDL::Z_LOWBYTE_IDX]; + packet[mgmLis3::Z_HIGHBYTE_IDX] << 8 | packet[mgmLis3::Z_LOWBYTE_IDX]; // Target value in microtesla float mgmX = static_cast(mgmMeasurementRawX) * sensitivityFactor * - MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR; + mgmLis3::GAUSS_TO_MICROTESLA_FACTOR; float mgmY = static_cast(mgmMeasurementRawY) * sensitivityFactor * - MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR; + mgmLis3::GAUSS_TO_MICROTESLA_FACTOR; float mgmZ = static_cast(mgmMeasurementRawZ) * sensitivityFactor * - MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR; + mgmLis3::GAUSS_TO_MICROTESLA_FACTOR; if (periodicPrintout) { if (debugDivider.checkAndIncrement()) { @@ -306,7 +291,7 @@ ReturnValue_t MgmLIS3MDLHandler::interpretDeviceReply(DeviceCommandId_t id, cons break; } - case MGMLIS3MDL::READ_TEMPERATURE: { + case mgmLis3::READ_TEMPERATURE: { int16_t tempValueRaw = packet[2] << 8 | packet[1]; float tempValue = 25.0 + ((static_cast(tempValueRaw)) / 8.0); if (periodicPrintout) { @@ -334,41 +319,6 @@ ReturnValue_t MgmLIS3MDLHandler::interpretDeviceReply(DeviceCommandId_t id, cons return returnvalue::OK; } -MGMLIS3MDL::Sensitivies MgmLIS3MDLHandler::getSensitivity(uint8_t ctrlRegister2) { - bool fs0Set = ctrlRegister2 & (1 << MGMLIS3MDL::FSO); // Checks if FS0 bit is set - bool fs1Set = ctrlRegister2 & (1 << MGMLIS3MDL::FS1); // Checks if FS1 bit is set - - if (fs0Set && fs1Set) - return MGMLIS3MDL::Sensitivies::GAUSS_16; - else if (!fs0Set && fs1Set) - return MGMLIS3MDL::Sensitivies::GAUSS_12; - else if (fs0Set && !fs1Set) - return MGMLIS3MDL::Sensitivies::GAUSS_8; - else - return MGMLIS3MDL::Sensitivies::GAUSS_4; -} - -float MgmLIS3MDLHandler::getSensitivityFactor(MGMLIS3MDL::Sensitivies sens) { - switch (sens) { - case (MGMLIS3MDL::GAUSS_4): { - return MGMLIS3MDL::FIELD_LSB_PER_GAUSS_4_SENS; - } - case (MGMLIS3MDL::GAUSS_8): { - return MGMLIS3MDL::FIELD_LSB_PER_GAUSS_8_SENS; - } - case (MGMLIS3MDL::GAUSS_12): { - return MGMLIS3MDL::FIELD_LSB_PER_GAUSS_12_SENS; - } - case (MGMLIS3MDL::GAUSS_16): { - return MGMLIS3MDL::FIELD_LSB_PER_GAUSS_16_SENS; - } - default: { - // Should never happen - return MGMLIS3MDL::FIELD_LSB_PER_GAUSS_4_SENS; - } - } -} - ReturnValue_t MgmLIS3MDLHandler::enableTemperatureSensor(const uint8_t *commandData, size_t commandDataLen) { if (commandData == nullptr) { @@ -376,16 +326,16 @@ ReturnValue_t MgmLIS3MDLHandler::enableTemperatureSensor(const uint8_t *commandD } triggerEvent(CHANGE_OF_SETUP_PARAMETER); uint32_t size = 2; - commandBuffer[0] = writeCommand(MGMLIS3MDL::CTRL_REG1); + commandBuffer[0] = mgmLis3::writeCommand(mgmLis3::CTRL_REG1); if (commandDataLen > 1) { return INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS; } switch (commandData[0]) { - case (MGMLIS3MDL::ON): { + case (mgmLis3::ON): { commandBuffer[1] = registers[0] | (1 << 7); break; } - case (MGMLIS3MDL::OFF): { + case (mgmLis3::OFF): { commandBuffer[1] = registers[0] & ~(1 << 7); break; } @@ -408,23 +358,23 @@ ReturnValue_t MgmLIS3MDLHandler::setOperatingMode(const uint8_t *commandData, } switch (commandData[0]) { - case MGMLIS3MDL::LOW: - registers[0] = (registers[0] & (~(1 << MGMLIS3MDL::OM1))) & (~(1 << MGMLIS3MDL::OM0)); - registers[3] = (registers[3] & (~(1 << MGMLIS3MDL::OMZ1))) & (~(1 << MGMLIS3MDL::OMZ0)); + case mgmLis3::LOW: + registers[0] = (registers[0] & (~(1 << mgmLis3::OM1))) & (~(1 << mgmLis3::OM0)); + registers[3] = (registers[3] & (~(1 << mgmLis3::OMZ1))) & (~(1 << mgmLis3::OMZ0)); break; - case MGMLIS3MDL::MEDIUM: - registers[0] = (registers[0] & (~(1 << MGMLIS3MDL::OM1))) | (1 << MGMLIS3MDL::OM0); - registers[3] = (registers[3] & (~(1 << MGMLIS3MDL::OMZ1))) | (1 << MGMLIS3MDL::OMZ0); + case mgmLis3::MEDIUM: + registers[0] = (registers[0] & (~(1 << mgmLis3::OM1))) | (1 << mgmLis3::OM0); + registers[3] = (registers[3] & (~(1 << mgmLis3::OMZ1))) | (1 << mgmLis3::OMZ0); break; - case MGMLIS3MDL::HIGH: - registers[0] = (registers[0] | (1 << MGMLIS3MDL::OM1)) & (~(1 << MGMLIS3MDL::OM0)); - registers[3] = (registers[3] | (1 << MGMLIS3MDL::OMZ1)) & (~(1 << MGMLIS3MDL::OMZ0)); + case mgmLis3::HIGH: + registers[0] = (registers[0] | (1 << mgmLis3::OM1)) & (~(1 << mgmLis3::OM0)); + registers[3] = (registers[3] | (1 << mgmLis3::OMZ1)) & (~(1 << mgmLis3::OMZ0)); break; - case MGMLIS3MDL::ULTRA: - registers[0] = (registers[0] | (1 << MGMLIS3MDL::OM1)) | (1 << MGMLIS3MDL::OM0); - registers[3] = (registers[3] | (1 << MGMLIS3MDL::OMZ1)) | (1 << MGMLIS3MDL::OMZ0); + case mgmLis3::ULTRA: + registers[0] = (registers[0] | (1 << mgmLis3::OM1)) | (1 << mgmLis3::OM0); + registers[3] = (registers[3] | (1 << mgmLis3::OMZ1)) | (1 << mgmLis3::OMZ0); break; default: break; @@ -434,24 +384,24 @@ ReturnValue_t MgmLIS3MDLHandler::setOperatingMode(const uint8_t *commandData, } void MgmLIS3MDLHandler::fillCommandAndReplyMap() { - insertInCommandAndReplyMap(MGMLIS3MDL::READ_CONFIG_AND_DATA, 1, &dataset); - insertInCommandAndReplyMap(MGMLIS3MDL::READ_TEMPERATURE, 1); - insertInCommandAndReplyMap(MGMLIS3MDL::SETUP_MGM, 1); - insertInCommandAndReplyMap(MGMLIS3MDL::IDENTIFY_DEVICE, 1); - insertInCommandAndReplyMap(MGMLIS3MDL::TEMP_SENSOR_ENABLE, 1); - insertInCommandAndReplyMap(MGMLIS3MDL::ACCURACY_OP_MODE_SET, 1); + insertInCommandAndReplyMap(mgmLis3::READ_CONFIG_AND_DATA, 1, &dataset); + insertInCommandAndReplyMap(mgmLis3::READ_TEMPERATURE, 1); + insertInCommandAndReplyMap(mgmLis3::SETUP_MGM, 1); + insertInCommandAndReplyMap(mgmLis3::IDENTIFY_DEVICE, 1); + insertInCommandAndReplyMap(mgmLis3::TEMP_SENSOR_ENABLE, 1); + insertInCommandAndReplyMap(mgmLis3::ACCURACY_OP_MODE_SET, 1); } void MgmLIS3MDLHandler::setToGoToNormalMode(bool enable) { this->goToNormalMode = enable; } ReturnValue_t MgmLIS3MDLHandler::prepareCtrlRegisterWrite() { - commandBuffer[0] = writeCommand(MGMLIS3MDL::CTRL_REG1, true); + commandBuffer[0] = mgmLis3::writeCommand(mgmLis3::CTRL_REG1, true); - for (size_t i = 0; i < MGMLIS3MDL::NR_OF_CTRL_REGISTERS; i++) { + for (size_t i = 0; i < mgmLis3::NR_OF_CTRL_REGISTERS; i++) { commandBuffer[i + 1] = registers[i]; } rawPacket = commandBuffer; - rawPacketLen = MGMLIS3MDL::NR_OF_CTRL_REGISTERS + 1; + rawPacketLen = mgmLis3::NR_OF_CTRL_REGISTERS + 1; // We dont have to check if this is working because we just did i return returnvalue::OK; @@ -467,8 +417,8 @@ void MgmLIS3MDLHandler::modeChanged(void) { internalState = InternalState::STATE ReturnValue_t MgmLIS3MDLHandler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTHS, &mgmXYZ); - localDataPoolMap.emplace(MGMLIS3MDL::TEMPERATURE_CELCIUS, &temperature); + localDataPoolMap.emplace(mgmLis3::FIELD_STRENGTHS, &mgmXYZ); + localDataPoolMap.emplace(mgmLis3::TEMPERATURE_CELCIUS, &temperature); poolManager.subscribeForRegularPeriodicPacket({dataset.getSid(), false, 10.0}); return returnvalue::OK; } diff --git a/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h b/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h index 3626a2b0..78b3b38c 100644 --- a/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h +++ b/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h @@ -1,7 +1,8 @@ #ifndef MISSION_DEVICES_MGMLIS3MDLHANDLER_H_ #define MISSION_DEVICES_MGMLIS3MDLHANDLER_H_ -#include "devicedefinitions/MgmLIS3HandlerDefs.h" +#include + #include "fsfw/devicehandlers/DeviceHandlerBase.h" #include "fsfw/globalfunctions/PeriodicOperationDivider.h" @@ -66,7 +67,7 @@ class MgmLIS3MDLHandler : public DeviceHandlerBase { LocalDataPoolManager &poolManager) override; private: - MGMLIS3MDL::MgmPrimaryDataset dataset; + mgmLis3::MgmPrimaryDataset dataset; // Length a single command SPI answer static const uint8_t SINGLE_COMMAND_ANSWER_LEN = 2; @@ -74,7 +75,7 @@ class MgmLIS3MDLHandler : public DeviceHandlerBase { // Single SPI command has 2 bytes, first for adress, second for content size_t singleComandSize = 2; // Has the size for all adresses of the lis3mdl + the continous write bit - uint8_t commandBuffer[MGMLIS3MDL::NR_OF_DATA_AND_CFG_REGISTERS + 1]; + uint8_t commandBuffer[mgmLis3::NR_OF_DATA_AND_CFG_REGISTERS + 1]; float absLimitX = 100; float absLimitY = 100; @@ -85,7 +86,7 @@ class MgmLIS3MDLHandler : public DeviceHandlerBase { * registers when we want to change something. * --> everytime we change set a register we have to save it */ - uint8_t registers[MGMLIS3MDL::NR_OF_CTRL_REGISTERS]; + uint8_t registers[mgmLis3::NR_OF_CTRL_REGISTERS]; uint8_t statusRegister = 0; bool goToNormalMode = false; @@ -107,35 +108,6 @@ class MgmLIS3MDLHandler : public DeviceHandlerBase { /*------------------------------------------------------------------------*/ /* Device specific commands and variables */ /*------------------------------------------------------------------------*/ - /** - * Sets the read bit for the command - * @param single command to set the read-bit at - * @param boolean to select a continuous read bit, default = false - */ - uint8_t readCommand(uint8_t command, bool continuousCom = false); - - /** - * Sets the write bit for the command - * @param single command to set the write-bit at - * @param boolean to select a continuous write bit, default = false - */ - uint8_t writeCommand(uint8_t command, bool continuousCom = false); - - /** - * This Method gets the full scale for the measurement range - * e.g.: +- 4 gauss. See p.25 datasheet. - * @return The ReturnValue does not contain the sign of the value - */ - MGMLIS3MDL::Sensitivies getSensitivity(uint8_t ctrlReg2); - - /** - * The 16 bit value needs to be multiplied with a sensitivity factor - * which depends on the sensitivity configuration - * - * @param sens Configured sensitivity of the LIS3 device - * @return Multiplication factor to get the sensor value from raw data. - */ - float getSensitivityFactor(MGMLIS3MDL::Sensitivies sens); /** * This Command detects the device ID diff --git a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp index 4becd420..c17e3abc 100644 --- a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp +++ b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp @@ -63,21 +63,21 @@ ReturnValue_t MgmRM3100Handler::buildTransitionDeviceCommand(DeviceCommandId_t * return NOTHING_TO_SEND; } case (InternalState::CONFIGURE_CMM): { - *id = RM3100::CONFIGURE_CMM; + *id = mgmRm3100::CONFIGURE_CMM; break; } case (InternalState::READ_CMM): { - *id = RM3100::READ_CMM; + *id = mgmRm3100::READ_CMM; break; } case (InternalState::STATE_CONFIGURE_TMRC): { - commandBuffer[0] = RM3100::TMRC_DEFAULT_VALUE; + commandBuffer[0] = mgmRm3100::TMRC_DEFAULT_VALUE; commandLen = 1; - *id = RM3100::CONFIGURE_TMRC; + *id = mgmRm3100::CONFIGURE_TMRC; break; } case (InternalState::STATE_READ_TMRC): { - *id = RM3100::READ_TMRC; + *id = mgmRm3100::READ_TMRC; break; } default: @@ -103,42 +103,42 @@ ReturnValue_t MgmRM3100Handler::buildCommandFromCommand(DeviceCommandId_t device const uint8_t *commandData, size_t commandDataLen) { switch (deviceCommand) { - case (RM3100::CONFIGURE_CMM): { - commandBuffer[0] = RM3100::CMM_REGISTER; - commandBuffer[1] = RM3100::CMM_VALUE; + case (mgmRm3100::CONFIGURE_CMM): { + commandBuffer[0] = mgmRm3100::CMM_REGISTER; + commandBuffer[1] = mgmRm3100::CMM_VALUE; rawPacket = commandBuffer; rawPacketLen = 2; break; } - case (RM3100::READ_CMM): { - commandBuffer[0] = RM3100::CMM_REGISTER | RM3100::READ_MASK; + case (mgmRm3100::READ_CMM): { + commandBuffer[0] = mgmRm3100::CMM_REGISTER | mgmRm3100::READ_MASK; commandBuffer[1] = 0; rawPacket = commandBuffer; rawPacketLen = 2; break; } - case (RM3100::CONFIGURE_TMRC): { + case (mgmRm3100::CONFIGURE_TMRC): { return handleTmrcConfigCommand(deviceCommand, commandData, commandDataLen); } - case (RM3100::READ_TMRC): { - commandBuffer[0] = RM3100::TMRC_REGISTER | RM3100::READ_MASK; + case (mgmRm3100::READ_TMRC): { + commandBuffer[0] = mgmRm3100::TMRC_REGISTER | mgmRm3100::READ_MASK; commandBuffer[1] = 0; rawPacket = commandBuffer; rawPacketLen = 2; break; } - case (RM3100::CONFIGURE_CYCLE_COUNT): { + case (mgmRm3100::CONFIGURE_CYCLE_COUNT): { return handleCycleCountConfigCommand(deviceCommand, commandData, commandDataLen); } - case (RM3100::READ_CYCLE_COUNT): { - commandBuffer[0] = RM3100::CYCLE_COUNT_START_REGISTER | RM3100::READ_MASK; + case (mgmRm3100::READ_CYCLE_COUNT): { + commandBuffer[0] = mgmRm3100::CYCLE_COUNT_START_REGISTER | mgmRm3100::READ_MASK; std::memset(commandBuffer + 1, 0, 6); rawPacket = commandBuffer; rawPacketLen = 7; break; } - case (RM3100::READ_DATA): { - commandBuffer[0] = RM3100::MEASUREMENT_REG_START | RM3100::READ_MASK; + case (mgmRm3100::READ_DATA): { + commandBuffer[0] = mgmRm3100::MEASUREMENT_REG_START | mgmRm3100::READ_MASK; std::memset(commandBuffer + 1, 0, 9); rawPacketLen = 10; break; @@ -150,7 +150,7 @@ ReturnValue_t MgmRM3100Handler::buildCommandFromCommand(DeviceCommandId_t device } ReturnValue_t MgmRM3100Handler::buildNormalDeviceCommand(DeviceCommandId_t *id) { - *id = RM3100::READ_DATA; + *id = mgmRm3100::READ_DATA; return buildCommandFromCommand(*id, nullptr, 0); } @@ -165,16 +165,16 @@ ReturnValue_t MgmRM3100Handler::scanForReply(const uint8_t *start, size_t len, ReturnValue_t MgmRM3100Handler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { ReturnValue_t result = returnvalue::OK; switch (id) { - case (RM3100::CONFIGURE_CMM): - case (RM3100::CONFIGURE_CYCLE_COUNT): - case (RM3100::CONFIGURE_TMRC): { + case (mgmRm3100::CONFIGURE_CMM): + case (mgmRm3100::CONFIGURE_CYCLE_COUNT): + case (mgmRm3100::CONFIGURE_TMRC): { // We can only check whether write was successful with read operation if (getMode() == _MODE_START_UP) { commandExecuted = true; } break; } - case (RM3100::READ_CMM): { + case (mgmRm3100::READ_CMM): { uint8_t cmmValue = packet[1]; // We clear the seventh bit in any case // because this one is zero sometimes for some reason @@ -188,7 +188,7 @@ ReturnValue_t MgmRM3100Handler::interpretDeviceReply(DeviceCommandId_t id, const } break; } - case (RM3100::READ_TMRC): { + case (mgmRm3100::READ_TMRC): { if (packet[1] == tmrcRegValue) { commandExecuted = true; // Reading TMRC was commanded. Trigger event to inform ground @@ -202,7 +202,7 @@ ReturnValue_t MgmRM3100Handler::interpretDeviceReply(DeviceCommandId_t id, const } break; } - case (RM3100::READ_CYCLE_COUNT): { + case (mgmRm3100::READ_CYCLE_COUNT): { uint16_t cycleCountX = packet[1] << 8 | packet[2]; uint16_t cycleCountY = packet[3] << 8 | packet[4]; uint16_t cycleCountZ = packet[5] << 8 | packet[6]; @@ -217,7 +217,7 @@ ReturnValue_t MgmRM3100Handler::interpretDeviceReply(DeviceCommandId_t id, const } break; } - case (RM3100::READ_DATA): { + case (mgmRm3100::READ_DATA): { result = handleDataReadout(packet); break; } @@ -244,7 +244,7 @@ ReturnValue_t MgmRM3100Handler::handleCycleCountConfigCommand(DeviceCommandId_t return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; } - commandBuffer[0] = RM3100::CYCLE_COUNT_VALUE; + commandBuffer[0] = mgmRm3100::CYCLE_COUNT_VALUE; std::memcpy(commandBuffer + 1, &cycleCountRegValueX, 2); std::memcpy(commandBuffer + 3, &cycleCountRegValueY, 2); std::memcpy(commandBuffer + 5, &cycleCountRegValueZ, 2); @@ -255,7 +255,7 @@ ReturnValue_t MgmRM3100Handler::handleCycleCountConfigCommand(DeviceCommandId_t ReturnValue_t MgmRM3100Handler::handleCycleCommand(bool oneCycleValue, const uint8_t *commandData, size_t commandDataLen) { - RM3100::CycleCountCommand command(oneCycleValue); + mgmRm3100::CycleCountCommand command(oneCycleValue); ReturnValue_t result = command.deSerialize(&commandData, &commandDataLen, SerializeIF::Endianness::BIG); if (result != returnvalue::OK) { @@ -284,7 +284,7 @@ ReturnValue_t MgmRM3100Handler::handleTmrcConfigCommand(DeviceCommandId_t device return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; } - commandBuffer[0] = RM3100::TMRC_REGISTER; + commandBuffer[0] = mgmRm3100::TMRC_REGISTER; commandBuffer[1] = commandData[0]; tmrcRegValue = commandData[0]; rawPacketLen = 2; @@ -293,23 +293,23 @@ ReturnValue_t MgmRM3100Handler::handleTmrcConfigCommand(DeviceCommandId_t device } void MgmRM3100Handler::fillCommandAndReplyMap() { - insertInCommandAndReplyMap(RM3100::CONFIGURE_CMM, 3); - insertInCommandAndReplyMap(RM3100::READ_CMM, 3); + insertInCommandAndReplyMap(mgmRm3100::CONFIGURE_CMM, 3); + insertInCommandAndReplyMap(mgmRm3100::READ_CMM, 3); - insertInCommandAndReplyMap(RM3100::CONFIGURE_TMRC, 3); - insertInCommandAndReplyMap(RM3100::READ_TMRC, 3); + insertInCommandAndReplyMap(mgmRm3100::CONFIGURE_TMRC, 3); + insertInCommandAndReplyMap(mgmRm3100::READ_TMRC, 3); - insertInCommandAndReplyMap(RM3100::CONFIGURE_CYCLE_COUNT, 3); - insertInCommandAndReplyMap(RM3100::READ_CYCLE_COUNT, 3); + insertInCommandAndReplyMap(mgmRm3100::CONFIGURE_CYCLE_COUNT, 3); + insertInCommandAndReplyMap(mgmRm3100::READ_CYCLE_COUNT, 3); - insertInCommandAndReplyMap(RM3100::READ_DATA, 3, &primaryDataset); + insertInCommandAndReplyMap(mgmRm3100::READ_DATA, 3, &primaryDataset); } void MgmRM3100Handler::modeChanged() { internalState = InternalState::NONE; } ReturnValue_t MgmRM3100Handler::initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(RM3100::FIELD_STRENGTHS, &mgmXYZ); + localDataPoolMap.emplace(mgmRm3100::FIELD_STRENGTHS, &mgmXYZ); poolManager.subscribeForRegularPeriodicPacket({primaryDataset.getSid(), false, 10.0}); return returnvalue::OK; } diff --git a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h index d45b2404..1e56bc9d 100644 --- a/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h +++ b/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h @@ -1,7 +1,8 @@ #ifndef MISSION_DEVICES_MGMRM3100HANDLER_H_ #define MISSION_DEVICES_MGMRM3100HANDLER_H_ -#include "devicedefinitions/MgmRM3100HandlerDefs.h" +#include + #include "fsfw/devicehandlers/DeviceHandlerBase.h" #include "fsfw/globalfunctions/PeriodicOperationDivider.h" @@ -69,19 +70,19 @@ class MgmRM3100Handler : public DeviceHandlerBase { }; InternalState internalState = InternalState::NONE; bool commandExecuted = false; - RM3100::Rm3100PrimaryDataset primaryDataset; + mgmRm3100::Rm3100PrimaryDataset primaryDataset; uint8_t commandBuffer[10]; uint8_t commandBufferLen = 0; - uint8_t cmmRegValue = RM3100::CMM_VALUE; - uint8_t tmrcRegValue = RM3100::TMRC_DEFAULT_VALUE; - uint16_t cycleCountRegValueX = RM3100::CYCLE_COUNT_VALUE; - uint16_t cycleCountRegValueY = RM3100::CYCLE_COUNT_VALUE; - uint16_t cycleCountRegValueZ = RM3100::CYCLE_COUNT_VALUE; - float scaleFactorX = 1.0 / RM3100::DEFAULT_GAIN; - float scaleFactorY = 1.0 / RM3100::DEFAULT_GAIN; - float scaleFactorZ = 1.0 / RM3100::DEFAULT_GAIN; + uint8_t cmmRegValue = mgmRm3100::CMM_VALUE; + uint8_t tmrcRegValue = mgmRm3100::TMRC_DEFAULT_VALUE; + uint16_t cycleCountRegValueX = mgmRm3100::CYCLE_COUNT_VALUE; + uint16_t cycleCountRegValueY = mgmRm3100::CYCLE_COUNT_VALUE; + uint16_t cycleCountRegValueZ = mgmRm3100::CYCLE_COUNT_VALUE; + float scaleFactorX = 1.0 / mgmRm3100::DEFAULT_GAIN; + float scaleFactorY = 1.0 / mgmRm3100::DEFAULT_GAIN; + float scaleFactorZ = 1.0 / mgmRm3100::DEFAULT_GAIN; bool goToNormalModeAtStartup = false; uint32_t transitionDelay; diff --git a/src/fsfw_hal/devicehandlers/devicedefinitions/CMakeLists.txt b/src/fsfw_hal/devicehandlers/devicedefinitions/CMakeLists.txt index e77862c1..2d874fb5 100644 --- a/src/fsfw_hal/devicehandlers/devicedefinitions/CMakeLists.txt +++ b/src/fsfw_hal/devicehandlers/devicedefinitions/CMakeLists.txt @@ -1 +1 @@ -target_sources(${LIB_FSFW_NAME} PRIVATE gyroL3gHelpers.cpp) +target_sources(${LIB_FSFW_NAME} PRIVATE gyroL3gHelpers.cpp mgmLis3Helpers.cpp) diff --git a/src/fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.cpp b/src/fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.cpp new file mode 100644 index 00000000..3609ea01 --- /dev/null +++ b/src/fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.cpp @@ -0,0 +1,52 @@ +#include "mgmLis3Helpers.h" + +uint8_t mgmLis3::readCommand(uint8_t command, bool continuousCom) { + command |= (1 << mgmLis3::RW_BIT); + if (continuousCom == true) { + command |= (1 << mgmLis3::MS_BIT); + } + return command; +} + +uint8_t mgmLis3::writeCommand(uint8_t command, bool continuousCom) { + command &= ~(1 << mgmLis3::RW_BIT); + if (continuousCom == true) { + command |= (1 << mgmLis3::MS_BIT); + } + return command; +} + +mgmLis3::Sensitivies mgmLis3::getSensitivity(uint8_t ctrlRegister2) { + bool fs0Set = ctrlRegister2 & (1 << mgmLis3::FSO); // Checks if FS0 bit is set + bool fs1Set = ctrlRegister2 & (1 << mgmLis3::FS1); // Checks if FS1 bit is set + + if (fs0Set && fs1Set) + return mgmLis3::Sensitivies::GAUSS_16; + else if (!fs0Set && fs1Set) + return mgmLis3::Sensitivies::GAUSS_12; + else if (fs0Set && !fs1Set) + return mgmLis3::Sensitivies::GAUSS_8; + else + return mgmLis3::Sensitivies::GAUSS_4; +} + +float mgmLis3::getSensitivityFactor(mgmLis3::Sensitivies sens) { + switch (sens) { + case (mgmLis3::GAUSS_4): { + return mgmLis3::FIELD_LSB_PER_GAUSS_4_SENS; + } + case (mgmLis3::GAUSS_8): { + return mgmLis3::FIELD_LSB_PER_GAUSS_8_SENS; + } + case (mgmLis3::GAUSS_12): { + return mgmLis3::FIELD_LSB_PER_GAUSS_12_SENS; + } + case (mgmLis3::GAUSS_16): { + return mgmLis3::FIELD_LSB_PER_GAUSS_16_SENS; + } + default: { + // Should never happen + return mgmLis3::FIELD_LSB_PER_GAUSS_4_SENS; + } + } +} diff --git a/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h b/src/fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h similarity index 86% rename from src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h rename to src/fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h index 34237637..8d47a200 100644 --- a/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h +++ b/src/fsfw_hal/devicehandlers/devicedefinitions/mgmLis3Helpers.h @@ -7,13 +7,43 @@ #include -namespace MGMLIS3MDL { +namespace mgmLis3 { enum Set { ON, OFF }; enum OpMode { LOW, MEDIUM, HIGH, ULTRA }; enum Sensitivies : uint8_t { GAUSS_4 = 4, GAUSS_8 = 8, GAUSS_12 = 12, GAUSS_16 = 16 }; +/** + * Sets the read bit for the command + * @param single command to set the read-bit at + * @param boolean to select a continuous read bit, default = false + */ +uint8_t readCommand(uint8_t command, bool continuousCom = false); + +/** + * Sets the write bit for the command + * @param single command to set the write-bit at + * @param boolean to select a continuous write bit, default = false + */ +uint8_t writeCommand(uint8_t command, bool continuousCom = false); + +/** + * This Method gets the full scale for the measurement range + * e.g.: +- 4 gauss. See p.25 datasheet. + * @return The ReturnValue does not contain the sign of the value + */ +mgmLis3::Sensitivies getSensitivity(uint8_t ctrlReg2); + +/** + * The 16 bit value needs to be multiplied with a sensitivity factor + * which depends on the sensitivity configuration + * + * @param sens Configured sensitivity of the LIS3 device + * @return Multiplication factor to get the sensor value from raw data. + */ +float getSensitivityFactor(mgmLis3::Sensitivies sens); + /* Actually 15, we just round up a bit */ static constexpr size_t MAX_BUFFER_SIZE = 16; @@ -154,6 +184,6 @@ class MgmPrimaryDataset : public StaticLocalDataSet<4> { lp_var_t temperature = lp_var_t(sid.objectId, TEMPERATURE_CELCIUS, this); }; -} // namespace MGMLIS3MDL +} // namespace mgmLis3 #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_MGMLIS3HANDLERDEFS_H_ */ diff --git a/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h b/src/fsfw_hal/devicehandlers/devicedefinitions/mgmRm3100Helpers.h similarity index 98% rename from src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h rename to src/fsfw_hal/devicehandlers/devicedefinitions/mgmRm3100Helpers.h index a2aa8ab0..680bd13d 100644 --- a/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h +++ b/src/fsfw_hal/devicehandlers/devicedefinitions/mgmRm3100Helpers.h @@ -8,7 +8,7 @@ #include -namespace RM3100 { +namespace mgmRm3100 { /* Actually 10, we round up a little bit */ static constexpr size_t MAX_BUFFER_SIZE = 12; @@ -115,6 +115,6 @@ class Rm3100PrimaryDataset : public StaticLocalDataSet<3> { lp_vec_t fieldStrengths = lp_vec_t(sid.objectId, FIELD_STRENGTHS, this); }; -} // namespace RM3100 +} // namespace mgmRm3100 #endif /* MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ */ From f84097543e59a3564eae4ac19b7118102728c8a9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 2 Mar 2023 15:20:59 +0100 Subject: [PATCH 328/404] allow passing context to mutex guard --- src/fsfw/ipc/MutexGuard.h | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/fsfw/ipc/MutexGuard.h b/src/fsfw/ipc/MutexGuard.h index f6d2f25f..b82b2d7f 100644 --- a/src/fsfw/ipc/MutexGuard.h +++ b/src/fsfw/ipc/MutexGuard.h @@ -7,14 +7,17 @@ class MutexGuard { public: MutexGuard(MutexIF* mutex, MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::BLOCKING, - uint32_t timeoutMs = 0) + uint32_t timeoutMs = 0, const char* context = nullptr) : internalMutex(mutex) { + if (context == nullptr) { + context = "unknown"; + } if (mutex == nullptr) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "MutexGuard: Passed mutex is invalid!" << std::endl; + sif::error << "MutexGuard::" << context << ": Passed mutex is invalid!" << std::endl; #else - sif::printError("MutexGuard: Passed mutex is invalid!\n"); + sif::printError("MutexGuard::%s: Passed mutex is invalid!\n", context); #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ #endif /* FSFW_VERBOSE_LEVEL >= 1 */ return; From 245886c55500b9e70ba71eab68c46d44af9f6836 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 2 Mar 2023 15:33:49 +0100 Subject: [PATCH 329/404] add lock context for pool manager --- src/fsfw/storagemanager/PoolManager.cpp | 6 +++--- src/fsfw/storagemanager/PoolManager.h | 1 + 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/fsfw/storagemanager/PoolManager.cpp b/src/fsfw/storagemanager/PoolManager.cpp index 840a7dcc..c724aa25 100644 --- a/src/fsfw/storagemanager/PoolManager.cpp +++ b/src/fsfw/storagemanager/PoolManager.cpp @@ -10,7 +10,7 @@ PoolManager::PoolManager(object_id_t setObjectId, const LocalPoolConfig& localPo PoolManager::~PoolManager() { MutexFactory::instance()->deleteMutex(mutex); } ReturnValue_t PoolManager::reserveSpace(const size_t size, store_address_t* address) { - MutexGuard mutexHelper(mutex, MutexIF::TimeoutType::WAITING, mutexTimeoutMs); + MutexGuard mutexHelper(mutex, MutexIF::TimeoutType::WAITING, mutexTimeoutMs, LOCK_CTX); ReturnValue_t status = LocalPool::reserveSpace(size, address); return status; } @@ -22,12 +22,12 @@ ReturnValue_t PoolManager::deleteData(store_address_t storeId) { << storeId.poolIndex << ". id is " << storeId.packetIndex << std::endl; #endif #endif - MutexGuard mutexHelper(mutex, MutexIF::TimeoutType::WAITING, mutexTimeoutMs); + MutexGuard mutexHelper(mutex, MutexIF::TimeoutType::WAITING, mutexTimeoutMs, LOCK_CTX); return LocalPool::deleteData(storeId); } ReturnValue_t PoolManager::deleteData(uint8_t* buffer, size_t size, store_address_t* storeId) { - MutexGuard mutexHelper(mutex, MutexIF::TimeoutType::WAITING, 20); + MutexGuard mutexHelper(mutex, MutexIF::TimeoutType::WAITING, mutexTimeoutMs, LOCK_CTX); ReturnValue_t status = LocalPool::deleteData(buffer, size, storeId); return status; } diff --git a/src/fsfw/storagemanager/PoolManager.h b/src/fsfw/storagemanager/PoolManager.h index aa8c93dd..d4bb9f0d 100644 --- a/src/fsfw/storagemanager/PoolManager.h +++ b/src/fsfw/storagemanager/PoolManager.h @@ -56,6 +56,7 @@ class PoolManager : public LocalPool { protected: //! Default mutex timeout value to prevent permanent blocking. uint32_t mutexTimeoutMs = 20; + static constexpr char LOCK_CTX[] = "PoolManager"; ReturnValue_t reserveSpace(size_t size, store_address_t* address) override; From 78cf00315d5bb9a05d47976a564d16c5978f1f41 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Fri, 3 Mar 2023 14:30:35 +0100 Subject: [PATCH 330/404] use sys clock for Countdown --- src/fsfw/fdir/FaultCounter.cpp | 2 +- src/fsfw/ipc/MutexGuard.h | 8 ++--- src/fsfw/osal/linux/Clock.cpp | 9 ++++-- src/fsfw/thermal/Heater.cpp | 2 +- src/fsfw/timemanager/Countdown.cpp | 51 +++++++++++++++++++----------- src/fsfw/timemanager/Countdown.h | 29 +++++++++++++---- 6 files changed, 67 insertions(+), 34 deletions(-) diff --git a/src/fsfw/fdir/FaultCounter.cpp b/src/fsfw/fdir/FaultCounter.cpp index eea08817..9ef359aa 100644 --- a/src/fsfw/fdir/FaultCounter.cpp +++ b/src/fsfw/fdir/FaultCounter.cpp @@ -68,7 +68,7 @@ ReturnValue_t FaultCounter::getParameter(uint8_t domainId, uint8_t uniqueId, parameterWrapper->set(faultCount); break; case ParameterIds::TIMEOUT: - parameterWrapper->set(timer.timeout); + parameterWrapper->set(timer.getTimeoutMs()); break; default: return INVALID_IDENTIFIER_ID; diff --git a/src/fsfw/ipc/MutexGuard.h b/src/fsfw/ipc/MutexGuard.h index b82b2d7f..9887f396 100644 --- a/src/fsfw/ipc/MutexGuard.h +++ b/src/fsfw/ipc/MutexGuard.h @@ -26,11 +26,11 @@ class MutexGuard { #if FSFW_VERBOSE_LEVEL >= 1 if (result == MutexIF::MUTEX_TIMEOUT) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::error << "MutexGuard: Lock of mutex failed with timeout of " << timeoutMs - << " milliseconds!" << std::endl; + sif::error << "MutexGuard::" << context << ": Lock of mutex failed with timeout of " + << timeoutMs << " milliseconds!" << std::endl; #else - sif::printError("MutexGuard: Lock of mutex failed with timeout of %lu milliseconds\n", - timeoutMs); + sif::printError("MutexGuard::%s: Lock of mutex failed with timeout of %lu milliseconds\n", + context, timeoutMs); #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ } else if (result != returnvalue::OK) { diff --git a/src/fsfw/osal/linux/Clock.cpp b/src/fsfw/osal/linux/Clock.cpp index bfdcf4e2..47099fb2 100644 --- a/src/fsfw/osal/linux/Clock.cpp +++ b/src/fsfw/osal/linux/Clock.cpp @@ -79,11 +79,16 @@ ReturnValue_t Clock::getUptime(timeval* uptime) { // TODO This is not posix compatible and delivers only seconds precision // Linux specific file read but more precise. double uptimeSeconds; - if (std::ifstream("/proc/uptime", std::ios::in) >> uptimeSeconds) { + std::ifstream ifile("/proc/uptime"); + if (ifile.bad()) { + return returnvalue::FAILED; + } + if (ifile >> uptimeSeconds) { uptime->tv_sec = uptimeSeconds; uptime->tv_usec = uptimeSeconds * (double)1e6 - (uptime->tv_sec * 1e6); + return returnvalue::OK; } - return returnvalue::OK; + return returnvalue::FAILED; } // Wait for new FSFW Clock function delivering seconds uptime. diff --git a/src/fsfw/thermal/Heater.cpp b/src/fsfw/thermal/Heater.cpp index 04abadc7..da78a74a 100644 --- a/src/fsfw/thermal/Heater.cpp +++ b/src/fsfw/thermal/Heater.cpp @@ -283,7 +283,7 @@ ReturnValue_t Heater::getParameter(uint8_t domainId, uint8_t uniqueId, } switch (uniqueId) { case 0: - parameterWrapper->set(heaterOnCountdown.timeout); + parameterWrapper->set(heaterOnCountdown.getTimeoutMs()); break; default: return INVALID_IDENTIFIER_ID; diff --git a/src/fsfw/timemanager/Countdown.cpp b/src/fsfw/timemanager/Countdown.cpp index 334883ae..662fc8f0 100644 --- a/src/fsfw/timemanager/Countdown.cpp +++ b/src/fsfw/timemanager/Countdown.cpp @@ -1,49 +1,62 @@ #include "fsfw/timemanager/Countdown.h" -Countdown::Countdown(uint32_t initialTimeout, bool startImmediately) : timeout(initialTimeout) { +#include "fsfw/globalfunctions/timevalOperations.h" + +Countdown::Countdown(uint32_t initialTimeout, bool startImmediately) { if (startImmediately) { setTimeout(initialTimeout); } else { - timeout = initialTimeout; + timeout.tv_sec = initialTimeout / 1000; + timeout.tv_usec = (initialTimeout % 1000) * 1000; } } -Countdown::~Countdown() {} +Countdown::~Countdown() = default; ReturnValue_t Countdown::setTimeout(uint32_t milliseconds) { - ReturnValue_t returnValue = Clock::getUptime(&startTime); - timeout = milliseconds; - return returnValue; + timeout.tv_sec = milliseconds / 1000; + timeout.tv_usec = (milliseconds % 1000) * 1000; + return Clock::getClock_timeval(&startTime); } bool Countdown::hasTimedOut() const { - if (uint32_t(this->getCurrentTime() - startTime) >= timeout) { + // Account for system clock going back in time. + if (getCurrentTime() < startTime) { return true; - } else { - return false; } + if (getCurrentTime() - startTime >= timeout) { + return true; + } + return false; } bool Countdown::isBusy() const { return !hasTimedOut(); } -ReturnValue_t Countdown::resetTimer() { return setTimeout(timeout); } +ReturnValue_t Countdown::resetTimer() { return setTimeoutTv(timeout); } void Countdown::timeOut() { startTime = this->getCurrentTime() - timeout; } uint32_t Countdown::getRemainingMillis() const { - // We fetch the time before the if-statement - // to be sure that the return is in - // range 0 <= number <= timeout - uint32_t currentTime = this->getCurrentTime(); if (this->hasTimedOut()) { return 0; - } else { - return (startTime + timeout) - currentTime; } + timeval remainingMillisTv = (startTime + timeout) - this->getCurrentTime(); + return remainingMillisTv.tv_sec * 1000 + remainingMillisTv.tv_usec / 1000; } -uint32_t Countdown::getCurrentTime() const { - uint32_t currentTime; - Clock::getUptime(¤tTime); +uint32_t Countdown::timevalToMs(timeval &tv) { return tv.tv_sec * 1000 + tv.tv_usec / 1000; } + +ReturnValue_t Countdown::setTimeoutTv(timeval tv) { + timeout = tv; + return Clock::getClock_timeval(&startTime); +} + +uint32_t Countdown::getTimeoutMs() const { return timeout.tv_sec * 1000 + timeout.tv_usec / 1000; } + +timeval Countdown::getTimeout() const { return timeout; } + +timeval Countdown::getCurrentTime() const { + timeval currentTime{}; + Clock::getClock_timeval(¤tTime); return currentTime; } diff --git a/src/fsfw/timemanager/Countdown.h b/src/fsfw/timemanager/Countdown.h index 26534789..44a1d4ad 100644 --- a/src/fsfw/timemanager/Countdown.h +++ b/src/fsfw/timemanager/Countdown.h @@ -6,6 +6,10 @@ /** * * Countdown keeps track of a timespan. + * This class uses the system clock internally to achieve + * a high resolution. This means that the API is only partially + * resistant against time jumps. The user must take care to account + * for time jumps in some from if this relevant. * * Countdown::resetTimer restarts the timer. * Countdown::setTimeout sets a new countdown duration and resets. @@ -39,6 +43,8 @@ class Countdown { * @return Returnvalue from Clock::getUptime */ ReturnValue_t setTimeout(uint32_t milliseconds); + ReturnValue_t setTimeoutTv(timeval tv); + /** * Returns true if the countdown duration has passed. * @@ -61,22 +67,31 @@ class Countdown { * Returns the remaining milliseconds (0 if timeout) */ uint32_t getRemainingMillis() const; + + uint32_t getTimeoutMs() const; + + timeval getTimeout() const; + /** * Makes hasTimedOut() return true */ void timeOut(); - /** - * Internal countdown duration in milliseconds - */ - uint32_t timeout; + + static inline uint32_t timevalToMs(timeval& tv); private: /** - * Last time the timer was started (uptime) + * Start time of the countdown. */ - uint32_t startTime = 0; + timeval startTime{}; - uint32_t getCurrentTime() const; + /** + * Timeout as timeval type. The countdown has timed out when the + * current time exceeds the start time plus the timeout. + */ + timeval timeout{}; + + timeval getCurrentTime() const; }; #endif /* FSFW_TIMEMANAGER_COUNTDOWN_H_ */ From 64537d442a335500bdc87ff382099de65f0b7aa7 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Fri, 3 Mar 2023 14:54:52 +0100 Subject: [PATCH 331/404] extneded and fixed countdown unittests --- unittests/timemanager/TestCountdown.cpp | 22 ++++++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/unittests/timemanager/TestCountdown.cpp b/unittests/timemanager/TestCountdown.cpp index 67f4ddb2..61d372e7 100644 --- a/unittests/timemanager/TestCountdown.cpp +++ b/unittests/timemanager/TestCountdown.cpp @@ -1,15 +1,18 @@ #include #include +#include +#include #include "CatchDefinitions.h" +static constexpr bool TEST_LONGER_CD = false; TEST_CASE("Countdown Tests", "[TestCountdown]") { INFO("Countdown Tests"); Countdown count(20); - REQUIRE(count.timeout == 20); + REQUIRE(count.getTimeoutMs() == 20); REQUIRE(count.setTimeout(100) == static_cast(returnvalue::OK)); - REQUIRE(count.timeout == 100); + REQUIRE(count.getTimeoutMs() == 100); REQUIRE(count.setTimeout(150) == static_cast(returnvalue::OK)); REQUIRE(count.isBusy()); REQUIRE(not count.hasTimedOut()); @@ -25,4 +28,19 @@ TEST_CASE("Countdown Tests", "[TestCountdown]") { count.resetTimer(); REQUIRE(not count.hasTimedOut()); REQUIRE(count.isBusy()); + count.setTimeout(100); + REQUIRE(not count.hasTimedOut()); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + REQUIRE(not count.hasTimedOut()); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + REQUIRE(count.hasTimedOut()); + + // Takes longer, disabled by default + if (TEST_LONGER_CD) { + count.setTimeout(2500); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + REQUIRE(not count.hasTimedOut()); + std::this_thread::sleep_for(std::chrono::milliseconds(1500)); + REQUIRE(count.hasTimedOut()); + } } From 6006c97e48b7e6dc3b45a832bdd027a510b67f16 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 3 Mar 2023 15:45:44 +0100 Subject: [PATCH 332/404] configurable queue depth --- src/fsfw/osal/common/TcpTmTcBridge.cpp | 6 +++--- src/fsfw/osal/common/TcpTmTcBridge.h | 2 +- src/fsfw/osal/common/UdpTmTcBridge.cpp | 6 +++--- src/fsfw/osal/common/UdpTmTcBridge.h | 2 +- src/fsfw/tmtcservices/TmTcBridge.cpp | 4 ++-- src/fsfw/tmtcservices/TmTcBridge.h | 4 ++-- 6 files changed, 12 insertions(+), 12 deletions(-) diff --git a/src/fsfw/osal/common/TcpTmTcBridge.cpp b/src/fsfw/osal/common/TcpTmTcBridge.cpp index f99a8bc1..0bf3ab28 100644 --- a/src/fsfw/osal/common/TcpTmTcBridge.cpp +++ b/src/fsfw/osal/common/TcpTmTcBridge.cpp @@ -16,9 +16,9 @@ #endif -TcpTmTcBridge::TcpTmTcBridge(object_id_t objectId, object_id_t tcDestination, object_id_t tmStoreId, - object_id_t tcStoreId) - : TmTcBridge("TCP TMTC Bridge", objectId, tcDestination, tmStoreId, tcStoreId) { +TcpTmTcBridge::TcpTmTcBridge(object_id_t objectId, object_id_t tcDestination, + uint32_t msgQueueDepth, object_id_t tmStoreId, object_id_t tcStoreId) + : TmTcBridge("TCP TMTC Bridge", objectId, tcDestination, msgQueueDepth, tmStoreId, tcStoreId) { mutex = MutexFactory::instance()->createMutex(); // Connection is always up, TM is requested by connecting to server and receiving packets registerCommConnect(); diff --git a/src/fsfw/osal/common/TcpTmTcBridge.h b/src/fsfw/osal/common/TcpTmTcBridge.h index 504592cc..b330ba2a 100644 --- a/src/fsfw/osal/common/TcpTmTcBridge.h +++ b/src/fsfw/osal/common/TcpTmTcBridge.h @@ -38,7 +38,7 @@ class TcpTmTcBridge : public TmTcBridge { * @param tmStoreId TM store object ID. It is recommended to the default object ID * @param tcStoreId TC store object ID. It is recommended to the default object ID */ - TcpTmTcBridge(object_id_t objectId, object_id_t tcDestination, + TcpTmTcBridge(object_id_t objectId, object_id_t tcDestination, uint32_t msgQueueDepth, object_id_t tmStoreId = objects::TM_STORE, object_id_t tcStoreId = objects::TC_STORE); virtual ~TcpTmTcBridge(); diff --git a/src/fsfw/osal/common/UdpTmTcBridge.cpp b/src/fsfw/osal/common/UdpTmTcBridge.cpp index ee4c806b..d6014ec7 100644 --- a/src/fsfw/osal/common/UdpTmTcBridge.cpp +++ b/src/fsfw/osal/common/UdpTmTcBridge.cpp @@ -20,9 +20,9 @@ const std::string UdpTmTcBridge::DEFAULT_SERVER_PORT = tcpip::DEFAULT_SERVER_PORT; UdpTmTcBridge::UdpTmTcBridge(object_id_t objectId, object_id_t tcDestination, - const std::string &udpServerPort_, object_id_t tmStoreId, - object_id_t tcStoreId) - : TmTcBridge("UDP TMTC Bridge", objectId, tcDestination, tmStoreId, tcStoreId) { + uint32_t msgQueueDepth, const std::string &udpServerPort_, + object_id_t tmStoreId, object_id_t tcStoreId) + : TmTcBridge("UDP TMTC Bridge", objectId, tcDestination, msgQueueDepth, tmStoreId, tcStoreId) { if (udpServerPort_.empty()) { udpServerPort = DEFAULT_SERVER_PORT; } else { diff --git a/src/fsfw/osal/common/UdpTmTcBridge.h b/src/fsfw/osal/common/UdpTmTcBridge.h index 92829c46..ce8adb4c 100644 --- a/src/fsfw/osal/common/UdpTmTcBridge.h +++ b/src/fsfw/osal/common/UdpTmTcBridge.h @@ -29,7 +29,7 @@ class UdpTmTcBridge : public TmTcBridge, public TcpIpBase { /* The ports chosen here should not be used by any other process. */ static const std::string DEFAULT_SERVER_PORT; - UdpTmTcBridge(object_id_t objectId, object_id_t tcDestination, + UdpTmTcBridge(object_id_t objectId, object_id_t tcDestination, uint32_t msgQueueDepth, const std::string& udpServerPort = "", object_id_t tmStoreId = objects::TM_STORE, object_id_t tcStoreId = objects::TC_STORE); ~UdpTmTcBridge() override; diff --git a/src/fsfw/tmtcservices/TmTcBridge.cpp b/src/fsfw/tmtcservices/TmTcBridge.cpp index ba851a85..f098103e 100644 --- a/src/fsfw/tmtcservices/TmTcBridge.cpp +++ b/src/fsfw/tmtcservices/TmTcBridge.cpp @@ -8,7 +8,7 @@ #define TMTCBRIDGE_WIRETAPPING 0 TmTcBridge::TmTcBridge(const char* name, object_id_t objectId, object_id_t tcDestination, - object_id_t tmStoreId, object_id_t tcStoreId) + uint32_t msgQueueDepth, object_id_t tmStoreId, object_id_t tcStoreId) : SystemObject(objectId), name(name), tmStoreId(tmStoreId), @@ -18,7 +18,7 @@ TmTcBridge::TmTcBridge(const char* name, object_id_t objectId, object_id_t tcDes { auto mqArgs = MqArgs(objectId, static_cast(this)); tmTcReceptionQueue = QueueFactory::instance()->createMessageQueue( - TMTC_RECEPTION_QUEUE_DEPTH, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); + msgQueueDepth, MessageQueueMessage::MAX_MESSAGE_SIZE, &mqArgs); } TmTcBridge::~TmTcBridge() { QueueFactory::instance()->deleteMessageQueue(tmTcReceptionQueue); } diff --git a/src/fsfw/tmtcservices/TmTcBridge.h b/src/fsfw/tmtcservices/TmTcBridge.h index 3df3419c..c8c880ba 100644 --- a/src/fsfw/tmtcservices/TmTcBridge.h +++ b/src/fsfw/tmtcservices/TmTcBridge.h @@ -15,7 +15,7 @@ class TmTcBridge : public AcceptsTelemetryIF, public ExecutableObjectIF, public SystemObject { public: - static constexpr uint8_t TMTC_RECEPTION_QUEUE_DEPTH = 20; + static constexpr uint8_t DEFAULT_TMTC_RECEPTION_QUEUE_DEPTH = 20; static constexpr uint8_t LIMIT_STORED_DATA_SENT_PER_CYCLE = 15; static constexpr unsigned int LIMIT_DOWNLINK_PACKETS_STORED = 500; @@ -23,7 +23,7 @@ class TmTcBridge : public AcceptsTelemetryIF, static constexpr uint8_t DEFAULT_DOWNLINK_PACKETS_STORED = 10; TmTcBridge(const char* name, object_id_t objectId, object_id_t tcDestination, - object_id_t tmStoreId, object_id_t tcStoreId); + uint32_t msgQueueDepth, object_id_t tmStoreId, object_id_t tcStoreId); ~TmTcBridge() override; /** From 4d353a1ad26ce3d72cb85369d069225309681617 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 3 Mar 2023 16:36:30 +0100 Subject: [PATCH 333/404] remove obsolete constant --- src/fsfw/tmtcservices/TmTcBridge.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/fsfw/tmtcservices/TmTcBridge.h b/src/fsfw/tmtcservices/TmTcBridge.h index c8c880ba..858793cc 100644 --- a/src/fsfw/tmtcservices/TmTcBridge.h +++ b/src/fsfw/tmtcservices/TmTcBridge.h @@ -15,7 +15,6 @@ class TmTcBridge : public AcceptsTelemetryIF, public ExecutableObjectIF, public SystemObject { public: - static constexpr uint8_t DEFAULT_TMTC_RECEPTION_QUEUE_DEPTH = 20; static constexpr uint8_t LIMIT_STORED_DATA_SENT_PER_CYCLE = 15; static constexpr unsigned int LIMIT_DOWNLINK_PACKETS_STORED = 500; From 95dab69b35cb7f7b086f8e5f528f66506c398838 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 4 Mar 2023 11:03:22 +0100 Subject: [PATCH 334/404] new monotonic clock API --- CHANGELOG.md | 1 + src/fsfw/osal/linux/Clock.cpp | 17 ++++++++++++++++- src/fsfw/timemanager/Clock.h | 20 +++++++++++++++++++- src/fsfw/timemanager/Countdown.cpp | 6 +++--- src/fsfw/timemanager/Stopwatch.cpp | 6 +++--- 5 files changed, 42 insertions(+), 8 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index e08cd12a..d2eff76e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/). ## Fixes +- Add monotonic watchdog Clock API and use it in `Countdown` and `Stopwatch` class. - Bugfix in `Service11TelecommandScheduling` which allowed commands time tagged in the past to be inserted. PR: https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/738 diff --git a/src/fsfw/osal/linux/Clock.cpp b/src/fsfw/osal/linux/Clock.cpp index 47099fb2..c625785b 100644 --- a/src/fsfw/osal/linux/Clock.cpp +++ b/src/fsfw/osal/linux/Clock.cpp @@ -42,7 +42,7 @@ ReturnValue_t Clock::setClock(const timeval* time) { return returnvalue::OK; } -ReturnValue_t Clock::getClock_timeval(timeval* time) { +ReturnValue_t Clock::getClock(timeval *time) { timespec timeUnix{}; int status = clock_gettime(CLOCK_REALTIME, &timeUnix); if (status != 0) { @@ -53,6 +53,10 @@ ReturnValue_t Clock::getClock_timeval(timeval* time) { return returnvalue::OK; } +ReturnValue_t Clock::getClock_timeval(timeval* time) { + return Clock::getClock(time); +} + ReturnValue_t Clock::getClock_usecs(uint64_t* time) { timeval timeVal{}; ReturnValue_t result = getClock_timeval(&timeVal); @@ -64,6 +68,17 @@ ReturnValue_t Clock::getClock_usecs(uint64_t* time) { return returnvalue::OK; } +ReturnValue_t Clock::getClockMonotonic(timeval *time) { + timespec timeUnix{}; + int status = clock_gettime(CLOCK_MONOTONIC_RAW, &timeUnix); + if (status != 0) { + return returnvalue::FAILED; + } + time->tv_sec = timeUnix.tv_sec; + time->tv_usec = timeUnix.tv_nsec / 1000.0; + return returnvalue::OK; +} + timeval Clock::getUptime() { timeval uptime{}; auto result = getUptime(&uptime); diff --git a/src/fsfw/timemanager/Clock.h b/src/fsfw/timemanager/Clock.h index 83bc3d9f..6fe6486c 100644 --- a/src/fsfw/timemanager/Clock.h +++ b/src/fsfw/timemanager/Clock.h @@ -49,6 +49,13 @@ class Clock { * @return -@c returnvalue::OK on success. Otherwise, the OS failure code is returned. */ static ReturnValue_t setClock(const timeval *time); + + /** + * @deprecated Use getClock instead, which does the same. + * @param time + * @return + */ + static ReturnValue_t getClock_timeval(timeval *time); /** * This system call returns the current system clock in timeval format. * The timval format has the fields @c tv_sec with seconds and @c tv_usec with @@ -56,7 +63,18 @@ class Clock { * @param time A pointer to a timeval struct where the current time is stored. * @return @c returnvalue::OK on success. Otherwise, the OS failure code is returned. */ - static ReturnValue_t getClock_timeval(timeval *time); + static ReturnValue_t getClock(timeval *time); + + /** + * Retrieve a monotonic clock. This clock this is also more suited for measuring elapsed times + * between two time points, but less suited when the absolute time is required. + * + * Implementation example: A generic UNIX implementation can use CLOCK_MONOTONIC_RAW with + * `clock_gettime`. + * @param time + * @return + */ + static ReturnValue_t getClockMonotonic(timeval *time); /** * Get the time since boot in a timeval struct diff --git a/src/fsfw/timemanager/Countdown.cpp b/src/fsfw/timemanager/Countdown.cpp index 662fc8f0..5e743efe 100644 --- a/src/fsfw/timemanager/Countdown.cpp +++ b/src/fsfw/timemanager/Countdown.cpp @@ -16,7 +16,7 @@ Countdown::~Countdown() = default; ReturnValue_t Countdown::setTimeout(uint32_t milliseconds) { timeout.tv_sec = milliseconds / 1000; timeout.tv_usec = (milliseconds % 1000) * 1000; - return Clock::getClock_timeval(&startTime); + return Clock::getClockMonotonic(&startTime); } bool Countdown::hasTimedOut() const { @@ -48,7 +48,7 @@ uint32_t Countdown::timevalToMs(timeval &tv) { return tv.tv_sec * 1000 + tv.tv_u ReturnValue_t Countdown::setTimeoutTv(timeval tv) { timeout = tv; - return Clock::getClock_timeval(&startTime); + return Clock::getClockMonotonic(&startTime); } uint32_t Countdown::getTimeoutMs() const { return timeout.tv_sec * 1000 + timeout.tv_usec / 1000; } @@ -57,6 +57,6 @@ timeval Countdown::getTimeout() const { return timeout; } timeval Countdown::getCurrentTime() const { timeval currentTime{}; - Clock::getClock_timeval(¤tTime); + Clock::getClockMonotonic(¤tTime); return currentTime; } diff --git a/src/fsfw/timemanager/Stopwatch.cpp b/src/fsfw/timemanager/Stopwatch.cpp index 4bc3a460..ad39f1b3 100644 --- a/src/fsfw/timemanager/Stopwatch.cpp +++ b/src/fsfw/timemanager/Stopwatch.cpp @@ -9,10 +9,10 @@ Stopwatch::Stopwatch(bool displayOnDestruction, StopwatchDisplayMode displayMode) : displayOnDestruction(displayOnDestruction), displayMode(displayMode) { // Measures start time on initialization. - Clock::getClock_timeval(&startTime); + Clock::getClockMonotonic(&startTime); } -void Stopwatch::start() { Clock::getUptime(&startTime); } +void Stopwatch::start() { Clock::getClockMonotonic(&startTime); } dur_millis_t Stopwatch::stop(bool display) { stopInternal(); @@ -63,6 +63,6 @@ StopwatchDisplayMode Stopwatch::getDisplayMode() const { return displayMode; } void Stopwatch::stopInternal() { timeval endTime; - Clock::getClock_timeval(&endTime); + Clock::getClockMonotonic(&endTime); elapsedTime = endTime - startTime; } From 04ee3c73626fae6c090762981096eaba11866ba8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 4 Mar 2023 11:04:55 +0100 Subject: [PATCH 335/404] renaming --- src/fsfw/osal/linux/Clock.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/fsfw/osal/linux/Clock.cpp b/src/fsfw/osal/linux/Clock.cpp index c625785b..050ec099 100644 --- a/src/fsfw/osal/linux/Clock.cpp +++ b/src/fsfw/osal/linux/Clock.cpp @@ -69,13 +69,13 @@ ReturnValue_t Clock::getClock_usecs(uint64_t* time) { } ReturnValue_t Clock::getClockMonotonic(timeval *time) { - timespec timeUnix{}; - int status = clock_gettime(CLOCK_MONOTONIC_RAW, &timeUnix); + timespec timeMonotonic{}; + int status = clock_gettime(CLOCK_MONOTONIC_RAW, &timeMonotonic); if (status != 0) { return returnvalue::FAILED; } - time->tv_sec = timeUnix.tv_sec; - time->tv_usec = timeUnix.tv_nsec / 1000.0; + time->tv_sec = timeMonotonic.tv_sec; + time->tv_usec = timeMonotonic.tv_nsec / 1000.0; return returnvalue::OK; } From bbf0d7a0d25de0f2d2f6a63af8c0ebfa79251dfb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 4 Mar 2023 11:31:54 +0100 Subject: [PATCH 336/404] add basic impl (no windows) for host --- src/fsfw/osal/host/Clock.cpp | 28 +++++++++++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) diff --git a/src/fsfw/osal/host/Clock.cpp b/src/fsfw/osal/host/Clock.cpp index dbf6529c..18cc4639 100644 --- a/src/fsfw/osal/host/Clock.cpp +++ b/src/fsfw/osal/host/Clock.cpp @@ -47,7 +47,29 @@ ReturnValue_t Clock::setClock(const timeval* time) { return returnvalue::OK; } -ReturnValue_t Clock::getClock_timeval(timeval* time) { +ReturnValue_t Clock::getClockMonotonic(timeval* time) { +#if defined(PLATFORM_WIN) + // TODO: Implement with std::chrono::steady_clock +#elif defined(PLATFORM_UNIX) + timespec timeMonotonic; + int status = clock_gettime(CLOCK_MONOTONIC_RAW, &timeMonotonic); + if (status != 0) { + return returnvalue::FAILED; + } + time->tv_sec = timeMonotonic.tv_sec; + time->tv_usec = timeMonotonic.tv_nsec / 1000.0; + return returnvalue::OK; +#else +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "Clock::getUptime: Not implemented for found OS!" << std::endl; +#else + sif::printWarning("Clock::getUptime: Not implemented for found OS!\n"); +#endif + return returnvalue::FAILED; +#endif +} + +ReturnValue_t Clock::getClock(timeval* time) { #if defined(PLATFORM_WIN) auto now = std::chrono::system_clock::now(); auto secondsChrono = std::chrono::time_point_cast(now); @@ -75,6 +97,10 @@ ReturnValue_t Clock::getClock_timeval(timeval* time) { #endif } +ReturnValue_t Clock::getClock_timeval(timeval* time) { + return Clock::getClock(time); +} + ReturnValue_t Clock::getClock_usecs(uint64_t* time) { if (time == nullptr) { return returnvalue::FAILED; From a39f1271ec5b7b2bd987c655705381d07a01baed Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 4 Mar 2023 11:32:32 +0100 Subject: [PATCH 337/404] return FAILED for win --- src/fsfw/osal/host/Clock.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/fsfw/osal/host/Clock.cpp b/src/fsfw/osal/host/Clock.cpp index 18cc4639..5e917600 100644 --- a/src/fsfw/osal/host/Clock.cpp +++ b/src/fsfw/osal/host/Clock.cpp @@ -50,6 +50,7 @@ ReturnValue_t Clock::setClock(const timeval* time) { ReturnValue_t Clock::getClockMonotonic(timeval* time) { #if defined(PLATFORM_WIN) // TODO: Implement with std::chrono::steady_clock + return returnvalue::FAILED; #elif defined(PLATFORM_UNIX) timespec timeMonotonic; int status = clock_gettime(CLOCK_MONOTONIC_RAW, &timeMonotonic); From 5cd7b98ba7a1f26702c3cf780e4cde6296164348 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 4 Mar 2023 11:38:16 +0100 Subject: [PATCH 338/404] more todo docs --- src/fsfw/osal/host/Clock.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/fsfw/osal/host/Clock.cpp b/src/fsfw/osal/host/Clock.cpp index 5e917600..ec66a11d 100644 --- a/src/fsfw/osal/host/Clock.cpp +++ b/src/fsfw/osal/host/Clock.cpp @@ -49,7 +49,9 @@ ReturnValue_t Clock::setClock(const timeval* time) { ReturnValue_t Clock::getClockMonotonic(timeval* time) { #if defined(PLATFORM_WIN) - // TODO: Implement with std::chrono::steady_clock + // TODO: Implement with std::chrono::steady_clock.. or in some other way. I am not even sure + // whether this is possible with steady_clock. The conversion we have to do here just to be + // generic is kind of awkward.. return returnvalue::FAILED; #elif defined(PLATFORM_UNIX) timespec timeMonotonic; From e9d9f446053699a91d89250910cc0d59d05fbe6b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 6 Mar 2023 14:01:45 +0100 Subject: [PATCH 339/404] added length check --- src/fsfw/pus/CServiceHealthCommanding.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/fsfw/pus/CServiceHealthCommanding.cpp b/src/fsfw/pus/CServiceHealthCommanding.cpp index 49284b73..ebb286cf 100644 --- a/src/fsfw/pus/CServiceHealthCommanding.cpp +++ b/src/fsfw/pus/CServiceHealthCommanding.cpp @@ -68,6 +68,9 @@ ReturnValue_t CServiceHealthCommanding::prepareCommand(CommandMessage *message, ReturnValue_t result = returnvalue::OK; switch (subservice) { case (Subservice::COMMAND_SET_HEALTH): { + if(tcDataLen != sizeof(object_id_t) + sizeof(HasHealthIF::HealthState)) { + return CommandingServiceBase::INVALID_TC; + } HealthSetCommand healthCommand; result = healthCommand.deSerialize(&tcData, &tcDataLen, SerializeIF::Endianness::BIG); if (result != returnvalue::OK) { From 2745b2080dc9c53ef6699c844ba1d000351d07ba Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Mar 2023 13:55:40 +0100 Subject: [PATCH 340/404] allow submode mask now --- src/fsfw/osal/host/Clock.cpp | 4 +- src/fsfw/osal/linux/Clock.cpp | 8 +- src/fsfw/pus/CServiceHealthCommanding.cpp | 2 +- src/fsfw/subsystem/SubsystemBase.cpp | 26 ++-- src/fsfw/subsystem/modes/ModeDefinitions.h | 155 ++++++++++----------- unittests/CMakeLists.txt | 1 + unittests/subsystem/CMakeLists.txt | 1 + unittests/subsystem/testModeDef.cpp | 49 +++++++ 8 files changed, 147 insertions(+), 99 deletions(-) create mode 100644 unittests/subsystem/CMakeLists.txt create mode 100644 unittests/subsystem/testModeDef.cpp diff --git a/src/fsfw/osal/host/Clock.cpp b/src/fsfw/osal/host/Clock.cpp index ec66a11d..2a3c94bc 100644 --- a/src/fsfw/osal/host/Clock.cpp +++ b/src/fsfw/osal/host/Clock.cpp @@ -100,9 +100,7 @@ ReturnValue_t Clock::getClock(timeval* time) { #endif } -ReturnValue_t Clock::getClock_timeval(timeval* time) { - return Clock::getClock(time); -} +ReturnValue_t Clock::getClock_timeval(timeval* time) { return Clock::getClock(time); } ReturnValue_t Clock::getClock_usecs(uint64_t* time) { if (time == nullptr) { diff --git a/src/fsfw/osal/linux/Clock.cpp b/src/fsfw/osal/linux/Clock.cpp index 050ec099..fd861da2 100644 --- a/src/fsfw/osal/linux/Clock.cpp +++ b/src/fsfw/osal/linux/Clock.cpp @@ -42,7 +42,7 @@ ReturnValue_t Clock::setClock(const timeval* time) { return returnvalue::OK; } -ReturnValue_t Clock::getClock(timeval *time) { +ReturnValue_t Clock::getClock(timeval* time) { timespec timeUnix{}; int status = clock_gettime(CLOCK_REALTIME, &timeUnix); if (status != 0) { @@ -53,9 +53,7 @@ ReturnValue_t Clock::getClock(timeval *time) { return returnvalue::OK; } -ReturnValue_t Clock::getClock_timeval(timeval* time) { - return Clock::getClock(time); -} +ReturnValue_t Clock::getClock_timeval(timeval* time) { return Clock::getClock(time); } ReturnValue_t Clock::getClock_usecs(uint64_t* time) { timeval timeVal{}; @@ -68,7 +66,7 @@ ReturnValue_t Clock::getClock_usecs(uint64_t* time) { return returnvalue::OK; } -ReturnValue_t Clock::getClockMonotonic(timeval *time) { +ReturnValue_t Clock::getClockMonotonic(timeval* time) { timespec timeMonotonic{}; int status = clock_gettime(CLOCK_MONOTONIC_RAW, &timeMonotonic); if (status != 0) { diff --git a/src/fsfw/pus/CServiceHealthCommanding.cpp b/src/fsfw/pus/CServiceHealthCommanding.cpp index ebb286cf..da4593ae 100644 --- a/src/fsfw/pus/CServiceHealthCommanding.cpp +++ b/src/fsfw/pus/CServiceHealthCommanding.cpp @@ -68,7 +68,7 @@ ReturnValue_t CServiceHealthCommanding::prepareCommand(CommandMessage *message, ReturnValue_t result = returnvalue::OK; switch (subservice) { case (Subservice::COMMAND_SET_HEALTH): { - if(tcDataLen != sizeof(object_id_t) + sizeof(HasHealthIF::HealthState)) { + if (tcDataLen != sizeof(object_id_t) + sizeof(HasHealthIF::HealthState)) { return CommandingServiceBase::INVALID_TC; } HealthSetCommand healthCommand; diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp index 66858c39..560cb769 100644 --- a/src/fsfw/subsystem/SubsystemBase.cpp +++ b/src/fsfw/subsystem/SubsystemBase.cpp @@ -21,8 +21,23 @@ SubsystemBase::~SubsystemBase() { QueueFactory::instance()->deleteMessageQueue(c ReturnValue_t SubsystemBase::checkStateAgainstTable(HybridIterator tableIter, Submode_t targetSubmode) { + using namespace mode; std::map::iterator childIter; + auto checkSubmode = [&]() { + if (tableIter.value->inheritSubmode()) { + if (childIter->second.submode != targetSubmode) { + return returnvalue::FAILED; + } + } + uint8_t mask; + if (tableIter.value->submodesAllowed(&mask)) { + if ((childIter->second.submode | mask) != mask) { + return returnvalue::FAILED; + } + } + return returnvalue::OK; + }; for (; tableIter.value != NULL; ++tableIter) { object_id_t object = tableIter.value->getObject(); @@ -33,14 +48,9 @@ ReturnValue_t SubsystemBase::checkStateAgainstTable(HybridIteratorsecond.mode != tableIter.value->getMode()) { return returnvalue::FAILED; } - - Submode_t submodeToCheckAgainst = tableIter.value->getSubmode(); - if (tableIter.value->inheritSubmode()) { - submodeToCheckAgainst = targetSubmode; - } - - if (childIter->second.submode != submodeToCheckAgainst) { - return returnvalue::FAILED; + ReturnValue_t result = checkSubmode(); + if (result != returnvalue::OK) { + return result; } } return returnvalue::OK; diff --git a/src/fsfw/subsystem/modes/ModeDefinitions.h b/src/fsfw/subsystem/modes/ModeDefinitions.h index d22bcd95..4b938a68 100644 --- a/src/fsfw/subsystem/modes/ModeDefinitions.h +++ b/src/fsfw/subsystem/modes/ModeDefinitions.h @@ -1,111 +1,102 @@ #ifndef FSFW_SUBSYSTEM_MODES_MODEDEFINITIONS_H_ #define FSFW_SUBSYSTEM_MODES_MODEDEFINITIONS_H_ -#include "../../modes/HasModesIF.h" -#include "../../objectmanager/SystemObjectIF.h" -#include "../../serialize/SerialLinkedListAdapter.h" -#include "../../serialize/SerializeIF.h" +#include "fsfw/modes/HasModesIF.h" +#include "fsfw/objectmanager/SystemObjectIF.h" +#include "fsfw/serialize/SerialLinkedListAdapter.h" +#include "fsfw/serialize/SerializeIF.h" -class ModeListEntry : public SerializeIF, public LinkedElement { +namespace mode { +enum SpecialSubmodeFlags : uint8_t { INHERIT = 1 << 0, ALLOWED_MASK = 1 << 1 }; +} + +class ModeListEntry : public SerialLinkedListAdapter, + public LinkedElement { public: - ModeListEntry() : LinkedElement(this) {} + ModeListEntry() : LinkedElement(this) { setLinks(); } - uint32_t value1 = 0; - uint32_t value2 = 0; - uint8_t value3 = 0; - uint8_t value4 = 0; + SerializeElement value1 = 0; + SerializeElement value2 = 0; + SerializeElement value3 = 0; + SerializeElement value4 = 0; + SerializeElement value5 = 0; - virtual ReturnValue_t serialize(uint8_t** buffer, size_t* size, size_t maxSize, - Endianness streamEndianness) const { - ReturnValue_t result; - - result = SerializeAdapter::serialize(&value1, buffer, size, maxSize, streamEndianness); - - if (result != returnvalue::OK) { - return result; - } - result = SerializeAdapter::serialize(&value2, buffer, size, maxSize, streamEndianness); - - if (result != returnvalue::OK) { - return result; - } - result = SerializeAdapter::serialize(&value3, buffer, size, maxSize, streamEndianness); - - if (result != returnvalue::OK) { - return result; - } - - result = SerializeAdapter::serialize(&value4, buffer, size, maxSize, streamEndianness); - - return result; - } - - virtual size_t getSerializedSize() const { - return sizeof(value1) + sizeof(value2) + sizeof(value3) + sizeof(value4); - } - - virtual ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size, - Endianness streamEndianness) { - ReturnValue_t result; - - result = SerializeAdapter::deSerialize(&value1, buffer, size, streamEndianness); - - if (result != returnvalue::OK) { - return result; - } - result = SerializeAdapter::deSerialize(&value2, buffer, size, streamEndianness); - - if (result != returnvalue::OK) { - return result; - } - result = SerializeAdapter::deSerialize(&value3, buffer, size, streamEndianness); - - if (result != returnvalue::OK) { - return result; - } - result = SerializeAdapter::deSerialize(&value4, buffer, size, streamEndianness); - - return result; + void setLinks() { + setStart(&value1); + value1.setNext(&value2); + value2.setNext(&value3); + value3.setNext(&value4); + value4.setNext(&value5); } // for Sequences - Mode_t getTableId() const { return value1; } + Mode_t getTableId() const { return value1.entry; } - void setTableId(Mode_t tableId) { this->value1 = tableId; } + void setTableId(Mode_t tableId) { this->value1.entry = tableId; } - uint8_t getWaitSeconds() const { return value2; } + uint8_t getWaitSeconds() const { return value2.entry; } - void setWaitSeconds(uint8_t waitSeconds) { this->value2 = waitSeconds; } + void setWaitSeconds(uint8_t waitSeconds) { this->value2.entry = waitSeconds; } - bool checkSuccess() const { return value3 == 1; } + bool checkSuccess() const { return value3.entry == 1; } - void setCheckSuccess(bool checkSuccess) { this->value3 = checkSuccess; } + void setCheckSuccess(bool checkSuccess) { this->value3.entry = checkSuccess; } // for Tables - object_id_t getObject() const { return value1; } + object_id_t getObject() const { return value1.entry; } - void setObject(object_id_t object) { this->value1 = object; } + void setObject(object_id_t object) { this->value1.entry = object; } - Mode_t getMode() const { return value2; } + Mode_t getMode() const { return value2.entry; } - void setMode(Mode_t mode) { this->value2 = mode; } + void setMode(Mode_t mode) { this->value2.entry = mode; } - Submode_t getSubmode() const { return value3; } + Submode_t getSubmode() const { return value3.entry; } - void setSubmode(Submode_t submode) { this->value3 = submode; } + void setSubmode(Submode_t submode) { this->value3.entry = submode; } - bool inheritSubmode() const { return value4 == 1; } - - void setInheritSubmode(bool inherit) { - if (inherit) { - value4 = 1; - } else { - value4 = 0; + bool inheritSubmode() const { + return (value4.entry & mode::SpecialSubmodeFlags::INHERIT) == + mode::SpecialSubmodeFlags::INHERIT; + } + bool submodesAllowed(uint8_t* mask) const { + bool submodesAllowed = (value4.entry & mode::SpecialSubmodeFlags::ALLOWED_MASK) == + mode::SpecialSubmodeFlags::ALLOWED_MASK; + if (submodesAllowed and mask != nullptr) { + *mask = value5.entry; } + return submodesAllowed; } - bool operator==(ModeListEntry other) { - return ((value1 == other.value1) && (value2 == other.value2) && (value3 == other.value3)); + /** + * Enable the inheritance of submodes. This is relevant for both the execution + * of mode tables and for mode checking. + */ + void enableInheritSubmode() { value4.entry |= mode::SpecialSubmodeFlags::INHERIT; } + /** + * Disable the inheritance of submodes. This is relevant for both the execution + * of mode tables and for mode checking. + */ + void disableInheritSubmode() { value4.entry &= ~mode::SpecialSubmodeFlags::INHERIT; } + + /** + * Specialization of @enableSubmodeAllowed which allows all submodes. + */ + void allowAllSubmodes() { enableSubmodeAllowed(0xff); } + + /** + * Enable an allowed submode mask for mode checks. + */ + void enableSubmodeAllowed(uint8_t mask) { + value4.entry |= mode::SpecialSubmodeFlags::ALLOWED_MASK; + value5.entry = mask; + } + /** + * Enforce the equality of submodes for mode checks. This is the default. + */ + void disableSubmodeAllowed() { + value4.entry &= ~mode::SpecialSubmodeFlags::ALLOWED_MASK; + value5.entry = 0; } }; diff --git a/unittests/CMakeLists.txt b/unittests/CMakeLists.txt index ad26e392..950b96b8 100644 --- a/unittests/CMakeLists.txt +++ b/unittests/CMakeLists.txt @@ -13,6 +13,7 @@ add_subdirectory(util) add_subdirectory(container) add_subdirectory(osal) add_subdirectory(pus) +add_subdirectory(subsystem) add_subdirectory(serialize) add_subdirectory(datapoollocal) add_subdirectory(storagemanager) diff --git a/unittests/subsystem/CMakeLists.txt b/unittests/subsystem/CMakeLists.txt new file mode 100644 index 00000000..724ebc05 --- /dev/null +++ b/unittests/subsystem/CMakeLists.txt @@ -0,0 +1 @@ +target_sources(${FSFW_TEST_TGT} PRIVATE testModeDef.cpp) diff --git a/unittests/subsystem/testModeDef.cpp b/unittests/subsystem/testModeDef.cpp new file mode 100644 index 00000000..758b3301 --- /dev/null +++ b/unittests/subsystem/testModeDef.cpp @@ -0,0 +1,49 @@ + +#include +#include + +#include "fsfw/subsystem/modes/ModeDefinitions.h" + +TEST_CASE("Mode Definitions", "[mode]") { + ModeListEntry entry; + + SECTION("Basic") { + entry.setMode(HasModesIF::MODE_OFF); + entry.setSubmode(2); + CHECK(entry.getMode() == HasModesIF::MODE_OFF); + CHECK(entry.getSubmode() == 2); + uint8_t mask; + CHECK(entry.submodesAllowed(&mask) == false); + } + + SECTION("Allowed submode mask") { + entry.allowAllSubmodes(); + uint8_t mask; + CHECK(entry.submodesAllowed(&mask) == true); + CHECK(mask == 0xff); + } + + SECTION("Serialization") { + std::array buf{}; + entry.setObject(0x1f2f3f4f); + entry.setMode(HasModesIF::MODE_ON); + entry.setSubmode(2); + entry.enableInheritSubmode(); + entry.enableSubmodeAllowed(0x1f); + uint8_t* serPtr = buf.data(); + size_t serLen = 0; + REQUIRE(entry.serialize(&serPtr, &serLen, buf.size(), SerializeIF::Endianness::NETWORK) == + returnvalue::OK); + CHECK(buf[0] == 0x1f); + CHECK(buf[1] == 0x2f); + CHECK(buf[2] == 0x3f); + CHECK(buf[3] == 0x4f); + CHECK(buf[4] == 0); + CHECK(buf[5] == 0); + CHECK(buf[6] == 0); + CHECK(buf[7] == HasModesIF::MODE_ON); + CHECK(buf[8] == 2); + CHECK(buf[9] == (mode::SpecialSubmodeFlags::ALLOWED_MASK | mode::SpecialSubmodeFlags::INHERIT)); + CHECK(buf[10] == 0x1f); + } +} From 4c48668125de7a43cb42b8513cada73c2fe3ffbd Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Mar 2023 16:03:28 +0100 Subject: [PATCH 341/404] add copy and assignment ctor for mode definition --- src/fsfw/subsystem/modes/ModeDefinitions.h | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/src/fsfw/subsystem/modes/ModeDefinitions.h b/src/fsfw/subsystem/modes/ModeDefinitions.h index 4b938a68..b60f01de 100644 --- a/src/fsfw/subsystem/modes/ModeDefinitions.h +++ b/src/fsfw/subsystem/modes/ModeDefinitions.h @@ -13,7 +13,7 @@ enum SpecialSubmodeFlags : uint8_t { INHERIT = 1 << 0, ALLOWED_MASK = 1 << 1 }; class ModeListEntry : public SerialLinkedListAdapter, public LinkedElement { public: - ModeListEntry() : LinkedElement(this) { setLinks(); } + ModeListEntry() : SerialLinkedListAdapter(), LinkedElement(this) { setLinks(); } SerializeElement value1 = 0; SerializeElement value2 = 0; @@ -21,6 +21,25 @@ class ModeListEntry : public SerialLinkedListAdapter, SerializeElement value4 = 0; SerializeElement value5 = 0; + ModeListEntry(const ModeListEntry& other) + : SerialLinkedListAdapter(), LinkedElement(this) { + value1.entry = other.value1.entry; + value2.entry = other.value2.entry; + value3.entry = other.value3.entry; + value4.entry = other.value4.entry; + value5.entry = other.value5.entry; + setLinks(); + } + + ModeListEntry& operator=(const ModeListEntry& other) { + this->value1.entry = other.value1.entry; + this->value2.entry = other.value2.entry; + this->value3.entry = other.value3.entry; + this->value4.entry = other.value4.entry; + this->value5.entry = other.value5.entry; + return *this; + } + void setLinks() { setStart(&value1); value1.setNext(&value2); From af58c414fc40f6151ecb3df3803f9820290e5399 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Mar 2023 16:29:10 +0100 Subject: [PATCH 342/404] bugfix in submode check logic --- src/fsfw/subsystem/SubsystemBase.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp index 560cb769..a876ae33 100644 --- a/src/fsfw/subsystem/SubsystemBase.cpp +++ b/src/fsfw/subsystem/SubsystemBase.cpp @@ -25,13 +25,18 @@ ReturnValue_t SubsystemBase::checkStateAgainstTable(HybridIterator::iterator childIter; auto checkSubmode = [&]() { + uint8_t mask; + bool submodesAllowedMask = tableIter.value->submodesAllowed(&mask); + uint8_t submodeToCheckAgainst = tableIter.value->getSubmode(); if (tableIter.value->inheritSubmode()) { - if (childIter->second.submode != targetSubmode) { + submodeToCheckAgainst = targetSubmode; + } + if (not submodesAllowedMask) { + if (childIter->second.submode != submodeToCheckAgainst) { return returnvalue::FAILED; } } - uint8_t mask; - if (tableIter.value->submodesAllowed(&mask)) { + if (submodesAllowedMask) { if ((childIter->second.submode | mask) != mask) { return returnvalue::FAILED; } From c80a3752d9f95cfdb9842f3e3d2c416efe66fdeb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Mar 2023 16:31:08 +0100 Subject: [PATCH 343/404] afmt --- src/fsfw/osal/host/Clock.cpp | 4 +--- src/fsfw/osal/linux/Clock.cpp | 8 +++----- src/fsfw/pus/CServiceHealthCommanding.cpp | 2 +- 3 files changed, 5 insertions(+), 9 deletions(-) diff --git a/src/fsfw/osal/host/Clock.cpp b/src/fsfw/osal/host/Clock.cpp index ec66a11d..2a3c94bc 100644 --- a/src/fsfw/osal/host/Clock.cpp +++ b/src/fsfw/osal/host/Clock.cpp @@ -100,9 +100,7 @@ ReturnValue_t Clock::getClock(timeval* time) { #endif } -ReturnValue_t Clock::getClock_timeval(timeval* time) { - return Clock::getClock(time); -} +ReturnValue_t Clock::getClock_timeval(timeval* time) { return Clock::getClock(time); } ReturnValue_t Clock::getClock_usecs(uint64_t* time) { if (time == nullptr) { diff --git a/src/fsfw/osal/linux/Clock.cpp b/src/fsfw/osal/linux/Clock.cpp index 050ec099..fd861da2 100644 --- a/src/fsfw/osal/linux/Clock.cpp +++ b/src/fsfw/osal/linux/Clock.cpp @@ -42,7 +42,7 @@ ReturnValue_t Clock::setClock(const timeval* time) { return returnvalue::OK; } -ReturnValue_t Clock::getClock(timeval *time) { +ReturnValue_t Clock::getClock(timeval* time) { timespec timeUnix{}; int status = clock_gettime(CLOCK_REALTIME, &timeUnix); if (status != 0) { @@ -53,9 +53,7 @@ ReturnValue_t Clock::getClock(timeval *time) { return returnvalue::OK; } -ReturnValue_t Clock::getClock_timeval(timeval* time) { - return Clock::getClock(time); -} +ReturnValue_t Clock::getClock_timeval(timeval* time) { return Clock::getClock(time); } ReturnValue_t Clock::getClock_usecs(uint64_t* time) { timeval timeVal{}; @@ -68,7 +66,7 @@ ReturnValue_t Clock::getClock_usecs(uint64_t* time) { return returnvalue::OK; } -ReturnValue_t Clock::getClockMonotonic(timeval *time) { +ReturnValue_t Clock::getClockMonotonic(timeval* time) { timespec timeMonotonic{}; int status = clock_gettime(CLOCK_MONOTONIC_RAW, &timeMonotonic); if (status != 0) { diff --git a/src/fsfw/pus/CServiceHealthCommanding.cpp b/src/fsfw/pus/CServiceHealthCommanding.cpp index ebb286cf..da4593ae 100644 --- a/src/fsfw/pus/CServiceHealthCommanding.cpp +++ b/src/fsfw/pus/CServiceHealthCommanding.cpp @@ -68,7 +68,7 @@ ReturnValue_t CServiceHealthCommanding::prepareCommand(CommandMessage *message, ReturnValue_t result = returnvalue::OK; switch (subservice) { case (Subservice::COMMAND_SET_HEALTH): { - if(tcDataLen != sizeof(object_id_t) + sizeof(HasHealthIF::HealthState)) { + if (tcDataLen != sizeof(object_id_t) + sizeof(HasHealthIF::HealthState)) { return CommandingServiceBase::INVALID_TC; } HealthSetCommand healthCommand; From 070b48ada28e471039dfbc3ac5b964f6cdb8d773 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 7 Mar 2023 17:15:34 +0100 Subject: [PATCH 344/404] review improvements --- src/fsfw/subsystem/SubsystemBase.cpp | 38 +++++++++------------- src/fsfw/subsystem/modes/ModeDefinitions.h | 9 +++-- 2 files changed, 23 insertions(+), 24 deletions(-) diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp index a876ae33..eccd447c 100644 --- a/src/fsfw/subsystem/SubsystemBase.cpp +++ b/src/fsfw/subsystem/SubsystemBase.cpp @@ -24,25 +24,6 @@ ReturnValue_t SubsystemBase::checkStateAgainstTable(HybridIterator::iterator childIter; - auto checkSubmode = [&]() { - uint8_t mask; - bool submodesAllowedMask = tableIter.value->submodesAllowed(&mask); - uint8_t submodeToCheckAgainst = tableIter.value->getSubmode(); - if (tableIter.value->inheritSubmode()) { - submodeToCheckAgainst = targetSubmode; - } - if (not submodesAllowedMask) { - if (childIter->second.submode != submodeToCheckAgainst) { - return returnvalue::FAILED; - } - } - if (submodesAllowedMask) { - if ((childIter->second.submode | mask) != mask) { - return returnvalue::FAILED; - } - } - return returnvalue::OK; - }; for (; tableIter.value != NULL; ++tableIter) { object_id_t object = tableIter.value->getObject(); @@ -53,9 +34,22 @@ ReturnValue_t SubsystemBase::checkStateAgainstTable(HybridIteratorsecond.mode != tableIter.value->getMode()) { return returnvalue::FAILED; } - ReturnValue_t result = checkSubmode(); - if (result != returnvalue::OK) { - return result; + + // Check submodes here. + uint8_t mask; + bool submodesAllowedMask = tableIter.value->submodesAllowed(&mask); + uint8_t submodeToCheckAgainst = tableIter.value->getSubmode(); + if (tableIter.value->inheritSubmode()) { + submodeToCheckAgainst = targetSubmode; + } + if (submodesAllowedMask) { + if ((childIter->second.submode | mask) != mask) { + return returnvalue::FAILED; + } + } else { + if (childIter->second.submode != submodeToCheckAgainst) { + return returnvalue::FAILED; + } } } return returnvalue::OK; diff --git a/src/fsfw/subsystem/modes/ModeDefinitions.h b/src/fsfw/subsystem/modes/ModeDefinitions.h index b60f01de..70a66fa6 100644 --- a/src/fsfw/subsystem/modes/ModeDefinitions.h +++ b/src/fsfw/subsystem/modes/ModeDefinitions.h @@ -13,6 +13,8 @@ enum SpecialSubmodeFlags : uint8_t { INHERIT = 1 << 0, ALLOWED_MASK = 1 << 1 }; class ModeListEntry : public SerialLinkedListAdapter, public LinkedElement { public: + static constexpr uint8_t ALL_SUBMODES_ALLOWED_MASK = 0xff; + ModeListEntry() : SerialLinkedListAdapter(), LinkedElement(this) { setLinks(); } SerializeElement value1 = 0; @@ -101,10 +103,13 @@ class ModeListEntry : public SerialLinkedListAdapter, /** * Specialization of @enableSubmodeAllowed which allows all submodes. */ - void allowAllSubmodes() { enableSubmodeAllowed(0xff); } + void allowAllSubmodes() { enableSubmodeAllowed(ALL_SUBMODES_ALLOWED_MASK); } /** - * Enable an allowed submode mask for mode checks. + * Enable an allowed submode mask for mode checks. Any submode which contains bits + * outside of the mask will be declined. + * + * For example, for a mask of 0b11, only the modes 0b00, 0b01 and 0b11 will be accepted. */ void enableSubmodeAllowed(uint8_t mask) { value4.entry |= mode::SpecialSubmodeFlags::ALLOWED_MASK; From 1b7493f945302b3785ceba6e7a34a727e3898a13 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Mar 2023 01:18:11 +0100 Subject: [PATCH 345/404] OFF -> NORMAL: Set transition source modes --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 3dbacc95..99b7496c 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -565,6 +565,9 @@ void DeviceHandlerBase::setMode(Mode_t newMode, uint8_t newSubmode) { */ if (newMode == MODE_ON and continueToNormal) { continueToNormal = false; + // TODO: Check whether the following two lines are okay to do so. + transitionSourceMode = MODE_ON; + transitionSourceSubMode = submode; mode = _MODE_TO_NORMAL; return; } From 26e4445189b676eaee11840e5a9d0ede25cf3896 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 8 Mar 2023 14:47:03 +0100 Subject: [PATCH 346/404] exceptionless host filesystem --- src/fsfw_hal/host/HostFilesystem.cpp | 24 ++++++++++++++++-------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/src/fsfw_hal/host/HostFilesystem.cpp b/src/fsfw_hal/host/HostFilesystem.cpp index d430d0f0..315ba422 100644 --- a/src/fsfw_hal/host/HostFilesystem.cpp +++ b/src/fsfw_hal/host/HostFilesystem.cpp @@ -15,7 +15,8 @@ ReturnValue_t HostFilesystem::writeToFile(FileOpParams params, const uint8_t *da return returnvalue::FAILED; } path path(params.path()); - if (not exists(path)) { + std::error_code e; + if (not exists(path, e)) { return HasFileSystemIF::FILE_DOES_NOT_EXIST; } // This is equivalent to "r+" mode, which is what we need here. Only using ::out would truncate @@ -35,7 +36,8 @@ ReturnValue_t HostFilesystem::readFromFile(FileOpParams params, uint8_t **buffer return returnvalue::FAILED; } path path(params.path()); - if (not exists(path)) { + std::error_code e; + if (not exists(path, e)) { return HasFileSystemIF::FILE_DOES_NOT_EXIST; } ifstream file(path); @@ -59,7 +61,8 @@ ReturnValue_t HostFilesystem::createFile(FilesystemParams params, const uint8_t return returnvalue::FAILED; } path path(params.path); - if (exists(path)) { + std::error_code e; + if (exists(path, e)) { return HasFileSystemIF::FILE_ALREADY_EXISTS; } ofstream file(path); @@ -74,7 +77,8 @@ ReturnValue_t HostFilesystem::removeFile(const char *path_, FileSystemArgsIF *ar return returnvalue::FAILED; } path path(path_); - if (not exists(path)) { + std::error_code e; + if (not exists(path, e)) { return HasFileSystemIF::FILE_DOES_NOT_EXIST; } if (remove(path, errorCode)) { @@ -89,7 +93,8 @@ ReturnValue_t HostFilesystem::createDirectory(FilesystemParams params, bool crea } path dirPath(params.path); - if (exists(dirPath)) { + std::error_code e; + if (exists(dirPath, e)) { return HasFileSystemIF::DIRECTORY_ALREADY_EXISTS; } @@ -110,7 +115,8 @@ ReturnValue_t HostFilesystem::removeDirectory(FilesystemParams params, bool dele return returnvalue::FAILED; } path dirPath(params.path); - if (not exists(dirPath)) { + std::error_code e; + if (not exists(dirPath, e)) { return HasFileSystemIF::DIRECTORY_DOES_NOT_EXIST; } if (is_regular_file(dirPath)) { @@ -149,12 +155,14 @@ ReturnValue_t HostFilesystem::rename(const char *oldPath_, const char *newPath_, bool HostFilesystem::fileExists(FilesystemParams params) { path path(params.path); - return filesystem::exists(path); + std::error_code e; + return filesystem::exists(path, e); } ReturnValue_t HostFilesystem::truncateFile(FilesystemParams params) { path path(params.path); - if (not filesystem::exists(path)) { + std::error_code e; + if (not filesystem::exists(path, e)) { return FILE_DOES_NOT_EXIST; } ofstream of(path); From f0bddfcb21bd2145a27030a40e16d04cce7dcf6a Mon Sep 17 00:00:00 2001 From: Ulrich Mohr Date: Thu, 9 Mar 2023 16:43:45 +0100 Subject: [PATCH 347/404] Modes: reusing submode for mode mask, more unittests --- src/fsfw/subsystem/modes/ModeDefinitions.h | 10 ++-- unittests/subsystem/testModeDef.cpp | 63 ++++++++++++++++++++-- 2 files changed, 61 insertions(+), 12 deletions(-) diff --git a/src/fsfw/subsystem/modes/ModeDefinitions.h b/src/fsfw/subsystem/modes/ModeDefinitions.h index 70a66fa6..ffeace5a 100644 --- a/src/fsfw/subsystem/modes/ModeDefinitions.h +++ b/src/fsfw/subsystem/modes/ModeDefinitions.h @@ -21,7 +21,6 @@ class ModeListEntry : public SerialLinkedListAdapter, SerializeElement value2 = 0; SerializeElement value3 = 0; SerializeElement value4 = 0; - SerializeElement value5 = 0; ModeListEntry(const ModeListEntry& other) : SerialLinkedListAdapter(), LinkedElement(this) { @@ -29,7 +28,6 @@ class ModeListEntry : public SerialLinkedListAdapter, value2.entry = other.value2.entry; value3.entry = other.value3.entry; value4.entry = other.value4.entry; - value5.entry = other.value5.entry; setLinks(); } @@ -38,7 +36,6 @@ class ModeListEntry : public SerialLinkedListAdapter, this->value2.entry = other.value2.entry; this->value3.entry = other.value3.entry; this->value4.entry = other.value4.entry; - this->value5.entry = other.value5.entry; return *this; } @@ -47,7 +44,6 @@ class ModeListEntry : public SerialLinkedListAdapter, value1.setNext(&value2); value2.setNext(&value3); value3.setNext(&value4); - value4.setNext(&value5); } // for Sequences @@ -84,7 +80,7 @@ class ModeListEntry : public SerialLinkedListAdapter, bool submodesAllowed = (value4.entry & mode::SpecialSubmodeFlags::ALLOWED_MASK) == mode::SpecialSubmodeFlags::ALLOWED_MASK; if (submodesAllowed and mask != nullptr) { - *mask = value5.entry; + *mask = value3.entry; } return submodesAllowed; } @@ -113,14 +109,14 @@ class ModeListEntry : public SerialLinkedListAdapter, */ void enableSubmodeAllowed(uint8_t mask) { value4.entry |= mode::SpecialSubmodeFlags::ALLOWED_MASK; - value5.entry = mask; + value3.entry = mask; } /** * Enforce the equality of submodes for mode checks. This is the default. */ void disableSubmodeAllowed() { value4.entry &= ~mode::SpecialSubmodeFlags::ALLOWED_MASK; - value5.entry = 0; + value3.entry = 0; } }; diff --git a/unittests/subsystem/testModeDef.cpp b/unittests/subsystem/testModeDef.cpp index 758b3301..e5a4e733 100644 --- a/unittests/subsystem/testModeDef.cpp +++ b/unittests/subsystem/testModeDef.cpp @@ -16,20 +16,30 @@ TEST_CASE("Mode Definitions", "[mode]") { CHECK(entry.submodesAllowed(&mask) == false); } + SECTION("Inherit submode") { + entry.enableInheritSubmode(); + CHECK(entry.inheritSubmode() == true); + entry.disableInheritSubmode(); + CHECK(entry.inheritSubmode() == false); + } + SECTION("Allowed submode mask") { entry.allowAllSubmodes(); uint8_t mask; CHECK(entry.submodesAllowed(&mask) == true); CHECK(mask == 0xff); + entry.enableSubmodeAllowed(0x32); + CHECK(entry.submodesAllowed(&mask) == true); + CHECK(mask == 0x32); + entry.disableSubmodeAllowed(); + CHECK(entry.submodesAllowed(&mask) == false); } - SECTION("Serialization") { + SECTION("Serialization nominal") { std::array buf{}; entry.setObject(0x1f2f3f4f); entry.setMode(HasModesIF::MODE_ON); entry.setSubmode(2); - entry.enableInheritSubmode(); - entry.enableSubmodeAllowed(0x1f); uint8_t* serPtr = buf.data(); size_t serLen = 0; REQUIRE(entry.serialize(&serPtr, &serLen, buf.size(), SerializeIF::Endianness::NETWORK) == @@ -43,7 +53,50 @@ TEST_CASE("Mode Definitions", "[mode]") { CHECK(buf[6] == 0); CHECK(buf[7] == HasModesIF::MODE_ON); CHECK(buf[8] == 2); - CHECK(buf[9] == (mode::SpecialSubmodeFlags::ALLOWED_MASK | mode::SpecialSubmodeFlags::INHERIT)); - CHECK(buf[10] == 0x1f); + CHECK(buf[9] == 0); + } + + SECTION("Serialization inherit submode") { + std::array buf{}; + entry.setObject(0x1f2f3f4f); + entry.setMode(HasModesIF::MODE_ON); + entry.setSubmode(2); + entry.enableInheritSubmode(); + uint8_t* serPtr = buf.data(); + size_t serLen = 0; + REQUIRE(entry.serialize(&serPtr, &serLen, buf.size(), SerializeIF::Endianness::NETWORK) == + returnvalue::OK); + CHECK(buf[0] == 0x1f); + CHECK(buf[1] == 0x2f); + CHECK(buf[2] == 0x3f); + CHECK(buf[3] == 0x4f); + CHECK(buf[4] == 0); + CHECK(buf[5] == 0); + CHECK(buf[6] == 0); + CHECK(buf[7] == HasModesIF::MODE_ON); + CHECK(buf[8] == 2); + CHECK(buf[9] == mode::SpecialSubmodeFlags::INHERIT); + } + + SECTION("Serialization submode mask") { + std::array buf{}; + entry.setObject(0x1f2f3f4f); + entry.setMode(HasModesIF::MODE_ON); + entry.setSubmode(2); + entry.enableSubmodeAllowed(0x1f); + uint8_t* serPtr = buf.data(); + size_t serLen = 0; + REQUIRE(entry.serialize(&serPtr, &serLen, buf.size(), SerializeIF::Endianness::NETWORK) == + returnvalue::OK); + CHECK(buf[0] == 0x1f); + CHECK(buf[1] == 0x2f); + CHECK(buf[2] == 0x3f); + CHECK(buf[3] == 0x4f); + CHECK(buf[4] == 0); + CHECK(buf[5] == 0); + CHECK(buf[6] == 0); + CHECK(buf[7] == HasModesIF::MODE_ON); + CHECK(buf[8] == 0x1f); + CHECK(buf[9] == mode::SpecialSubmodeFlags::ALLOWED_MASK); } } From 87462afe6d367cba4e3e28d24b4732657638ca9f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 10 Mar 2023 14:23:40 +0100 Subject: [PATCH 348/404] better error printout for i2c write error --- src/fsfw_hal/linux/i2c/I2cComIF.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/src/fsfw_hal/linux/i2c/I2cComIF.cpp index b1d54701..d343000a 100644 --- a/src/fsfw_hal/linux/i2c/I2cComIF.cpp +++ b/src/fsfw_hal/linux/i2c/I2cComIF.cpp @@ -112,7 +112,7 @@ ReturnValue_t I2cComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s if (i2cCookie->errorCounter < 3) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "I2cComIF::sendMessage: Failed to send data to I2C " - "device with error code " + "device from " << deviceFile << " with error code " << errno << ". Error description: " << strerror(errno) << std::endl; #endif } From 55f6825a03eb2f1cc5d9abeab084d8ea7abbac63 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 10 Mar 2023 14:50:16 +0100 Subject: [PATCH 349/404] Revert "Modes: reusing submode for mode mask, more unittests" This reverts commit f0bddfcb21bd2145a27030a40e16d04cce7dcf6a. --- src/fsfw/subsystem/modes/ModeDefinitions.h | 10 ++-- unittests/subsystem/testModeDef.cpp | 61 ++-------------------- 2 files changed, 11 insertions(+), 60 deletions(-) diff --git a/src/fsfw/subsystem/modes/ModeDefinitions.h b/src/fsfw/subsystem/modes/ModeDefinitions.h index ffeace5a..70a66fa6 100644 --- a/src/fsfw/subsystem/modes/ModeDefinitions.h +++ b/src/fsfw/subsystem/modes/ModeDefinitions.h @@ -21,6 +21,7 @@ class ModeListEntry : public SerialLinkedListAdapter, SerializeElement value2 = 0; SerializeElement value3 = 0; SerializeElement value4 = 0; + SerializeElement value5 = 0; ModeListEntry(const ModeListEntry& other) : SerialLinkedListAdapter(), LinkedElement(this) { @@ -28,6 +29,7 @@ class ModeListEntry : public SerialLinkedListAdapter, value2.entry = other.value2.entry; value3.entry = other.value3.entry; value4.entry = other.value4.entry; + value5.entry = other.value5.entry; setLinks(); } @@ -36,6 +38,7 @@ class ModeListEntry : public SerialLinkedListAdapter, this->value2.entry = other.value2.entry; this->value3.entry = other.value3.entry; this->value4.entry = other.value4.entry; + this->value5.entry = other.value5.entry; return *this; } @@ -44,6 +47,7 @@ class ModeListEntry : public SerialLinkedListAdapter, value1.setNext(&value2); value2.setNext(&value3); value3.setNext(&value4); + value4.setNext(&value5); } // for Sequences @@ -80,7 +84,7 @@ class ModeListEntry : public SerialLinkedListAdapter, bool submodesAllowed = (value4.entry & mode::SpecialSubmodeFlags::ALLOWED_MASK) == mode::SpecialSubmodeFlags::ALLOWED_MASK; if (submodesAllowed and mask != nullptr) { - *mask = value3.entry; + *mask = value5.entry; } return submodesAllowed; } @@ -109,14 +113,14 @@ class ModeListEntry : public SerialLinkedListAdapter, */ void enableSubmodeAllowed(uint8_t mask) { value4.entry |= mode::SpecialSubmodeFlags::ALLOWED_MASK; - value3.entry = mask; + value5.entry = mask; } /** * Enforce the equality of submodes for mode checks. This is the default. */ void disableSubmodeAllowed() { value4.entry &= ~mode::SpecialSubmodeFlags::ALLOWED_MASK; - value3.entry = 0; + value5.entry = 0; } }; diff --git a/unittests/subsystem/testModeDef.cpp b/unittests/subsystem/testModeDef.cpp index e5a4e733..758b3301 100644 --- a/unittests/subsystem/testModeDef.cpp +++ b/unittests/subsystem/testModeDef.cpp @@ -16,73 +16,19 @@ TEST_CASE("Mode Definitions", "[mode]") { CHECK(entry.submodesAllowed(&mask) == false); } - SECTION("Inherit submode") { - entry.enableInheritSubmode(); - CHECK(entry.inheritSubmode() == true); - entry.disableInheritSubmode(); - CHECK(entry.inheritSubmode() == false); - } - SECTION("Allowed submode mask") { entry.allowAllSubmodes(); uint8_t mask; CHECK(entry.submodesAllowed(&mask) == true); CHECK(mask == 0xff); - entry.enableSubmodeAllowed(0x32); - CHECK(entry.submodesAllowed(&mask) == true); - CHECK(mask == 0x32); - entry.disableSubmodeAllowed(); - CHECK(entry.submodesAllowed(&mask) == false); } - SECTION("Serialization nominal") { - std::array buf{}; - entry.setObject(0x1f2f3f4f); - entry.setMode(HasModesIF::MODE_ON); - entry.setSubmode(2); - uint8_t* serPtr = buf.data(); - size_t serLen = 0; - REQUIRE(entry.serialize(&serPtr, &serLen, buf.size(), SerializeIF::Endianness::NETWORK) == - returnvalue::OK); - CHECK(buf[0] == 0x1f); - CHECK(buf[1] == 0x2f); - CHECK(buf[2] == 0x3f); - CHECK(buf[3] == 0x4f); - CHECK(buf[4] == 0); - CHECK(buf[5] == 0); - CHECK(buf[6] == 0); - CHECK(buf[7] == HasModesIF::MODE_ON); - CHECK(buf[8] == 2); - CHECK(buf[9] == 0); - } - - SECTION("Serialization inherit submode") { + SECTION("Serialization") { std::array buf{}; entry.setObject(0x1f2f3f4f); entry.setMode(HasModesIF::MODE_ON); entry.setSubmode(2); entry.enableInheritSubmode(); - uint8_t* serPtr = buf.data(); - size_t serLen = 0; - REQUIRE(entry.serialize(&serPtr, &serLen, buf.size(), SerializeIF::Endianness::NETWORK) == - returnvalue::OK); - CHECK(buf[0] == 0x1f); - CHECK(buf[1] == 0x2f); - CHECK(buf[2] == 0x3f); - CHECK(buf[3] == 0x4f); - CHECK(buf[4] == 0); - CHECK(buf[5] == 0); - CHECK(buf[6] == 0); - CHECK(buf[7] == HasModesIF::MODE_ON); - CHECK(buf[8] == 2); - CHECK(buf[9] == mode::SpecialSubmodeFlags::INHERIT); - } - - SECTION("Serialization submode mask") { - std::array buf{}; - entry.setObject(0x1f2f3f4f); - entry.setMode(HasModesIF::MODE_ON); - entry.setSubmode(2); entry.enableSubmodeAllowed(0x1f); uint8_t* serPtr = buf.data(); size_t serLen = 0; @@ -96,7 +42,8 @@ TEST_CASE("Mode Definitions", "[mode]") { CHECK(buf[5] == 0); CHECK(buf[6] == 0); CHECK(buf[7] == HasModesIF::MODE_ON); - CHECK(buf[8] == 0x1f); - CHECK(buf[9] == mode::SpecialSubmodeFlags::ALLOWED_MASK); + CHECK(buf[8] == 2); + CHECK(buf[9] == (mode::SpecialSubmodeFlags::ALLOWED_MASK | mode::SpecialSubmodeFlags::INHERIT)); + CHECK(buf[10] == 0x1f); } } From 4d6f6e6b23b5c0486dad6be8abba7681114a05fe Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 10 Mar 2023 14:58:20 +0100 Subject: [PATCH 350/404] event manager queue depth configurable --- src/fsfw/events/EventManager.cpp | 4 ++-- src/fsfw/events/EventManager.h | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/fsfw/events/EventManager.cpp b/src/fsfw/events/EventManager.cpp index ab999158..f2a099ed 100644 --- a/src/fsfw/events/EventManager.cpp +++ b/src/fsfw/events/EventManager.cpp @@ -15,12 +15,12 @@ const LocalPool::LocalPoolConfig EventManager::poolConfig = { {fsfwconfig::FSFW_EVENTMGMT_EVENTIDMATCHERS, sizeof(EventIdRangeMatcher)}, {fsfwconfig::FSFW_EVENTMGMR_RANGEMATCHERS, sizeof(ReporterRangeMatcher)}}; -EventManager::EventManager(object_id_t setObjectId) +EventManager::EventManager(object_id_t setObjectId, uint32_t eventQueueDepth) : 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); + eventQueueDepth, EventMessage::EVENT_MESSAGE_SIZE, &mqArgs); } EventManager::~EventManager() { diff --git a/src/fsfw/events/EventManager.h b/src/fsfw/events/EventManager.h index 555afa0e..9064adda 100644 --- a/src/fsfw/events/EventManager.h +++ b/src/fsfw/events/EventManager.h @@ -21,9 +21,9 @@ extern const char* translateEvents(Event event); class EventManager : public EventManagerIF, public ExecutableObjectIF, public SystemObject { public: - static const uint16_t MAX_EVENTS_PER_CYCLE = 80; + static const uint16_t DEFAULT_MAX_EVENTS_PER_CYCLE = 80; - EventManager(object_id_t setObjectId); + EventManager(object_id_t setObjectId, uint32_t eventQueueDepth); virtual ~EventManager(); void setMutexTimeout(MutexIF::TimeoutType timeoutType, uint32_t timeoutMs); From 9a8d775eb1a8788ad844215bf2a42d9f707767c0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 12 Mar 2023 20:49:34 +0100 Subject: [PATCH 351/404] optimization for cmd executor --- src/fsfw_hal/linux/CommandExecutor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw_hal/linux/CommandExecutor.cpp b/src/fsfw_hal/linux/CommandExecutor.cpp index 27cf8aca..e8f64432 100644 --- a/src/fsfw_hal/linux/CommandExecutor.cpp +++ b/src/fsfw_hal/linux/CommandExecutor.cpp @@ -17,7 +17,7 @@ ReturnValue_t CommandExecutor::load(std::string command, bool blocking, bool pri return COMMAND_PENDING; } - currentCmd = command; + currentCmd = std::move(command); this->blocking = blocking; this->printOutput = printOutput; if (state == States::IDLE) { From 7208343b6d41d70c264918ef1ef0277d3663c8c1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 13 Mar 2023 16:01:13 +0100 Subject: [PATCH 352/404] basic faulty cb for power switcher component --- src/fsfw/power/PowerSwitcherComponent.cpp | 5 +++++ src/fsfw/power/PowerSwitcherComponent.h | 20 ++++++++++---------- src/fsfw_hal/linux/i2c/I2cComIF.cpp | 5 +++-- 3 files changed, 18 insertions(+), 12 deletions(-) diff --git a/src/fsfw/power/PowerSwitcherComponent.cpp b/src/fsfw/power/PowerSwitcherComponent.cpp index b07a7296..c273e6c3 100644 --- a/src/fsfw/power/PowerSwitcherComponent.cpp +++ b/src/fsfw/power/PowerSwitcherComponent.cpp @@ -28,6 +28,9 @@ ReturnValue_t PowerSwitcherComponent::performOperation(uint8_t opCode) { continue; } } + if (getHealth() == FAULTY) { + performFaultyOperation(); + } if (switcher.active()) { switcher.doStateMachine(); auto currState = switcher.getState(); @@ -117,3 +120,5 @@ ReturnValue_t PowerSwitcherComponent::connectModeTreeParent(HasModeTreeChildrenI object_id_t PowerSwitcherComponent::getObjectId() const { return SystemObject::getObjectId(); } ModeTreeChildIF& PowerSwitcherComponent::getModeTreeChildIF() { return *this; } + +void PowerSwitcherComponent::performFaultyOperation() {} diff --git a/src/fsfw/power/PowerSwitcherComponent.h b/src/fsfw/power/PowerSwitcherComponent.h index 38dddc37..d6fc06cd 100644 --- a/src/fsfw/power/PowerSwitcherComponent.h +++ b/src/fsfw/power/PowerSwitcherComponent.h @@ -1,5 +1,4 @@ -#ifndef _FSFW_POWER_POWERSWITCHERCOMPONENT_H_ -#define _FSFW_POWER_POWERSWITCHERCOMPONENT_H_ +#pragma once #include #include @@ -37,9 +36,11 @@ class PowerSwitcherComponent : public SystemObject, ReturnValue_t connectModeTreeParent(HasModeTreeChildrenIF &parent) override; ModeTreeChildIF &getModeTreeChildIF() override; + protected: + PowerSwitcher switcher; + private: MessageQueueIF *queue = nullptr; - PowerSwitcher switcher; Mode_t mode = MODE_OFF; Submode_t submode = 0; @@ -49,24 +50,23 @@ class PowerSwitcherComponent : public SystemObject, void setMode(Mode_t newMode, Submode_t newSubmode); - virtual ReturnValue_t performOperation(uint8_t opCode) override; + ReturnValue_t performOperation(uint8_t opCode) override; ReturnValue_t initialize() override; - MessageQueueId_t getCommandQueue() const override; + [[nodiscard]] MessageQueueId_t getCommandQueue() const override; void getMode(Mode_t *mode, Submode_t *submode) override; ReturnValue_t checkModeCommand(Mode_t mode, Submode_t submode, uint32_t *msToReachTheMode) override; void startTransition(Mode_t mode, Submode_t submode) override; + virtual void performFaultyOperation(); void setToExternalControl() override; void announceMode(bool recursive) override; ReturnValue_t setHealth(HealthState health) override; HasHealthIF::HealthState getHealth() override; - object_id_t getObjectId() const override; - const HasHealthIF *getOptHealthIF() const override; - const HasModesIF &getModeIF() const override; + [[nodiscard]] object_id_t getObjectId() const override; + [[nodiscard]] const HasHealthIF *getOptHealthIF() const override; + [[nodiscard]] const HasModesIF &getModeIF() const override; }; - -#endif /* _FSFW_POWER_POWERSWITCHERCOMPONENT_H_ */ diff --git a/src/fsfw_hal/linux/i2c/I2cComIF.cpp b/src/fsfw_hal/linux/i2c/I2cComIF.cpp index d343000a..1a85d4d3 100644 --- a/src/fsfw_hal/linux/i2c/I2cComIF.cpp +++ b/src/fsfw_hal/linux/i2c/I2cComIF.cpp @@ -112,8 +112,9 @@ ReturnValue_t I2cComIF::sendMessage(CookieIF* cookie, const uint8_t* sendData, s if (i2cCookie->errorCounter < 3) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "I2cComIF::sendMessage: Failed to send data to I2C " - "device from " << deviceFile << " with error code " - << errno << ". Error description: " << strerror(errno) << std::endl; + "device from " + << deviceFile << " with error code " << errno + << ". Error description: " << strerror(errno) << std::endl; #endif } return returnvalue::FAILED; From 8382d61b9206c0259439eeddcad3759f1cd9bd2f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Mar 2023 11:44:13 +0100 Subject: [PATCH 353/404] possible fix for power switch component --- CHANGELOG.md | 4 ++++ src/fsfw/power/PowerSwitcherComponent.cpp | 3 ++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index d2eff76e..5bb71595 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,10 @@ and this project adheres to [Semantic Versioning](http://semver.org/). # [unreleased] +## Fixed + +- Use `modetree::connectModeTreeParent` in `PowerSwitcherComponent` to connect mode tree parent. + # [v6.0.0] ## Fixes diff --git a/src/fsfw/power/PowerSwitcherComponent.cpp b/src/fsfw/power/PowerSwitcherComponent.cpp index b07a7296..6478c873 100644 --- a/src/fsfw/power/PowerSwitcherComponent.cpp +++ b/src/fsfw/power/PowerSwitcherComponent.cpp @@ -2,6 +2,7 @@ #include #include +#include PowerSwitcherComponent::PowerSwitcherComponent(object_id_t objectId, PowerSwitchIF* pwrSwitcher, power::Switch_t pwrSwitch) @@ -111,7 +112,7 @@ const HasHealthIF* PowerSwitcherComponent::getOptHealthIF() const { return this; const HasModesIF& PowerSwitcherComponent::getModeIF() const { return *this; } ReturnValue_t PowerSwitcherComponent::connectModeTreeParent(HasModeTreeChildrenIF& parent) { - return parent.registerChild(*this); + return modetree::connectModeTreeParent(parent, *this, &healthHelper, modeHelper); } object_id_t PowerSwitcherComponent::getObjectId() const { return SystemObject::getObjectId(); } From 0b0a0299bc044e67413d8912ceb76a779d76ee4e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Mar 2023 13:45:03 +0100 Subject: [PATCH 354/404] power switch IF improvements --- CHANGELOG.md | 5 +++++ src/fsfw/events/fwSubsystemIdRanges.h | 2 +- src/fsfw/power/Fuse.h | 2 +- src/fsfw/power/PowerSwitchIF.h | 5 ++++- 4 files changed, 11 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5bb71595..f73fd68a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,11 @@ and this project adheres to [Semantic Versioning](http://semver.org/). - Use `modetree::connectModeTreeParent` in `PowerSwitcherComponent` to connect mode tree parent. +## Changed + +- Rename FW subsystem ID from `PCDU_2` to `POWER_SYSTEM_IF`. +- Add new `SWITCH_UNKNOWN` returnvalue in `PowerSwitchIF`. + # [v6.0.0] ## Fixes diff --git a/src/fsfw/events/fwSubsystemIdRanges.h b/src/fsfw/events/fwSubsystemIdRanges.h index 574ea070..e395c2f9 100644 --- a/src/fsfw/events/fwSubsystemIdRanges.h +++ b/src/fsfw/events/fwSubsystemIdRanges.h @@ -10,7 +10,7 @@ enum : uint8_t { CDH = 28, TCS_1 = 59, PCDU_1 = 42, - PCDU_2 = 43, + POWER_SWITCH_IF = 43, HEATER = 50, T_SENSORS = 52, FDIR = 70, diff --git a/src/fsfw/power/Fuse.h b/src/fsfw/power/Fuse.h index e8b86cfd..c1b35899 100644 --- a/src/fsfw/power/Fuse.h +++ b/src/fsfw/power/Fuse.h @@ -32,7 +32,7 @@ class Fuse : public SystemObject, gp_id_t poolIdPower; }; - static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PCDU_1; + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::POWER_SWITCH_IF; //! 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. diff --git a/src/fsfw/power/PowerSwitchIF.h b/src/fsfw/power/PowerSwitchIF.h index 43746218..0b331e11 100644 --- a/src/fsfw/power/PowerSwitchIF.h +++ b/src/fsfw/power/PowerSwitchIF.h @@ -28,7 +28,9 @@ class PowerSwitchIF { static const ReturnValue_t SWITCH_TIMEOUT = MAKE_RETURN_CODE(2); static const ReturnValue_t FUSE_ON = MAKE_RETURN_CODE(3); static const ReturnValue_t FUSE_OFF = MAKE_RETURN_CODE(4); - static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PCDU_2; + static const ReturnValue_t SWITCH_UNKNOWN = MAKE_RETURN_CODE(5); + + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::POWER_SWITCH_IF; //!< Someone detected that a switch went off which shouldn't. Severity: //!< Low, Parameter1: switchId1, Parameter2: switchId2 static const Event SWITCH_WENT_OFF = MAKE_EVENT(0, severity::LOW); @@ -50,6 +52,7 @@ class PowerSwitchIF { * @return * - @c SWITCH_ON if the specified switch is on. * - @c SWITCH_OFF if the specified switch is off. + * - @c SWITCH_UNKNOWN if the state of the specified switch is unknown. * - @c returnvalue::FAILED if an error occured */ virtual ReturnValue_t getSwitchState(power::Switch_t switchNr) const = 0; From b6b9d1d7901c794f771771c1c3b6ac6d5fa2a08f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Mar 2023 16:49:46 +0100 Subject: [PATCH 355/404] make dataset enable idempotent --- src/fsfw/datapoollocal/LocalDataPoolManager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/datapoollocal/LocalDataPoolManager.cpp b/src/fsfw/datapoollocal/LocalDataPoolManager.cpp index 67c2b4b8..a2712909 100644 --- a/src/fsfw/datapoollocal/LocalDataPoolManager.cpp +++ b/src/fsfw/datapoollocal/LocalDataPoolManager.cpp @@ -718,7 +718,7 @@ ReturnValue_t LocalDataPoolManager::togglePeriodicGeneration(sid_t sid, bool ena if ((LocalPoolDataSetAttorney::getReportingEnabled(*dataSet) and enable) or (not LocalPoolDataSetAttorney::getReportingEnabled(*dataSet) and not enable)) { - return REPORTING_STATUS_UNCHANGED; + return returnvalue::OK; } LocalPoolDataSetAttorney::setReportingEnabled(*dataSet, enable); From bf980d74c075c46ae50963619f7ef33746aaddaa Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Mar 2023 17:40:39 +0100 Subject: [PATCH 356/404] HK helper simplification --- CHANGELOG.md | 1 + .../PeriodicHousekeepingHelper.cpp | 34 +++---------------- .../housekeeping/PeriodicHousekeepingHelper.h | 4 +-- 3 files changed, 6 insertions(+), 33 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index f73fd68a..8a6afee3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/). - Rename FW subsystem ID from `PCDU_2` to `POWER_SYSTEM_IF`. - Add new `SWITCH_UNKNOWN` returnvalue in `PowerSwitchIF`. +- Simplify `PeriodicHousekeepingHelper`: No non-diagnostic handling # [v6.0.0] diff --git a/src/fsfw/housekeeping/PeriodicHousekeepingHelper.cpp b/src/fsfw/housekeeping/PeriodicHousekeepingHelper.cpp index 0c2bef96..25a9df96 100644 --- a/src/fsfw/housekeeping/PeriodicHousekeepingHelper.cpp +++ b/src/fsfw/housekeeping/PeriodicHousekeepingHelper.cpp @@ -8,10 +8,8 @@ PeriodicHousekeepingHelper::PeriodicHousekeepingHelper(LocalPoolDataSetBase* own : owner(owner) {} void PeriodicHousekeepingHelper::initialize(float collectionInterval, - dur_millis_t minimumPeriodicInterval, - uint8_t nonDiagIntervalFactor) { + dur_millis_t minimumPeriodicInterval) { this->minimumPeriodicInterval = minimumPeriodicInterval; - this->nonDiagIntervalFactor = nonDiagIntervalFactor; collectionIntervalTicks = intervalSecondsToIntervalTicks(collectionInterval); /* This will cause a checkOpNecessary call to be true immediately. I think it's okay if a HK packet is generated immediately instead of waiting one generation cycle. */ @@ -40,38 +38,14 @@ uint32_t PeriodicHousekeepingHelper::intervalSecondsToIntervalTicks( /* Avoid division by zero */ if (minimumPeriodicInterval == 0) { - if (isDiagnostics) { - /* Perform operation each cycle */ - return 1; - } else { - return nonDiagIntervalFactor; - } + /* Perform operation each cycle */ + return 1; + } else { dur_millis_t intervalInMs = collectionIntervalSeconds * 1000; uint32_t divisor = minimumPeriodicInterval; - if (not isDiagnostics) { - /* We need to multiply the divisor because non-diagnostics only - allow a multiple of the minimum periodic interval */ - divisor *= nonDiagIntervalFactor; - } uint32_t ticks = std::ceil(static_cast(intervalInMs) / divisor); - if (not isDiagnostics) { - /* Now we need to multiply the calculated ticks with the factor as as well - because the minimum tick count to generate a non-diagnostic is the factor itself. - Example calculation for non-diagnostic with - 0.4 second interval and 0.2 second task interval. - Resultant tick count of 5 is equal to operation each second. - - Examle calculation for non-diagnostic with 2.0 second interval and 0.2 second - task interval. - Resultant tick count of 10 is equal to operatin every 2 seconds. - - Example calculation for diagnostic with 0.4 second interval and 0.3 - second task interval. Resulting tick count of 2 is equal to operation - every 0.6 seconds. */ - ticks *= nonDiagIntervalFactor; - } return ticks; } } diff --git a/src/fsfw/housekeeping/PeriodicHousekeepingHelper.h b/src/fsfw/housekeeping/PeriodicHousekeepingHelper.h index 8b6245ba..1ec0febf 100644 --- a/src/fsfw/housekeeping/PeriodicHousekeepingHelper.h +++ b/src/fsfw/housekeeping/PeriodicHousekeepingHelper.h @@ -11,8 +11,7 @@ class PeriodicHousekeepingHelper { public: PeriodicHousekeepingHelper(LocalPoolDataSetBase* owner); - void initialize(float collectionInterval, dur_millis_t minimumPeriodicInterval, - uint8_t nonDiagIntervalFactor); + void initialize(float collectionInterval, dur_millis_t minimumPeriodicInterval); void changeCollectionInterval(float newInterval); float getCollectionIntervalInSeconds() const; @@ -20,7 +19,6 @@ class PeriodicHousekeepingHelper { private: LocalPoolDataSetBase* owner = nullptr; - uint8_t nonDiagIntervalFactor = 0; uint32_t intervalSecondsToIntervalTicks(float collectionIntervalSeconds); float intervalTicksToSeconds(uint32_t collectionInterval) const; From aac32e763cca43e93182b7f0b24ada4c38bc5365 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Mar 2023 18:20:17 +0100 Subject: [PATCH 357/404] some compile fixes --- src/fsfw/datapoollocal/LocalPoolDataSetBase.cpp | 5 ++--- src/fsfw/datapoollocal/LocalPoolDataSetBase.h | 3 +-- src/fsfw/datapoollocal/internal/LocalPoolDataSetAttorney.h | 6 ++---- src/fsfw/housekeeping/PeriodicHousekeepingHelper.cpp | 1 - 4 files changed, 5 insertions(+), 10 deletions(-) diff --git a/src/fsfw/datapoollocal/LocalPoolDataSetBase.cpp b/src/fsfw/datapoollocal/LocalPoolDataSetBase.cpp index 38aad828..28995446 100644 --- a/src/fsfw/datapoollocal/LocalPoolDataSetBase.cpp +++ b/src/fsfw/datapoollocal/LocalPoolDataSetBase.cpp @@ -250,9 +250,8 @@ void LocalPoolDataSetBase::setReportingEnabled(bool reportingEnabled) { bool LocalPoolDataSetBase::getReportingEnabled() const { return reportingEnabled; } void LocalPoolDataSetBase::initializePeriodicHelper(float collectionInterval, - dur_millis_t minimumPeriodicInterval, - uint8_t nonDiagIntervalFactor) { - periodicHelper->initialize(collectionInterval, minimumPeriodicInterval, nonDiagIntervalFactor); + dur_millis_t minimumPeriodicInterval) { + periodicHelper->initialize(collectionInterval, minimumPeriodicInterval); } void LocalPoolDataSetBase::setChanged(bool changed) { this->changed = changed; } diff --git a/src/fsfw/datapoollocal/LocalPoolDataSetBase.h b/src/fsfw/datapoollocal/LocalPoolDataSetBase.h index 9166cf34..7aec77ea 100644 --- a/src/fsfw/datapoollocal/LocalPoolDataSetBase.h +++ b/src/fsfw/datapoollocal/LocalPoolDataSetBase.h @@ -191,8 +191,7 @@ class LocalPoolDataSetBase : public PoolDataSetBase, public MarkChangedIF { */ bool reportingEnabled = false; - void initializePeriodicHelper(float collectionInterval, dur_millis_t minimumPeriodicInterval, - uint8_t nonDiagIntervalFactor = 5); + void initializePeriodicHelper(float collectionInterval, dur_millis_t minimumPeriodicInterval); /** * If the valid state of a dataset is always relevant to the whole diff --git a/src/fsfw/datapoollocal/internal/LocalPoolDataSetAttorney.h b/src/fsfw/datapoollocal/internal/LocalPoolDataSetAttorney.h index e7dbd0c7..6f9a3b27 100644 --- a/src/fsfw/datapoollocal/internal/LocalPoolDataSetAttorney.h +++ b/src/fsfw/datapoollocal/internal/LocalPoolDataSetAttorney.h @@ -12,10 +12,8 @@ class LocalPoolDataSetAttorney { static bool isDiagnostics(LocalPoolDataSetBase& set) { return set.isDiagnostics(); } static void initializePeriodicHelper(LocalPoolDataSetBase& set, float collectionInterval, - uint32_t minimumPeriodicIntervalMs, - uint8_t nonDiagIntervalFactor = 5) { - set.initializePeriodicHelper(collectionInterval, minimumPeriodicIntervalMs, - nonDiagIntervalFactor); + uint32_t minimumPeriodicIntervalMs) { + set.initializePeriodicHelper(collectionInterval, minimumPeriodicIntervalMs); } static void setReportingEnabled(LocalPoolDataSetBase& set, bool enabled) { diff --git a/src/fsfw/housekeeping/PeriodicHousekeepingHelper.cpp b/src/fsfw/housekeeping/PeriodicHousekeepingHelper.cpp index 25a9df96..b39e45c3 100644 --- a/src/fsfw/housekeeping/PeriodicHousekeepingHelper.cpp +++ b/src/fsfw/housekeeping/PeriodicHousekeepingHelper.cpp @@ -34,7 +34,6 @@ uint32_t PeriodicHousekeepingHelper::intervalSecondsToIntervalTicks( if (owner == nullptr) { return 0; } - bool isDiagnostics = owner->isDiagnostics(); /* Avoid division by zero */ if (minimumPeriodicInterval == 0) { From 40405fe6c753eb484bcc1ea360160700ee233b6e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Mar 2023 18:21:16 +0100 Subject: [PATCH 358/404] add spill option --- src/fsfw/storagemanager/PoolManager.cpp | 4 ++-- src/fsfw/storagemanager/PoolManager.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fsfw/storagemanager/PoolManager.cpp b/src/fsfw/storagemanager/PoolManager.cpp index c724aa25..ed92dfef 100644 --- a/src/fsfw/storagemanager/PoolManager.cpp +++ b/src/fsfw/storagemanager/PoolManager.cpp @@ -2,8 +2,8 @@ #include "fsfw/FSFW.h" -PoolManager::PoolManager(object_id_t setObjectId, const LocalPoolConfig& localPoolConfig) - : LocalPool(setObjectId, localPoolConfig, true) { +PoolManager::PoolManager(object_id_t setObjectId, const LocalPoolConfig& localPoolConfig, bool spillToHigherPools) + : LocalPool(setObjectId, localPoolConfig, true, spillToHigherPools) { mutex = MutexFactory::instance()->createMutex(); } diff --git a/src/fsfw/storagemanager/PoolManager.h b/src/fsfw/storagemanager/PoolManager.h index d4bb9f0d..798e92dd 100644 --- a/src/fsfw/storagemanager/PoolManager.h +++ b/src/fsfw/storagemanager/PoolManager.h @@ -21,7 +21,7 @@ */ class PoolManager : public LocalPool { public: - PoolManager(object_id_t setObjectId, const LocalPoolConfig& poolConfig); + PoolManager(object_id_t setObjectId, const LocalPoolConfig& poolConfig, bool spillToHigherPools); /** * @brief In the PoolManager's destructor all allocated memory From 5ba69b169f786881c952716fdd61fff09e233e92 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Mar 2023 18:22:00 +0100 Subject: [PATCH 359/404] changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index f73fd68a..d2ad777a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,6 +16,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/). - Rename FW subsystem ID from `PCDU_2` to `POWER_SYSTEM_IF`. - Add new `SWITCH_UNKNOWN` returnvalue in `PowerSwitchIF`. +- Pool Manager now allows enabling the `spillToHigherPools` option. # [v6.0.0] From c61c85280b81d3506b5b4163e11f8f9d436cc3fb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Mar 2023 18:26:04 +0100 Subject: [PATCH 360/404] remove more code --- src/fsfw/datapoollocal/LocalDataPoolManager.cpp | 7 +------ src/fsfw/datapoollocal/LocalDataPoolManager.h | 13 +------------ 2 files changed, 2 insertions(+), 18 deletions(-) diff --git a/src/fsfw/datapoollocal/LocalDataPoolManager.cpp b/src/fsfw/datapoollocal/LocalDataPoolManager.cpp index a2712909..f66a328c 100644 --- a/src/fsfw/datapoollocal/LocalDataPoolManager.cpp +++ b/src/fsfw/datapoollocal/LocalDataPoolManager.cpp @@ -70,8 +70,7 @@ ReturnValue_t LocalDataPoolManager::initialize(MessageQueueIF* queueToUse) { return returnvalue::OK; } -ReturnValue_t LocalDataPoolManager::initializeAfterTaskCreation(uint8_t nonDiagInvlFactor) { - setNonDiagnosticIntervalFactor(nonDiagInvlFactor); +ReturnValue_t LocalDataPoolManager::initializeAfterTaskCreation() { return initializeHousekeepingPoolEntriesOnce(); } @@ -661,10 +660,6 @@ ReturnValue_t LocalDataPoolManager::serializeHkPacketIntoStore(HousekeepingPacke return hkPacket.serialize(&dataPtr, serializedSize, maxSize, SerializeIF::Endianness::MACHINE); } -void LocalDataPoolManager::setNonDiagnosticIntervalFactor(uint8_t nonDiagInvlFactor) { - this->nonDiagnosticIntervalFactor = nonDiagInvlFactor; -} - void LocalDataPoolManager::performPeriodicHkGeneration(HkReceiver& receiver) { sid_t sid = receiver.dataId.sid; LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid); diff --git a/src/fsfw/datapoollocal/LocalDataPoolManager.h b/src/fsfw/datapoollocal/LocalDataPoolManager.h index 8f369ea0..65d58d59 100644 --- a/src/fsfw/datapoollocal/LocalDataPoolManager.h +++ b/src/fsfw/datapoollocal/LocalDataPoolManager.h @@ -102,7 +102,7 @@ class LocalDataPoolManager : public ProvidesDataPoolSubscriptionIF, public Acces * @param nonDiagInvlFactor * @return */ - ReturnValue_t initializeAfterTaskCreation(uint8_t nonDiagInvlFactor = 5); + ReturnValue_t initializeAfterTaskCreation(); /** * @brief This should be called in the periodic handler of the owner. @@ -152,17 +152,6 @@ class LocalDataPoolManager : public ProvidesDataPoolSubscriptionIF, public Acces MessageQueueId_t targetQueueId, bool generateSnapshot) override; - /** - * Non-Diagnostics packets usually have a lower minimum sampling frequency - * than diagnostic packets. - * A factor can be specified to determine the minimum sampling frequency - * for non-diagnostic packets. The minimum sampling frequency of the - * diagnostics packets,which is usually jusst the period of the - * performOperation calls, is multiplied with that factor. - * @param factor - */ - void setNonDiagnosticIntervalFactor(uint8_t nonDiagInvlFactor); - /** * @brief The manager is also able to handle housekeeping messages. * @details From 522bd41d6e760a9905a0346908cb8d891a506285 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Mar 2023 18:26:44 +0100 Subject: [PATCH 361/404] changelog --- CHANGELOG.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 2ab90249..d11ef73a 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,7 +16,9 @@ and this project adheres to [Semantic Versioning](http://semver.org/). - Rename FW subsystem ID from `PCDU_2` to `POWER_SYSTEM_IF`. - Add new `SWITCH_UNKNOWN` returnvalue in `PowerSwitchIF`. -- Simplify `PeriodicHousekeepingHelper`: No non-diagnostic handling +- Simplify `PeriodicHousekeepingHelper`: No non-diagnostic handling. + Start removing the distinction between diagnostics and regular + HK packets. - Pool Manager now allows enabling the `spillToHigherPools` option. # [v6.0.0] From f84e3284abdf323735fecb3c02a2cf3c7f4bdc98 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Mar 2023 19:54:21 +0100 Subject: [PATCH 362/404] just hardcode spill option to true --- src/fsfw/storagemanager/PoolManager.cpp | 4 ++-- src/fsfw/storagemanager/PoolManager.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/fsfw/storagemanager/PoolManager.cpp b/src/fsfw/storagemanager/PoolManager.cpp index ed92dfef..665edbd9 100644 --- a/src/fsfw/storagemanager/PoolManager.cpp +++ b/src/fsfw/storagemanager/PoolManager.cpp @@ -2,8 +2,8 @@ #include "fsfw/FSFW.h" -PoolManager::PoolManager(object_id_t setObjectId, const LocalPoolConfig& localPoolConfig, bool spillToHigherPools) - : LocalPool(setObjectId, localPoolConfig, true, spillToHigherPools) { +PoolManager::PoolManager(object_id_t setObjectId, const LocalPoolConfig& localPoolConfig) + : LocalPool(setObjectId, localPoolConfig, true, true) { mutex = MutexFactory::instance()->createMutex(); } diff --git a/src/fsfw/storagemanager/PoolManager.h b/src/fsfw/storagemanager/PoolManager.h index 798e92dd..d4bb9f0d 100644 --- a/src/fsfw/storagemanager/PoolManager.h +++ b/src/fsfw/storagemanager/PoolManager.h @@ -21,7 +21,7 @@ */ class PoolManager : public LocalPool { public: - PoolManager(object_id_t setObjectId, const LocalPoolConfig& poolConfig, bool spillToHigherPools); + PoolManager(object_id_t setObjectId, const LocalPoolConfig& poolConfig); /** * @brief In the PoolManager's destructor all allocated memory From cf27954a867a1c16b4e5b0fe72cd79df946ff903 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 14 Mar 2023 19:55:01 +0100 Subject: [PATCH 363/404] changelog --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index d11ef73a..417fa1a4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,6 +11,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/). ## Fixed - Use `modetree::connectModeTreeParent` in `PowerSwitcherComponent` to connect mode tree parent. +- Pool Manager now enables the `spillToHigherPools` option in `LocalPool` parent. ## Changed @@ -19,7 +20,6 @@ and this project adheres to [Semantic Versioning](http://semver.org/). - Simplify `PeriodicHousekeepingHelper`: No non-diagnostic handling. Start removing the distinction between diagnostics and regular HK packets. -- Pool Manager now allows enabling the `spillToHigherPools` option. # [v6.0.0] From 43fd0b2f59c3aeb2d3f4db10cfad56ee3709d68d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 15 Mar 2023 11:48:50 +0100 Subject: [PATCH 364/404] resolve some more merge conflicts --- src/fsfw/pus/CServiceHealthCommanding.h | 15 --------------- 1 file changed, 15 deletions(-) diff --git a/src/fsfw/pus/CServiceHealthCommanding.h b/src/fsfw/pus/CServiceHealthCommanding.h index 70023671..2cc16589 100644 --- a/src/fsfw/pus/CServiceHealthCommanding.h +++ b/src/fsfw/pus/CServiceHealthCommanding.h @@ -6,11 +6,7 @@ #include "fsfw/tmtcservices/CommandingServiceBase.h" struct HealthServiceCfg { -<<<<<<< HEAD - HealthServiceCfg(object_id_t objectId, uint16_t apid, HealthTable &healthTable, -======= HealthServiceCfg(object_id_t objectId, uint16_t apid, object_id_t healthTable, ->>>>>>> upstream/development uint16_t maxNumHealthInfoPerCycle) : objectId(objectId), apid(apid), @@ -18,11 +14,7 @@ struct HealthServiceCfg { maxNumHealthInfoPerCycle(maxNumHealthInfoPerCycle) {} object_id_t objectId; uint16_t apid; -<<<<<<< HEAD - HealthTable &table; -======= object_id_t table; ->>>>>>> upstream/development uint16_t maxNumHealthInfoPerCycle; uint8_t service = 201; uint8_t numParallelCommands = 4; @@ -47,11 +39,8 @@ class CServiceHealthCommanding : public CommandingServiceBase { public: CServiceHealthCommanding(HealthServiceCfg args); ~CServiceHealthCommanding() override = default; -<<<<<<< HEAD -======= ReturnValue_t initialize() override; ->>>>>>> upstream/development protected: /* CSB abstract function implementations */ @@ -70,12 +59,8 @@ class CServiceHealthCommanding : public CommandingServiceBase { void doPeriodicOperation() override; private: -<<<<<<< HEAD - HealthTable &healthTable; -======= const object_id_t healthTableId; HealthTable *healthTable; ->>>>>>> upstream/development uint16_t maxNumHealthInfoPerCycle = 0; bool reportAllHealth = false; ReturnValue_t iterateHealthTable(bool reset); From 227524a21da755d125bcb1a5ff67bcbc452f8cf9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 20 Mar 2023 15:49:09 +0100 Subject: [PATCH 365/404] transition source submode is tricky --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 99b7496c..00d6c451 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -567,7 +567,7 @@ void DeviceHandlerBase::setMode(Mode_t newMode, uint8_t newSubmode) { continueToNormal = false; // TODO: Check whether the following two lines are okay to do so. transitionSourceMode = MODE_ON; - transitionSourceSubMode = submode; + transitionSourceSubMode = newSubmode; mode = _MODE_TO_NORMAL; return; } From 341437df1387aaf0128dd5304b5c0cc8ad734e69 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 21 Mar 2023 20:20:13 +0100 Subject: [PATCH 366/404] add flush functions for serial helpers --- src/fsfw_hal/linux/serial/helper.cpp | 4 ++++ src/fsfw_hal/linux/serial/helper.h | 3 +++ 2 files changed, 7 insertions(+) diff --git a/src/fsfw_hal/linux/serial/helper.cpp b/src/fsfw_hal/linux/serial/helper.cpp index ba975f2f..bc344608 100644 --- a/src/fsfw_hal/linux/serial/helper.cpp +++ b/src/fsfw_hal/linux/serial/helper.cpp @@ -161,3 +161,7 @@ void uart::setStopbits(struct termios& options, StopBits bits) { options.c_cflag &= ~CSTOPB; } } + +void uart::flushRxBuf(int fd) { tcflush(fd, TCIFLUSH); } + +void uart::flushTxRxBuf(int fd) { tcflush(fd, TCIOFLUSH); } diff --git a/src/fsfw_hal/linux/serial/helper.h b/src/fsfw_hal/linux/serial/helper.h index 7f067d00..833886b1 100644 --- a/src/fsfw_hal/linux/serial/helper.h +++ b/src/fsfw_hal/linux/serial/helper.h @@ -64,6 +64,9 @@ void setParity(struct termios& options, Parity parity); void ignoreCtrlLines(struct termios& options); +void flushRxBuf(int fd); +void flushTxRxBuf(int fd); + int readCountersAndErrors(int serialPort, serial_icounter_struct& icounter); } // namespace uart From f8a7c1d4ed621a3375db0da9b9e9f8d5484abbc1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 22 Mar 2023 01:03:49 +0100 Subject: [PATCH 367/404] rename namespace --- src/fsfw_hal/linux/serial/SerialComIF.cpp | 6 +++--- src/fsfw_hal/linux/serial/helper.cpp | 20 ++++++++++---------- src/fsfw_hal/linux/serial/helper.h | 2 +- 3 files changed, 14 insertions(+), 14 deletions(-) diff --git a/src/fsfw_hal/linux/serial/SerialComIF.cpp b/src/fsfw_hal/linux/serial/SerialComIF.cpp index 177d74e4..f272e787 100644 --- a/src/fsfw_hal/linux/serial/SerialComIF.cpp +++ b/src/fsfw_hal/linux/serial/SerialComIF.cpp @@ -88,11 +88,11 @@ int SerialComIF::configureUartPort(SerialCookie* uartCookie) { return fd; } - uart::setParity(options, uartCookie->getParity()); + serial::setParity(options, uartCookie->getParity()); setStopBitOptions(&options, uartCookie); setDatasizeOptions(&options, uartCookie); setFixedOptions(&options); - uart::setMode(options, uartCookie->getUartMode()); + serial::setMode(options, uartCookie->getUartMode()); if (uartCookie->getInputShouldBeFlushed()) { tcflush(fd, TCIFLUSH); } @@ -101,7 +101,7 @@ int SerialComIF::configureUartPort(SerialCookie* uartCookie) { options.c_cc[VTIME] = 0; options.c_cc[VMIN] = 0; - uart::setBaudrate(options, uartCookie->getBaudrate()); + serial::setBaudrate(options, uartCookie->getBaudrate()); /* Save option settings */ if (tcsetattr(fd, TCSANOW, &options) != 0) { diff --git a/src/fsfw_hal/linux/serial/helper.cpp b/src/fsfw_hal/linux/serial/helper.cpp index bc344608..c58689c0 100644 --- a/src/fsfw_hal/linux/serial/helper.cpp +++ b/src/fsfw_hal/linux/serial/helper.cpp @@ -3,7 +3,7 @@ #include "fsfw/serviceinterface.h" -void uart::setMode(struct termios& options, UartModes mode) { +void serial::setMode(struct termios& options, UartModes mode) { if (mode == UartModes::NON_CANONICAL) { /* Disable canonical mode */ options.c_lflag &= ~ICANON; @@ -12,7 +12,7 @@ void uart::setMode(struct termios& options, UartModes mode) { } } -void uart::setBaudrate(struct termios& options, UartBaudRate baud) { +void serial::setBaudrate(struct termios& options, UartBaudRate baud) { switch (baud) { case UartBaudRate::RATE_50: cfsetspeed(&options, B50); @@ -114,7 +114,7 @@ void uart::setBaudrate(struct termios& options, UartBaudRate baud) { } } -void uart::setBitsPerWord(struct termios& options, BitsPerWord bits) { +void serial::setBitsPerWord(struct termios& options, BitsPerWord bits) { options.c_cflag &= ~CSIZE; // Clear all the size bits if (bits == BitsPerWord::BITS_5) { options.c_cflag |= CS5; @@ -127,11 +127,11 @@ void uart::setBitsPerWord(struct termios& options, BitsPerWord bits) { } } -void uart::enableRead(struct termios& options) { options.c_cflag |= CREAD; } +void serial::enableRead(struct termios& options) { options.c_cflag |= CREAD; } -void uart::ignoreCtrlLines(struct termios& options) { options.c_cflag |= CLOCAL; } +void serial::ignoreCtrlLines(struct termios& options) { options.c_cflag |= CLOCAL; } -void uart::setParity(struct termios& options, Parity parity) { +void serial::setParity(struct termios& options, Parity parity) { /* Clear parity bit */ options.c_cflag &= ~PARENB; switch (parity) { @@ -148,11 +148,11 @@ void uart::setParity(struct termios& options, Parity parity) { } } -int uart::readCountersAndErrors(int serialPort, serial_icounter_struct& icounter) { +int serial::readCountersAndErrors(int serialPort, serial_icounter_struct& icounter) { return ioctl(serialPort, TIOCGICOUNT, &icounter); } -void uart::setStopbits(struct termios& options, StopBits bits) { +void serial::setStopbits(struct termios& options, StopBits bits) { if (bits == StopBits::TWO_STOP_BITS) { // Use two stop bits options.c_cflag |= CSTOPB; @@ -162,6 +162,6 @@ void uart::setStopbits(struct termios& options, StopBits bits) { } } -void uart::flushRxBuf(int fd) { tcflush(fd, TCIFLUSH); } +void serial::flushRxBuf(int fd) { tcflush(fd, TCIFLUSH); } -void uart::flushTxRxBuf(int fd) { tcflush(fd, TCIOFLUSH); } +void serial::flushTxRxBuf(int fd) { tcflush(fd, TCIOFLUSH); } diff --git a/src/fsfw_hal/linux/serial/helper.h b/src/fsfw_hal/linux/serial/helper.h index 833886b1..ee59ac66 100644 --- a/src/fsfw_hal/linux/serial/helper.h +++ b/src/fsfw_hal/linux/serial/helper.h @@ -45,7 +45,7 @@ enum class UartBaudRate { RATE_4000000 }; -namespace uart { +namespace serial { void setMode(struct termios& options, UartModes mode); /** From 33ac395072f0145b6e80e12deae978a5e0432f08 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Mar 2023 15:42:14 +0100 Subject: [PATCH 368/404] use RR sched instead of FIFO for Linux RT --- src/fsfw/osal/linux/PosixThread.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/fsfw/osal/linux/PosixThread.cpp b/src/fsfw/osal/linux/PosixThread.cpp index 811d58e2..9851876d 100644 --- a/src/fsfw/osal/linux/PosixThread.cpp +++ b/src/fsfw/osal/linux/PosixThread.cpp @@ -179,8 +179,8 @@ void PosixThread::createTask(void* (*fnc_)(void*), void* arg_) { #error "Please define FSFW_USE_REALTIME_FOR_LINUX with either 0 or 1" #endif #if FSFW_USE_REALTIME_FOR_LINUX == 1 - // FIFO -> This needs root privileges for the process - status = pthread_attr_setschedpolicy(&attributes, SCHED_FIFO); + // RR -> This needs root privileges for the process + status = pthread_attr_setschedpolicy(&attributes, SCHED_RR); if (status != 0) { utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_attr_setschedpolicy"); } From db4587bb59603bdc83e6720193fd1782649a0678 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Mar 2023 18:29:17 +0100 Subject: [PATCH 369/404] allow creating regular threads --- src/fsfw/osal/linux/PosixThread.cpp | 24 ++++++++++++++++++------ 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/src/fsfw/osal/linux/PosixThread.cpp b/src/fsfw/osal/linux/PosixThread.cpp index 9851876d..e90070bb 100644 --- a/src/fsfw/osal/linux/PosixThread.cpp +++ b/src/fsfw/osal/linux/PosixThread.cpp @@ -178,20 +178,32 @@ void PosixThread::createTask(void* (*fnc_)(void*), void* arg_) { #ifndef FSFW_USE_REALTIME_FOR_LINUX #error "Please define FSFW_USE_REALTIME_FOR_LINUX with either 0 or 1" #endif + if (priority < 100) { + // RR -> This needs root privileges for the process #if FSFW_USE_REALTIME_FOR_LINUX == 1 - // RR -> This needs root privileges for the process - status = pthread_attr_setschedpolicy(&attributes, SCHED_RR); - if (status != 0) { - utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_attr_setschedpolicy"); + status = pthread_attr_setschedpolicy(&attributes, SCHED_RR); + if (status != 0) { + utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_attr_setschedpolicy"); + } +#else +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning + << "Real time priorities are only allowed if FSFW_USE_REALTIME_FOR_LINUX is set to 1" + << std::endl; +#endif +#endif + } else { + priority = 0; } sched_param scheduleParams; scheduleParams.__sched_priority = priority; status = pthread_attr_setschedparam(&attributes, &scheduleParams); if (status != 0) { - utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_attr_setschedparam"); - } +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "PosixThread: Setting priority failed" << std::endl; #endif + } // Set Signal Mask for suspend until startTask is called sigset_t waitSignal; sigemptyset(&waitSignal); From cf6150cc184656481eaeb2beff23cd8e92a6ada9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 24 Mar 2023 00:57:44 +0100 Subject: [PATCH 370/404] add BUSY retval --- src/fsfw/devicehandlers/DeviceCommunicationIF.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/fsfw/devicehandlers/DeviceCommunicationIF.h b/src/fsfw/devicehandlers/DeviceCommunicationIF.h index a5546a36..cf827240 100644 --- a/src/fsfw/devicehandlers/DeviceCommunicationIF.h +++ b/src/fsfw/devicehandlers/DeviceCommunicationIF.h @@ -49,6 +49,7 @@ class DeviceCommunicationIF { // is this needed if there is no open/close call? static const ReturnValue_t NOT_ACTIVE = MAKE_RETURN_CODE(0x05); static const ReturnValue_t TOO_MUCH_DATA = MAKE_RETURN_CODE(0x06); + static constexpr ReturnValue_t BUSY = MAKE_RETURN_CODE(0x07); virtual ~DeviceCommunicationIF() {} From d16b3c7e6707276dcc5a7ce6b100a88b0ef84b74 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 24 Mar 2023 11:53:41 +0100 Subject: [PATCH 371/404] try to do this cleanly --- src/fsfw/osal/linux/FixedTimeslotTask.cpp | 11 ++++++++--- src/fsfw/osal/linux/FixedTimeslotTask.h | 3 ++- src/fsfw/osal/linux/PeriodicPosixTask.cpp | 11 ++++++++--- src/fsfw/osal/linux/PeriodicPosixTask.h | 2 +- src/fsfw/osal/linux/PosixThread.cpp | 11 ++++++----- src/fsfw/osal/linux/PosixThread.h | 11 ++++++++++- src/fsfw/osal/linux/TaskFactory.cpp | 14 ++++++++------ src/fsfw/tasks/TaskFactory.h | 6 ++++-- src/fsfw_hal/linux/serial/helper.h | 2 +- 9 files changed, 48 insertions(+), 23 deletions(-) diff --git a/src/fsfw/osal/linux/FixedTimeslotTask.cpp b/src/fsfw/osal/linux/FixedTimeslotTask.cpp index 34729c22..0709f52f 100644 --- a/src/fsfw/osal/linux/FixedTimeslotTask.cpp +++ b/src/fsfw/osal/linux/FixedTimeslotTask.cpp @@ -7,10 +7,15 @@ const size_t PeriodicTaskIF::MINIMUM_STACK_SIZE = PTHREAD_STACK_MIN; FixedTimeslotTask::FixedTimeslotTask(const char* name_, TaskPriority priority_, size_t stackSize_, - TaskPeriod periodSeconds_, TaskDeadlineMissedFunction dlmFunc_) + TaskPeriod periodSeconds_, TaskDeadlineMissedFunction dlmFunc_, + PosixThreadArgs* args) : FixedTimeslotTaskBase(periodSeconds_, dlmFunc_), - posixThread(name_, priority_, stackSize_), - started(false) {} + posixThread(name_, SchedulingPolicy::REGULAR, priority_, stackSize_), + started(false) { + if (args != nullptr) { + posixThread.setSchedPolicy(args->policy); + } +} void* FixedTimeslotTask::taskEntryPoint(void* arg) { // The argument is re-interpreted as PollingTask. diff --git a/src/fsfw/osal/linux/FixedTimeslotTask.h b/src/fsfw/osal/linux/FixedTimeslotTask.h index 1f5766a2..0957bedc 100644 --- a/src/fsfw/osal/linux/FixedTimeslotTask.h +++ b/src/fsfw/osal/linux/FixedTimeslotTask.h @@ -23,7 +23,8 @@ class FixedTimeslotTask : public FixedTimeslotTaskBase { * @param deadlineMissedFunc_ */ FixedTimeslotTask(const char* name_, TaskPriority priority_, size_t stackSize_, - TaskPeriod periodSeconds_, TaskDeadlineMissedFunction dlmFunc_); + TaskPeriod periodSeconds_, TaskDeadlineMissedFunction dlmFunc_, + PosixThreadArgs* args); ~FixedTimeslotTask() override = default; ReturnValue_t startTask() override; diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.cpp b/src/fsfw/osal/linux/PeriodicPosixTask.cpp index 556a0367..5f1256d7 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.cpp +++ b/src/fsfw/osal/linux/PeriodicPosixTask.cpp @@ -4,10 +4,15 @@ #include "fsfw/tasks/ExecutableObjectIF.h" PeriodicPosixTask::PeriodicPosixTask(const char* name_, int priority_, size_t stackSize_, - TaskPeriod period_, TaskDeadlineMissedFunction dlmFunc_) + TaskPeriod period_, TaskDeadlineMissedFunction dlmFunc_, + PosixThreadArgs* args) : PeriodicTaskBase(period_, dlmFunc_), - posixThread(name_, priority_, stackSize_), - started(false) {} + posixThread(name_, SchedulingPolicy::REGULAR, priority_, stackSize_), + started(false) { + if (args != nullptr) { + posixThread.setSchedPolicy(args->policy); + } +} void* PeriodicPosixTask::taskEntryPoint(void* arg) { // The argument is re-interpreted as PollingTask. diff --git a/src/fsfw/osal/linux/PeriodicPosixTask.h b/src/fsfw/osal/linux/PeriodicPosixTask.h index 085c10b9..eac14f0c 100644 --- a/src/fsfw/osal/linux/PeriodicPosixTask.h +++ b/src/fsfw/osal/linux/PeriodicPosixTask.h @@ -24,7 +24,7 @@ class PeriodicPosixTask : public PeriodicTaskBase { * @param deadlineMissedFunc_ */ PeriodicPosixTask(const char* name_, int priority_, size_t stackSize_, TaskPeriod period_, - TaskDeadlineMissedFunction dlmFunc_); + TaskDeadlineMissedFunction dlmFunc_, PosixThreadArgs* args); ~PeriodicPosixTask() override = default; /** diff --git a/src/fsfw/osal/linux/PosixThread.cpp b/src/fsfw/osal/linux/PosixThread.cpp index e90070bb..385b4e1c 100644 --- a/src/fsfw/osal/linux/PosixThread.cpp +++ b/src/fsfw/osal/linux/PosixThread.cpp @@ -7,8 +7,9 @@ #include "fsfw/osal/linux/unixUtility.h" #include "fsfw/serviceinterface/ServiceInterface.h" -PosixThread::PosixThread(const char* name_, int priority_, size_t stackSize_) - : thread(0), priority(priority_), stackSize(stackSize_) { +PosixThread::PosixThread(const char* name_, SchedulingPolicy schedPolciy, int priority_, + size_t stackSize_) + : thread(0), schedPolicy(schedPolciy), priority(priority_), stackSize(stackSize_) { name[0] = '\0'; std::strncat(name, name_, PTHREAD_MAX_NAMELEN - 1); } @@ -178,7 +179,7 @@ void PosixThread::createTask(void* (*fnc_)(void*), void* arg_) { #ifndef FSFW_USE_REALTIME_FOR_LINUX #error "Please define FSFW_USE_REALTIME_FOR_LINUX with either 0 or 1" #endif - if (priority < 100) { + if (schedPolicy == SchedulingPolicy::RR) { // RR -> This needs root privileges for the process #if FSFW_USE_REALTIME_FOR_LINUX == 1 status = pthread_attr_setschedpolicy(&attributes, SCHED_RR); @@ -192,8 +193,6 @@ void PosixThread::createTask(void* (*fnc_)(void*), void* arg_) { << std::endl; #endif #endif - } else { - priority = 0; } sched_param scheduleParams; @@ -255,3 +254,5 @@ void PosixThread::createTask(void* (*fnc_)(void*), void* arg_) { utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_attr_destroy"); } } + +void PosixThread::setSchedPolicy(SchedulingPolicy policy) { this->schedPolicy = policy; } diff --git a/src/fsfw/osal/linux/PosixThread.h b/src/fsfw/osal/linux/PosixThread.h index add41bf6..1ae0ec0c 100644 --- a/src/fsfw/osal/linux/PosixThread.h +++ b/src/fsfw/osal/linux/PosixThread.h @@ -9,10 +9,15 @@ #include "../../returnvalues/returnvalue.h" +enum SchedulingPolicy { REGULAR, RR }; +struct PosixThreadArgs { + SchedulingPolicy policy; +}; + class PosixThread { public: static constexpr uint8_t PTHREAD_MAX_NAMELEN = 16; - PosixThread(const char* name_, int priority_, size_t stackSize_); + PosixThread(const char* name_, SchedulingPolicy schedPolicy, int priority_, size_t stackSize_); virtual ~PosixThread(); /** * Set the Thread to sleep state @@ -20,6 +25,9 @@ class PosixThread { * @return Returns Failed if sleep fails */ static ReturnValue_t sleep(uint64_t ns); + + void setSchedPolicy(SchedulingPolicy policy); + /** * @brief Function to suspend the task until SIGUSR1 was received * @@ -72,6 +80,7 @@ class PosixThread { private: char name[PTHREAD_MAX_NAMELEN]; + SchedulingPolicy schedPolicy; int priority; size_t stackSize = 0; diff --git a/src/fsfw/osal/linux/TaskFactory.cpp b/src/fsfw/osal/linux/TaskFactory.cpp index bacc4311..c93f3fde 100644 --- a/src/fsfw/osal/linux/TaskFactory.cpp +++ b/src/fsfw/osal/linux/TaskFactory.cpp @@ -12,18 +12,20 @@ TaskFactory::~TaskFactory() = default; TaskFactory* TaskFactory::instance() { return TaskFactory::factoryInstance; } -PeriodicTaskIF* TaskFactory::createPeriodicTask( - TaskName name_, TaskPriority taskPriority_, TaskStackSize stackSize_, - TaskPeriod periodInSeconds_, TaskDeadlineMissedFunction deadLineMissedFunction_) { +PeriodicTaskIF* TaskFactory::createPeriodicTask(TaskName name_, TaskPriority taskPriority_, + TaskStackSize stackSize_, + TaskPeriod periodInSeconds_, + TaskDeadlineMissedFunction deadLineMissedFunction_, + void* args) { return new PeriodicPosixTask(name_, taskPriority_, stackSize_, periodInSeconds_, - deadLineMissedFunction_); + deadLineMissedFunction_, reinterpret_cast(args)); } FixedTimeslotTaskIF* TaskFactory::createFixedTimeslotTask( TaskName name_, TaskPriority taskPriority_, TaskStackSize stackSize_, - TaskPeriod periodInSeconds_, TaskDeadlineMissedFunction deadLineMissedFunction_) { + TaskPeriod periodInSeconds_, TaskDeadlineMissedFunction deadLineMissedFunction_, void* args) { return new FixedTimeslotTask(name_, taskPriority_, stackSize_, periodInSeconds_, - deadLineMissedFunction_); + deadLineMissedFunction_, reinterpret_cast(args)); } ReturnValue_t TaskFactory::deleteTask(PeriodicTaskIF* task) { diff --git a/src/fsfw/tasks/TaskFactory.h b/src/fsfw/tasks/TaskFactory.h index 828c533e..9198f100 100644 --- a/src/fsfw/tasks/TaskFactory.h +++ b/src/fsfw/tasks/TaskFactory.h @@ -47,7 +47,8 @@ class TaskFactory { */ PeriodicTaskIF* createPeriodicTask(TaskName name_, TaskPriority taskPriority_, TaskStackSize stackSize_, TaskPeriod periodInSeconds_, - TaskDeadlineMissedFunction deadLineMissedFunction_); + TaskDeadlineMissedFunction deadLineMissedFunction_, + void* args); /** * The meaning for the variables for fixed timeslot tasks is the same as for periodic tasks. @@ -62,7 +63,8 @@ class TaskFactory { FixedTimeslotTaskIF* createFixedTimeslotTask(TaskName name_, TaskPriority taskPriority_, TaskStackSize stackSize_, TaskPeriod periodInSeconds_, - TaskDeadlineMissedFunction deadLineMissedFunction_); + TaskDeadlineMissedFunction deadLineMissedFunction_, + void* args); /** * Function to be called to delete a task diff --git a/src/fsfw_hal/linux/serial/helper.h b/src/fsfw_hal/linux/serial/helper.h index ee59ac66..623612ad 100644 --- a/src/fsfw_hal/linux/serial/helper.h +++ b/src/fsfw_hal/linux/serial/helper.h @@ -69,6 +69,6 @@ void flushTxRxBuf(int fd); int readCountersAndErrors(int serialPort, serial_icounter_struct& icounter); -} // namespace uart +} // namespace serial #endif /* FSFW_HAL_LINUX_UART_HELPER_H_ */ From e704295cce76048d65bbb8893840b9ecedb05c94 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 24 Mar 2023 11:58:23 +0100 Subject: [PATCH 372/404] default value --- src/fsfw/osal/linux/PosixThread.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/osal/linux/PosixThread.h b/src/fsfw/osal/linux/PosixThread.h index 1ae0ec0c..133a9884 100644 --- a/src/fsfw/osal/linux/PosixThread.h +++ b/src/fsfw/osal/linux/PosixThread.h @@ -11,7 +11,7 @@ enum SchedulingPolicy { REGULAR, RR }; struct PosixThreadArgs { - SchedulingPolicy policy; + SchedulingPolicy policy = SchedulingPolicy::REGULAR; }; class PosixThread { From 4415dc24e1bee0252e5282b583fcade7f37a97bf Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 24 Mar 2023 13:25:34 +0100 Subject: [PATCH 373/404] fix host OSAL --- src/fsfw/osal/host/TaskFactory.cpp | 10 ++++++---- src/fsfw/tasks/TaskFactory.h | 4 ++-- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/src/fsfw/osal/host/TaskFactory.cpp b/src/fsfw/osal/host/TaskFactory.cpp index 0a27241b..d9552923 100644 --- a/src/fsfw/osal/host/TaskFactory.cpp +++ b/src/fsfw/osal/host/TaskFactory.cpp @@ -20,16 +20,18 @@ TaskFactory::~TaskFactory() = default; TaskFactory* TaskFactory::instance() { return TaskFactory::factoryInstance; } -PeriodicTaskIF* TaskFactory::createPeriodicTask( - TaskName name_, TaskPriority taskPriority_, TaskStackSize stackSize_, - TaskPeriod periodInSeconds_, TaskDeadlineMissedFunction deadLineMissedFunction_) { +PeriodicTaskIF* TaskFactory::createPeriodicTask(TaskName name_, TaskPriority taskPriority_, + TaskStackSize stackSize_, + TaskPeriod periodInSeconds_, + TaskDeadlineMissedFunction deadLineMissedFunction_, + void* args) { return new PeriodicTask(name_, taskPriority_, stackSize_, periodInSeconds_, deadLineMissedFunction_); } FixedTimeslotTaskIF* TaskFactory::createFixedTimeslotTask( TaskName name_, TaskPriority taskPriority_, TaskStackSize stackSize_, - TaskPeriod periodInSeconds_, TaskDeadlineMissedFunction deadLineMissedFunction_) { + TaskPeriod periodInSeconds_, TaskDeadlineMissedFunction deadLineMissedFunction_, void* args) { return new FixedTimeslotTask(name_, taskPriority_, stackSize_, periodInSeconds_, deadLineMissedFunction_); } diff --git a/src/fsfw/tasks/TaskFactory.h b/src/fsfw/tasks/TaskFactory.h index 9198f100..e9b96a77 100644 --- a/src/fsfw/tasks/TaskFactory.h +++ b/src/fsfw/tasks/TaskFactory.h @@ -48,7 +48,7 @@ class TaskFactory { PeriodicTaskIF* createPeriodicTask(TaskName name_, TaskPriority taskPriority_, TaskStackSize stackSize_, TaskPeriod periodInSeconds_, TaskDeadlineMissedFunction deadLineMissedFunction_, - void* args); + void* args = nullptr); /** * The meaning for the variables for fixed timeslot tasks is the same as for periodic tasks. @@ -64,7 +64,7 @@ class TaskFactory { TaskStackSize stackSize_, TaskPeriod periodInSeconds_, TaskDeadlineMissedFunction deadLineMissedFunction_, - void* args); + void* args = nullptr); /** * Function to be called to delete a task From a937b457f900a6a6b26bc0b42a5357f014840a67 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 24 Mar 2023 14:10:47 +0100 Subject: [PATCH 374/404] this is so confusing --- src/fsfw/osal/linux/PosixThread.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/fsfw/osal/linux/PosixThread.cpp b/src/fsfw/osal/linux/PosixThread.cpp index 385b4e1c..f420b326 100644 --- a/src/fsfw/osal/linux/PosixThread.cpp +++ b/src/fsfw/osal/linux/PosixThread.cpp @@ -186,6 +186,14 @@ void PosixThread::createTask(void* (*fnc_)(void*), void* arg_) { if (status != 0) { utility::printUnixErrorGeneric(CLASS_NAME, "createTask", "pthread_attr_setschedpolicy"); } + sched_param scheduleParams; + scheduleParams.sched_priority = priority; + status = pthread_attr_setschedparam(&attributes, &scheduleParams); + if (status != 0) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "PosixThread: Setting priority failed" << std::endl; +#endif + } #else #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning @@ -195,14 +203,6 @@ void PosixThread::createTask(void* (*fnc_)(void*), void* arg_) { #endif } - sched_param scheduleParams; - scheduleParams.__sched_priority = priority; - status = pthread_attr_setschedparam(&attributes, &scheduleParams); - if (status != 0) { -#if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << "PosixThread: Setting priority failed" << std::endl; -#endif - } // Set Signal Mask for suspend until startTask is called sigset_t waitSignal; sigemptyset(&waitSignal); From b31e1037fb3d436e6a7f213050af8f64d194a85d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 26 Mar 2023 20:05:10 +0200 Subject: [PATCH 375/404] HK service configurable queue depth --- src/fsfw/pus/Service3Housekeeping.cpp | 5 +++-- src/fsfw/pus/Service3Housekeeping.h | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/fsfw/pus/Service3Housekeeping.cpp b/src/fsfw/pus/Service3Housekeeping.cpp index 0a51aa4b..485c83f4 100644 --- a/src/fsfw/pus/Service3Housekeeping.cpp +++ b/src/fsfw/pus/Service3Housekeeping.cpp @@ -4,9 +4,10 @@ #include "fsfw/objectmanager/ObjectManager.h" #include "fsfw/pus/servicepackets/Service3Packets.h" -Service3Housekeeping::Service3Housekeeping(object_id_t objectId, uint16_t apid, uint8_t serviceId) +Service3Housekeeping::Service3Housekeeping(object_id_t objectId, uint16_t apid, uint8_t serviceId, + uint32_t queueDepth) : CommandingServiceBase(objectId, apid, "PUS 3 HK", serviceId, NUM_OF_PARALLEL_COMMANDS, - COMMAND_TIMEOUT_SECONDS) {} + COMMAND_TIMEOUT_SECONDS, queueDepth) {} Service3Housekeeping::~Service3Housekeeping() {} diff --git a/src/fsfw/pus/Service3Housekeeping.h b/src/fsfw/pus/Service3Housekeeping.h index 70f15762..38f808f4 100644 --- a/src/fsfw/pus/Service3Housekeeping.h +++ b/src/fsfw/pus/Service3Housekeeping.h @@ -28,7 +28,7 @@ class Service3Housekeeping : public CommandingServiceBase, public AcceptsHkPacke static constexpr uint8_t NUM_OF_PARALLEL_COMMANDS = 4; static constexpr uint16_t COMMAND_TIMEOUT_SECONDS = 60; - Service3Housekeeping(object_id_t objectId, uint16_t apid, uint8_t serviceId); + Service3Housekeeping(object_id_t objectId, uint16_t apid, uint8_t serviceId, uint32_t queueDepth); virtual ~Service3Housekeeping(); protected: From 314f0fa2cde749ee1021d311e222bb0044cc2e5b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Mar 2023 15:27:48 +0200 Subject: [PATCH 376/404] start power switch component in undefined mode --- src/fsfw/power/PowerSwitcherComponent.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/power/PowerSwitcherComponent.h b/src/fsfw/power/PowerSwitcherComponent.h index d6fc06cd..8c74e76f 100644 --- a/src/fsfw/power/PowerSwitcherComponent.h +++ b/src/fsfw/power/PowerSwitcherComponent.h @@ -42,7 +42,7 @@ class PowerSwitcherComponent : public SystemObject, private: MessageQueueIF *queue = nullptr; - Mode_t mode = MODE_OFF; + Mode_t mode = MODE_UNDEFINED; Submode_t submode = 0; ModeHelper modeHelper; From 4f632e2c6866cee88dd9920a965aa0d079799aa3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 28 Mar 2023 19:37:47 +0200 Subject: [PATCH 377/404] ctrl base bugfix --- src/fsfw/controller/ControllerBase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/controller/ControllerBase.cpp b/src/fsfw/controller/ControllerBase.cpp index c41c4121..2c151f5a 100644 --- a/src/fsfw/controller/ControllerBase.cpp +++ b/src/fsfw/controller/ControllerBase.cpp @@ -58,7 +58,7 @@ void ControllerBase::handleQueue() { void ControllerBase::startTransition(Mode_t mode_, Submode_t submode_) { changeHK(this->mode, this->submode, false); - triggerEvent(CHANGING_MODE, mode, submode); + triggerEvent(CHANGING_MODE, mode_, submode_); mode = mode_; submode = submode_; modeHelper.modeChanged(mode, submode); From e2e87b149d91c51196c76d6b84243fce1c77a28a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 3 Apr 2023 14:31:45 +0200 Subject: [PATCH 378/404] initialize switch state list --- src/fsfw/power/DummyPowerSwitcher.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/fsfw/power/DummyPowerSwitcher.cpp b/src/fsfw/power/DummyPowerSwitcher.cpp index 952a5a57..51adde71 100644 --- a/src/fsfw/power/DummyPowerSwitcher.cpp +++ b/src/fsfw/power/DummyPowerSwitcher.cpp @@ -6,7 +6,11 @@ DummyPowerSwitcher::DummyPowerSwitcher(object_id_t objectId, size_t numberOfSwit : SystemObject(objectId, registerGlobally), switcherList(numberOfSwitches), fuseList(numberOfFuses), - switchDelayMs(switchDelayMs) {} + switchDelayMs(switchDelayMs) { + for(auto &switchState: switcherList) { + switchState = PowerSwitchIF::SWITCH_UNKNOWN; + } +} void DummyPowerSwitcher::setInitialSwitcherList(std::vector switcherList) { this->switcherList = switcherList; From 1e3c89b672e17700a9b50a312d9df29c54977685 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 3 Apr 2023 17:59:14 +0200 Subject: [PATCH 379/404] i dont think ths needs to be public --- src/fsfw/devicehandlers/HealthDevice.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/fsfw/devicehandlers/HealthDevice.h b/src/fsfw/devicehandlers/HealthDevice.h index f4c3520f..00ea44c5 100644 --- a/src/fsfw/devicehandlers/HealthDevice.h +++ b/src/fsfw/devicehandlers/HealthDevice.h @@ -31,8 +31,6 @@ class HealthDevice : public SystemObject, public ExecutableObjectIF, public HasH MessageQueueId_t parentQueue; MessageQueueIF* commandQueue; - - public: HealthHelper healthHelper; }; From 7a392dc33a7e406986fa3684e114a41b0e91de9a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 3 Apr 2023 18:54:35 +0200 Subject: [PATCH 380/404] new register function --- src/fsfw/devicehandlers/HealthDevice.cpp | 5 ++-- src/fsfw/devicehandlers/HealthDevice.h | 2 +- src/fsfw/subsystem/SubsystemBase.cpp | 31 +++++++++++++----------- src/fsfw/subsystem/SubsystemBase.h | 2 ++ 4 files changed, 22 insertions(+), 18 deletions(-) diff --git a/src/fsfw/devicehandlers/HealthDevice.cpp b/src/fsfw/devicehandlers/HealthDevice.cpp index 717fadd1..d3a70c77 100644 --- a/src/fsfw/devicehandlers/HealthDevice.cpp +++ b/src/fsfw/devicehandlers/HealthDevice.cpp @@ -29,11 +29,10 @@ ReturnValue_t HealthDevice::initialize() { if (result != returnvalue::OK) { return result; } - if (parentQueue != 0) { + if (parentQueue != MessageQueueIF::NO_QUEUE) { return healthHelper.initialize(parentQueue); - } else { - return healthHelper.initialize(); } + return healthHelper.initialize(); } MessageQueueId_t HealthDevice::getCommandQueue() const { return commandQueue->getId(); } diff --git a/src/fsfw/devicehandlers/HealthDevice.h b/src/fsfw/devicehandlers/HealthDevice.h index 00ea44c5..a5c28cf7 100644 --- a/src/fsfw/devicehandlers/HealthDevice.h +++ b/src/fsfw/devicehandlers/HealthDevice.h @@ -29,7 +29,7 @@ class HealthDevice : public SystemObject, public ExecutableObjectIF, public HasH protected: HealthState lastHealth; - MessageQueueId_t parentQueue; + MessageQueueId_t parentQueue = MessageQueueIF::NO_QUEUE; MessageQueueIF* commandQueue; HealthHelper healthHelper; }; diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp index eccd447c..b8c09230 100644 --- a/src/fsfw/subsystem/SubsystemBase.cpp +++ b/src/fsfw/subsystem/SubsystemBase.cpp @@ -315,20 +315,7 @@ object_id_t SubsystemBase::getObjectId() const { return SystemObject::getObjectI void SubsystemBase::modeChanged() {} ReturnValue_t SubsystemBase::registerChild(const ModeTreeChildIF& child) { - ChildInfo info; - - const HasModesIF& modeChild = child.getModeIF(); - // intentional to force an initial command during system startup - info.commandQueue = modeChild.getCommandQueue(); - info.mode = HasModesIF::MODE_UNDEFINED; - info.submode = SUBMODE_NONE; - info.healthChanged = false; - - auto resultPair = childrenMap.emplace(child.getObjectId(), info); - if (not resultPair.second) { - return COULD_NOT_INSERT_CHILD; - } - return returnvalue::OK; + return registerChild(child.getObjectId(), child.getModeIF().getCommandQueue()); } const HasHealthIF* SubsystemBase::getOptHealthIF() const { return this; } @@ -336,3 +323,19 @@ const HasHealthIF* SubsystemBase::getOptHealthIF() const { return this; } const HasModesIF& SubsystemBase::getModeIF() const { return *this; } ModeTreeChildIF& SubsystemBase::getModeTreeChildIF() { return *this; } + +ReturnValue_t SubsystemBase::registerChild(object_id_t childObjectId, MessageQueueId_t childQueue) { + ChildInfo info; + + // intentional to force an initial command during system startup + info.commandQueue = childQueue; + info.mode = HasModesIF::MODE_UNDEFINED; + info.submode = SUBMODE_NONE; + info.healthChanged = false; + + auto resultPair = childrenMap.emplace(childObjectId, info); + if (not resultPair.second) { + return COULD_NOT_INSERT_CHILD; + } + return returnvalue::OK; +} diff --git a/src/fsfw/subsystem/SubsystemBase.h b/src/fsfw/subsystem/SubsystemBase.h index c3886d61..072b4ca4 100644 --- a/src/fsfw/subsystem/SubsystemBase.h +++ b/src/fsfw/subsystem/SubsystemBase.h @@ -61,6 +61,8 @@ class SubsystemBase : public SystemObject, * COULD_NOT_INSERT_CHILD If the Child could not be added to the ChildrenMap */ ReturnValue_t registerChild(const ModeTreeChildIF &child) override; + // TODO: Add this to HasModeTreeChildrenIF. + ReturnValue_t registerChild(object_id_t childObjectId, MessageQueueId_t childQueue); ReturnValue_t initialize() override; From 7966ede11b1eb038cbf0111f865bfbf830183c24 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 3 Apr 2023 21:57:18 +0200 Subject: [PATCH 381/404] add O_SYNC flag for UioMapper --- src/fsfw_hal/linux/uio/UioMapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw_hal/linux/uio/UioMapper.cpp b/src/fsfw_hal/linux/uio/UioMapper.cpp index 33e4fd97..c6da5488 100644 --- a/src/fsfw_hal/linux/uio/UioMapper.cpp +++ b/src/fsfw_hal/linux/uio/UioMapper.cpp @@ -36,7 +36,7 @@ UioMapper::~UioMapper() {} ReturnValue_t UioMapper::getMappedAdress(uint32_t** address, Permissions permissions) { ReturnValue_t result = returnvalue::OK; - int fd = open(uioFile.c_str(), O_RDWR); + int fd = open(uioFile.c_str(), O_RDWR | O_SYNC); if (fd < 1) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << "UioMapper::getMappedAdress: Invalid UIO device file " << uioFile << std::endl; From 94cdf67a80e3379783df57954e354d858f2f8e03 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 4 Apr 2023 01:51:58 +0200 Subject: [PATCH 382/404] make health functions virtual --- src/fsfw/devicehandlers/DeviceHandlerBase.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index 57e9d982..08298bdc 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -257,8 +257,8 @@ class DeviceHandlerBase : public DeviceHandlerIF, Mode_t getTransitionSourceMode() const; Submode_t getTransitionSourceSubMode() const; virtual void getMode(Mode_t *mode, Submode_t *submode); - HealthState getHealth(); - ReturnValue_t setHealth(HealthState health); + virtual HealthState getHealth() override; + virtual ReturnValue_t setHealth(HealthState health) override; virtual ReturnValue_t getParameter(uint8_t domainId, uint8_t uniqueId, ParameterWrapper *parameterWrapper, const ParameterWrapper *newValues, From 4af90f99f3daf108ea123afc7c0f0372e5d0da5d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 4 Apr 2023 01:52:26 +0200 Subject: [PATCH 383/404] changelog --- CHANGELOG.md | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index d3e03c4f..a841e38d 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,10 @@ and this project adheres to [Semantic Versioning](http://semver.org/). # [unreleased] +## Changed + +- Health functions are virtual now. + # [v6.0.0] 2023-02-10 ## Fixes From 6650c293da09d8851c2bd6c4d6e6c5a8390d003e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 4 Apr 2023 15:59:26 +0200 Subject: [PATCH 384/404] change collection interval is public now --- src/fsfw/datapoollocal/LocalDataPoolManager.cpp | 12 +++--------- src/fsfw/datapoollocal/LocalDataPoolManager.h | 3 +-- 2 files changed, 4 insertions(+), 11 deletions(-) diff --git a/src/fsfw/datapoollocal/LocalDataPoolManager.cpp b/src/fsfw/datapoollocal/LocalDataPoolManager.cpp index f66a328c..178d5654 100644 --- a/src/fsfw/datapoollocal/LocalDataPoolManager.cpp +++ b/src/fsfw/datapoollocal/LocalDataPoolManager.cpp @@ -505,9 +505,9 @@ ReturnValue_t LocalDataPoolManager::handleHousekeepingMessage(CommandMessage* me float newCollIntvl = 0; HousekeepingMessage::getCollectionIntervalModificationCommand(message, &newCollIntvl); if (command == HousekeepingMessage::MODIFY_DIAGNOSTICS_REPORT_COLLECTION_INTERVAL) { - result = changeCollectionInterval(sid, newCollIntvl, true); + result = changeCollectionInterval(sid, newCollIntvl); } else { - result = changeCollectionInterval(sid, newCollIntvl, false); + result = changeCollectionInterval(sid, newCollIntvl); } break; } @@ -720,8 +720,7 @@ ReturnValue_t LocalDataPoolManager::togglePeriodicGeneration(sid_t sid, bool ena return returnvalue::OK; } -ReturnValue_t LocalDataPoolManager::changeCollectionInterval(sid_t sid, float newCollectionInterval, - bool isDiagnostics) { +ReturnValue_t LocalDataPoolManager::changeCollectionInterval(sid_t sid, float newCollectionInterval) { LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid); if (dataSet == nullptr) { printWarningOrError(sif::OutputTypes::OUT_WARNING, "changeCollectionInterval", @@ -729,11 +728,6 @@ ReturnValue_t LocalDataPoolManager::changeCollectionInterval(sid_t sid, float ne return DATASET_NOT_FOUND; } - bool targetIsDiagnostics = LocalPoolDataSetAttorney::isDiagnostics(*dataSet); - if ((targetIsDiagnostics and not isDiagnostics) or (not targetIsDiagnostics and isDiagnostics)) { - return WRONG_HK_PACKET_TYPE; - } - PeriodicHousekeepingHelper* periodicHelper = LocalPoolDataSetAttorney::getPeriodicHelper(*dataSet); diff --git a/src/fsfw/datapoollocal/LocalDataPoolManager.h b/src/fsfw/datapoollocal/LocalDataPoolManager.h index 65d58d59..cd0d4f62 100644 --- a/src/fsfw/datapoollocal/LocalDataPoolManager.h +++ b/src/fsfw/datapoollocal/LocalDataPoolManager.h @@ -174,6 +174,7 @@ class LocalDataPoolManager : public ProvidesDataPoolSubscriptionIF, public Acces ReturnValue_t generateHousekeepingPacket(sid_t sid, LocalPoolDataSetBase* dataSet, bool forDownlink, MessageQueueId_t destination = MessageQueueIF::NO_QUEUE); + ReturnValue_t changeCollectionInterval(sid_t sid, float newCollectionInterval); HasLocalDataPoolIF* getOwner(); @@ -337,8 +338,6 @@ class LocalDataPoolManager : public ProvidesDataPoolSubscriptionIF, public Acces void performPeriodicHkGeneration(HkReceiver& hkReceiver); ReturnValue_t togglePeriodicGeneration(sid_t sid, bool enable, bool isDiagnostics); - ReturnValue_t changeCollectionInterval(sid_t sid, float newCollectionInterval, - bool isDiagnostics); ReturnValue_t generateSetStructurePacket(sid_t sid, bool isDiagnostics); void handleHkUpdateResetListInsertion(DataType dataType, DataId dataId); From 5a9304765f670685b6ca27cc9e6064012a17bab7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 6 Apr 2023 22:34:57 +0200 Subject: [PATCH 385/404] accepts action commands without ipc data --- src/fsfw/action/ActionHelper.cpp | 21 ++++++++++++++------- 1 file changed, 14 insertions(+), 7 deletions(-) diff --git a/src/fsfw/action/ActionHelper.cpp b/src/fsfw/action/ActionHelper.cpp index fd6c8afb..5e0a9a5f 100644 --- a/src/fsfw/action/ActionHelper.cpp +++ b/src/fsfw/action/ActionHelper.cpp @@ -59,17 +59,24 @@ void ActionHelper::setQueueToUse(MessageQueueIF* queue) { queueToUse = queue; } void ActionHelper::prepareExecution(MessageQueueId_t commandedBy, ActionId_t actionId, store_address_t dataAddress) { + bool hasAdditionalData = false; const uint8_t* dataPtr = nullptr; size_t size = 0; - ReturnValue_t result = ipcStore->getData(dataAddress, &dataPtr, &size); - if (result != returnvalue::OK) { - CommandMessage reply; - ActionMessage::setStepReply(&reply, actionId, 0, result); - queueToUse->sendMessage(commandedBy, &reply); - return; + ReturnValue_t result; + if(dataAddress != store_address_t::invalid()) { + hasAdditionalData = true; + ReturnValue_t result = ipcStore->getData(dataAddress, &dataPtr, &size); + if (result != returnvalue::OK) { + CommandMessage reply; + ActionMessage::setStepReply(&reply, actionId, 0, result); + queueToUse->sendMessage(commandedBy, &reply); + return; + } } result = owner->executeAction(actionId, commandedBy, dataPtr, size); - ipcStore->deleteData(dataAddress); + if(hasAdditionalData) { + ipcStore->deleteData(dataAddress); + } if (result == HasActionsIF::EXECUTION_FINISHED) { CommandMessage reply; ActionMessage::setCompletionReply(&reply, actionId, true, result); From e97fa5ac84db7ab5c10a31c2c78b26057cfacb71 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 7 Apr 2023 11:03:46 +0200 Subject: [PATCH 386/404] add skip directive for retvals --- src/fsfw/globalfunctions/DleParser.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/fsfw/globalfunctions/DleParser.h b/src/fsfw/globalfunctions/DleParser.h index 9802017a..48df0543 100644 --- a/src/fsfw/globalfunctions/DleParser.h +++ b/src/fsfw/globalfunctions/DleParser.h @@ -18,8 +18,11 @@ */ class DleParser { public: + //! [EXPORT] : [SKIP] static constexpr ReturnValue_t NO_PACKET_FOUND = returnvalue::makeCode(1, 1); + //! [EXPORT] : [SKIP] static constexpr ReturnValue_t POSSIBLE_PACKET_LOSS = returnvalue::makeCode(1, 2); + using BufPair = std::pair; enum class ContextType { NONE, PACKET_FOUND, ERROR }; From 285d327b97514946f0714e477289f67ee8bd413f Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 7 Apr 2023 17:42:44 +0200 Subject: [PATCH 387/404] clean up spi retval defs --- src/fsfw/action/ActionHelper.cpp | 4 ++-- src/fsfw/datapoollocal/LocalDataPoolManager.cpp | 3 ++- src/fsfw/power/DummyPowerSwitcher.cpp | 2 +- src/fsfw/subsystem/SubsystemBase.cpp | 2 +- src/fsfw_hal/common/spi/spiCommon.h | 12 +++++++++++- src/fsfw_hal/linux/spi/SpiComIF.cpp | 10 +++++----- src/fsfw_hal/linux/spi/SpiComIF.h | 9 --------- src/fsfw_hal/stm32h7/spi/SpiComIF.cpp | 6 +++--- 8 files changed, 25 insertions(+), 23 deletions(-) diff --git a/src/fsfw/action/ActionHelper.cpp b/src/fsfw/action/ActionHelper.cpp index 5e0a9a5f..81a1a727 100644 --- a/src/fsfw/action/ActionHelper.cpp +++ b/src/fsfw/action/ActionHelper.cpp @@ -63,7 +63,7 @@ void ActionHelper::prepareExecution(MessageQueueId_t commandedBy, ActionId_t act const uint8_t* dataPtr = nullptr; size_t size = 0; ReturnValue_t result; - if(dataAddress != store_address_t::invalid()) { + if (dataAddress != store_address_t::invalid()) { hasAdditionalData = true; ReturnValue_t result = ipcStore->getData(dataAddress, &dataPtr, &size); if (result != returnvalue::OK) { @@ -74,7 +74,7 @@ void ActionHelper::prepareExecution(MessageQueueId_t commandedBy, ActionId_t act } } result = owner->executeAction(actionId, commandedBy, dataPtr, size); - if(hasAdditionalData) { + if (hasAdditionalData) { ipcStore->deleteData(dataAddress); } if (result == HasActionsIF::EXECUTION_FINISHED) { diff --git a/src/fsfw/datapoollocal/LocalDataPoolManager.cpp b/src/fsfw/datapoollocal/LocalDataPoolManager.cpp index 178d5654..32de8e6b 100644 --- a/src/fsfw/datapoollocal/LocalDataPoolManager.cpp +++ b/src/fsfw/datapoollocal/LocalDataPoolManager.cpp @@ -720,7 +720,8 @@ ReturnValue_t LocalDataPoolManager::togglePeriodicGeneration(sid_t sid, bool ena return returnvalue::OK; } -ReturnValue_t LocalDataPoolManager::changeCollectionInterval(sid_t sid, float newCollectionInterval) { +ReturnValue_t LocalDataPoolManager::changeCollectionInterval(sid_t sid, + float newCollectionInterval) { LocalPoolDataSetBase* dataSet = HasLocalDpIFManagerAttorney::getDataSetHandle(owner, sid); if (dataSet == nullptr) { printWarningOrError(sif::OutputTypes::OUT_WARNING, "changeCollectionInterval", diff --git a/src/fsfw/power/DummyPowerSwitcher.cpp b/src/fsfw/power/DummyPowerSwitcher.cpp index 51adde71..80b4cc4a 100644 --- a/src/fsfw/power/DummyPowerSwitcher.cpp +++ b/src/fsfw/power/DummyPowerSwitcher.cpp @@ -7,7 +7,7 @@ DummyPowerSwitcher::DummyPowerSwitcher(object_id_t objectId, size_t numberOfSwit switcherList(numberOfSwitches), fuseList(numberOfFuses), switchDelayMs(switchDelayMs) { - for(auto &switchState: switcherList) { + for (auto &switchState : switcherList) { switchState = PowerSwitchIF::SWITCH_UNKNOWN; } } diff --git a/src/fsfw/subsystem/SubsystemBase.cpp b/src/fsfw/subsystem/SubsystemBase.cpp index b8c09230..4f62a889 100644 --- a/src/fsfw/subsystem/SubsystemBase.cpp +++ b/src/fsfw/subsystem/SubsystemBase.cpp @@ -325,7 +325,7 @@ const HasModesIF& SubsystemBase::getModeIF() const { return *this; } ModeTreeChildIF& SubsystemBase::getModeTreeChildIF() { return *this; } ReturnValue_t SubsystemBase::registerChild(object_id_t childObjectId, MessageQueueId_t childQueue) { - ChildInfo info; + ChildInfo info; // intentional to force an initial command during system startup info.commandQueue = childQueue; diff --git a/src/fsfw_hal/common/spi/spiCommon.h b/src/fsfw_hal/common/spi/spiCommon.h index bee86823..072363a5 100644 --- a/src/fsfw_hal/common/spi/spiCommon.h +++ b/src/fsfw_hal/common/spi/spiCommon.h @@ -5,8 +5,18 @@ namespace spi { +static constexpr uint8_t CLASS_ID = CLASS_ID::HAL_SPI; +static constexpr ReturnValue_t OPENING_FILE_FAILED = returnvalue::makeCode(CLASS_ID, 0); +/* Full duplex (ioctl) transfer failure */ +static constexpr ReturnValue_t FULL_DUPLEX_TRANSFER_FAILED = returnvalue::makeCode(CLASS_ID, 1); +/* Half duplex (read/write) transfer failure */ +static constexpr ReturnValue_t HALF_DUPLEX_TRANSFER_FAILED = returnvalue::makeCode(CLASS_ID, 2); +static constexpr ReturnValue_t TIMEOUT = returnvalue::makeCode(CLASS_ID, 3); +static constexpr ReturnValue_t BUSY = returnvalue::makeCode(CLASS_ID, 4); +static constexpr ReturnValue_t GENERIC_ERROR = returnvalue::makeCode(CLASS_ID, 5); + enum SpiModes : uint8_t { MODE_0, MODE_1, MODE_2, MODE_3 }; -} +} // namespace spi #endif /* FSFW_HAL_COMMON_SPI_SPICOMMON_H_ */ diff --git a/src/fsfw_hal/linux/spi/SpiComIF.cpp b/src/fsfw_hal/linux/spi/SpiComIF.cpp index d285b120..f227c685 100644 --- a/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -173,7 +173,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const int fileDescriptor = 0; UnixFileGuard fileHelper(dev, fileDescriptor, O_RDWR, "SpiComIF::sendMessage"); if (fileHelper.getOpenResult() != returnvalue::OK) { - return OPENING_FILE_FAILED; + return spi::OPENING_FILE_FAILED; } spi::SpiModes spiMode = spi::SpiModes::MODE_0; uint32_t spiSpeed = 0; @@ -234,7 +234,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const if (retval < 0) { spiCookie->setTransferSize(0); utility::handleIoctlError("SpiComIF::sendMessage: ioctl error."); - result = FULL_DUPLEX_TRANSFER_FAILED; + result = spi::FULL_DUPLEX_TRANSFER_FAILED; } #if FSFW_HAL_SPI_WIRETAPPING == 1 performSpiWiretapping(spiCookie); @@ -250,7 +250,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie* spiCookie, const sif::printWarning("SpiComIF::sendMessage: Half-Duplex write operation failed!\n"); #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ #endif /* FSFW_VERBOSE_LEVEL >= 1 */ - result = HALF_DUPLEX_TRANSFER_FAILED; + result = spi::HALF_DUPLEX_TRANSFER_FAILED; } } @@ -287,7 +287,7 @@ ReturnValue_t SpiComIF::performHalfDuplexReception(SpiCookie* spiCookie) { int fileDescriptor = 0; UnixFileGuard fileHelper(dev, fileDescriptor, O_RDWR, "SpiComIF::requestReceiveMessage"); if (fileHelper.getOpenResult() != returnvalue::OK) { - return OPENING_FILE_FAILED; + return spi::OPENING_FILE_FAILED; } uint8_t* rxBuf = nullptr; @@ -327,7 +327,7 @@ ReturnValue_t SpiComIF::performHalfDuplexReception(SpiCookie* spiCookie) { sif::printWarning("SpiComIF::sendMessage: Half-Duplex read operation failed!\n"); #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ #endif /* FSFW_VERBOSE_LEVEL >= 1 */ - result = HALF_DUPLEX_TRANSFER_FAILED; + result = spi::HALF_DUPLEX_TRANSFER_FAILED; } if (gpioId != gpio::NO_GPIO and not csLockManual) { diff --git a/src/fsfw_hal/linux/spi/SpiComIF.h b/src/fsfw_hal/linux/spi/SpiComIF.h index 14a3355a..3b72a59d 100644 --- a/src/fsfw_hal/linux/spi/SpiComIF.h +++ b/src/fsfw_hal/linux/spi/SpiComIF.h @@ -22,15 +22,6 @@ class SpiCookie; */ class SpiComIF : public DeviceCommunicationIF, public SystemObject { public: - static constexpr uint8_t spiRetvalId = CLASS_ID::HAL_SPI; - static constexpr ReturnValue_t OPENING_FILE_FAILED = returnvalue::makeCode(spiRetvalId, 0); - /* Full duplex (ioctl) transfer failure */ - static constexpr ReturnValue_t FULL_DUPLEX_TRANSFER_FAILED = - returnvalue::makeCode(spiRetvalId, 1); - /* Half duplex (read/write) transfer failure */ - static constexpr ReturnValue_t HALF_DUPLEX_TRANSFER_FAILED = - returnvalue::makeCode(spiRetvalId, 2); - SpiComIF(object_id_t objectId, std::string devname, GpioIF& gpioComIF); ReturnValue_t initializeInterface(CookieIF* cookie) override; diff --git a/src/fsfw_hal/stm32h7/spi/SpiComIF.cpp b/src/fsfw_hal/stm32h7/spi/SpiComIF.cpp index 724917bd..4d688bd5 100644 --- a/src/fsfw_hal/stm32h7/spi/SpiComIF.cpp +++ b/src/fsfw_hal/stm32h7/spi/SpiComIF.cpp @@ -357,13 +357,13 @@ ReturnValue_t SpiComIF::halErrorHandler(HAL_StatusTypeDef status, spi::TransferM sif::printWarning("SpiComIF::handle%sSendOperation: HAL error %d occured\n", modeString, status); switch (status) { case (HAL_BUSY): { - return spi::HAL_BUSY_RETVAL; + return spi::BUSY; } case (HAL_ERROR): { - return spi::HAL_ERROR_RETVAL; + return spi::GENERIC_ERROR; } case (HAL_TIMEOUT): { - return spi::HAL_TIMEOUT_RETVAL; + return spi::TIMEOUT; } default: { return returnvalue::FAILED; From 894d1e3b8744bb59b498e8ac56f19065759af366 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 11 Apr 2023 16:36:54 +0200 Subject: [PATCH 388/404] move CFDP handler --- src/fsfw/cfdp.h | 1 - src/fsfw/cfdp/handler/CMakeLists.txt | 5 +- src/fsfw/cfdp/handler/CfdpHandler.cpp | 134 -------------------------- src/fsfw/cfdp/handler/CfdpHandler.h | 71 -------------- 4 files changed, 2 insertions(+), 209 deletions(-) delete mode 100644 src/fsfw/cfdp/handler/CfdpHandler.cpp delete mode 100644 src/fsfw/cfdp/handler/CfdpHandler.h diff --git a/src/fsfw/cfdp.h b/src/fsfw/cfdp.h index f6c01ad0..b2645978 100644 --- a/src/fsfw/cfdp.h +++ b/src/fsfw/cfdp.h @@ -2,7 +2,6 @@ #define FSFW_CFDP_H #include "cfdp/definitions.h" -#include "cfdp/handler/CfdpHandler.h" #include "cfdp/handler/DestHandler.h" #include "cfdp/handler/FaultHandlerBase.h" #include "cfdp/helpers.h" diff --git a/src/fsfw/cfdp/handler/CMakeLists.txt b/src/fsfw/cfdp/handler/CMakeLists.txt index d96ce91c..7ad995c0 100644 --- a/src/fsfw/cfdp/handler/CMakeLists.txt +++ b/src/fsfw/cfdp/handler/CMakeLists.txt @@ -1,3 +1,2 @@ -target_sources( - ${LIB_FSFW_NAME} PRIVATE SourceHandler.cpp DestHandler.cpp - FaultHandlerBase.cpp UserBase.cpp CfdpHandler.cpp) +target_sources(${LIB_FSFW_NAME} PRIVATE SourceHandler.cpp DestHandler.cpp + FaultHandlerBase.cpp UserBase.cpp) diff --git a/src/fsfw/cfdp/handler/CfdpHandler.cpp b/src/fsfw/cfdp/handler/CfdpHandler.cpp deleted file mode 100644 index 902097b6..00000000 --- a/src/fsfw/cfdp/handler/CfdpHandler.cpp +++ /dev/null @@ -1,134 +0,0 @@ -#include "CfdpHandler.h" - -#include "fsfw/cfdp/pdu/AckPduReader.h" -#include "fsfw/cfdp/pdu/PduHeaderReader.h" -#include "fsfw/globalfunctions/arrayprinter.h" -#include "fsfw/ipc/QueueFactory.h" -#include "fsfw/tmtcservices/TmTcMessage.h" - -using namespace returnvalue; -using namespace cfdp; - -CfdpHandler::CfdpHandler(const FsfwHandlerParams& fsfwParams, const CfdpHandlerCfg& cfdpCfg) - : SystemObject(fsfwParams.objectId), - msgQueue(fsfwParams.msgQueue), - destHandler( - DestHandlerParams(LocalEntityCfg(cfdpCfg.id, cfdpCfg.indicCfg, cfdpCfg.faultHandler), - cfdpCfg.userHandler, cfdpCfg.remoteCfgProvider, cfdpCfg.packetInfoList, - cfdpCfg.lostSegmentsList), - FsfwParams(fsfwParams.packetDest, nullptr, this, fsfwParams.tcStore, - fsfwParams.tmStore)) { - destHandler.setMsgQueue(msgQueue); -} - -[[nodiscard]] const char* CfdpHandler::getName() const { return "CFDP Handler"; } - -[[nodiscard]] uint32_t CfdpHandler::getIdentifier() const { - return destHandler.getDestHandlerParams().cfg.localId.getValue(); -} - -[[nodiscard]] MessageQueueId_t CfdpHandler::getRequestQueue() const { return msgQueue.getId(); } - -ReturnValue_t CfdpHandler::initialize() { - ReturnValue_t result = destHandler.initialize(); - if (result != OK) { - return result; - } - tcStore = destHandler.getTcStore(); - tmStore = destHandler.getTmStore(); - - return SystemObject::initialize(); -} - -ReturnValue_t CfdpHandler::performOperation(uint8_t operationCode) { - // TODO: Receive TC packets and route them to source and dest handler, depending on which is - // correct or more appropriate - ReturnValue_t status; - ReturnValue_t result = OK; - TmTcMessage tmtcMsg; - for (status = msgQueue.receiveMessage(&tmtcMsg); status == returnvalue::OK; - status = msgQueue.receiveMessage(&tmtcMsg)) { - result = handleCfdpPacket(tmtcMsg); - if (result != OK) { - status = result; - } - } - auto& fsmRes = destHandler.performStateMachine(); - // TODO: Error handling? - while (fsmRes.callStatus == CallStatus::CALL_AGAIN) { - destHandler.performStateMachine(); - // TODO: Error handling? - } - return status; -} - -ReturnValue_t CfdpHandler::handleCfdpPacket(TmTcMessage& msg) { - auto accessorPair = tcStore->getData(msg.getStorageId()); - if (accessorPair.first != OK) { - return accessorPair.first; - } - PduHeaderReader reader(accessorPair.second.data(), accessorPair.second.size()); - ReturnValue_t result = reader.parseData(); - if (result != returnvalue::OK) { - return INVALID_PDU_FORMAT; - } - // The CFDP distributor should have taken care of ensuring the destination ID is correct - PduType type = reader.getPduType(); - // Only the destination handler can process these PDUs - if (type == PduType::FILE_DATA) { - // Disable auto-deletion of packet - accessorPair.second.release(); - PacketInfo info(type, msg.getStorageId()); - result = destHandler.passPacket(info); - } else { - // Route depending on PDU type and directive type if applicable. It retrieves directive type - // from the raw stream for better performance (with sanity and directive code check). - // The routing is based on section 4.5 of the CFDP standard which specifies the PDU forwarding - // procedure. - - // PDU header only. Invalid supplied data. A directive packet should have a valid data field - // with at least one byte being the directive code - const uint8_t* pduDataField = reader.getPduDataField(); - if (pduDataField == nullptr) { - return INVALID_PDU_FORMAT; - } - if (not FileDirectiveReader::checkFileDirective(pduDataField[0])) { - return INVALID_DIRECTIVE_FIELD; - } - auto directive = static_cast(pduDataField[0]); - - auto passToDestHandler = [&]() { - accessorPair.second.release(); - PacketInfo info(type, msg.getStorageId(), directive); - result = destHandler.passPacket(info); - }; - auto passToSourceHandler = [&]() { - - }; - if (directive == FileDirective::METADATA or directive == FileDirective::EOF_DIRECTIVE or - directive == FileDirective::PROMPT) { - // Section b) of 4.5.3: These PDUs should always be targeted towards the file receiver a.k.a. - // the destination handler - passToDestHandler(); - } else if (directive == FileDirective::FINISH or directive == FileDirective::NAK or - directive == FileDirective::KEEP_ALIVE) { - // Section c) of 4.5.3: These PDUs should always be targeted towards the file sender a.k.a. - // the source handler - passToSourceHandler(); - } else if (directive == FileDirective::ACK) { - // Section a): Recipient depends of the type of PDU that is being acknowledged. We can simply - // extract the PDU type from the raw stream. If it is an EOF PDU, this packet is passed to - // the source handler, for a Finished PDU, it is passed to the destination handler. - FileDirective ackedDirective; - if (not AckPduReader::checkAckedDirectiveField(pduDataField[1], ackedDirective)) { - return INVALID_ACK_DIRECTIVE_FIELDS; - } - if (ackedDirective == FileDirective::EOF_DIRECTIVE) { - passToSourceHandler(); - } else if (ackedDirective == FileDirective::FINISH) { - passToDestHandler(); - } - } - } - return result; -} diff --git a/src/fsfw/cfdp/handler/CfdpHandler.h b/src/fsfw/cfdp/handler/CfdpHandler.h deleted file mode 100644 index 2de9f7dd..00000000 --- a/src/fsfw/cfdp/handler/CfdpHandler.h +++ /dev/null @@ -1,71 +0,0 @@ -#ifndef FSFW_EXAMPLE_HOSTED_CFDPHANDLER_H -#define FSFW_EXAMPLE_HOSTED_CFDPHANDLER_H - -#include - -#include "fsfw/cfdp/handler/DestHandler.h" -#include "fsfw/objectmanager/SystemObject.h" -#include "fsfw/tasks/ExecutableObjectIF.h" -#include "fsfw/tmtcservices/AcceptsTelecommandsIF.h" -#include "fsfw/tmtcservices/TmTcMessage.h" - -struct FsfwHandlerParams { - FsfwHandlerParams(object_id_t objectId, HasFileSystemIF& vfs, AcceptsTelemetryIF& packetDest, - StorageManagerIF& tcStore, StorageManagerIF& tmStore, MessageQueueIF& msgQueue) - : objectId(objectId), - vfs(vfs), - packetDest(packetDest), - tcStore(tcStore), - tmStore(tmStore), - msgQueue(msgQueue) {} - object_id_t objectId{}; - HasFileSystemIF& vfs; - AcceptsTelemetryIF& packetDest; - StorageManagerIF& tcStore; - StorageManagerIF& tmStore; - MessageQueueIF& msgQueue; -}; - -struct CfdpHandlerCfg { - CfdpHandlerCfg(cfdp::EntityId localId, cfdp::IndicationCfg indicationCfg, - cfdp::UserBase& userHandler, cfdp::FaultHandlerBase& userFaultHandler, - cfdp::PacketInfoListBase& packetInfo, cfdp::LostSegmentsListBase& lostSegmentsList, - cfdp::RemoteConfigTableIF& remoteCfgProvider) - : id(std::move(localId)), - indicCfg(indicationCfg), - packetInfoList(packetInfo), - lostSegmentsList(lostSegmentsList), - remoteCfgProvider(remoteCfgProvider), - userHandler(userHandler), - faultHandler(userFaultHandler) {} - - cfdp::EntityId id; - cfdp::IndicationCfg indicCfg; - cfdp::PacketInfoListBase& packetInfoList; - cfdp::LostSegmentsListBase& lostSegmentsList; - cfdp::RemoteConfigTableIF& remoteCfgProvider; - cfdp::UserBase& userHandler; - cfdp::FaultHandlerBase& faultHandler; -}; - -class CfdpHandler : public SystemObject, public ExecutableObjectIF, public AcceptsTelecommandsIF { - public: - explicit CfdpHandler(const FsfwHandlerParams& fsfwParams, const CfdpHandlerCfg& cfdpCfg); - - [[nodiscard]] const char* getName() const override; - [[nodiscard]] uint32_t getIdentifier() const override; - [[nodiscard]] MessageQueueId_t getRequestQueue() const override; - - ReturnValue_t initialize() override; - ReturnValue_t performOperation(uint8_t operationCode) override; - - private: - MessageQueueIF& msgQueue; - cfdp::DestHandler destHandler; - StorageManagerIF* tcStore = nullptr; - StorageManagerIF* tmStore = nullptr; - - ReturnValue_t handleCfdpPacket(TmTcMessage& msg); -}; - -#endif // FSFW_EXAMPLE_HOSTED_CFDPHANDLER_H From 7f61d17ceeee76ffd2206831a9288e5c8da5b6f0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 14 Apr 2023 21:08:44 +0200 Subject: [PATCH 389/404] even better event manager printout --- src/fsfw/events/EventManager.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/fsfw/events/EventManager.cpp b/src/fsfw/events/EventManager.cpp index f2a099ed..045b2416 100644 --- a/src/fsfw/events/EventManager.cpp +++ b/src/fsfw/events/EventManager.cpp @@ -55,8 +55,9 @@ void EventManager::notifyListeners(EventMessage* message) { if (result != returnvalue::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 << " for event " << std::setw(4) + << message->getEventId() << " 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); From 5eb9ee8bc1e6f8640cbea46febd11e4b92241881 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 14 Apr 2023 21:22:24 +0200 Subject: [PATCH 390/404] DHB fdir: event queue depth confgurable --- src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp | 5 +++-- src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.h | 3 ++- src/fsfw/events/EventManager.cpp | 2 +- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp b/src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp index 20e141c3..d3e5753f 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.cpp @@ -10,8 +10,9 @@ object_id_t DeviceHandlerFailureIsolation::powerConfirmationId = objects::NO_OBJECT; -DeviceHandlerFailureIsolation::DeviceHandlerFailureIsolation(object_id_t owner, object_id_t parent) - : FailureIsolationBase(owner, parent), +DeviceHandlerFailureIsolation::DeviceHandlerFailureIsolation(object_id_t owner, object_id_t parent, + uint8_t eventQueueDepth) + : FailureIsolationBase(owner, parent, eventQueueDepth), strangeReplyCount(DEFAULT_MAX_STRANGE_REPLIES, DEFAULT_STRANGE_REPLIES_TIME_MS, parameterDomainBase++), missedReplyCount(DEFAULT_MAX_MISSED_REPLY_COUNT, DEFAULT_MISSED_REPLY_TIME_MS, diff --git a/src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.h b/src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.h index 6007ceb8..4835af99 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.h +++ b/src/fsfw/devicehandlers/DeviceHandlerFailureIsolation.h @@ -13,7 +13,8 @@ class DeviceHandlerFailureIsolation : public FailureIsolationBase { friend class Heater; public: - DeviceHandlerFailureIsolation(object_id_t owner, object_id_t parent); + DeviceHandlerFailureIsolation(object_id_t owner, object_id_t parent, + uint8_t eventQueueDepth = 10); ~DeviceHandlerFailureIsolation(); ReturnValue_t initialize(); void triggerEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0); diff --git a/src/fsfw/events/EventManager.cpp b/src/fsfw/events/EventManager.cpp index 045b2416..f5cc99c6 100644 --- a/src/fsfw/events/EventManager.cpp +++ b/src/fsfw/events/EventManager.cpp @@ -55,7 +55,7 @@ void EventManager::notifyListeners(EventMessage* message) { if (result != returnvalue::OK) { #if FSFW_CPP_OSTREAM_ENABLED == 1 sif::error << std::hex << "EventManager::notifyListeners: MSG to 0x" << std::setfill('0') - << std::setw(8) << listener.first << " for event " << std::setw(4) + << std::setw(8) << listener.first << " for event 0x" << std::setw(4) << message->getEventId() << " failed with result 0x" << std::setw(4) << result << std::setfill(' ') << std::endl; #else From 258f0d331329d67e13eec9d7f4053fd269e3f9b6 Mon Sep 17 00:00:00 2001 From: meggert Date: Wed, 19 Apr 2023 15:25:14 +0200 Subject: [PATCH 391/404] use both LPFs --- .../devicehandlers/devicedefinitions/gyroL3gHelpers.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h b/src/fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h index 4aef6811..0135a04c 100644 --- a/src/fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h +++ b/src/fsfw_hal/devicehandlers/devicedefinitions/gyroL3gHelpers.h @@ -73,8 +73,10 @@ static constexpr uint8_t CTRL_REG_4_VAL = SET_BLE; /* Register 5 */ static constexpr uint8_t SET_REBOOT_MEM = 1 << 7; static constexpr uint8_t SET_FIFO_ENB = 1 << 6; +static constexpr uint8_t SET_OUT_SEL_1 = 1 << 1; +static constexpr uint8_t SET_OUT_SEL_0 = 1 << 0; -static constexpr uint8_t CTRL_REG_5_VAL = 0b00000000; +static constexpr uint8_t CTRL_REG_5_VAL = SET_OUT_SEL_1 | SET_OUT_SEL_0; /* Possible range values in degrees per second (DPS). */ static constexpr uint16_t RANGE_DPS_00 = 245; From cae131edcf95e6ab8c6415abb31e529aea553ba7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 May 2023 16:02:55 +0200 Subject: [PATCH 392/404] CFDP and unittest bugfixes --- src/fsfw/cfdp/pdu/HeaderCreator.cpp | 4 ++-- src/fsfw/cfdp/pdu/HeaderReader.cpp | 4 ++-- unittests/CatchFactory.cpp | 2 +- unittests/action/TestActionHelper.cpp | 22 +++++----------------- unittests/cfdp/pdu/testCfdpHeader.cpp | 10 +++++----- unittests/cfdp/pdu/testFileData.cpp | 2 +- unittests/cfdp/pdu/testFileDirective.cpp | 4 ++-- unittests/cfdp/testCfdp.cpp | 4 ++-- 8 files changed, 20 insertions(+), 32 deletions(-) diff --git a/src/fsfw/cfdp/pdu/HeaderCreator.cpp b/src/fsfw/cfdp/pdu/HeaderCreator.cpp index 29688575..2db3953c 100644 --- a/src/fsfw/cfdp/pdu/HeaderCreator.cpp +++ b/src/fsfw/cfdp/pdu/HeaderCreator.cpp @@ -24,8 +24,8 @@ ReturnValue_t HeaderCreator::serialize(uint8_t **buffer, size_t *size, size_t ma *buffer += 1; **buffer = pduDataFieldLen & 0x00ff; *buffer += 1; - **buffer = segmentationCtrl << 7 | pduConf.sourceId.getWidth() << 4 | segmentMetadataFlag << 3 | - pduConf.seqNum.getWidth(); + **buffer = segmentationCtrl << 7 | ((pduConf.sourceId.getWidth() - 1) << 4) | + segmentMetadataFlag << 3 | (pduConf.seqNum.getWidth() - 1); *buffer += 1; *size += 4; ReturnValue_t result = pduConf.sourceId.serialize(buffer, size, maxSize, streamEndianness); diff --git a/src/fsfw/cfdp/pdu/HeaderReader.cpp b/src/fsfw/cfdp/pdu/HeaderReader.cpp index 9edf2394..de3d2906 100644 --- a/src/fsfw/cfdp/pdu/HeaderReader.cpp +++ b/src/fsfw/cfdp/pdu/HeaderReader.cpp @@ -78,11 +78,11 @@ cfdp::SegmentationControl PduHeaderReader::getSegmentationControl() const { } cfdp::WidthInBytes PduHeaderReader::getLenEntityIds() const { - return static_cast((pointers.fixedHeader->fourthByte >> 4) & 0x07); + return static_cast(((pointers.fixedHeader->fourthByte >> 4) & 0b111) + 1); } cfdp::WidthInBytes PduHeaderReader::getLenSeqNum() const { - return static_cast(pointers.fixedHeader->fourthByte & 0x07); + return static_cast((pointers.fixedHeader->fourthByte & 0b111) + 1); } cfdp::SegmentMetadataFlag PduHeaderReader::getSegmentMetadataFlag() const { diff --git a/unittests/CatchFactory.cpp b/unittests/CatchFactory.cpp index 0d855cb3..37c62f94 100644 --- a/unittests/CatchFactory.cpp +++ b/unittests/CatchFactory.cpp @@ -30,7 +30,7 @@ */ void Factory::produceFrameworkObjects(void* args) { setStaticFrameworkObjectIds(); - new EventManager(objects::EVENT_MANAGER); + new EventManager(objects::EVENT_MANAGER, 120); new HealthTable(objects::HEALTH_TABLE); new InternalErrorReporter(objects::INTERNAL_ERROR_REPORTER); diff --git a/unittests/action/TestActionHelper.cpp b/unittests/action/TestActionHelper.cpp index d99700d7..57166d5e 100644 --- a/unittests/action/TestActionHelper.cpp +++ b/unittests/action/TestActionHelper.cpp @@ -27,12 +27,8 @@ TEST_CASE("Action Helper", "[action]") { CHECK(not testDhMock.executeActionCalled); REQUIRE(actionHelper.handleActionMessage(&actionMessage) == returnvalue::OK); CHECK(testDhMock.executeActionCalled); - // No message is sent if everything is alright. CHECK(not testMqMock.wasMessageSent()); - store_address_t invalidAddress; - ActionMessage::setCommand(&actionMessage, testActionId, invalidAddress); - actionHelper.handleActionMessage(&actionMessage); - CHECK(testMqMock.wasMessageSent()); + CHECK(not testMqMock.wasMessageSent()); const uint8_t* ptr = nullptr; size_t size = 0; REQUIRE(ipcStore->getData(paramAddress, &ptr, &size) == @@ -44,6 +40,10 @@ TEST_CASE("Action Helper", "[action]") { for (uint8_t i = 0; i < 3; i++) { REQUIRE(ptr[i] == (i + 1)); } + // Action message without application data is also valid + store_address_t invalidAddress; + ActionMessage::setCommand(&actionMessage, testActionId, invalidAddress); + actionHelper.handleActionMessage(&actionMessage); testDhMock.clearBuffer(); } @@ -95,17 +95,5 @@ TEST_CASE("Action Helper", "[action]") { REQUIRE(ActionMessage::getActionId(&testMessage) == testActionId); } - SECTION("Missing IPC Data") { - ActionMessage::setCommand(&actionMessage, testActionId, store_address_t::invalid()); - CHECK(not testDhMock.executeActionCalled); - REQUIRE(actionHelper.handleActionMessage(&actionMessage) == returnvalue::OK); - CommandMessage testMessage; - REQUIRE(testMqMock.getNextSentMessage(testMessage) == returnvalue::OK); - REQUIRE(testMessage.getCommand() == static_cast(ActionMessage::STEP_FAILED)); - REQUIRE(ActionMessage::getReturnCode(&testMessage) == - static_cast(StorageManagerIF::ILLEGAL_STORAGE_ID)); - REQUIRE(ActionMessage::getStep(&testMessage) == 0); - } - SECTION("Data Reply") {} } diff --git a/unittests/cfdp/pdu/testCfdpHeader.cpp b/unittests/cfdp/pdu/testCfdpHeader.cpp index 5f81bec9..1fc7dfd4 100644 --- a/unittests/cfdp/pdu/testCfdpHeader.cpp +++ b/unittests/cfdp/pdu/testCfdpHeader.cpp @@ -97,7 +97,7 @@ TEST_CASE("CFDP Header", "[cfdp]") { REQUIRE(creator.serialize(&serTarget, &serSize, serBuf.size(), SerializeIF::Endianness::BIG) == returnvalue::OK); CHECK(serBuf[0] == 0x3f); - CHECK(serBuf[3] == 0x99); + CHECK(serBuf[3] == 0x88); REQUIRE(creator.getCrcFlag() == true); REQUIRE(creator.getDirection() == cfdp::Direction::TOWARDS_SENDER); REQUIRE(creator.getLargeFileFlag() == true); @@ -127,7 +127,7 @@ TEST_CASE("CFDP Header", "[cfdp]") { REQUIRE(creator.getTransmissionMode() == cfdp::TransmissionMode::UNACKNOWLEDGED); REQUIRE(creator.getSegmentationControl() == true); // Last three bits are 2 now (length of seq number) and bit 1 to bit 3 is 4 (len entity IDs) - REQUIRE(serBuf[3] == 0b11001010); + REQUIRE(serBuf[3] == 0b10111001); uint32_t entityId = 0; size_t deSerSize = 0; SerializeAdapter::deSerialize(&entityId, serBuf.data() + 4, &deSerSize, @@ -175,7 +175,7 @@ TEST_CASE("CFDP Header", "[cfdp]") { REQUIRE(serBuf[1] == 0); REQUIRE(serBuf[2] == 0); // Entity and Transaction Sequence number are 1 byte large - REQUIRE(serBuf[3] == 0b00010001); + REQUIRE(serBuf[3] == 0b00000000); // Source ID REQUIRE(serBuf[4] == 0); // Transaction Seq Number @@ -220,7 +220,7 @@ TEST_CASE("CFDP Header", "[cfdp]") { REQUIRE(serBuf[1] == 0); REQUIRE(serBuf[2] == 0); // Entity and Transaction Sequence number are 1 byte large - REQUIRE(serBuf[3] == 0b00010001); + REQUIRE(serBuf[3] == 0b00000000); REQUIRE(serSize == 7); // Deser call not strictly necessary auto reader = PduHeaderReader(serBuf.data(), serBuf.size()); @@ -270,7 +270,7 @@ TEST_CASE("CFDP Header", "[cfdp]") { REQUIRE(reader.parseData() == returnvalue::OK); // Everything except version bit flipped to one now REQUIRE(serBuf[0] == 0x3f); - REQUIRE(serBuf[3] == 0b11001010); + REQUIRE(serBuf[3] == 0b10111001); REQUIRE(reader.getWholePduSize() == 14); REQUIRE(reader.getCrcFlag() == true); diff --git a/unittests/cfdp/pdu/testFileData.cpp b/unittests/cfdp/pdu/testFileData.cpp index 258ef9c1..39139378 100644 --- a/unittests/cfdp/pdu/testFileData.cpp +++ b/unittests/cfdp/pdu/testFileData.cpp @@ -68,7 +68,7 @@ TEST_CASE("File Data PDU", "[cfdp][pdu]") { // Bits 1 to 3 length of enitity IDs is 2 // Bit 4: Segment metadata flag is set // Bit 5 to seven: length of transaction seq num is 2 - REQUIRE(fileDataBuffer[3] == 0b10101010); + REQUIRE(fileDataBuffer[3] == 0b10011001); REQUIRE((fileDataBuffer[10] >> 6) & 0b11 == cfdp::RecordContinuationState::CONTAINS_START_AND_END); // Segment metadata length diff --git a/unittests/cfdp/pdu/testFileDirective.cpp b/unittests/cfdp/pdu/testFileDirective.cpp index e1158a1a..17e0d699 100644 --- a/unittests/cfdp/pdu/testFileDirective.cpp +++ b/unittests/cfdp/pdu/testFileDirective.cpp @@ -30,7 +30,7 @@ TEST_CASE("CFDP File Directive", "[cfdp][pdu]") { REQUIRE(serBuf[1] == 0); REQUIRE(serBuf[2] == 5); // Entity and Transaction Sequence number are 1 byte large - REQUIRE(serBuf[3] == 0b00010001); + REQUIRE(serBuf[3] == 0b00000000); // Source ID REQUIRE(serBuf[4] == 0); // Transaction Seq Number @@ -82,4 +82,4 @@ TEST_CASE("CFDP File Directive", "[cfdp][pdu]") { // Invalid file directive REQUIRE(fdDeser.parseData() == cfdp::INVALID_DIRECTIVE_FIELD); } -} \ No newline at end of file +} diff --git a/unittests/cfdp/testCfdp.cpp b/unittests/cfdp/testCfdp.cpp index eacc83de..467b5b2d 100644 --- a/unittests/cfdp/testCfdp.cpp +++ b/unittests/cfdp/testCfdp.cpp @@ -33,8 +33,8 @@ TEST_CASE("CFDP Base", "[cfdp]") { // PDU data field length is 5 (4 + Directive code octet) REQUIRE(serBuf[1] == 0); REQUIRE(serBuf[2] == 5); - // Entity and Transaction Sequence number are 1 byte large - REQUIRE(serBuf[3] == 0b00010001); + // Entity and Transaction Sequence number are 1 byte large, value minus one is stored + REQUIRE(serBuf[3] == 0b00000000); // Source ID REQUIRE(serBuf[4] == 0); // Transaction Seq Number From 1a77e6bb095b275f8223d7f71f74eb6de188ec54 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 May 2023 16:13:05 +0200 Subject: [PATCH 393/404] proper floating point comparison --- unittests/datapoollocal/testLocalPoolManager.cpp | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/unittests/datapoollocal/testLocalPoolManager.cpp b/unittests/datapoollocal/testLocalPoolManager.cpp index 91cd011d..e463a123 100644 --- a/unittests/datapoollocal/testLocalPoolManager.cpp +++ b/unittests/datapoollocal/testLocalPoolManager.cpp @@ -9,6 +9,7 @@ #include #include +#include #include #include "CatchDefinitions.h" @@ -309,9 +310,7 @@ TEST_CASE("Local Pool Manager Tests", "[LocManTest]") { HousekeepingMessage::setCollectionIntervalModificationCommand(&hkCmd, lpool::testSid, 0.4, false); CHECK(poolOwner.poolManager.handleHousekeepingMessage(&hkCmd) == returnvalue::OK); - /* For non-diagnostics and a specified minimum frequency of 0.2 seconds, the - resulting collection interval should be 1.0 second */ - CHECK(poolOwner.dataset.getCollectionInterval() == 1.0); + CHECK_THAT(poolOwner.dataset.getCollectionInterval(), Catch::Matchers::WithinAbs(0.4, 0.01)); REQUIRE(poolOwnerMock.wasMessageSent()); REQUIRE(poolOwnerMock.numberOfSentMessages() == 1); CHECK(poolOwnerMock.clearLastSentMessage() == returnvalue::OK); @@ -348,14 +347,6 @@ TEST_CASE("Local Pool Manager Tests", "[LocManTest]") { REQUIRE(poolOwnerMock.numberOfSentMessages() == 1); CHECK(poolOwnerMock.clearLastSentMessage() == returnvalue::OK); - HousekeepingMessage::setCollectionIntervalModificationCommand(&hkCmd, lpool::testSid, 0.4, - false); - CHECK(poolOwner.poolManager.handleHousekeepingMessage(&hkCmd) == - static_cast(LocalDataPoolManager::WRONG_HK_PACKET_TYPE)); - REQUIRE(poolOwnerMock.wasMessageSent()); - REQUIRE(poolOwnerMock.numberOfSentMessages() == 1); - CHECK(poolOwnerMock.clearLastSentMessage() == returnvalue::OK); - HousekeepingMessage::setStructureReportingCommand(&hkCmd, lpool::testSid, false); CHECK(poolOwner.poolManager.handleHousekeepingMessage(&hkCmd) == static_cast(LocalDataPoolManager::WRONG_HK_PACKET_TYPE)); From 4391823f01d792bcc078d47b60f7df7624f8cbe4 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 15 May 2023 16:14:36 +0200 Subject: [PATCH 394/404] changelog --- CHANGELOG.md | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index a841e38d..2250a301 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,12 @@ and this project adheres to [Semantic Versioning](http://semver.org/). # [unreleased] +## Fixed + +- Important bugfix in CFDP PDU header format: The entity length field and the transaction sequence + number fields stored the actual length of the field instead of the length minus 1 like specified + in the CFDP standard. + ## Changed - Health functions are virtual now. From e0adb3325f4a26f6c112ef4b83114319b67cdbff Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 22 May 2023 10:37:26 +0200 Subject: [PATCH 395/404] werks --- unittests/action/TestActionHelper.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/unittests/action/TestActionHelper.cpp b/unittests/action/TestActionHelper.cpp index 57166d5e..de021bb8 100644 --- a/unittests/action/TestActionHelper.cpp +++ b/unittests/action/TestActionHelper.cpp @@ -28,7 +28,6 @@ TEST_CASE("Action Helper", "[action]") { REQUIRE(actionHelper.handleActionMessage(&actionMessage) == returnvalue::OK); CHECK(testDhMock.executeActionCalled); CHECK(not testMqMock.wasMessageSent()); - CHECK(not testMqMock.wasMessageSent()); const uint8_t* ptr = nullptr; size_t size = 0; REQUIRE(ipcStore->getData(paramAddress, &ptr, &size) == From b442ca09b91d0cfdc6acd5a30349a25c4051972c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 7 Jun 2023 11:58:31 +0200 Subject: [PATCH 396/404] configurable queue depth --- src/fsfw/pus/Service8FunctionManagement.cpp | 4 ++-- src/fsfw/pus/Service8FunctionManagement.h | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/fsfw/pus/Service8FunctionManagement.cpp b/src/fsfw/pus/Service8FunctionManagement.cpp index 8b7c6972..2607adca 100644 --- a/src/fsfw/pus/Service8FunctionManagement.cpp +++ b/src/fsfw/pus/Service8FunctionManagement.cpp @@ -9,11 +9,11 @@ #include "fsfw/serviceinterface/ServiceInterface.h" Service8FunctionManagement::Service8FunctionManagement(object_id_t objectId, uint16_t apid, - uint8_t serviceId, + uint8_t serviceId, size_t queueDepth, uint8_t numParallelCommands, uint16_t commandTimeoutSeconds) : CommandingServiceBase(objectId, apid, "PUS 8 Functional Commanding", serviceId, - numParallelCommands, commandTimeoutSeconds) {} + numParallelCommands, commandTimeoutSeconds, queueDepth) {} Service8FunctionManagement::~Service8FunctionManagement() {} diff --git a/src/fsfw/pus/Service8FunctionManagement.h b/src/fsfw/pus/Service8FunctionManagement.h index 38aaf4f4..0ec715b8 100644 --- a/src/fsfw/pus/Service8FunctionManagement.h +++ b/src/fsfw/pus/Service8FunctionManagement.h @@ -31,7 +31,8 @@ class Service8FunctionManagement : public CommandingServiceBase { public: Service8FunctionManagement(object_id_t objectId, uint16_t apid, uint8_t serviceId, - uint8_t numParallelCommands = 4, uint16_t commandTimeoutSeconds = 60); + size_t queueDepth, uint8_t numParallelCommands = 4, + uint16_t commandTimeoutSeconds = 60); ~Service8FunctionManagement() override; protected: From 74b164b1da9958a5b34a8c3cea05e74d111a6d4d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 11 Jun 2023 18:02:27 +0200 Subject: [PATCH 397/404] make number of parallel cmds configurable --- src/fsfw/pus/Service3Housekeeping.cpp | 4 ++-- src/fsfw/pus/Service3Housekeeping.h | 3 ++- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/fsfw/pus/Service3Housekeeping.cpp b/src/fsfw/pus/Service3Housekeeping.cpp index 485c83f4..c6fac20c 100644 --- a/src/fsfw/pus/Service3Housekeeping.cpp +++ b/src/fsfw/pus/Service3Housekeeping.cpp @@ -5,8 +5,8 @@ #include "fsfw/pus/servicepackets/Service3Packets.h" Service3Housekeeping::Service3Housekeeping(object_id_t objectId, uint16_t apid, uint8_t serviceId, - uint32_t queueDepth) - : CommandingServiceBase(objectId, apid, "PUS 3 HK", serviceId, NUM_OF_PARALLEL_COMMANDS, + uint32_t queueDepth, uint8_t numParallelCommands) + : CommandingServiceBase(objectId, apid, "PUS 3 HK", serviceId, numParallelCommands, COMMAND_TIMEOUT_SECONDS, queueDepth) {} Service3Housekeeping::~Service3Housekeeping() {} diff --git a/src/fsfw/pus/Service3Housekeeping.h b/src/fsfw/pus/Service3Housekeeping.h index 38f808f4..10004bc6 100644 --- a/src/fsfw/pus/Service3Housekeeping.h +++ b/src/fsfw/pus/Service3Housekeeping.h @@ -28,7 +28,8 @@ class Service3Housekeeping : public CommandingServiceBase, public AcceptsHkPacke static constexpr uint8_t NUM_OF_PARALLEL_COMMANDS = 4; static constexpr uint16_t COMMAND_TIMEOUT_SECONDS = 60; - Service3Housekeeping(object_id_t objectId, uint16_t apid, uint8_t serviceId, uint32_t queueDepth); + Service3Housekeeping(object_id_t objectId, uint16_t apid, uint8_t serviceId, uint32_t queueDepth, + uint8_t numParallelCommands); virtual ~Service3Housekeeping(); protected: From f2947bc78e91802e93eee970817b6ca47356225a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 18 Jun 2023 19:04:36 +0200 Subject: [PATCH 398/404] specify truncate flag explicitely --- src/fsfw_hal/host/HostFilesystem.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/fsfw_hal/host/HostFilesystem.cpp b/src/fsfw_hal/host/HostFilesystem.cpp index 315ba422..f9f3bdd6 100644 --- a/src/fsfw_hal/host/HostFilesystem.cpp +++ b/src/fsfw_hal/host/HostFilesystem.cpp @@ -165,7 +165,8 @@ ReturnValue_t HostFilesystem::truncateFile(FilesystemParams params) { if (not filesystem::exists(path, e)) { return FILE_DOES_NOT_EXIST; } - ofstream of(path); + // Specify truncaion flug explicitely. + ofstream of(path, std::ios::out | std::ios::trunc); return returnvalue::OK; } From f80c5980ea5bf84828a22dc6b4985a8747f85df4 Mon Sep 17 00:00:00 2001 From: meggert Date: Mon, 19 Jun 2023 17:04:45 +0200 Subject: [PATCH 399/404] max value calc fix --- src/fsfw/globalfunctions/math/VectorOperations.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/fsfw/globalfunctions/math/VectorOperations.h b/src/fsfw/globalfunctions/math/VectorOperations.h index 197cd46a..1e84330e 100644 --- a/src/fsfw/globalfunctions/math/VectorOperations.h +++ b/src/fsfw/globalfunctions/math/VectorOperations.h @@ -54,7 +54,7 @@ class VectorOperations { } static T maxAbsValue(const T *vector, uint8_t size, uint8_t *index = 0) { - T max = -1; + T max = vector[size - 1]; for (; size > 0; size--) { T abs = vector[size - 1]; @@ -72,7 +72,7 @@ class VectorOperations { } static T maxValue(const T *vector, uint8_t size, uint8_t *index = 0) { - T max = -1; + T max = vector[size - 1]; for (; size > 0; size--) { if (vector[size - 1] > max) { From c7037d417a8dbc0e0b7d2c5244c02bb1fc3c1312 Mon Sep 17 00:00:00 2001 From: Ulrich Mohr Date: Tue, 20 Jun 2023 11:54:15 +0200 Subject: [PATCH 400/404] nullptr check for optional argument --- .../globalfunctions/math/VectorOperations.h | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/src/fsfw/globalfunctions/math/VectorOperations.h b/src/fsfw/globalfunctions/math/VectorOperations.h index 1e84330e..b8f6b00f 100644 --- a/src/fsfw/globalfunctions/math/VectorOperations.h +++ b/src/fsfw/globalfunctions/math/VectorOperations.h @@ -53,8 +53,9 @@ class VectorOperations { mulScalar(vector, 1 / norm(vector, size), normalizedVector, size); } - static T maxAbsValue(const T *vector, uint8_t size, uint8_t *index = 0) { + static T maxAbsValue(const T *vector, uint8_t size, uint8_t *index = nullptr) { T max = vector[size - 1]; + uint8_t foundIndex = size - 1; for (; size > 0; size--) { T abs = vector[size - 1]; @@ -64,24 +65,35 @@ class VectorOperations { if (abs > max) { max = abs; if (index != 0) { - *index = size - 1; + foundIndex = size - 1; } } } + + if (index != nullptr) { + *index = foundIndex; + } + return max; } - static T maxValue(const T *vector, uint8_t size, uint8_t *index = 0) { + static T maxValue(const T *vector, uint8_t size, uint8_t *index = nullptr) { T max = vector[size - 1]; + uint8_t foundIndex = size - 1; for (; size > 0; size--) { if (vector[size - 1] > max) { max = vector[size - 1]; if (index != 0) { - *index = size - 1; + foundIndex = size - 1; } } } + + if (index != nullptr) { + *index = foundIndex; + } + return max; } From 0f76cdb3ba54f5e90a8eee4316c49cf0f581f996 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 20 Jun 2023 20:36:03 +0200 Subject: [PATCH 401/404] typo --- src/fsfw_hal/host/HostFilesystem.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw_hal/host/HostFilesystem.cpp b/src/fsfw_hal/host/HostFilesystem.cpp index f9f3bdd6..20984f77 100644 --- a/src/fsfw_hal/host/HostFilesystem.cpp +++ b/src/fsfw_hal/host/HostFilesystem.cpp @@ -165,7 +165,7 @@ ReturnValue_t HostFilesystem::truncateFile(FilesystemParams params) { if (not filesystem::exists(path, e)) { return FILE_DOES_NOT_EXIST; } - // Specify truncaion flug explicitely. + // Specify truncation flug explicitely. ofstream of(path, std::ios::out | std::ios::trunc); return returnvalue::OK; } From a85a38c882af968200a0dd54ded9fd99b3961e7a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 16:08:29 +0200 Subject: [PATCH 402/404] some thnings are configurable now --- src/fsfw/internalerror/InternalErrorReporter.cpp | 10 ++++++---- src/fsfw/internalerror/InternalErrorReporter.h | 5 ++++- 2 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/fsfw/internalerror/InternalErrorReporter.cpp b/src/fsfw/internalerror/InternalErrorReporter.cpp index 69faacdd..44910801 100644 --- a/src/fsfw/internalerror/InternalErrorReporter.cpp +++ b/src/fsfw/internalerror/InternalErrorReporter.cpp @@ -5,9 +5,12 @@ #include "fsfw/ipc/QueueFactory.h" #include "fsfw/serviceinterface/ServiceInterface.h" -InternalErrorReporter::InternalErrorReporter(object_id_t setObjectId, uint32_t messageQueueDepth) +InternalErrorReporter::InternalErrorReporter(object_id_t setObjectId, uint32_t messageQueueDepth, + bool enableSetByDefault, float generationFrequency) : SystemObject(setObjectId), poolManager(this, commandQueue), + enableSetByDefault(enableSetByDefault), + generationFrequency(generationFrequency), internalErrorSid(setObjectId, InternalErrorDataset::ERROR_SET_ID), internalErrorDataset(this) { commandQueue = QueueFactory::instance()->createMessageQueue(messageQueueDepth); @@ -134,9 +137,8 @@ ReturnValue_t InternalErrorReporter::initializeLocalDataPool(localpool::DataPool localDataPoolMap.emplace(errorPoolIds::TM_HITS, &tmHitsEntry); localDataPoolMap.emplace(errorPoolIds::QUEUE_HITS, &queueHitsEntry); localDataPoolMap.emplace(errorPoolIds::STORE_HITS, &storeHitsEntry); - poolManager.subscribeForDiagPeriodicPacket(subdp::DiagnosticsHkPeriodicParams( - internalErrorSid, false, - static_cast(getPeriodicOperationFrequency()) / static_cast(1000.0))); + poolManager.subscribeForRegularPeriodicPacket( + subdp::RegularHkPeriodicParams(internalErrorSid, enableSetByDefault, generationFrequency)); internalErrorDataset.setValidity(true, true); return returnvalue::OK; } diff --git a/src/fsfw/internalerror/InternalErrorReporter.h b/src/fsfw/internalerror/InternalErrorReporter.h index ca82d1a4..210f2cb1 100644 --- a/src/fsfw/internalerror/InternalErrorReporter.h +++ b/src/fsfw/internalerror/InternalErrorReporter.h @@ -21,7 +21,8 @@ class InternalErrorReporter : public SystemObject, public InternalErrorReporterIF, public HasLocalDataPoolIF { public: - InternalErrorReporter(object_id_t setObjectId, uint32_t messageQueueDepth = 5); + InternalErrorReporter(object_id_t setObjectId, uint32_t messageQueueDepth, + bool enableSetByDefault, float generatonFrequency); /** * Enable diagnostic printout. Please note that this feature will @@ -63,6 +64,8 @@ class InternalErrorReporter : public SystemObject, MutexIF* mutex = nullptr; MutexIF::TimeoutType timeoutType = MutexIF::TimeoutType::WAITING; uint32_t timeoutMs = 20; + bool enableSetByDefault; + float generationFrequency; sid_t internalErrorSid; InternalErrorDataset internalErrorDataset; From 2293e7f2bb31c4bf81ad8e41c358c94fc9a5f921 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 22 Jun 2023 16:12:41 +0200 Subject: [PATCH 403/404] typo --- src/fsfw/internalerror/InternalErrorReporter.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/internalerror/InternalErrorReporter.h b/src/fsfw/internalerror/InternalErrorReporter.h index 210f2cb1..f845410b 100644 --- a/src/fsfw/internalerror/InternalErrorReporter.h +++ b/src/fsfw/internalerror/InternalErrorReporter.h @@ -22,7 +22,7 @@ class InternalErrorReporter : public SystemObject, public HasLocalDataPoolIF { public: InternalErrorReporter(object_id_t setObjectId, uint32_t messageQueueDepth, - bool enableSetByDefault, float generatonFrequency); + bool enableSetByDefault, float generationFrequency); /** * Enable diagnostic printout. Please note that this feature will From c3572e31a8d6845442cf8e54802d94477b2db64e Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 25 Jun 2023 12:35:50 +0200 Subject: [PATCH 404/404] add API to set msg counter --- CHANGELOG.md | 2 +- src/fsfw/tmtcpacket/pus/tm/PusTmZcWriter.cpp | 6 ++++++ src/fsfw/tmtcpacket/pus/tm/PusTmZcWriter.h | 1 + 3 files changed, 8 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 487f3736..b4e9ab8e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,7 +10,6 @@ and this project adheres to [Semantic Versioning](http://semver.org/). ## Fixes - - Important bugfix in CFDP PDU header format: The entity length field and the transaction sequence number fields stored the actual length of the field instead of the length minus 1 like specified in the CFDP standard. @@ -26,6 +25,7 @@ and this project adheres to [Semantic Versioning](http://semver.org/). - add CFDP subsystem ID https://egit.irs.uni-stuttgart.de/fsfw/fsfw/pulls/742 +- `PusTmZcWriter` now exposes API to set message counter field. ## Changed - Bump ETL version to 20.35.14 diff --git a/src/fsfw/tmtcpacket/pus/tm/PusTmZcWriter.cpp b/src/fsfw/tmtcpacket/pus/tm/PusTmZcWriter.cpp index 6a9f6235..d897aff9 100644 --- a/src/fsfw/tmtcpacket/pus/tm/PusTmZcWriter.cpp +++ b/src/fsfw/tmtcpacket/pus/tm/PusTmZcWriter.cpp @@ -24,3 +24,9 @@ void PusTmZeroCopyWriter::updateErrorControl() { crcStart[0] = (crc16 >> 8) & 0xff; crcStart[1] = crc16 & 0xff; } + +void PusTmZeroCopyWriter::setMessageCount(uint16_t msgCount) { + size_t serSize = 0; + SerializeAdapter::serialize(&msgCount, const_cast(pointers.secHeaderStart + 3), + &serSize, 2, SerializeIF::Endianness::NETWORK); +} diff --git a/src/fsfw/tmtcpacket/pus/tm/PusTmZcWriter.h b/src/fsfw/tmtcpacket/pus/tm/PusTmZcWriter.h index 36c43f51..66ef1405 100644 --- a/src/fsfw/tmtcpacket/pus/tm/PusTmZcWriter.h +++ b/src/fsfw/tmtcpacket/pus/tm/PusTmZcWriter.h @@ -14,6 +14,7 @@ class PusTmZeroCopyWriter : public PusTmReader { PusTmZeroCopyWriter(TimeReaderIF& timeReader, uint8_t* data, size_t size); void setSequenceCount(uint16_t seqCount); + void setMessageCount(uint16_t msgCount); void updateErrorControl(); private: