From 517d52f55df804ef22fab9c7be6708e2560b180d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 16 Aug 2021 11:27:46 +0200 Subject: [PATCH 01/14] using better defines --- .../src/fsfw_tests/internal/InternalUnitTester.cpp | 9 ++++++--- tests/src/fsfw_tests/internal/InternalUnitTester.h | 1 + tests/src/fsfw_tests/internal/osal/IntTestMutex.cpp | 11 ++++++----- .../fsfw_tests/internal/osal/IntTestSemaphore.cpp | 13 +++++++------ 4 files changed, 20 insertions(+), 14 deletions(-) diff --git a/tests/src/fsfw_tests/internal/InternalUnitTester.cpp b/tests/src/fsfw_tests/internal/InternalUnitTester.cpp index f9fc1932..20998d64 100644 --- a/tests/src/fsfw_tests/internal/InternalUnitTester.cpp +++ b/tests/src/fsfw_tests/internal/InternalUnitTester.cpp @@ -16,15 +16,18 @@ InternalUnitTester::~InternalUnitTester() {} ReturnValue_t InternalUnitTester::performTests( const struct InternalUnitTester::TestConfig& testConfig) { #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::info << "Running internal unit tests.." << std::endl; + sif::info << "Running internal unit tests.. Error messages might follow" << + std::endl; #else sif::printInfo("Running internal unit tests..\n"); #endif testserialize::test_serialization(); testmq::testMq(); - testsemaph::testBinSemaph(); - testsemaph::testCountingSemaph(); + if(testConfig.testSemaphores) { + testsemaph::testBinSemaph(); + testsemaph::testCountingSemaph(); + } testmutex::testMutex(); if(testConfig.testArrayPrinter) { arrayprinter::testArrayPrinter(); diff --git a/tests/src/fsfw_tests/internal/InternalUnitTester.h b/tests/src/fsfw_tests/internal/InternalUnitTester.h index 50c89d77..433a0f1f 100644 --- a/tests/src/fsfw_tests/internal/InternalUnitTester.h +++ b/tests/src/fsfw_tests/internal/InternalUnitTester.h @@ -18,6 +18,7 @@ class InternalUnitTester: public HasReturnvaluesIF { public: struct TestConfig { bool testArrayPrinter = false; + bool testSemaphores = true; }; InternalUnitTester(); diff --git a/tests/src/fsfw_tests/internal/osal/IntTestMutex.cpp b/tests/src/fsfw_tests/internal/osal/IntTestMutex.cpp index b1699a46..d9184cd8 100644 --- a/tests/src/fsfw_tests/internal/osal/IntTestMutex.cpp +++ b/tests/src/fsfw_tests/internal/osal/IntTestMutex.cpp @@ -1,10 +1,12 @@ #include "fsfw_tests/internal/osal/IntTestMutex.h" #include "fsfw_tests/internal/UnittDefinitions.h" +#include "fsfw/platform.h" #include -#if defined(WIN32) || defined(UNIX) -#include +#if defined PLATFORM_WIN || defined PLATFORM_UNIX +#include "fsfw/osal/host/Mutex.h" + #include #include #endif @@ -20,7 +22,7 @@ void testmutex::testMutex() { // timed_mutex from the C++ library specifies undefined behaviour if // the timed mutex is locked twice from the same thread. // TODO: we should pass a define like FSFW_OSAL_HOST to the build. -#if defined(WIN32) || defined(UNIX) +#if defined PLATFORM_WIN || defined PLATFORM_UNIX // This calls the function from // another thread and stores the returnvalue in a future. auto future = std::async(&MutexIF::lockMutex, mutex, MutexIF::TimeoutType::WAITING, 1); @@ -37,8 +39,7 @@ void testmutex::testMutex() { unitt::put_error(id); } - // TODO: we should pass a define like FSFW_OSAL_HOST to the build. -#if !defined(WIN32) && !defined(UNIX) +#if !defined PLATFORM_WIN && !defined PLATFORM_UNIX result = mutex->unlockMutex(); if(result != MutexIF::CURR_THREAD_DOES_NOT_OWN_MUTEX) { unitt::put_error(id); diff --git a/tests/src/fsfw_tests/internal/osal/IntTestSemaphore.cpp b/tests/src/fsfw_tests/internal/osal/IntTestSemaphore.cpp index 8b79f33b..4b28f961 100644 --- a/tests/src/fsfw_tests/internal/osal/IntTestSemaphore.cpp +++ b/tests/src/fsfw_tests/internal/osal/IntTestSemaphore.cpp @@ -1,9 +1,10 @@ +#include "fsfw/FSFW.h" #include "fsfw_tests/internal/osal/IntTestSemaphore.h" #include "fsfw_tests/internal/UnittDefinitions.h" -#include -#include -#include +#include "fsfw/tasks/SemaphoreFactory.h" +#include "fsfw/serviceinterface/ServiceInterface.h" +#include "fsfw/timemanager/Stopwatch.h" #include @@ -16,7 +17,7 @@ void testsemaph::testBinSemaph() { } testBinSemaphoreImplementation(binSemaph, id); SemaphoreFactory::instance()->deleteSemaphore(binSemaph); -#if defined(freeRTOS) +#if defined FSFW_OSAL_FREERTOS SemaphoreIF* binSemaphUsingTask = SemaphoreFactory::instance()->createBinarySemaphore(1); testBinSemaphoreImplementation(binSemaphUsingTask, id); @@ -36,7 +37,7 @@ void testsemaph::testCountingSemaph() { } testBinSemaphoreImplementation(countingSemaph, id); SemaphoreFactory::instance()->deleteSemaphore(countingSemaph); -#if defined(freeRTOS) +#if defined FSFW_OSAL_FREERTOS countingSemaph = SemaphoreFactory::instance()-> createCountingSemaphore(1, 1, 1); testBinSemaphoreImplementation(countingSemaph, id); @@ -50,7 +51,7 @@ void testsemaph::testCountingSemaph() { createCountingSemaphore(3,3); testCountingSemaphImplementation(countingSemaph, id); SemaphoreFactory::instance()->deleteSemaphore(countingSemaph); -#if defined(freeRTOS) +#if defined FSFW_OSAL_FREERTOS countingSemaph = SemaphoreFactory::instance()-> createCountingSemaphore(3, 0, 1); uint8_t semaphCount = countingSemaph->getSemaphoreCounter(); From 21b5eaa891c58f3e52de3acbf138c3ec128a1789 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 15 Sep 2021 17:09:42 +0200 Subject: [PATCH 02/14] Some changes and improvements for DHB 1. Renamed getCommanderId to getCommanderQueueId. 2. Some indentation 3. Correct preprocessor define for warning printout used now --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 28 ++++++++++----- src/fsfw/devicehandlers/DeviceHandlerBase.h | 34 +++++++++---------- 2 files changed, 36 insertions(+), 26 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index c3fc023e..535113fd 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -461,7 +461,7 @@ size_t DeviceHandlerBase::getNextReplyLength(DeviceCommandId_t commandId){ return iter->second.replyLen; }else{ return 0; - } + } } ReturnValue_t DeviceHandlerBase::updateReplyMapEntry(DeviceCommandId_t deviceReply, @@ -612,15 +612,15 @@ void DeviceHandlerBase::replyToReply(const DeviceCommandId_t command, DeviceRepl } DeviceCommandInfo* info = &replyInfo.command->second; if (info == nullptr){ - printWarningOrError(sif::OutputTypes::OUT_ERROR, - "replyToReply", HasReturnvaluesIF::RETURN_FAILED, - "Command pointer not found"); - return; + printWarningOrError(sif::OutputTypes::OUT_ERROR, + "replyToReply", HasReturnvaluesIF::RETURN_FAILED, + "Command pointer not found"); + return; } if (info->expectedReplies > 0){ - // Check before to avoid underflow - info->expectedReplies--; + // Check before to avoid underflow + info->expectedReplies--; } // Check if more replies are expected. If so, do nothing. if (info->expectedReplies == 0) { @@ -1355,10 +1355,20 @@ void DeviceHandlerBase::buildInternalCommand(void) { DeviceCommandMap::iterator iter = deviceCommandMap.find( deviceCommandId); if (iter == deviceCommandMap.end()) { +#if FSFW_VERBOSE_LEVEL >= 1 + char output[36]; + sprintf(output, "Command 0x%08x unknown", + static_cast(deviceCommandId)); + // so we can track misconfigurations + printWarningOrError(sif::OutputTypes::OUT_WARNING, + "buildInternalCommand", + COMMAND_NOT_SUPPORTED, + output); +#endif result = COMMAND_NOT_SUPPORTED; } else if (iter->second.isExecuting) { -#if FSFW_DISABLE_PRINTOUT == 0 +#if FSFW_VERBOSE_LEVEL >= 1 char output[36]; sprintf(output, "Command 0x%08x is executing", static_cast(deviceCommandId)); @@ -1569,7 +1579,7 @@ LocalDataPoolManager* DeviceHandlerBase::getHkManagerHandle() { return &poolManager; } -MessageQueueId_t DeviceHandlerBase::getCommanderId(DeviceCommandId_t replyId) const { +MessageQueueId_t DeviceHandlerBase::getCommanderQueueId(DeviceCommandId_t replyId) const { auto commandIter = deviceCommandMap.find(replyId); if(commandIter == deviceCommandMap.end()) { return MessageQueueIF::NO_QUEUE; diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index 30d36ef0..b182b611 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -6,22 +6,22 @@ #include "DeviceHandlerFailureIsolation.h" #include "DeviceHandlerThermalSet.h" -#include "../serviceinterface/ServiceInterface.h" -#include "../serviceinterface/serviceInterfaceDefintions.h" -#include "../objectmanager/SystemObject.h" -#include "../tasks/ExecutableObjectIF.h" -#include "../returnvalues/HasReturnvaluesIF.h" -#include "../action/HasActionsIF.h" -#include "../datapool/PoolVariableIF.h" -#include "../modes/HasModesIF.h" -#include "../power/PowerSwitchIF.h" -#include "../ipc/MessageQueueIF.h" -#include "../tasks/PeriodicTaskIF.h" -#include "../action/ActionHelper.h" -#include "../health/HealthHelper.h" -#include "../parameters/ParameterHelper.h" -#include "../datapoollocal/HasLocalDataPoolIF.h" -#include "../datapoollocal/LocalDataPoolManager.h" +#include "fsfw/serviceinterface/ServiceInterface.h" +#include "fsfw/serviceinterface/serviceInterfaceDefintions.h" +#include "fsfw/objectmanager/SystemObject.h" +#include "fsfw/tasks/ExecutableObjectIF.h" +#include "fsfw/returnvalues/HasReturnvaluesIF.h" +#include "fsfw/action/HasActionsIF.h" +#include "fsfw/datapool/PoolVariableIF.h" +#include "fsfw/modes/HasModesIF.h" +#include "fsfw/power/PowerSwitchIF.h" +#include "fsfw/ipc/MessageQueueIF.h" +#include "fsfw/tasks/PeriodicTaskIF.h" +#include "fsfw/action/ActionHelper.h" +#include "fsfw/health/HealthHelper.h" +#include "fsfw/parameters/ParameterHelper.h" +#include "fsfw/datapoollocal/HasLocalDataPoolIF.h" +#include "fsfw/datapoollocal/LocalDataPoolManager.h" #include @@ -399,7 +399,7 @@ protected: */ virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) = 0; - MessageQueueId_t getCommanderId(DeviceCommandId_t replyId) const; + MessageQueueId_t getCommanderQueueId(DeviceCommandId_t replyId) const; /** * Helper function to get pending command. This is useful for devices * like SPI sensors to identify the last sent command. From e5db64cbb91bc72c63f0f3963bba9d89df9e1e6c Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 15 Sep 2021 17:15:18 +0200 Subject: [PATCH 03/14] set transfer size to 0, better name --- hal/src/fsfw_hal/linux/spi/SpiComIF.cpp | 8 ++------ hal/src/fsfw_hal/linux/spi/SpiComIF.h | 1 + hal/src/fsfw_hal/linux/spi/SpiCookie.cpp | 2 +- hal/src/fsfw_hal/linux/spi/SpiCookie.h | 6 ++---- 4 files changed, 6 insertions(+), 11 deletions(-) diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp index 48bf7449..6cf6675f 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -15,11 +15,6 @@ #include #include -/* Can be used for low-level debugging of the SPI bus */ -#ifndef FSFW_HAL_LINUX_SPI_WIRETAPPING -#define FSFW_HAL_LINUX_SPI_WIRETAPPING 0 -#endif - SpiComIF::SpiComIF(object_id_t objectId, GpioIF* gpioComIF): SystemObject(objectId), gpioComIF(gpioComIF) { if(gpioComIF == nullptr) { @@ -193,7 +188,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie *spiCookie, const spiCookie->getSpiParameters(spiMode, spiSpeed, nullptr); setSpiSpeedAndMode(fileDescriptor, spiMode, spiSpeed); spiCookie->assignWriteBuffer(sendData); - spiCookie->assignTransferSize(sendLen); + spiCookie->setTransferSize(sendLen); bool fullDuplex = spiCookie->isFullDuplex(); gpioId_t gpioId = spiCookie->getChipSelectPin(); @@ -335,6 +330,7 @@ ReturnValue_t SpiComIF::readReceivedMessage(CookieIF *cookie, uint8_t **buffer, *buffer = rxBuf; *size = spiCookie->getCurrentTransferSize(); + spiCookie->setTransferSize(0); return HasReturnvaluesIF::RETURN_OK; } diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.h b/hal/src/fsfw_hal/linux/spi/SpiComIF.h index d43e2505..bcca7462 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.h +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.h @@ -1,6 +1,7 @@ #ifndef LINUX_SPI_SPICOMIF_H_ #define LINUX_SPI_SPICOMIF_H_ +#include "fsfw/FSFW.h" #include "spiDefinitions.h" #include "returnvalues/classIds.h" #include "fsfw_hal/common/gpio/GpioIF.h" diff --git a/hal/src/fsfw_hal/linux/spi/SpiCookie.cpp b/hal/src/fsfw_hal/linux/spi/SpiCookie.cpp index 54d8aa16..f07954e9 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiCookie.cpp +++ b/hal/src/fsfw_hal/linux/spi/SpiCookie.cpp @@ -121,7 +121,7 @@ bool SpiCookie::isFullDuplex() const { return not this->halfDuplex; } -void SpiCookie::assignTransferSize(size_t transferSize) { +void SpiCookie::setTransferSize(size_t transferSize) { spiTransferStruct.len = transferSize; } diff --git a/hal/src/fsfw_hal/linux/spi/SpiCookie.h b/hal/src/fsfw_hal/linux/spi/SpiCookie.h index acf7c77c..844fd421 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiCookie.h +++ b/hal/src/fsfw_hal/linux/spi/SpiCookie.h @@ -103,10 +103,10 @@ public: void assignReadBuffer(uint8_t* rx); void assignWriteBuffer(const uint8_t* tx); /** - * Assign size for the next transfer. + * Set size for the next transfer. Set to 0 for no transfer * @param transferSize */ - void assignTransferSize(size_t transferSize); + void setTransferSize(size_t transferSize); size_t getCurrentTransferSize() const; struct UncommonParameters { @@ -158,8 +158,6 @@ private: std::string spiDev, const size_t maxSize, spi::SpiModes spiMode, uint32_t spiSpeed, spi::send_callback_function_t callback, void* args); - size_t currentTransferSize = 0; - address_t spiAddress; gpioId_t chipSelectPin; std::string spiDevice; From 665be0d417a04b7bea5a4e18f1434153b166e37a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Sep 2021 18:05:17 +0200 Subject: [PATCH 04/14] better name for wiretapping define --- hal/src/fsfw_hal/linux/spi/SpiComIF.cpp | 2 +- src/fsfw/FSFW.h.in | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp index 1fa87121..9c4e66ae 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -227,7 +227,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie *spiCookie, const utility::handleIoctlError("SpiComIF::sendMessage: ioctl error."); result = FULL_DUPLEX_TRANSFER_FAILED; } -#if FSFW_HAL_LINUX_SPI_WIRETAPPING == 1 +#if FSFW_HAL_SPI_WIRETAPPING == 1 performSpiWiretapping(spiCookie); #endif /* FSFW_LINUX_SPI_WIRETAPPING == 1 */ } diff --git a/src/fsfw/FSFW.h.in b/src/fsfw/FSFW.h.in index 3f005387..628cd521 100644 --- a/src/fsfw/FSFW.h.in +++ b/src/fsfw/FSFW.h.in @@ -20,9 +20,9 @@ #define FSFW_TCP_RECV_WIRETAPPING_ENABLED 0 #endif -/* Can be used for low-level debugging of the SPI bus */ -#ifndef FSFW_HAL_LINUX_SPI_WIRETAPPING -#define FSFW_HAL_LINUX_SPI_WIRETAPPING 0 +// Can be used for low-level debugging of the SPI bus +#ifndef FSFW_HAL_SPI_WIRETAPPING +#define FSFW_HAL_SPI_WIRETAPPING 0 #endif #ifndef FSFW_HAL_L3GD20_GYRO_DEBUG From f2bc374f0ffd349c3c02552885433735eb632f92 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Sep 2021 18:12:59 +0200 Subject: [PATCH 05/14] Device handler updates --- .../fsfw_hal/devicehandlers/CMakeLists.txt | 2 + .../devicehandlers/GyroL3GD20Handler.cpp | 43 ++++- .../devicehandlers/GyroL3GD20Handler.h | 21 ++- .../devicedefinitions/MgmLIS3HandlerDefs.h | 178 ++++++++++++++++++ .../devicedefinitions/MgmRM3100HandlerDefs.h | 132 +++++++++++++ 5 files changed, 364 insertions(+), 12 deletions(-) create mode 100644 hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h create mode 100644 hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h diff --git a/hal/src/fsfw_hal/devicehandlers/CMakeLists.txt b/hal/src/fsfw_hal/devicehandlers/CMakeLists.txt index cf542d8d..94e67c72 100644 --- a/hal/src/fsfw_hal/devicehandlers/CMakeLists.txt +++ b/hal/src/fsfw_hal/devicehandlers/CMakeLists.txt @@ -1,3 +1,5 @@ target_sources(${LIB_FSFW_NAME} PRIVATE GyroL3GD20Handler.cpp + MgmRM3100Handler.cpp + MgmLIS3MDLHandler.cpp ) diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp index 4a492e5d..70ba49eb 100644 --- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp @@ -3,11 +3,11 @@ #include "fsfw/datapool/PoolReadGuard.h" GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, - CookieIF *comCookie, uint8_t switchId, uint32_t transitionDelayMs): + CookieIF *comCookie, uint32_t transitionDelayMs): DeviceHandlerBase(objectId, deviceCommunication, comCookie), - switchId(switchId), transitionDelayMs(transitionDelayMs), dataset(this) { + transitionDelayMs(transitionDelayMs), dataset(this) { #if FSFW_HAL_L3GD20_GYRO_DEBUG == 1 - debugDivider = new PeriodicOperationDivider(5); + debugDivider = new PeriodicOperationDivider(3); #endif } @@ -215,11 +215,32 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, PoolReadGuard readSet(&dataset); if(readSet.getReadResult() == HasReturnvaluesIF::RETURN_OK) { - dataset.angVelocX = angVelocX; - dataset.angVelocY = angVelocY; - dataset.angVelocZ = angVelocZ; + if(std::abs(angVelocX) < this->absLimitX) { + dataset.angVelocX = angVelocX; + dataset.angVelocX.setValid(true); + } + else { + dataset.angVelocX.setValid(false); + } + + if(std::abs(angVelocY) < this->absLimitY) { + dataset.angVelocY = angVelocY; + dataset.angVelocY.setValid(true); + } + else { + dataset.angVelocY.setValid(false); + } + + if(std::abs(angVelocZ) < this->absLimitZ) { + dataset.angVelocZ = angVelocZ; + dataset.angVelocZ.setValid(true); + } + else { + dataset.angVelocZ.setValid(false); + } + dataset.temperature = temperature; - dataset.setValidity(true, true); + dataset.temperature.setValid(true); } break; } @@ -234,7 +255,7 @@ uint32_t GyroHandlerL3GD20H::getTransitionDelayMs(Mode_t from, Mode_t to) { return this->transitionDelayMs; } -void GyroHandlerL3GD20H::setGoNormalModeAtStartup() { +void GyroHandlerL3GD20H::setToGoToNormalMode(bool enable) { this->goNormalModeImmediately = true; } @@ -256,3 +277,9 @@ void GyroHandlerL3GD20H::fillCommandAndReplyMap() { void GyroHandlerL3GD20H::modeChanged() { internalState = InternalState::NONE; } + +void GyroHandlerL3GD20H::setAbsoluteLimits(float limitX, float limitY, float limitZ) { + this->absLimitX = limitX; + this->absLimitY = limitY; + this->absLimitZ = limitZ; +} diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h index bc1d9c1c..870f551d 100644 --- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h +++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h @@ -19,13 +19,22 @@ class GyroHandlerL3GD20H: public DeviceHandlerBase { public: GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, - CookieIF* comCookie, uint8_t switchId, uint32_t transitionDelayMs = 10000); + CookieIF* comCookie, uint32_t transitionDelayMs); virtual ~GyroHandlerL3GD20H(); + /** + * 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 + * @param xLimit + * @param yLimit + * @param zLimit + */ + void setAbsoluteLimits(float limitX, float limitY, float limitZ); + /** * @brief Configure device handler to go to normal mode immediately */ - void setGoNormalModeAtStartup(); + void setToGoToNormalMode(bool enable); protected: /* DeviceHandlerBase overrides */ @@ -40,12 +49,12 @@ protected: size_t commandDataLen) override; ReturnValue_t scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) override; - ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, + virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; void fillCommandAndReplyMap() override; void modeChanged() override; - uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) override; @@ -54,6 +63,10 @@ private: uint32_t transitionDelayMs = 0; GyroPrimaryDataset dataset; + float absLimitX = L3GD20H::RANGE_DPS_00; + float absLimitY = L3GD20H::RANGE_DPS_00; + float absLimitZ = L3GD20H::RANGE_DPS_00; + enum class InternalState { NONE, CONFIGURE, diff --git a/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h b/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h new file mode 100644 index 00000000..b6f3fd84 --- /dev/null +++ b/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h @@ -0,0 +1,178 @@ +#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_MGMLIS3HANDLERDEFS_H_ +#define MISSION_DEVICES_DEVICEDEFINITIONS_MGMLIS3HANDLERDEFS_H_ + +#include +#include +#include +#include + +namespace MGMLIS3MDL { + +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 +}; + +/* Actually 15, we just round up a bit */ +static constexpr size_t MAX_BUFFER_SIZE = 16; + +/* Field data register scaling */ +static constexpr uint8_t GAUSS_TO_MICROTESLA_FACTOR = 100; +static constexpr float FIELD_LSB_PER_GAUSS_4_SENS = 1.0 / 6842.0; +static constexpr float FIELD_LSB_PER_GAUSS_8_SENS = 1.0 / 3421.0; +static constexpr float FIELD_LSB_PER_GAUSS_12_SENS = 1.0 / 2281.0; +static constexpr float FIELD_LSB_PER_GAUSS_16_SENS = 1.0 / 1711.0; + +static const DeviceCommandId_t READ_CONFIG_AND_DATA = 0x00; +static const DeviceCommandId_t SETUP_MGM = 0x01; +static const DeviceCommandId_t READ_TEMPERATURE = 0x02; +static const DeviceCommandId_t IDENTIFY_DEVICE = 0x03; +static const DeviceCommandId_t TEMP_SENSOR_ENABLE = 0x04; +static const DeviceCommandId_t ACCURACY_OP_MODE_SET = 0x05; + +/* Number of all control registers */ +static const uint8_t NR_OF_CTRL_REGISTERS = 5; +/* Number of registers in the MGM */ +static const uint8_t NR_OF_REGISTERS = 19; +/* Total number of adresses for all registers */ +static const uint8_t TOTAL_NR_OF_ADRESSES = 52; +static const uint8_t NR_OF_DATA_AND_CFG_REGISTERS = 14; +static const uint8_t TEMPERATURE_REPLY_LEN = 3; +static const uint8_t SETUP_REPLY_LEN = 6; + +/*------------------------------------------------------------------------*/ +/* Register adresses */ +/*------------------------------------------------------------------------*/ +/* Register adress returns identifier of device with default 0b00111101 */ +static const uint8_t IDENTIFY_DEVICE_REG_ADDR = 0b00001111; +static const uint8_t DEVICE_ID = 0b00111101; // Identifier for Device + +/* Register adress to access register 1 */ +static const uint8_t CTRL_REG1 = 0b00100000; +/* Register adress to access register 2 */ +static const uint8_t CTRL_REG2 = 0b00100001; +/* Register adress to access register 3 */ +static const uint8_t CTRL_REG3 = 0b00100010; +/* Register adress to access register 4 */ +static const uint8_t CTRL_REG4 = 0b00100011; +/* Register adress to access register 5 */ +static const uint8_t CTRL_REG5 = 0b00100100; + +/* Register adress to access status register */ +static const uint8_t STATUS_REG_IDX = 8; +static const uint8_t STATUS_REG = 0b00100111; + +/* Register adress to access low byte of x-axis */ +static const uint8_t X_LOWBYTE_IDX = 9; +static const uint8_t X_LOWBYTE = 0b00101000; +/* Register adress to access high byte of x-axis */ +static const uint8_t X_HIGHBYTE_IDX = 10; +static const uint8_t X_HIGHBYTE = 0b00101001; +/* Register adress to access low byte of y-axis */ +static const uint8_t Y_LOWBYTE_IDX = 11; +static const uint8_t Y_LOWBYTE = 0b00101010; +/* Register adress to access high byte of y-axis */ +static const uint8_t Y_HIGHBYTE_IDX = 12; +static const uint8_t Y_HIGHBYTE = 0b00101011; +/* Register adress to access low byte of z-axis */ +static const uint8_t Z_LOWBYTE_IDX = 13; +static const uint8_t Z_LOWBYTE = 0b00101100; +/* Register adress to access high byte of z-axis */ +static const uint8_t Z_HIGHBYTE_IDX = 14; +static const uint8_t Z_HIGHBYTE = 0b00101101; + +/* Register adress to access low byte of temperature sensor */ +static const uint8_t TEMP_LOWBYTE = 0b00101110; +/* Register adress to access high byte of temperature sensor */ +static const uint8_t TEMP_HIGHBYTE = 0b00101111; + +/*------------------------------------------------------------------------*/ +/* Initialize Setup Register set bits */ +/*------------------------------------------------------------------------*/ +/* General transfer bits */ +// Read=1 / Write=0 Bit +static const uint8_t RW_BIT = 7; +// Continous Read/Write Bit, increment adress +static const uint8_t MS_BIT = 6; + +/* CTRL_REG1 bits */ +static const uint8_t ST = 0; // Self test enable bit, enabled = 1 +// Enable rates higher than 80 Hz enabled = 1 +static const uint8_t FAST_ODR = 1; +static const uint8_t DO0 = 2; // Output data rate bit 2 +static const uint8_t DO1 = 3; // Output data rate bit 3 +static const uint8_t DO2 = 4; // Output data rate bit 4 +static const uint8_t OM0 = 5; // XY operating mode bit 5 +static const uint8_t OM1 = 6; // XY operating mode bit 6 +static const uint8_t TEMP_EN = 7; // Temperature sensor enable enabled = 1 +static const uint8_t CTRL_REG1_DEFAULT = (1 << TEMP_EN) | (1 << OM1) | + (1 << DO0) | (1 << DO1) | (1 << DO2); + +/* CTRL_REG2 bits */ +//reset configuration registers and user registers +static const uint8_t SOFT_RST = 2; +static const uint8_t REBOOT = 3; //reboot memory content +static const uint8_t FSO = 5; //full-scale selection bit 5 +static const uint8_t FS1 = 6; //full-scale selection bit 6 +static const uint8_t CTRL_REG2_DEFAULT = 0; + +/* CTRL_REG3 bits */ +static const uint8_t MD0 = 0; //Operating mode bit 0 +static const uint8_t MD1 = 1; //Operating mode bit 1 +//SPI serial interface mode selection enabled = 3-wire-mode +static const uint8_t SIM = 2; +static const uint8_t LP = 5; //low-power mode +static const uint8_t CTRL_REG3_DEFAULT = 0; + +/* CTRL_REG4 bits */ +//big/little endian data selection enabled = MSb at lower adress +static const uint8_t BLE = 1; +static const uint8_t OMZ0 = 2; //Z operating mode bit 2 +static const uint8_t OMZ1 = 3; //Z operating mode bit 3 +static const uint8_t CTRL_REG4_DEFAULT = (1 << OMZ1); + +/* CTRL_REG5 bits */ +static const uint8_t BDU = 6; //Block data update +static const uint8_t FAST_READ = 7; //Fast read enabled = 1 +static const uint8_t CTRL_REG5_DEFAULT = 0; + +static const uint32_t MGM_DATA_SET_ID = READ_CONFIG_AND_DATA; + +enum MgmPoolIds: lp_id_t { + FIELD_STRENGTH_X, + FIELD_STRENGTH_Y, + FIELD_STRENGTH_Z, + TEMPERATURE_CELCIUS +}; + +class MgmPrimaryDataset: public StaticLocalDataSet<5> { +public: + MgmPrimaryDataset(HasLocalDataPoolIF* hkOwner): + StaticLocalDataSet(hkOwner, MGM_DATA_SET_ID) {} + + MgmPrimaryDataset(object_id_t mgmId): + StaticLocalDataSet(sid_t(mgmId, MGM_DATA_SET_ID)) {} + + lp_var_t fieldStrengthX = lp_var_t(sid.objectId, + FIELD_STRENGTH_X, this); + lp_var_t fieldStrengthY = lp_var_t(sid.objectId, + FIELD_STRENGTH_Y, this); + lp_var_t fieldStrengthZ = lp_var_t(sid.objectId, + FIELD_STRENGTH_Z, this); + lp_var_t temperature = lp_var_t(sid.objectId, + TEMPERATURE_CELCIUS, this); +}; + +} + + +#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_MGMLIS3HANDLERDEFS_H_ */ diff --git a/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h b/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h new file mode 100644 index 00000000..08f80dd9 --- /dev/null +++ b/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h @@ -0,0 +1,132 @@ +#ifndef MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ +#define MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ + +#include +#include +#include +#include +#include + +namespace RM3100 { + +/* Actually 10, we round up a little bit */ +static constexpr size_t MAX_BUFFER_SIZE = 12; + +static constexpr uint8_t READ_MASK = 0x80; + +/*----------------------------------------------------------------------------*/ +/* CMM Register */ +/*----------------------------------------------------------------------------*/ +static constexpr uint8_t SET_CMM_CMZ = 1 << 6; +static constexpr uint8_t SET_CMM_CMY = 1 << 5; +static constexpr uint8_t SET_CMM_CMX = 1 << 4; +static constexpr uint8_t SET_CMM_DRDM = 1 << 2; +static constexpr uint8_t SET_CMM_START = 1; +static constexpr uint8_t CMM_REGISTER = 0x01; + +static constexpr uint8_t CMM_VALUE = SET_CMM_CMZ | SET_CMM_CMY | SET_CMM_CMX | + SET_CMM_DRDM | SET_CMM_START; + +/*----------------------------------------------------------------------------*/ +/* Cycle count register */ +/*----------------------------------------------------------------------------*/ +// Default value (200) +static constexpr uint8_t CYCLE_COUNT_VALUE = 0xC8; + +static constexpr float DEFAULT_GAIN = static_cast(CYCLE_COUNT_VALUE) / + 100 * 38; +static constexpr uint8_t CYCLE_COUNT_START_REGISTER = 0x04; + +/*----------------------------------------------------------------------------*/ +/* TMRC register */ +/*----------------------------------------------------------------------------*/ +static constexpr uint8_t TMRC_150HZ_VALUE = 0x94; +static constexpr uint8_t TMRC_75HZ_VALUE = 0x95; +static constexpr uint8_t TMRC_DEFAULT_37HZ_VALUE = 0x96; + +static constexpr uint8_t TMRC_REGISTER = 0x0B; +static constexpr uint8_t TMRC_DEFAULT_VALUE = TMRC_DEFAULT_37HZ_VALUE; + +static constexpr uint8_t MEASUREMENT_REG_START = 0x24; +static constexpr uint8_t BIST_REGISTER = 0x33; +static constexpr uint8_t DATA_READY_VAL = 0b1000'0000; +static constexpr uint8_t STATUS_REGISTER = 0x34; +static constexpr uint8_t REVID_REGISTER = 0x36; + +// Range in Microtesla. 1 T equals 10000 Gauss (for comparison with LIS3 MGM) +static constexpr uint16_t RANGE = 800; + +static constexpr DeviceCommandId_t READ_DATA = 0; + +static constexpr DeviceCommandId_t CONFIGURE_CMM = 1; +static constexpr DeviceCommandId_t READ_CMM = 2; + +static constexpr DeviceCommandId_t CONFIGURE_TMRC = 3; +static constexpr DeviceCommandId_t READ_TMRC = 4; + +static constexpr DeviceCommandId_t CONFIGURE_CYCLE_COUNT = 5; +static constexpr DeviceCommandId_t READ_CYCLE_COUNT = 6; + +class CycleCountCommand: public SerialLinkedListAdapter { +public: + CycleCountCommand(bool oneCycleCount = true): oneCycleCount(oneCycleCount) { + setLinks(oneCycleCount); + } + + ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size, + Endianness streamEndianness) override { + ReturnValue_t result = SerialLinkedListAdapter::deSerialize(buffer, + size, streamEndianness); + if(oneCycleCount) { + cycleCountY = cycleCountX; + cycleCountZ = cycleCountX; + } + return result; + } + + SerializeElement cycleCountX; + SerializeElement cycleCountY; + SerializeElement cycleCountZ; + +private: + void setLinks(bool oneCycleCount) { + setStart(&cycleCountX); + if(not oneCycleCount) { + cycleCountX.setNext(&cycleCountY); + cycleCountY.setNext(&cycleCountZ); + } + } + + bool oneCycleCount; +}; + +static constexpr uint32_t MGM_DATASET_ID = READ_DATA; + +enum MgmPoolIds: lp_id_t { + FIELD_STRENGTH_X, + FIELD_STRENGTH_Y, + FIELD_STRENGTH_Z, +}; + +class Rm3100PrimaryDataset: public StaticLocalDataSet<3 * sizeof(float)> { +public: + Rm3100PrimaryDataset(HasLocalDataPoolIF* hkOwner): + StaticLocalDataSet(hkOwner, MGM_DATASET_ID) {} + + Rm3100PrimaryDataset(object_id_t mgmId): + StaticLocalDataSet(sid_t(mgmId, MGM_DATASET_ID)) {} + + // Field strengths in micro Tesla. + lp_var_t fieldStrengthX = lp_var_t(sid.objectId, + FIELD_STRENGTH_X, this); + lp_var_t fieldStrengthY = lp_var_t(sid.objectId, + FIELD_STRENGTH_Y, this); + lp_var_t fieldStrengthZ = lp_var_t(sid.objectId, + FIELD_STRENGTH_Z, this); +}; + +} + + + +#endif /* MISSION_DEVICES_DEVICEDEFINITIONS_MGMHANDLERRM3100DEFINITIONS_H_ */ From a6bd7c0d6ea035c5acb38139ed9bac78228ab1ac Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Sep 2021 18:13:51 +0200 Subject: [PATCH 06/14] added missing defines for debug output --- src/fsfw/FSFW.h.in | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/fsfw/FSFW.h.in b/src/fsfw/FSFW.h.in index f0eb9365..f124e647 100644 --- a/src/fsfw/FSFW.h.in +++ b/src/fsfw/FSFW.h.in @@ -17,7 +17,15 @@ #cmakedefine FSFW_ADD_SGP4_PROPAGATOR #ifndef FSFW_HAL_L3GD20_GYRO_DEBUG -#define FSFW_HAL_L3GD20_GYRO_DEBUG 0 +#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 e0671a395ebea9d2297b2096293db80a2e412f4a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Sep 2021 18:14:40 +0200 Subject: [PATCH 07/14] indentation --- src/fsfw/FSFW.h.in | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/fsfw/FSFW.h.in b/src/fsfw/FSFW.h.in index 628cd521..12703add 100644 --- a/src/fsfw/FSFW.h.in +++ b/src/fsfw/FSFW.h.in @@ -22,7 +22,7 @@ // Can be used for low-level debugging of the SPI bus #ifndef FSFW_HAL_SPI_WIRETAPPING -#define FSFW_HAL_SPI_WIRETAPPING 0 +#define FSFW_HAL_SPI_WIRETAPPING 0 #endif #ifndef FSFW_HAL_L3GD20_GYRO_DEBUG @@ -34,7 +34,6 @@ #endif /* FSFW_HAL_RM3100_MGM_DEBUG */ #ifndef FSFW_HAL_LIS3MDL_MGM_DEBUG -#define FSFW_HAL_LIS3MDL_MGM_DEBUG 0 +#define FSFW_HAL_LIS3MDL_MGM_DEBUG 0 #endif /* FSFW_HAL_LIS3MDL_MGM_DEBUG */ - #endif /* FSFW_FSFW_H_ */ From f6b03dee6ab7755df53d415fe75802a30fd2c435 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Fri, 24 Sep 2021 12:11:12 +0200 Subject: [PATCH 08/14] removed unused variable switchId from GyroL3GD20Handler class --- hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h | 1 - 1 file changed, 1 deletion(-) diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h index 870f551d..e73d2fbb 100644 --- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h +++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h @@ -59,7 +59,6 @@ protected: LocalDataPoolManager &poolManager) override; private: - uint8_t switchId = 0; uint32_t transitionDelayMs = 0; GyroPrimaryDataset dataset; From a37b6184fcddf7792aaa5b70b5882a6d838a65ab Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 25 Sep 2021 16:40:22 +0200 Subject: [PATCH 09/14] fix dataset sizes --- .../devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h | 2 +- .../devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h b/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h index b6f3fd84..9d65aae2 100644 --- a/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h +++ b/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmLIS3HandlerDefs.h @@ -154,7 +154,7 @@ enum MgmPoolIds: lp_id_t { TEMPERATURE_CELCIUS }; -class MgmPrimaryDataset: public StaticLocalDataSet<5> { +class MgmPrimaryDataset: public StaticLocalDataSet<4> { public: MgmPrimaryDataset(HasLocalDataPoolIF* hkOwner): StaticLocalDataSet(hkOwner, MGM_DATA_SET_ID) {} diff --git a/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h b/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h index 08f80dd9..0ee2c7f6 100644 --- a/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h +++ b/hal/src/fsfw_hal/devicehandlers/devicedefinitions/MgmRM3100HandlerDefs.h @@ -108,7 +108,7 @@ enum MgmPoolIds: lp_id_t { FIELD_STRENGTH_Z, }; -class Rm3100PrimaryDataset: public StaticLocalDataSet<3 * sizeof(float)> { +class Rm3100PrimaryDataset: public StaticLocalDataSet<3> { public: Rm3100PrimaryDataset(HasLocalDataPoolIF* hkOwner): StaticLocalDataSet(hkOwner, MGM_DATASET_ID) {} From a84e60a37a6fef4147837bd6732d74722d4708c1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 26 Sep 2021 22:22:55 +0200 Subject: [PATCH 10/14] Added missing devicehandlers These devicehandlers were missing from the last PR --- .../devicehandlers/MgmLIS3MDLHandler.cpp | 518 ++++++++++++++++++ .../devicehandlers/MgmLIS3MDLHandler.h | 186 +++++++ .../devicehandlers/MgmRM3100Handler.cpp | 363 ++++++++++++ .../devicehandlers/MgmRM3100Handler.h | 111 ++++ 4 files changed, 1178 insertions(+) create mode 100644 hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp create mode 100644 hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h create mode 100644 hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp create mode 100644 hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h diff --git a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp new file mode 100644 index 00000000..891d6fdc --- /dev/null +++ b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp @@ -0,0 +1,518 @@ +#include "MgmLIS3MDLHandler.h" + +#include "fsfw/datapool/PoolReadGuard.h" +#if FSFW_HAL_LIS3MDL_MGM_DEBUG == 1 +#include "fsfw/globalfunctions/PeriodicOperationDivider.h" +#endif + +MgmLIS3MDLHandler::MgmLIS3MDLHandler(object_id_t objectId, object_id_t deviceCommunication, + CookieIF* comCookie, uint32_t transitionDelay): + DeviceHandlerBase(objectId, deviceCommunication, comCookie), + dataset(this), transitionDelay(transitionDelay) { +#if FSFW_HAL_LIS3MDL_MGM_DEBUG == 1 + debugDivider = new PeriodicOperationDivider(3); +#endif + // Set to default values right away + registers[0] = MGMLIS3MDL::CTRL_REG1_DEFAULT; + registers[1] = MGMLIS3MDL::CTRL_REG2_DEFAULT; + registers[2] = MGMLIS3MDL::CTRL_REG3_DEFAULT; + registers[3] = MGMLIS3MDL::CTRL_REG4_DEFAULT; + registers[4] = MGMLIS3MDL::CTRL_REG5_DEFAULT; + +} + +MgmLIS3MDLHandler::~MgmLIS3MDLHandler() { +} + + +void MgmLIS3MDLHandler::doStartUp() { + switch (internalState) { + case(InternalState::STATE_NONE): { + internalState = InternalState::STATE_FIRST_CONTACT; + break; + } + case(InternalState::STATE_FIRST_CONTACT): { + /* Will be set by checking device ID (WHO AM I register) */ + if(commandExecuted) { + commandExecuted = false; + internalState = InternalState::STATE_SETUP; + } + break; + } + case(InternalState::STATE_SETUP): { + internalState = InternalState::STATE_CHECK_REGISTERS; + break; + } + case(InternalState::STATE_CHECK_REGISTERS): { + /* Set up cached registers which will be used to configure the MGM. */ + if(commandExecuted) { + commandExecuted = false; + if(goToNormalMode) { + setMode(MODE_NORMAL); + } + else { + setMode(_MODE_TO_ON); + } + } + break; + } + default: + break; + } + +} + +void MgmLIS3MDLHandler::doShutDown() { + setMode(_MODE_POWER_DOWN); +} + +ReturnValue_t MgmLIS3MDLHandler::buildTransitionDeviceCommand( + DeviceCommandId_t *id) { + switch (internalState) { + case(InternalState::STATE_NONE): + case(InternalState::STATE_NORMAL): { + return HasReturnvaluesIF::RETURN_OK; + } + case(InternalState::STATE_FIRST_CONTACT): { + *id = MGMLIS3MDL::IDENTIFY_DEVICE; + break; + } + case(InternalState::STATE_SETUP): { + *id = MGMLIS3MDL::SETUP_MGM; + break; + } + case(InternalState::STATE_CHECK_REGISTERS): { + *id = MGMLIS3MDL::READ_CONFIG_AND_DATA; + break; + } + default: { + /* might be a configuration error. */ +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "GyroHandler::buildTransitionDeviceCommand: Unknown internal state!" << + std::endl; +#else + sif::printWarning("GyroHandler::buildTransitionDeviceCommand: Unknown internal state!\n"); +#endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ + return HasReturnvaluesIF::RETURN_OK; + } + + } + 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; + + prepareCtrlRegisterWrite(); +} + +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; + communicationStep = CommunicationStep::TEMPERATURE; + return buildCommandFromCommand(*id, NULL, 0); + } + else { + *id = MGMLIS3MDL::READ_TEMPERATURE; + communicationStep = CommunicationStep::DATA; + return buildCommandFromCommand(*id, NULL, 0); + } + +} + +ReturnValue_t MgmLIS3MDLHandler::buildCommandFromCommand( + DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) { + switch(deviceCommand) { + case(MGMLIS3MDL::READ_CONFIG_AND_DATA): { + std::memset(commandBuffer, 0, sizeof(commandBuffer)); + commandBuffer[0] = readCommand(MGMLIS3MDL::CTRL_REG1, true); + + rawPacket = commandBuffer; + rawPacketLen = MGMLIS3MDL::NR_OF_DATA_AND_CFG_REGISTERS + 1; + return RETURN_OK; + } + case(MGMLIS3MDL::READ_TEMPERATURE): { + std::memset(commandBuffer, 0, 3); + commandBuffer[0] = readCommand(MGMLIS3MDL::TEMP_LOWBYTE, true); + + rawPacket = commandBuffer; + rawPacketLen = 3; + return RETURN_OK; + } + case(MGMLIS3MDL::IDENTIFY_DEVICE): { + return identifyDevice(); + } + case(MGMLIS3MDL::TEMP_SENSOR_ENABLE): { + return enableTemperatureSensor(commandData, commandDataLen); + } + case(MGMLIS3MDL::SETUP_MGM): { + setupMgm(); + return HasReturnvaluesIF::RETURN_OK; + } + case(MGMLIS3MDL::ACCURACY_OP_MODE_SET): { + return setOperatingMode(commandData, commandDataLen); + } + default: + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } + return HasReturnvaluesIF::RETURN_FAILED; +} + +ReturnValue_t MgmLIS3MDLHandler::identifyDevice() { + uint32_t size = 2; + commandBuffer[0] = readCommand(MGMLIS3MDL::IDENTIFY_DEVICE_REG_ADDR); + commandBuffer[1] = 0x00; + + rawPacket = commandBuffer; + rawPacketLen = size; + + return RETURN_OK; +} + +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) { + *foundLen = len; + *foundId = MGMLIS3MDL::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]) { +#if FSFW_VERBOSE_LEVEL >= 1 +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "MGMHandlerLIS3MDL::scanForReply: Invalid registers!" << std::endl; +#else + sif::printWarning("MGMHandlerLIS3MDL::scanForReply: Invalid registers!\n"); +#endif +#endif + return DeviceHandlerIF::INVALID_DATA; + } + if(mode == _MODE_START_UP) { + commandExecuted = true; + } + + } + else if(len == MGMLIS3MDL::TEMPERATURE_REPLY_LEN) { + *foundLen = len; + *foundId = MGMLIS3MDL::READ_TEMPERATURE; + } + else if (len == MGMLIS3MDL::SETUP_REPLY_LEN) { + *foundLen = len; + *foundId = MGMLIS3MDL::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 FSFW_VERBOSE_LEVEL >= 1 +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "MGMHandlerLIS3MDL::scanForReply: " + "Device identification failed!" << std::endl; +#else + sif::printWarning("MGMHandlerLIS3MDL::scanForReply: " + "Device identification failed!\n"); +#endif +#endif + return DeviceHandlerIF::INVALID_DATA; + } + + if(mode == _MODE_START_UP) { + commandExecuted = true; + } + } + } + else { + return DeviceHandlerIF::INVALID_DATA; + } + + /* Data with SPI Interface always has this answer */ + if (start[0] == 0b11111111) { + return RETURN_OK; + } + else { + return DeviceHandlerIF::INVALID_DATA; + } + +} +ReturnValue_t MgmLIS3MDLHandler::interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) { + + switch (id) { + case MGMLIS3MDL::IDENTIFY_DEVICE: { + break; + } + case MGMLIS3MDL::SETUP_MGM: { + break; + } + case MGMLIS3MDL::READ_CONFIG_AND_DATA: { + // 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] ; + int16_t mgmMeasurementRawY = packet[MGMLIS3MDL::Y_HIGHBYTE_IDX] << 8 + | packet[MGMLIS3MDL::Y_LOWBYTE_IDX] ; + int16_t mgmMeasurementRawZ = packet[MGMLIS3MDL::Z_HIGHBYTE_IDX] << 8 + | packet[MGMLIS3MDL::Z_LOWBYTE_IDX] ; + + /* Target value in microtesla */ + float mgmX = static_cast(mgmMeasurementRawX) * sensitivityFactor + * MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR; + float mgmY = static_cast(mgmMeasurementRawY) * sensitivityFactor + * MGMLIS3MDL::GAUSS_TO_MICROTESLA_FACTOR; + 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 */ + } +#endif /* OBSW_VERBOSE_LEVEL >= 1 */ + PoolReadGuard readHelper(&dataset); + if(readHelper.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + if(std::abs(mgmX) < absLimitX) { + dataset.fieldStrengthX = mgmX; + dataset.fieldStrengthX.setValid(true); + } + else { + dataset.fieldStrengthX.setValid(false); + } + + if(std::abs(mgmY) < absLimitY) { + dataset.fieldStrengthY = mgmY; + dataset.fieldStrengthY.setValid(true); + } + else { + dataset.fieldStrengthY.setValid(false); + } + + if(std::abs(mgmZ) < absLimitZ) { + dataset.fieldStrengthZ = mgmZ; + dataset.fieldStrengthZ.setValid(true); + } + else { + dataset.fieldStrengthZ.setValid(false); + } + } + break; + } + + 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 + } +#endif + ReturnValue_t result = dataset.read(); + if(result == HasReturnvaluesIF::RETURN_OK) { + dataset.temperature = tempValue; + dataset.commit(); + } + break; + } + + default: { + return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; + } + + } + return RETURN_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) { + 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) { + case (MGMLIS3MDL::ON): { + commandBuffer[1] = registers[0] | (1 << 7); + break; + } + case (MGMLIS3MDL::OFF): { + commandBuffer[1] = registers[0] & ~(1 << 7); + break; + } + default: + return INVALID_COMMAND_PARAMETER; + } + registers[0] = commandBuffer[1]; + + rawPacket = commandBuffer; + rawPacketLen = size; + + return RETURN_OK; +} + +ReturnValue_t MgmLIS3MDLHandler::setOperatingMode(const uint8_t *commandData, + size_t commandDataLen) { + triggerEvent(CHANGE_OF_SETUP_PARAMETER); + if (commandDataLen != 1) { + return INVALID_NUMBER_OR_LENGTH_OF_PARAMETERS; + } + + 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)); + break; + case MGMLIS3MDL::MEDIUM: + registers[0] = (registers[0] & (~(1 << MGMLIS3MDL::OM1))) | (1 << MGMLIS3MDL::OM0); + registers[3] = (registers[3] & (~(1 << MGMLIS3MDL::OMZ1))) | (1 << MGMLIS3MDL::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)); + break; + + case MGMLIS3MDL::ULTRA: + registers[0] = (registers[0] | (1 << MGMLIS3MDL::OM1)) | (1 << MGMLIS3MDL::OM0); + registers[3] = (registers[3] | (1 << MGMLIS3MDL::OMZ1)) | (1 << MGMLIS3MDL::OMZ0); + break; + default: + break; + } + + return prepareCtrlRegisterWrite(); +} + +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); +} + +void MgmLIS3MDLHandler::setToGoToNormalMode(bool enable) { + this->goToNormalMode = enable; +} + +ReturnValue_t MgmLIS3MDLHandler::prepareCtrlRegisterWrite() { + commandBuffer[0] = writeCommand(MGMLIS3MDL::CTRL_REG1, true); + + for (size_t i = 0; i < MGMLIS3MDL::NR_OF_CTRL_REGISTERS; i++) { + commandBuffer[i + 1] = registers[i]; + } + rawPacket = commandBuffer; + rawPacketLen = MGMLIS3MDL::NR_OF_CTRL_REGISTERS + 1; + + // We dont have to check if this is working because we just did i + return RETURN_OK; +} + +void MgmLIS3MDLHandler::doTransition(Mode_t modeFrom, Submode_t subModeFrom) { + +} + +uint32_t MgmLIS3MDLHandler::getTransitionDelayMs(Mode_t from, Mode_t to) { + return transitionDelay; +} + +void MgmLIS3MDLHandler::modeChanged(void) { + internalState = InternalState::STATE_NONE; +} + +ReturnValue_t MgmLIS3MDLHandler::initializeLocalDataPool( + localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + localDataPoolMap.emplace(MGMLIS3MDL::FIELD_STRENGTH_X, + new PoolEntry({0.0})); + 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})); + return HasReturnvaluesIF::RETURN_OK; +} + +void MgmLIS3MDLHandler::setAbsoluteLimits(float xLimit, float yLimit, float zLimit) { + this->absLimitX = xLimit; + this->absLimitY = yLimit; + this->absLimitZ = zLimit; +} diff --git a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h new file mode 100644 index 00000000..6bf89a49 --- /dev/null +++ b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.h @@ -0,0 +1,186 @@ +#ifndef MISSION_DEVICES_MGMLIS3MDLHANDLER_H_ +#define MISSION_DEVICES_MGMLIS3MDLHANDLER_H_ + +#include "fsfw/FSFW.h" +#include "events/subsystemIdRanges.h" +#include "devicedefinitions/MgmLIS3HandlerDefs.h" + +#include "fsfw/devicehandlers/DeviceHandlerBase.h" + +class PeriodicOperationDivider; + +/** + * @brief Device handler object for the LIS3MDL 3-axis magnetometer + * by STMicroeletronics + * @details + * Datasheet can be found online by googling LIS3MDL. + * Flight manual: + * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/LIS3MDL_MGM + * @author L. Loidold, R. Mueller + */ +class MgmLIS3MDLHandler: public DeviceHandlerBase { +public: + enum class CommunicationStep { + DATA, + TEMPERATURE + }; + + static const uint8_t INTERFACE_ID = CLASS_ID::MGM_LIS3MDL; + static const uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::MGM_LIS3MDL; + //Notifies a command to change the setup parameters + static const Event CHANGE_OF_SETUP_PARAMETER = MAKE_EVENT(0, severity::LOW); + + MgmLIS3MDLHandler(uint32_t objectId, object_id_t deviceCommunication, CookieIF* comCookie, + uint32_t transitionDelay); + virtual ~MgmLIS3MDLHandler(); + + /** + * 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 + * @param xLimit + * @param yLimit + * @param zLimit + */ + void setAbsoluteLimits(float xLimit, float yLimit, float zLimit); + void setToGoToNormalMode(bool enable); + +protected: + + /** DeviceHandlerBase overrides */ + void doShutDown() override; + void doStartUp() override; + void doTransition(Mode_t modeFrom, Submode_t subModeFrom) override; + virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + ReturnValue_t buildCommandFromCommand( + DeviceCommandId_t deviceCommand, const uint8_t *commandData, + size_t commandDataLen) override; + ReturnValue_t buildTransitionDeviceCommand( + DeviceCommandId_t *id) override; + ReturnValue_t buildNormalDeviceCommand( + DeviceCommandId_t *id) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t len, + DeviceCommandId_t *foundId, size_t *foundLen) override; + /** + * This implementation is tailored towards space applications and will flag values larger + * than 100 microtesla on X,Y and 150 microtesla on Z as invalid + * @param id + * @param packet + * @return + */ + virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, + const uint8_t *packet) override; + void fillCommandAndReplyMap() override; + void modeChanged(void) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; + +private: + MGMLIS3MDL::MgmPrimaryDataset dataset; + //Length a single command SPI answer + static const uint8_t SINGLE_COMMAND_ANSWER_LEN = 2; + + uint32_t transitionDelay; + // 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]; + + float absLimitX = 100; + float absLimitY = 100; + float absLimitZ = 150; + + /** + * We want to save the registers we set, so we dont have to read the + * 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 statusRegister = 0; + bool goToNormalMode = false; + + enum class InternalState { + STATE_NONE, + STATE_FIRST_CONTACT, + STATE_SETUP, + STATE_CHECK_REGISTERS, + STATE_NORMAL + }; + + InternalState internalState = InternalState::STATE_NONE; + CommunicationStep communicationStep = CommunicationStep::DATA; + bool commandExecuted = false; + + /*------------------------------------------------------------------------*/ + /* 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 + */ + ReturnValue_t identifyDevice(); + + virtual void setupMgm(); + + /*------------------------------------------------------------------------*/ + /* Non normal commands */ + /*------------------------------------------------------------------------*/ + /** + * Enables/Disables the integrated Temperaturesensor + * @param commandData On or Off + * @param length of the commandData: has to be 1 + */ + virtual ReturnValue_t enableTemperatureSensor(const uint8_t *commandData, + size_t commandDataLen); + + /** + * Sets the accuracy of the measurement of the axis. The noise is changing. + * @param commandData LOW, MEDIUM, HIGH, ULTRA + * @param length of the command, has to be 1 + */ + virtual ReturnValue_t setOperatingMode(const uint8_t *commandData, + size_t commandDataLen); + + /** + * We always update all registers together, so this method updates + * the rawpacket and rawpacketLen, so we just manipulate the local + * saved register + * + */ + ReturnValue_t prepareCtrlRegisterWrite(); + +#if FSFW_HAL_LIS3MDL_MGM_DEBUG == 1 + PeriodicOperationDivider* debugDivider; +#endif +}; + +#endif /* MISSION_DEVICES_MGMLIS3MDLHANDLER_H_ */ diff --git a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp b/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp new file mode 100644 index 00000000..20cf95d2 --- /dev/null +++ b/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp @@ -0,0 +1,363 @@ +#include "MgmRM3100Handler.h" + +#include "fsfw/datapool/PoolReadGuard.h" +#include "fsfw/globalfunctions/bitutility.h" +#include "fsfw/devicehandlers/DeviceHandlerMessage.h" +#include "fsfw/objectmanager/SystemObjectIF.h" +#include "fsfw/returnvalues/HasReturnvaluesIF.h" + + +MgmRM3100Handler::MgmRM3100Handler(object_id_t objectId, + object_id_t deviceCommunication, CookieIF* comCookie, uint32_t transitionDelay): + DeviceHandlerBase(objectId, deviceCommunication, comCookie), + primaryDataset(this), transitionDelay(transitionDelay) { +#if FSFW_HAL_RM3100_MGM_DEBUG == 1 + debugDivider = new PeriodicOperationDivider(3); +#endif +} + +MgmRM3100Handler::~MgmRM3100Handler() {} + +void MgmRM3100Handler::doStartUp() { + switch(internalState) { + case(InternalState::NONE): { + internalState = InternalState::CONFIGURE_CMM; + break; + } + case(InternalState::CONFIGURE_CMM): { + internalState = InternalState::READ_CMM; + break; + } + case(InternalState::READ_CMM): { + if(commandExecuted) { + internalState = InternalState::STATE_CONFIGURE_TMRC; + } + break; + } + case(InternalState::STATE_CONFIGURE_TMRC): { + if(commandExecuted) { + internalState = InternalState::STATE_READ_TMRC; + } + break; + } + case(InternalState::STATE_READ_TMRC): { + if(commandExecuted) { + internalState = InternalState::NORMAL; + if(goToNormalModeAtStartup) { + setMode(MODE_NORMAL); + } + else { + setMode(_MODE_TO_ON); + } + } + break; + } + default: { + break; + } + } +} + +void MgmRM3100Handler::doShutDown() { + setMode(_MODE_POWER_DOWN); +} + +ReturnValue_t MgmRM3100Handler::buildTransitionDeviceCommand( + DeviceCommandId_t *id) { + size_t commandLen = 0; + switch(internalState) { + case(InternalState::NONE): + case(InternalState::NORMAL): { + return NOTHING_TO_SEND; + } + case(InternalState::CONFIGURE_CMM): { + *id = RM3100::CONFIGURE_CMM; + break; + } + case(InternalState::READ_CMM): { + *id = RM3100::READ_CMM; + break; + } + case(InternalState::STATE_CONFIGURE_TMRC): { + commandBuffer[0] = RM3100::TMRC_DEFAULT_VALUE; + commandLen = 1; + *id = RM3100::CONFIGURE_TMRC; + break; + } + case(InternalState::STATE_READ_TMRC): { + *id = RM3100::READ_TMRC; + break; + } + default: + // Might be a configuration error + sif::warning << "MgmRM3100Handler::buildTransitionDeviceCommand: Unknown internal state!" << + std::endl; + return HasReturnvaluesIF::RETURN_OK; + } + + return buildCommandFromCommand(*id, commandBuffer, commandLen); +} + +ReturnValue_t MgmRM3100Handler::buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, size_t commandDataLen) { + switch(deviceCommand) { + case(RM3100::CONFIGURE_CMM): { + commandBuffer[0] = RM3100::CMM_REGISTER; + commandBuffer[1] = RM3100::CMM_VALUE; + rawPacket = commandBuffer; + rawPacketLen = 2; + break; + } + case(RM3100::READ_CMM): { + commandBuffer[0] = RM3100::CMM_REGISTER | RM3100::READ_MASK; + commandBuffer[1] = 0; + rawPacket = commandBuffer; + rawPacketLen = 2; + break; + } + case(RM3100::CONFIGURE_TMRC): { + return handleTmrcConfigCommand(deviceCommand, commandData, commandDataLen); + } + case(RM3100::READ_TMRC): { + commandBuffer[0] = RM3100::TMRC_REGISTER | RM3100::READ_MASK; + commandBuffer[1] = 0; + rawPacket = commandBuffer; + rawPacketLen = 2; + break; + } + case(RM3100::CONFIGURE_CYCLE_COUNT): { + return handleCycleCountConfigCommand(deviceCommand, commandData, commandDataLen); + } + case(RM3100::READ_CYCLE_COUNT): { + commandBuffer[0] = RM3100::CYCLE_COUNT_START_REGISTER | RM3100::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; + std::memset(commandBuffer + 1, 0, 9); + rawPacketLen = 10; + break; + } + default: + return DeviceHandlerIF::COMMAND_NOT_IMPLEMENTED; + } + return RETURN_OK; +} + +ReturnValue_t MgmRM3100Handler::buildNormalDeviceCommand( + DeviceCommandId_t *id) { + *id = RM3100::READ_DATA; + return buildCommandFromCommand(*id, nullptr, 0); +} + +ReturnValue_t MgmRM3100Handler::scanForReply(const uint8_t *start, + size_t len, DeviceCommandId_t *foundId, + size_t *foundLen) { + + // For SPI, ID will always be the one of the last sent command + *foundId = this->getPendingCommand(); + *foundLen = len; + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t MgmRM3100Handler::interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) { + ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; + switch(id) { + case(RM3100::CONFIGURE_CMM): + 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) { + commandExecuted = true; + } + break; + } + case(RM3100::READ_CMM): { + uint8_t cmmValue = packet[1]; + // We clear the seventh bit in any case + // because this one is zero sometimes for some reason + bitutil::bitClear(&cmmValue, 6); + if(cmmValue == cmmRegValue and internalState == InternalState::READ_CMM) { + commandExecuted = true; + } + else { + // Attempt reconfiguration + internalState = InternalState::CONFIGURE_CMM; + return DeviceHandlerIF::DEVICE_REPLY_INVALID; + } + break; + } + case(RM3100::READ_TMRC): { + if(packet[1] == tmrcRegValue) { + commandExecuted = true; + // Reading TMRC was commanded. Trigger event to inform ground + if(mode != _MODE_START_UP) { + triggerEvent(tmrcSet, tmrcRegValue, 0); + } + } + else { + // Attempt reconfiguration + internalState = InternalState::STATE_CONFIGURE_TMRC; + return DeviceHandlerIF::DEVICE_REPLY_INVALID; + } + break; + } + case(RM3100::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]; + if(cycleCountX != cycleCountRegValueX or cycleCountY != cycleCountRegValueY or + cycleCountZ != cycleCountRegValueZ) { + return DeviceHandlerIF::DEVICE_REPLY_INVALID; + } + // Reading TMRC was commanded. Trigger event to inform ground + if(mode != _MODE_START_UP) { + uint32_t eventParam1 = (cycleCountX << 16) | cycleCountY; + triggerEvent(cycleCountersSet, eventParam1, cycleCountZ); + } + break; + } + case(RM3100::READ_DATA): { + result = handleDataReadout(packet); + break; + } + default: + return DeviceHandlerIF::UNKNOWN_DEVICE_REPLY; + } + + return result; +} + +ReturnValue_t MgmRM3100Handler::handleCycleCountConfigCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, size_t commandDataLen) { + if(commandData == nullptr) { + return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; + } + + // Set cycle count + if(commandDataLen == 2) { + handleCycleCommand(true, commandData, commandDataLen); + } + else if(commandDataLen == 6) { + handleCycleCommand(false, commandData, commandDataLen); + } + else { + return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; + } + + commandBuffer[0] = RM3100::CYCLE_COUNT_VALUE; + std::memcpy(commandBuffer + 1, &cycleCountRegValueX, 2); + std::memcpy(commandBuffer + 3, &cycleCountRegValueY, 2); + std::memcpy(commandBuffer + 5, &cycleCountRegValueZ, 2); + rawPacketLen = 7; + rawPacket = commandBuffer; + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t MgmRM3100Handler::handleCycleCommand(bool oneCycleValue, + const uint8_t *commandData, size_t commandDataLen) { + RM3100::CycleCountCommand command(oneCycleValue); + ReturnValue_t result = command.deSerialize(&commandData, &commandDataLen, + SerializeIF::Endianness::BIG); + if(result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + // Data sheet p.30 "while noise limits the useful upper range to ~400 cycle counts." + if(command.cycleCountX > 450 ) { + return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; + } + + if(not oneCycleValue and (command.cycleCountY > 450 or command.cycleCountZ > 450)) { + return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; + } + + cycleCountRegValueX = command.cycleCountX; + cycleCountRegValueY = command.cycleCountY; + cycleCountRegValueZ = command.cycleCountZ; + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t MgmRM3100Handler::handleTmrcConfigCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, size_t commandDataLen) { + if(commandData == nullptr or commandDataLen != 1) { + return DeviceHandlerIF::INVALID_COMMAND_PARAMETER; + } + + commandBuffer[0] = RM3100::TMRC_REGISTER; + commandBuffer[1] = commandData[0]; + tmrcRegValue = commandData[0]; + rawPacketLen = 2; + rawPacket = commandBuffer; + return HasReturnvaluesIF::RETURN_OK; +} + +void MgmRM3100Handler::fillCommandAndReplyMap() { + insertInCommandAndReplyMap(RM3100::CONFIGURE_CMM, 3); + insertInCommandAndReplyMap(RM3100::READ_CMM, 3); + + insertInCommandAndReplyMap(RM3100::CONFIGURE_TMRC, 3); + insertInCommandAndReplyMap(RM3100::READ_TMRC, 3); + + insertInCommandAndReplyMap(RM3100::CONFIGURE_CYCLE_COUNT, 3); + insertInCommandAndReplyMap(RM3100::READ_CYCLE_COUNT, 3); + + insertInCommandAndReplyMap(RM3100::READ_DATA, 3, &primaryDataset); +} + +void MgmRM3100Handler::modeChanged(void) { + internalState = InternalState::NONE; +} + +ReturnValue_t MgmRM3100Handler::initializeLocalDataPool( + localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { + 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})); + return HasReturnvaluesIF::RETURN_OK; +} + +uint32_t MgmRM3100Handler::getTransitionDelayMs(Mode_t from, Mode_t to) { + return this->transitionDelay; +} + +void MgmRM3100Handler::setToGoToNormalMode(bool enable) { + goToNormalModeAtStartup = enable; +} + +ReturnValue_t MgmRM3100Handler::handleDataReadout(const uint8_t *packet) { + // Analyze data here. The sensor generates 24 bit signed values so we need to do some bitshift + // 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; + + // Now scale to physical value in microtesla + float fieldStrengthX = fieldStrengthRawX * scaleFactorX; + float fieldStrengthY = fieldStrengthRawY * scaleFactorX; + float fieldStrengthZ = fieldStrengthRawZ * scaleFactorX; + +#if FSFW_HAL_RM3100_MGM_DEBUG == 1 + if(debugDivider->checkAndIncrement()) { + sif::info << "MgmRM3100Handler: Magnetic field strength in" + " microtesla:" << std::endl; + /* Set terminal to utf-8 if there is an issue with micro printout. */ + sif::info << "X: " << fieldStrengthX << " uT" << std::endl; + sif::info << "Y: " << fieldStrengthY << " uT" << std::endl; + sif::info << "Z: " << fieldStrengthZ << " uT" << std::endl; + } +#endif + + // TODO: Sanity check on values? + PoolReadGuard readGuard(&primaryDataset); + if(readGuard.getReadResult() == HasReturnvaluesIF::RETURN_OK) { + primaryDataset.fieldStrengthX = fieldStrengthX; + primaryDataset.fieldStrengthY = fieldStrengthY; + primaryDataset.fieldStrengthZ = fieldStrengthZ; + primaryDataset.setValidity(true, true); + } + return RETURN_OK; +} diff --git a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h b/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h new file mode 100644 index 00000000..1ba680cb --- /dev/null +++ b/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.h @@ -0,0 +1,111 @@ +#ifndef MISSION_DEVICES_MGMRM3100HANDLER_H_ +#define MISSION_DEVICES_MGMRM3100HANDLER_H_ + +#include "fsfw/FSFW.h" +#include "devices/powerSwitcherList.h" +#include "devicedefinitions/MgmRM3100HandlerDefs.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 + * (https://www.pnicorp.com/rm3100/) + * @details + * Flight manual: + * https://egit.irs.uni-stuttgart.de/redmine/projects/eive-flight-manual/wiki/RM3100_MGM + */ +class MgmRM3100Handler: public DeviceHandlerBase { +public: + static const uint8_t INTERFACE_ID = CLASS_ID::MGM_RM3100; + + //! [EXPORT] : [COMMENT] P1: TMRC value which was set, P2: 0 + static constexpr Event tmrcSet = event::makeEvent(SUBSYSTEM_ID::MGM_RM3100, + 0x00, severity::INFO); + + //! [EXPORT] : [COMMENT] Cycle counter set. P1: First two bytes new Cycle Count X + //! P1: Second two bytes new Cycle Count Y + //! P2: New cycle count Z + static constexpr Event cycleCountersSet = event::makeEvent( + SUBSYSTEM_ID::MGM_RM3100, 0x01, severity::INFO); + + MgmRM3100Handler(object_id_t objectId, object_id_t deviceCommunication, + CookieIF* comCookie, uint32_t transitionDelay); + virtual ~MgmRM3100Handler(); + + /** + * Configure device handler to go to normal mode after startup immediately + * @param enable + */ + void setToGoToNormalMode(bool enable); + +protected: + + /* DeviceHandlerBase overrides */ + ReturnValue_t buildTransitionDeviceCommand( + DeviceCommandId_t *id) override; + void doStartUp() override; + void doShutDown() override; + ReturnValue_t buildNormalDeviceCommand(DeviceCommandId_t *id) override; + ReturnValue_t buildCommandFromCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData, size_t commandDataLen) override; + ReturnValue_t scanForReply(const uint8_t *start, size_t len, + DeviceCommandId_t *foundId, size_t *foundLen) override; + ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) override; + + void fillCommandAndReplyMap() override; + void modeChanged(void) override; + virtual uint32_t getTransitionDelayMs(Mode_t from, Mode_t to) override; + ReturnValue_t initializeLocalDataPool(localpool::DataPool &localDataPoolMap, + LocalDataPoolManager &poolManager) override; + +private: + + enum class InternalState { + NONE, + CONFIGURE_CMM, + READ_CMM, + // The cycle count states are propably not going to be used because + // the default cycle count will be used. + STATE_CONFIGURE_CYCLE_COUNT, + STATE_READ_CYCLE_COUNT, + STATE_CONFIGURE_TMRC, + STATE_READ_TMRC, + NORMAL + }; + InternalState internalState = InternalState::NONE; + bool commandExecuted = false; + RM3100::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; + + bool goToNormalModeAtStartup = false; + uint32_t transitionDelay; + + ReturnValue_t handleCycleCountConfigCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData,size_t commandDataLen); + ReturnValue_t handleCycleCommand(bool oneCycleValue, + const uint8_t *commandData, size_t commandDataLen); + + ReturnValue_t handleTmrcConfigCommand(DeviceCommandId_t deviceCommand, + const uint8_t *commandData,size_t commandDataLen); + + ReturnValue_t handleDataReadout(const uint8_t* packet); +#if FSFW_HAL_RM3100_MGM_DEBUG == 1 + PeriodicOperationDivider* debugDivider; +#endif +}; + +#endif /* MISSION_DEVICEHANDLING_MGMRM3100HANDLER_H_ */ From 59feaa4b5c272899e07915accc4c3b46d3f4d45d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 26 Sep 2021 22:38:47 +0200 Subject: [PATCH 11/14] moved class id and subsystem ID --- src/fsfw/events/fwSubsystemIdRanges.h | 2 ++ src/fsfw/returnvalues/FwClassIds.h | 2 ++ 2 files changed, 4 insertions(+) diff --git a/src/fsfw/events/fwSubsystemIdRanges.h b/src/fsfw/events/fwSubsystemIdRanges.h index 88dee9b4..08fb878d 100644 --- a/src/fsfw/events/fwSubsystemIdRanges.h +++ b/src/fsfw/events/fwSubsystemIdRanges.h @@ -29,6 +29,8 @@ enum: uint8_t { PUS_SERVICE_9 = 89, PUS_SERVICE_17 = 97, PUS_SERVICE_23 = 103, + MGM_LIS3MDL = 106, + MGM_RM3100 = 107, FW_SUBSYSTEM_ID_RANGE }; diff --git a/src/fsfw/returnvalues/FwClassIds.h b/src/fsfw/returnvalues/FwClassIds.h index af32f9a7..cdbf5657 100644 --- a/src/fsfw/returnvalues/FwClassIds.h +++ b/src/fsfw/returnvalues/FwClassIds.h @@ -76,6 +76,8 @@ enum: uint8_t { HAL_UART, //HURT HAL_I2C, //HI2C HAL_GPIO, //HGIO + MGM_LIS3MDL, //MGMLIS3 + MGM_RM3100, //MGMRM3100 FW_CLASS_ID_COUNT // [EXPORT] : [END] }; From 423f7c828189701230fe7161a680590e37bf40fb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 26 Sep 2021 22:45:32 +0200 Subject: [PATCH 12/14] missing include and printer compatbility fixes --- .../devicehandlers/GyroL3GD20Handler.cpp | 4 +++- .../devicehandlers/MgmLIS3MDLHandler.cpp | 2 ++ .../devicehandlers/MgmRM3100Handler.cpp | 19 ++++++++++++++++--- 3 files changed, 21 insertions(+), 4 deletions(-) diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp index 70ba49eb..d27351d7 100644 --- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp @@ -1,7 +1,9 @@ -#include "fsfw_hal/devicehandlers/GyroL3GD20Handler.h" +#include "GyroL3GD20Handler.h" #include "fsfw/datapool/PoolReadGuard.h" +#include + GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, CookieIF *comCookie, uint32_t transitionDelayMs): DeviceHandlerBase(objectId, deviceCommunication, comCookie), diff --git a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp index 891d6fdc..804e83f2 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/MgmLIS3MDLHandler.cpp @@ -5,6 +5,8 @@ #include "fsfw/globalfunctions/PeriodicOperationDivider.h" #endif +#include + MgmLIS3MDLHandler::MgmLIS3MDLHandler(object_id_t objectId, object_id_t deviceCommunication, CookieIF* comCookie, uint32_t transitionDelay): DeviceHandlerBase(objectId, deviceCommunication, comCookie), diff --git a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp b/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp index 20cf95d2..124eebbc 100644 --- a/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/MgmRM3100Handler.cpp @@ -89,9 +89,16 @@ ReturnValue_t MgmRM3100Handler::buildTransitionDeviceCommand( break; } default: +#if FSFW_VERBOSE_LEVEL >= 1 +#if FSFW_CPP_OSTREAM_ENABLED == 1 // Might be a configuration error - sif::warning << "MgmRM3100Handler::buildTransitionDeviceCommand: Unknown internal state!" << - std::endl; + sif::warning << "MgmRM3100Handler::buildTransitionDeviceCommand: " + "Unknown internal state" << std::endl; +#else + sif::printWarning("MgmRM3100Handler::buildTransitionDeviceCommand: " + "Unknown internal state\n"); +#endif +#endif return HasReturnvaluesIF::RETURN_OK; } @@ -342,12 +349,18 @@ ReturnValue_t MgmRM3100Handler::handleDataReadout(const uint8_t *packet) { #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; - /* Set terminal to utf-8 if there is an issue with micro printout. */ 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 From 95464955079b7c704b0073ae73699aafb1cebafe Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 27 Sep 2021 10:38:47 +0200 Subject: [PATCH 13/14] improvements for linux libgpioIF --- .../fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp | 169 +++++++++++------- hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h | 7 +- 2 files changed, 108 insertions(+), 68 deletions(-) diff --git a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp index 43cd63ce..3f8e1f3c 100644 --- a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp +++ b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp @@ -122,7 +122,7 @@ ReturnValue_t LinuxLibgpioIF::configureGpioByLineName(gpioId_t gpioId, int result = gpiod_ctxless_find_line(lineName.c_str(), chipname, MAX_CHIPNAME_LENGTH, &lineOffset); - if (result != LINE_FOUND) { + if (result != LINE_FOUND) { parseFindeLineResult(result, lineName); return RETURN_FAILED; } @@ -140,7 +140,7 @@ ReturnValue_t LinuxLibgpioIF::configureGpioByLineName(gpioId_t gpioId, } ReturnValue_t LinuxLibgpioIF::configureRegularGpio(gpioId_t gpioId, struct gpiod_chip* chip, - GpiodRegularBase& regularGpio, std::string failOutput) { + GpiodRegularBase& regularGpio, std::string failOutput) { unsigned int lineNum; gpio::Direction direction; std::string consumer; @@ -165,22 +165,10 @@ ReturnValue_t LinuxLibgpioIF::configureRegularGpio(gpioId_t gpioId, struct gpiod case(gpio::OUT): { result = gpiod_line_request_output(lineHandle, consumer.c_str(), regularGpio.initValue); - if (result < 0) { - sif::error << "LinuxLibgpioIF::configureRegularGpio: Failed to request line " << lineNum << - " from GPIO instance with ID: " << gpioId << std::endl; - gpiod_line_release(lineHandle); - return RETURN_FAILED; - } break; } case(gpio::IN): { result = gpiod_line_request_input(lineHandle, consumer.c_str()); - if (result < 0) { - sif::error << "LinuxLibgpioIF::configureGpios: Failed to request line " - << lineNum << " from GPIO instance with ID: " << gpioId << std::endl; - gpiod_line_release(lineHandle); - return RETURN_FAILED; - } break; } default: { @@ -189,6 +177,18 @@ ReturnValue_t LinuxLibgpioIF::configureRegularGpio(gpioId_t gpioId, struct gpiod return GPIO_INVALID_INSTANCE; } + if (result < 0) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::error << "LinuxLibgpioIF::configureRegularGpio: Failed to request line " << + lineNum << " from GPIO instance with ID: " << gpioId << std::endl; +#else + sif::printError("LinuxLibgpioIF::configureRegularGpio: " + "Failed to request line %d from GPIO instance with ID: %d\n", lineNum, gpioId); +#endif + gpiod_line_release(lineHandle); + return RETURN_FAILED; + } + } /** * Write line handle to GPIO configuration instance so it can later be used to set or @@ -230,7 +230,11 @@ ReturnValue_t LinuxLibgpioIF::pullHigh(gpioId_t gpioId) { ReturnValue_t LinuxLibgpioIF::pullLow(gpioId_t gpioId) { gpioMapIter = gpioMap.find(gpioId); if (gpioMapIter == gpioMap.end()) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "LinuxLibgpioIF::pullLow: Unknown GPIO ID " << gpioId << std::endl; +#else + sif::printWarning("LinuxLibgpioIF::pullLow: Unknown GPIO ID %d\n", gpioId); +#endif return UNKNOWN_GPIO_ID; } @@ -260,8 +264,13 @@ ReturnValue_t LinuxLibgpioIF::driveGpio(gpioId_t gpioId, GpiodRegularBase& regularGpio, gpio::Levels logicLevel) { int result = gpiod_line_set_value(regularGpio.lineHandle, logicLevel); if (result < 0) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "LinuxLibgpioIF::driveGpio: Failed to pull GPIO with ID " << gpioId << " to logic level " << logicLevel << std::endl; +#else + sif::printWarning("LinuxLibgpioIF::driveGpio: Failed to pull GPIO with ID %d to " + "logic level %d\n", gpioId, logicLevel); +#endif return DRIVE_GPIO_FAILURE; } @@ -271,7 +280,11 @@ ReturnValue_t LinuxLibgpioIF::driveGpio(gpioId_t gpioId, ReturnValue_t LinuxLibgpioIF::readGpio(gpioId_t gpioId, int* gpioState) { gpioMapIter = gpioMap.find(gpioId); if (gpioMapIter == gpioMap.end()){ +#if FSFW_CPP_OSTREAM_ENABLED == 1 sif::warning << "LinuxLibgpioIF::readGpio: Unknown GPIOD ID " << gpioId << std::endl; +#else + sif::printWarning("LinuxLibgpioIF::readGpio: Unknown GPIOD ID %d\n", gpioId); +#endif return UNKNOWN_GPIO_ID; } @@ -299,13 +312,14 @@ ReturnValue_t LinuxLibgpioIF::checkForConflicts(GpioMap& mapToAdd){ for(auto& gpioConfig: mapToAdd) { switch(gpioConfig.second->gpioType) { case(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP): - case(gpio::GpioTypes::GPIO_REGULAR_BY_LABEL): { + case(gpio::GpioTypes::GPIO_REGULAR_BY_LABEL): + case(gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME): { auto regularGpio = dynamic_cast(gpioConfig.second); if(regularGpio == nullptr) { return GPIO_TYPE_FAILURE; } - /* Check for conflicts and remove duplicates if necessary */ - result = checkForConflictsRegularGpio(gpioConfig.first, *regularGpio, mapToAdd); + // Check for conflicts and remove duplicates if necessary + result = checkForConflictsById(gpioConfig.first, gpioConfig.second->gpioType, mapToAdd); if(result != HasReturnvaluesIF::RETURN_OK) { status = result; } @@ -316,15 +330,22 @@ ReturnValue_t LinuxLibgpioIF::checkForConflicts(GpioMap& mapToAdd){ if(callbackGpio == nullptr) { return GPIO_TYPE_FAILURE; } - /* Check for conflicts and remove duplicates if necessary */ - result = checkForConflictsCallbackGpio(gpioConfig.first, callbackGpio, mapToAdd); + // Check for conflicts and remove duplicates if necessary + result = checkForConflictsById(gpioConfig.first, + gpioConfig.second->gpioType, mapToAdd); if(result != HasReturnvaluesIF::RETURN_OK) { status = result; } break; } default: { - +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "Invalid GPIO type detected for GPIO ID " << gpioConfig.first + << std::endl; +#else + sif::printWarning("Invalid GPIO type detected for GPIO ID %d\n", gpioConfig.first); +#endif + status = GPIO_TYPE_FAILURE; } } } @@ -332,68 +353,86 @@ ReturnValue_t LinuxLibgpioIF::checkForConflicts(GpioMap& mapToAdd){ } -ReturnValue_t LinuxLibgpioIF::checkForConflictsRegularGpio(gpioId_t gpioIdToCheck, - GpiodRegularBase& gpioToCheck, GpioMap& mapToAdd) { - /* Cross check with private map */ +ReturnValue_t LinuxLibgpioIF::checkForConflictsById(gpioId_t gpioIdToCheck, + gpio::GpioTypes expectedType, GpioMap& mapToAdd) { + // Cross check with private map gpioMapIter = gpioMap.find(gpioIdToCheck); if(gpioMapIter != gpioMap.end()) { auto& gpioType = gpioMapIter->second->gpioType; - if(gpioType != gpio::GpioTypes::GPIO_REGULAR_BY_CHIP and - gpioType != gpio::GpioTypes::GPIO_REGULAR_BY_LABEL) { - sif::warning << "LinuxLibgpioIF::checkForConflicts: ID already exists for different " - "GPIO type" << gpioIdToCheck << ". Removing duplicate." << std::endl; + bool eraseDuplicateDifferentType = false; + switch(expectedType) { + case(gpio::GpioTypes::NONE): { + break; + } + 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) { + eraseDuplicateDifferentType = true; + } + break; + } + case(gpio::GpioTypes::CALLBACK): { + if(gpioType != gpio::GpioTypes::CALLBACK) { + eraseDuplicateDifferentType = true; + } + } + } + if(eraseDuplicateDifferentType) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "LinuxLibgpioIF::checkForConflicts: ID already exists for " + "different GPIO type " << gpioIdToCheck << + ". Removing duplicate from map to add" << std::endl; +#else + sif::printWarning("LinuxLibgpioIF::checkForConflicts: ID already exists for " + "different GPIO type %d. Removing duplicate from map to add\n", gpioIdToCheck); +#endif mapToAdd.erase(gpioIdToCheck); - return HasReturnvaluesIF::RETURN_OK; - } - auto ownRegularGpio = dynamic_cast(gpioMapIter->second); - if(ownRegularGpio == nullptr) { - return GPIO_TYPE_FAILURE; + return GPIO_DUPLICATE_DETECTED; } - /* Remove element from map to add because a entry for this GPIO - already exists */ - sif::warning << "LinuxLibgpioIF::checkForConflictsRegularGpio: Duplicate GPIO definition" - << " detected. Duplicate will be removed from map to add." << std::endl; - mapToAdd.erase(gpioIdToCheck); - } - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t LinuxLibgpioIF::checkForConflictsCallbackGpio(gpioId_t gpioIdToCheck, - GpioCallback *callbackGpio, GpioMap& mapToAdd) { - /* Cross check with private map */ - gpioMapIter = gpioMap.find(gpioIdToCheck); - if(gpioMapIter != gpioMap.end()) { - if(gpioMapIter->second->gpioType != gpio::GpioTypes::CALLBACK) { - sif::warning << "LinuxLibgpioIF::checkForConflicts: ID already exists for different " - "GPIO type" << gpioIdToCheck << ". Removing duplicate." << std::endl; - mapToAdd.erase(gpioIdToCheck); - return HasReturnvaluesIF::RETURN_OK; - } - - /* Remove element from map to add because a entry for this GPIO - already exists */ - sif::warning << "LinuxLibgpioIF::checkForConflictsRegularGpio: Duplicate GPIO definition" - << " detected. Duplicate will be removed from map to add." << std::endl; + // Remove element from map to add because a entry for this GPIO already exists +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "LinuxLibgpioIF::checkForConflictsRegularGpio: Duplicate GPIO " + "definition with ID " << gpioIdToCheck << " detected. " << + "Duplicate will be removed from map to add" << std::endl; +#else + sif::printWarning("LinuxLibgpioIF::checkForConflictsRegularGpio: Duplicate GPIO definition " + "with ID %d detected. Duplicate will be removed from map to add\n", gpioIdToCheck); +#endif mapToAdd.erase(gpioIdToCheck); + return GPIO_DUPLICATE_DETECTED; } return HasReturnvaluesIF::RETURN_OK; } void LinuxLibgpioIF::parseFindeLineResult(int result, std::string& lineName) { switch (result) { +#if FSFW_CPP_OSTREAM_ENABLED == 1 case LINE_NOT_EXISTS: - sif::warning << "LinuxLibgpioIF::parseFindeLineResult: Line with name " << lineName - << " does not exist" << std::endl; + case LINE_ERROR: { + sif::warning << "LinuxLibgpioIF::parseFindeLineResult: Line with name " << lineName << + " does not exist" << std::endl; break; - case LINE_ERROR: - sif::warning << "LinuxLibgpioIF::parseFindeLineResult: " << "Line with name " - << lineName << " does not exist" << std::endl; + } + default: { + sif::warning << "LinuxLibgpioIF::parseFindeLineResult: Unknown return code for line " + "with name " << lineName << std::endl; break; - default: - sif::warning << "LinuxLibgpioIF::parseFindeLineResult: Unknown return code for line with " - << "name " << lineName << std::endl; + } +#else + case LINE_NOT_EXISTS: + case LINE_ERROR: { + sif::printWarning("LinuxLibgpioIF::parseFindeLineResult: Line with name %s " + "does not exist\n", lineName); break; } + default: { + sif::printWarning("LinuxLibgpioIF::parseFindeLineResult: Unknown return code for line " + "with name %s\n", lineName); + break; + } +#endif + } } diff --git a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h index d458c362..6489d749 100644 --- a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +++ b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h @@ -28,6 +28,8 @@ public: HasReturnvaluesIF::makeReturnCode(gpioRetvalId, 3); static constexpr ReturnValue_t GPIO_INVALID_INSTANCE = HasReturnvaluesIF::makeReturnCode(gpioRetvalId, 4); + static constexpr ReturnValue_t GPIO_DUPLICATE_DETECTED = + HasReturnvaluesIF::makeReturnCode(gpioRetvalId, 5); LinuxLibgpioIF(object_id_t objectId); virtual ~LinuxLibgpioIF(); @@ -75,10 +77,9 @@ private: */ ReturnValue_t checkForConflicts(GpioMap& mapToAdd); - ReturnValue_t checkForConflictsRegularGpio(gpioId_t gpiodId, GpiodRegularBase& regularGpio, + ReturnValue_t checkForConflictsById(gpioId_t gpiodId, gpio::GpioTypes type, GpioMap& mapToAdd); - ReturnValue_t checkForConflictsCallbackGpio(gpioId_t gpiodId, GpioCallback* regularGpio, - GpioMap& mapToAdd); + //ReturnValue_t checkForConflictsCallbackGpio(gpioId_t gpiodId, GpioMap& mapToAdd); /** * @brief Performs the initial configuration of all GPIOs specified in the GpioMap mapToAdd. From a84c770dfb8447325c263da42d5cecd99b38d1f0 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 27 Sep 2021 10:51:31 +0200 Subject: [PATCH 14/14] type improvements and bugfixes --- .../fsfw_hal/common/gpio/gpioDefinitions.h | 34 ++++++++++--------- .../fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp | 19 +++++++---- hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h | 19 +++++------ 3 files changed, 39 insertions(+), 33 deletions(-) diff --git a/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h b/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h index 53182058..c6f21195 100644 --- a/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h +++ b/hal/src/fsfw_hal/common/gpio/gpioDefinitions.h @@ -9,12 +9,13 @@ using gpioId_t = uint16_t; namespace gpio { -enum Levels { +enum Levels: uint8_t { LOW = 0, - HIGH = 1 + HIGH = 1, + NONE = 99 }; -enum Direction { +enum Direction: uint8_t { IN = 0, OUT = 1 }; @@ -24,7 +25,7 @@ enum GpioOperation { WRITE }; -enum GpioTypes { +enum class GpioTypes { NONE, GPIO_REGULAR_BY_CHIP, GPIO_REGULAR_BY_LABEL, @@ -34,7 +35,8 @@ enum GpioTypes { static constexpr gpioId_t NO_GPIO = -1; -using gpio_cb_t = void (*) (gpioId_t gpioId, gpio::GpioOperation gpioOp, int value, void* args); +using gpio_cb_t = void (*) (gpioId_t gpioId, gpio::GpioOperation gpioOp, gpio::Levels value, + void* args); } @@ -58,7 +60,7 @@ public: GpioBase() = default; GpioBase(gpio::GpioTypes gpioType, std::string consumer, gpio::Direction direction, - int initValue): + gpio::Levels initValue): gpioType(gpioType), consumer(consumer),direction(direction), initValue(initValue) {} virtual~ GpioBase() {}; @@ -67,19 +69,19 @@ public: gpio::GpioTypes gpioType = gpio::GpioTypes::NONE; std::string consumer; gpio::Direction direction = gpio::Direction::IN; - int initValue = 0; + gpio::Levels initValue = gpio::Levels::NONE; }; class GpiodRegularBase: public GpioBase { public: GpiodRegularBase(gpio::GpioTypes gpioType, std::string consumer, gpio::Direction direction, - int initValue, int lineNum): GpioBase(gpioType, consumer, direction, initValue), - lineNum(lineNum) { + gpio::Levels initValue, int lineNum): + GpioBase(gpioType, consumer, direction, initValue), lineNum(lineNum) { } // line number will be configured at a later point for the open by line name configuration GpiodRegularBase(gpio::GpioTypes gpioType, std::string consumer, gpio::Direction direction, - int initValue): GpioBase(gpioType, consumer, direction, initValue) { + gpio::Levels initValue): GpioBase(gpioType, consumer, direction, initValue) { } int lineNum = 0; @@ -94,7 +96,7 @@ public: } GpiodRegularByChip(std::string chipname_, int lineNum_, std::string consumer_, - gpio::Direction direction_, int initValue_) : + gpio::Direction direction_, gpio::Levels initValue_) : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_CHIP, consumer_, direction_, initValue_, lineNum_), chipname(chipname_){ @@ -112,7 +114,7 @@ public: class GpiodRegularByLabel: public GpiodRegularBase { public: GpiodRegularByLabel(std::string label_, int lineNum_, std::string consumer_, - gpio::Direction direction_, int initValue_) : + gpio::Direction direction_, gpio::Levels initValue_) : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LABEL, consumer_, direction_, initValue_, lineNum_), label(label_) { @@ -135,14 +137,14 @@ public: class GpiodRegularByLineName: public GpiodRegularBase { public: GpiodRegularByLineName(std::string lineName_, std::string consumer_, gpio::Direction direction_, - int initValue_) : + gpio::Levels initValue_) : GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME, consumer_, direction_, initValue_), lineName(lineName_) { } GpiodRegularByLineName(std::string lineName_, std::string consumer_) : - GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME, consumer_, gpio::Direction::IN, - gpio::LOW), lineName(lineName_) { + GpiodRegularBase(gpio::GpioTypes::GPIO_REGULAR_BY_LINE_NAME, consumer_, + gpio::Direction::IN, gpio::LOW), lineName(lineName_) { } std::string lineName; @@ -150,7 +152,7 @@ public: class GpioCallback: public GpioBase { public: - GpioCallback(std::string consumer, gpio::Direction direction_, int initValue_, + GpioCallback(std::string consumer, gpio::Direction direction_, gpio::Levels initValue_, gpio::gpio_cb_t callback, void* callbackArgs): 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 3f8e1f3c..004e1e7f 100644 --- a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp +++ b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.cpp @@ -1,8 +1,9 @@ -#include "fsfw_hal/linux/gpio/LinuxLibgpioIF.h" +#include "LinuxLibgpioIF.h" + #include "fsfw_hal/common/gpio/gpioDefinitions.h" #include "fsfw_hal/common/gpio/GpioCookie.h" -#include +#include "fsfw/serviceinterface/ServiceInterface.h" #include #include @@ -221,7 +222,7 @@ ReturnValue_t LinuxLibgpioIF::pullHigh(gpioId_t gpioId) { return GPIO_INVALID_INSTANCE; } gpioCallback->callback(gpioMapIter->first, gpio::GpioOperation::WRITE, - 1, gpioCallback->callbackArgs); + gpio::Levels::HIGH, gpioCallback->callbackArgs); return RETURN_OK; } return GPIO_TYPE_FAILURE; @@ -254,7 +255,7 @@ ReturnValue_t LinuxLibgpioIF::pullLow(gpioId_t gpioId) { return GPIO_INVALID_INSTANCE; } gpioCallback->callback(gpioMapIter->first, gpio::GpioOperation::WRITE, - 0, gpioCallback->callbackArgs); + gpio::Levels::LOW, gpioCallback->callbackArgs); return RETURN_OK; } return GPIO_TYPE_FAILURE; @@ -299,10 +300,14 @@ ReturnValue_t LinuxLibgpioIF::readGpio(gpioId_t gpioId, int* gpioState) { *gpioState = gpiod_line_get_value(regularGpio->lineHandle); } else { - + auto gpioCallback = dynamic_cast(gpioMapIter->second); + if(gpioCallback->callback == nullptr) { + return GPIO_INVALID_INSTANCE; + } + gpioCallback->callback(gpioMapIter->first, gpio::GpioOperation::READ, + gpio::Levels::NONE, gpioCallback->callbackArgs); + return RETURN_OK; } - - return RETURN_OK; } diff --git a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h index 6489d749..cc32bd70 100644 --- a/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h +++ b/hal/src/fsfw_hal/linux/gpio/LinuxLibgpioIF.h @@ -1,19 +1,19 @@ #ifndef LINUX_GPIO_LINUXLIBGPIOIF_H_ #define LINUX_GPIO_LINUXLIBGPIOIF_H_ -#include "../../common/gpio/GpioIF.h" -#include -#include +#include "fsfw/returnvalues/FwClassIds.h" +#include "fsfw_hal/common/gpio/GpioIF.h" +#include "fsfw/objectmanager/SystemObject.h" class GpioCookie; class GpiodRegularIF; /** - * @brief This class implements the GpioIF for a linux based system. The - * implementation is based on the libgpiod lib which requires linux 4.8 - * or higher. - * @note The Petalinux SDK from Xilinx supports libgpiod since Petalinux - * 2019.1. + * @brief This class implements the GpioIF for a linux based system. + * @details + * This implementation is based on the libgpiod lib which requires Linux 4.8 or higher. + * @note + * The Petalinux SDK from Xilinx supports libgpiod since Petalinux 2019.1. */ class LinuxLibgpioIF : public GpioIF, public SystemObject { public: @@ -46,7 +46,7 @@ private: static const int LINE_ERROR = -1; static const int LINE_FOUND = 1; - /* Holds the information and configuration of all used GPIOs */ + // Holds the information and configuration of all used GPIOs GpioUnorderedMap gpioMap; GpioUnorderedMapIter gpioMapIter; @@ -79,7 +79,6 @@ private: ReturnValue_t checkForConflictsById(gpioId_t gpiodId, gpio::GpioTypes type, GpioMap& mapToAdd); - //ReturnValue_t checkForConflictsCallbackGpio(gpioId_t gpiodId, GpioMap& mapToAdd); /** * @brief Performs the initial configuration of all GPIOs specified in the GpioMap mapToAdd.