From 20adc1c9813697744a37c375027114370131695c Mon Sep 17 00:00:00 2001 From: "Jakob.Meier" <–meierj@irs.uni-stuttgart.de> Date: Sat, 7 Aug 2021 14:28:12 +0200 Subject: [PATCH 01/19] queue nullptr check in action helper --- action/ActionHelper.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/action/ActionHelper.cpp b/action/ActionHelper.cpp index 73007ea3..aaa19ac4 100644 --- a/action/ActionHelper.cpp +++ b/action/ActionHelper.cpp @@ -33,6 +33,17 @@ ReturnValue_t ActionHelper::initialize(MessageQueueIF* queueToUse_) { setQueueToUse(queueToUse_); } + if(queueToUse == nullptr) { +#if FSFW_VERBOSE_LEVEL >= 1 +#if FSFW_CPP_OSTREAM_ENABLED == 1 + sif::warning << "ActionHelper::initialize: No queue set" << std::endl; +#else + sif::printWarning("ActionHelper::initialize: No queue set\n"); +#endif +#endif /* FSFW_VERBOSE_LEVEL >= 1 */ + return HasReturnvaluesIF::RETURN_FAILED; + } + return HasReturnvaluesIF::RETURN_OK; } From db3284c2b8cc6492a50476025a93cab509e02d77 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 16 Aug 2021 14:52:11 +0200 Subject: [PATCH 02/19] subversion update --- src/fsfw/FSFWVersion.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/fsfw/FSFWVersion.h b/src/fsfw/FSFWVersion.h index 1d0da280..f8e89694 100644 --- a/src/fsfw/FSFWVersion.h +++ b/src/fsfw/FSFWVersion.h @@ -4,7 +4,7 @@ const char* const FSFW_VERSION_NAME = "ASTP"; #define FSFW_VERSION 1 -#define FSFW_SUBVERSION 3 +#define FSFW_SUBVERSION 2 #define FSFW_REVISION 0 #endif /* FSFW_VERSION_H_ */ From fa14ebbe1f0aa5f8888e53d3476b93acf1a82cdb Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 16 Aug 2021 15:19:03 +0200 Subject: [PATCH 03/19] additional check --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9a1f7d1a..4a1ec8c4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -38,7 +38,7 @@ elseif(${CMAKE_CXX_STANDARD} LESS 11) endif() # Backwards comptability -if(OS_FSFW) +if(OS_FSFW AND NOT FSFW_OSAL) message(WARNING "Please pass the FSFW OSAL as FSFW_OSAL instead of OS_FSFW") set(FSFW_OSAL OS_FSFW) endif() From 845c00044ecbb36683c7dbc1c662d24e49bad380 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 18 Aug 2021 11:27:39 +0200 Subject: [PATCH 04/19] printout fixes for UnixFileGuard --- hal/src/fsfw_hal/linux/UnixFileGuard.cpp | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/hal/src/fsfw_hal/linux/UnixFileGuard.cpp b/hal/src/fsfw_hal/linux/UnixFileGuard.cpp index f7901018..ad875623 100644 --- a/hal/src/fsfw_hal/linux/UnixFileGuard.cpp +++ b/hal/src/fsfw_hal/linux/UnixFileGuard.cpp @@ -1,5 +1,10 @@ +#include "fsfw/FSFW.h" +#include "fsfw/serviceinterface.h" #include "fsfw_hal/linux/UnixFileGuard.h" +#include +#include + UnixFileGuard::UnixFileGuard(std::string device, int* fileDescriptor, int flags, std::string diagnosticPrefix): fileDescriptor(fileDescriptor) { @@ -10,12 +15,11 @@ UnixFileGuard::UnixFileGuard(std::string device, int* fileDescriptor, int flags, if (*fileDescriptor < 0) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << diagnosticPrefix <<"Opening device failed with error code " << errno << - "." << std::endl; - sif::warning << "Error description: " << strerror(errno) << std::endl; + sif::warning << diagnosticPrefix << "Opening device failed with error code " << + errno << ": " << strerror(errno) << std::endl; #else - sif::printError("%sOpening device failed with error code %d.\n", diagnosticPrefix); - sif::printWarning("Error description: %s\n", strerror(errno)); + sif::printWarning("%sOpening device failed with error code %d: %s\n", + diagnosticPrefix, errno, strerror(errno)); #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ #endif /* FSFW_VERBOSE_LEVEL >= 1 */ openStatus = OPEN_FILE_FAILED; From b6aebb3061a8417ffe09111eced07b8f17dddd3d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 19 Aug 2021 17:08:35 +0200 Subject: [PATCH 05/19] format adapted --- hal/src/fsfw_hal/linux/UnixFileGuard.cpp | 4 ++-- hal/src/fsfw_hal/linux/spi/SpiComIF.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/hal/src/fsfw_hal/linux/UnixFileGuard.cpp b/hal/src/fsfw_hal/linux/UnixFileGuard.cpp index ad875623..5019343e 100644 --- a/hal/src/fsfw_hal/linux/UnixFileGuard.cpp +++ b/hal/src/fsfw_hal/linux/UnixFileGuard.cpp @@ -15,10 +15,10 @@ UnixFileGuard::UnixFileGuard(std::string device, int* fileDescriptor, int flags, if (*fileDescriptor < 0) { #if FSFW_VERBOSE_LEVEL >= 1 #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::warning << diagnosticPrefix << "Opening device failed with error code " << + sif::warning << diagnosticPrefix << ": Opening device failed with error code " << errno << ": " << strerror(errno) << std::endl; #else - sif::printWarning("%sOpening device failed with error code %d: %s\n", + sif::printWarning("%s: Opening device failed with error code %d: %s\n", diagnosticPrefix, errno, strerror(errno)); #endif /* FSFW_CPP_OSTREAM_ENABLED == 1 */ #endif /* FSFW_VERBOSE_LEVEL >= 1 */ diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp index fafe67be..6697bc92 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -184,7 +184,7 @@ ReturnValue_t SpiComIF::performRegularSendOperation(SpiCookie *spiCookie, const /* Prepare transfer */ int fileDescriptor = 0; std::string device = spiCookie->getSpiDevice(); - UnixFileGuard fileHelper(device, &fileDescriptor, O_RDWR, "SpiComIF::sendMessage: "); + UnixFileGuard fileHelper(device, &fileDescriptor, O_RDWR, "SpiComIF::sendMessage"); if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) { return OPENING_FILE_FAILED; } @@ -273,7 +273,7 @@ ReturnValue_t SpiComIF::performHalfDuplexReception(SpiCookie* spiCookie) { std::string device = spiCookie->getSpiDevice(); int fileDescriptor = 0; UnixFileGuard fileHelper(device, &fileDescriptor, O_RDWR, - "SpiComIF::requestReceiveMessage: "); + "SpiComIF::requestReceiveMessage"); if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) { return OPENING_FILE_FAILED; } From 98e3ed897c9f81837cffa24da41d4cbb50cea72d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 19 Aug 2021 17:17:19 +0200 Subject: [PATCH 06/19] small tweak --- hal/src/fsfw_hal/linux/spi/SpiComIF.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp index 6697bc92..48bf7449 100644 --- a/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp +++ b/hal/src/fsfw_hal/linux/spi/SpiComIF.cpp @@ -90,7 +90,7 @@ ReturnValue_t SpiComIF::initializeInterface(CookieIF *cookie) { int fileDescriptor = 0; UnixFileGuard fileHelper(spiCookie->getSpiDevice(), &fileDescriptor, O_RDWR, - "SpiComIF::initializeInterface: "); + "SpiComIF::initializeInterface"); if(fileHelper.getOpenResult() != HasReturnvaluesIF::RETURN_OK) { return fileHelper.getOpenResult(); } From a6d744c9c8df4e2cd69c170a4bd6806f54b835f9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Mon, 6 Sep 2021 12:05:30 +0200 Subject: [PATCH 07/19] Possible bugfix in DHB The delayCycles variables needs to be initialized differently for periodic replies. It is initialized to the maxDelayCycles value now --- devicehandlers/DeviceHandlerBase.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp index 5665b101..733701b2 100644 --- a/devicehandlers/DeviceHandlerBase.cpp +++ b/devicehandlers/DeviceHandlerBase.cpp @@ -430,7 +430,12 @@ ReturnValue_t DeviceHandlerBase::insertInReplyMap(DeviceCommandId_t replyId, DeviceReplyInfo info; info.maxDelayCycles = maxDelayCycles; info.periodic = periodic; - info.delayCycles = 0; + if(info.periodic) { + info.delayCycles = info.maxDelayCycles; + } + else { + info.delayCycles = 0; + } info.replyLen = replyLen; info.dataSet = dataSet; info.command = deviceCommandMap.end(); From c9bfc8464aede0f79c167e18c5c0c79ded2444ce Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 11 Sep 2021 17:39:42 +0200 Subject: [PATCH 08/19] added function to enable periodic reply --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 23 +++++++++++------ src/fsfw/devicehandlers/DeviceHandlerBase.h | 25 +++++++++++++------ 2 files changed, 33 insertions(+), 15 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 1a7fbc91..95e25b74 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -430,12 +430,7 @@ ReturnValue_t DeviceHandlerBase::insertInReplyMap(DeviceCommandId_t replyId, DeviceReplyInfo info; info.maxDelayCycles = maxDelayCycles; info.periodic = periodic; - if(info.periodic) { - info.delayCycles = info.maxDelayCycles; - } - else { - info.delayCycles = 0; - } + info.delayCycles = 0; info.replyLen = replyLen; info.dataSet = dataSet; info.command = deviceCommandMap.end(); @@ -474,7 +469,7 @@ ReturnValue_t DeviceHandlerBase::updateReplyMapEntry(DeviceCommandId_t deviceRep auto replyIter = deviceReplyMap.find(deviceReply); if (replyIter == deviceReplyMap.end()) { triggerEvent(INVALID_DEVICE_COMMAND, deviceReply); - return RETURN_FAILED; + return COMMAND_NOT_SUPPORTED; } else { DeviceReplyInfo *info = &(replyIter->second); if (maxDelayCycles != 0) { @@ -486,6 +481,20 @@ ReturnValue_t DeviceHandlerBase::updateReplyMapEntry(DeviceCommandId_t deviceRep } } +ReturnValue_t DeviceHandlerBase::enablePeriodicReply(DeviceCommandId_t deviceReply) { + auto replyIter = deviceReplyMap.find(deviceReply); + if (replyIter == deviceReplyMap.end()) { + triggerEvent(INVALID_DEVICE_COMMAND, deviceReply); + return COMMAND_NOT_SUPPORTED; + } else { + DeviceReplyInfo *info = &(replyIter->second); + if(not info->periodic) { + return COMMAND_NOT_SUPPORTED; + } + info->delayCycles = info->maxDelayCycles; + } + return HasReturnvaluesIF::RETURN_OK; +} ReturnValue_t DeviceHandlerBase::setReplyDataset(DeviceCommandId_t replyId, LocalPoolDataSetBase *dataSet) { diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index 53bd1e65..d478b0fa 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -449,7 +449,9 @@ protected: * @param replyLen Will be supplied to the requestReceiveMessage call of * the communication interface. * @param periodic Indicates if the command is periodic (i.e. it is sent - * by the device repeatedly without request) or not. Default is aperiodic (0) + * by the device repeatedly without request) or not. Default is aperiodic (0). + * Please note that periodic replies are disabled by default. You can enable them with + * #enablePeriodicReplies * @return - @c RETURN_OK when the command was successfully inserted, * - @c RETURN_FAILED else. */ @@ -464,7 +466,9 @@ protected: * @param maxDelayCycles The maximum number of delay cycles the reply waits * until it times out. * @param periodic Indicates if the command is periodic (i.e. it is sent - * by the device repeatedly without request) or not. Default is aperiodic (0) + * by the device repeatedly without request) or not. Default is aperiodic (0). + * Please note that periodic replies are disabled by default. You can enable them with + * #enablePeriodicReplies * @return - @c RETURN_OK when the command was successfully inserted, * - @c RETURN_FAILED else. */ @@ -480,6 +484,13 @@ protected: */ ReturnValue_t insertInCommandMap(DeviceCommandId_t deviceCommand); + /** + * Enables a periodic reply for a given command. It sets to delay cycles to the specified + * maximum delay cycles for a given reply ID. + * @return + */ + ReturnValue_t enablePeriodicReply(DeviceCommandId_t deviceReply); + /** * @brief This function returns the reply length of the next reply to read. * @@ -493,16 +504,14 @@ protected: virtual size_t getNextReplyLength(DeviceCommandId_t deviceCommand); /** - * @brief This is a helper method to facilitate updating entries - * in the reply map. + * @brief This is a helper method to facilitate updating entries in the reply map. * @param deviceCommand Identifier of the reply to update. - * @param delayCycles The current number of delay cycles to wait. - * As stated in #fillCommandAndCookieMap, to disable periodic commands, - * this is set to zero. + * @param delayCycles The current number of delay cycles to wait. As stated in + * #fillCommandAndReplyMap, to disable periodic commands, this is set to zero. * @param maxDelayCycles The maximum number of delay cycles the reply waits * until it times out. By passing 0 the entry remains untouched. * @param periodic Indicates if the command is periodic (i.e. it is sent - * by the device repeatedly without request) or not.Default is aperiodic (0). + * by the device repeatedly without request) or not. Default is aperiodic (0). * Warning: The setting always overrides the value that was entered in the map. * @return - @c RETURN_OK when the command was successfully inserted, * - @c RETURN_FAILED else. From 11a3c8c21f8a80d5c0872e99e39bd655f18acfa7 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 11 Sep 2021 17:42:29 +0200 Subject: [PATCH 09/19] added option to disable it as well --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 9 +++++++-- src/fsfw/devicehandlers/DeviceHandlerBase.h | 5 +++-- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index 95e25b74..aab3aa16 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -481,7 +481,7 @@ ReturnValue_t DeviceHandlerBase::updateReplyMapEntry(DeviceCommandId_t deviceRep } } -ReturnValue_t DeviceHandlerBase::enablePeriodicReply(DeviceCommandId_t deviceReply) { +ReturnValue_t DeviceHandlerBase::enablePeriodicReply(bool enable, DeviceCommandId_t deviceReply) { auto replyIter = deviceReplyMap.find(deviceReply); if (replyIter == deviceReplyMap.end()) { triggerEvent(INVALID_DEVICE_COMMAND, deviceReply); @@ -491,7 +491,12 @@ ReturnValue_t DeviceHandlerBase::enablePeriodicReply(DeviceCommandId_t deviceRep if(not info->periodic) { return COMMAND_NOT_SUPPORTED; } - info->delayCycles = info->maxDelayCycles; + if(enable) { + info->delayCycles = info->maxDelayCycles; + } + else { + info->delayCycles = 0; + } } return HasReturnvaluesIF::RETURN_OK; } diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index d478b0fa..138b429e 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -486,10 +486,11 @@ protected: /** * Enables a periodic reply for a given command. It sets to delay cycles to the specified - * maximum delay cycles for a given reply ID. + * maximum delay cycles for a given reply ID if enabled or to 0 if disabled. + * @param enable Specify whether to enable or disable a given periodic reply * @return */ - ReturnValue_t enablePeriodicReply(DeviceCommandId_t deviceReply); + ReturnValue_t enablePeriodicReply(bool enable, DeviceCommandId_t deviceReply); /** * @brief This function returns the reply length of the next reply to read. From 134deb3f433988513617a0d510fbbf8629b34fbc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sat, 11 Sep 2021 17:43:58 +0200 Subject: [PATCH 10/19] renamed function --- src/fsfw/devicehandlers/DeviceHandlerBase.cpp | 2 +- src/fsfw/devicehandlers/DeviceHandlerBase.h | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index aab3aa16..c3fc023e 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -481,7 +481,7 @@ ReturnValue_t DeviceHandlerBase::updateReplyMapEntry(DeviceCommandId_t deviceRep } } -ReturnValue_t DeviceHandlerBase::enablePeriodicReply(bool enable, DeviceCommandId_t deviceReply) { +ReturnValue_t DeviceHandlerBase::updatePeriodicReply(bool enable, DeviceCommandId_t deviceReply) { auto replyIter = deviceReplyMap.find(deviceReply); if (replyIter == deviceReplyMap.end()) { triggerEvent(INVALID_DEVICE_COMMAND, deviceReply); diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index 138b429e..30d36ef0 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -451,7 +451,7 @@ protected: * @param periodic Indicates if the command is periodic (i.e. it is sent * by the device repeatedly without request) or not. Default is aperiodic (0). * Please note that periodic replies are disabled by default. You can enable them with - * #enablePeriodicReplies + * #updatePeriodicReply * @return - @c RETURN_OK when the command was successfully inserted, * - @c RETURN_FAILED else. */ @@ -468,7 +468,7 @@ protected: * @param periodic Indicates if the command is periodic (i.e. it is sent * by the device repeatedly without request) or not. Default is aperiodic (0). * Please note that periodic replies are disabled by default. You can enable them with - * #enablePeriodicReplies + * #updatePeriodicReply * @return - @c RETURN_OK when the command was successfully inserted, * - @c RETURN_FAILED else. */ @@ -490,7 +490,7 @@ protected: * @param enable Specify whether to enable or disable a given periodic reply * @return */ - ReturnValue_t enablePeriodicReply(bool enable, DeviceCommandId_t deviceReply); + ReturnValue_t updatePeriodicReply(bool enable, DeviceCommandId_t deviceReply); /** * @brief This function returns the reply length of the next reply to read. From abacfbf2d5550c189e5c0b48f3eb794eb8525f02 Mon Sep 17 00:00:00 2001 From: Ulrich Mohr Date: Mon, 13 Sep 2021 10:38:36 +0200 Subject: [PATCH 11/19] added setting of readLen according to review --- src/fsfw/globalfunctions/DleEncoder.cpp | 23 ++++++++++++++++++++--- 1 file changed, 20 insertions(+), 3 deletions(-) diff --git a/src/fsfw/globalfunctions/DleEncoder.cpp b/src/fsfw/globalfunctions/DleEncoder.cpp index df9a909e..47ea5c4e 100644 --- a/src/fsfw/globalfunctions/DleEncoder.cpp +++ b/src/fsfw/globalfunctions/DleEncoder.cpp @@ -154,6 +154,11 @@ ReturnValue_t DleEncoder::decodeStreamEscaped(const uint8_t *sourceStream, size_ size_t encodedIndex = 0; size_t decodedIndex = 0; uint8_t nextByte; + + //init to 0 so that we can just return in the first checks (which do not consume anything from + //the source stream) + *readLen = 0; + if(maxDestStreamlen < 1) { return STREAM_TOO_SHORT; } @@ -166,6 +171,8 @@ ReturnValue_t DleEncoder::decodeStreamEscaped(const uint8_t *sourceStream, size_ and (sourceStream[encodedIndex] != STX_CHAR)) { if (sourceStream[encodedIndex] == DLE_CHAR) { if(encodedIndex + 1 >= sourceStreamLen) { + //reached the end of the sourceStream + *readLen = sourceStreamLen; return DECODING_ERROR; } nextByte = sourceStream[encodedIndex + 1]; @@ -200,6 +207,8 @@ ReturnValue_t DleEncoder::decodeStreamEscaped(const uint8_t *sourceStream, size_ } if (sourceStream[encodedIndex] != ETX_CHAR) { if(decodedIndex == maxDestStreamlen) { + //so far we did not find anything wrong here, so let user try again + *readLen = 0; return STREAM_TOO_SHORT; } else { @@ -220,6 +229,11 @@ ReturnValue_t DleEncoder::decodeStreamNonEscaped(const uint8_t *sourceStream, size_t encodedIndex = 0; size_t decodedIndex = 0; uint8_t nextByte; + + //init to 0 so that we can just return in the first checks (which do not consume anything from + //the source stream) + *readLen = 0; + if(maxDestStreamlen < 2) { return STREAM_TOO_SHORT; } @@ -227,6 +241,7 @@ ReturnValue_t DleEncoder::decodeStreamNonEscaped(const uint8_t *sourceStream, return DECODING_ERROR; } if (sourceStream[encodedIndex++] != STX_CHAR) { + *readLen = 1; return DECODING_ERROR; } while ((encodedIndex < sourceStreamLen) && (decodedIndex < maxDestStreamlen)) { @@ -265,11 +280,13 @@ ReturnValue_t DleEncoder::decodeStreamNonEscaped(const uint8_t *sourceStream, ++encodedIndex; ++decodedIndex; } - *readLen = encodedIndex; + if(decodedIndex == maxDestStreamlen) { + //so far we did not find anything wrong here, so let user try again + *readLen = 0; return STREAM_TOO_SHORT; - } - else { + } else { + *readLen = encodedIndex; return DECODING_ERROR; } } From 21b5eaa891c58f3e52de3acbf138c3ec128a1789 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 15 Sep 2021 17:09:42 +0200 Subject: [PATCH 12/19] 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 13/19] 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 a6e4eb9ad4f9c2367c09d72182176d43b67260a8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 15 Sep 2021 17:18:47 +0200 Subject: [PATCH 14/19] improvements for L3GD20H device handler --- .../devicehandlers/GyroL3GD20Handler.cpp | 42 +++++++++---------- .../devicehandlers/GyroL3GD20Handler.h | 11 ++--- 2 files changed, 25 insertions(+), 28 deletions(-) diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp index 96d284e1..4a492e5d 100644 --- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp +++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.cpp @@ -3,9 +3,9 @@ #include "fsfw/datapool/PoolReadGuard.h" GyroHandlerL3GD20H::GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, - CookieIF *comCookie): + CookieIF *comCookie, uint8_t switchId, uint32_t transitionDelayMs): DeviceHandlerBase(objectId, deviceCommunication, comCookie), - dataset(this) { + switchId(switchId), transitionDelayMs(transitionDelayMs), dataset(this) { #if FSFW_HAL_L3GD20_GYRO_DEBUG == 1 debugDivider = new PeriodicOperationDivider(5); #endif @@ -47,7 +47,7 @@ ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t switch(internalState) { case(InternalState::NONE): case(InternalState::NORMAL): { - return HasReturnvaluesIF::RETURN_OK; + return NOTHING_TO_SEND; } case(InternalState::CONFIGURE): { *id = L3GD20H::CONFIGURE_CTRL_REGS; @@ -66,10 +66,11 @@ ReturnValue_t GyroHandlerL3GD20H::buildTransitionDeviceCommand(DeviceCommandId_t default: #if FSFW_CPP_OSTREAM_ENABLED == 1 /* Might be a configuration error. */ - sif::debug << "GyroHandler::buildTransitionDeviceCommand: Unknown internal state!" << - std::endl; + sif::warning << "GyroL3GD20Handler::buildTransitionDeviceCommand: " + "Unknown internal state!" << std::endl; #else - sif::printDebug("GyroHandler::buildTransitionDeviceCommand: Unknown internal state!\n"); + sif::printDebug("GyroL3GD20Handler::buildTransitionDeviceCommand: " + "Unknown internal state!\n"); #endif return HasReturnvaluesIF::RETURN_OK; } @@ -144,7 +145,7 @@ ReturnValue_t GyroHandlerL3GD20H::buildCommandFromCommand( ReturnValue_t GyroHandlerL3GD20H::scanForReply(const uint8_t *start, size_t len, DeviceCommandId_t *foundId, size_t *foundLen) { - /* For SPI, the ID will always be the one of the last sent command. */ + // For SPI, the ID will always be the one of the last sent command *foundId = this->getPendingCommand(); *foundLen = this->rawPacketLen; @@ -166,7 +167,7 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, commandExecuted = true; } else { - /* Attempt reconfiguration. */ + // Attempt reconfiguration internalState = InternalState::CONFIGURE; return DeviceHandlerIF::DEVICE_REPLY_INVALID; } @@ -199,13 +200,12 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, if(debugDivider->checkAndIncrement()) { /* Set terminal to utf-8 if there is an issue with micro printout. */ #if FSFW_CPP_OSTREAM_ENABLED == 1 - sif::info << "GyroHandlerL3GD20H: Angular velocities in degrees per second:" << - std::endl; - sif::info << "X: " << angVelocX << " \xC2\xB0" << std::endl; - sif::info << "Y: " << angVelocY << " \xC2\xB0" << std::endl; - sif::info << "Z: " << angVelocZ << " \xC2\xB0" << std::endl; + sif::info << "GyroHandlerL3GD20H: Angular velocities (deg/s):" << std::endl; + sif::info << "X: " << angVelocX << std::endl; + sif::info << "Y: " << angVelocY << std::endl; + sif::info << "Z: " << angVelocZ << std::endl; #else - sif::printInfo("GyroHandlerL3GD20H: Angular velocities in degrees per second:\n"); + sif::printInfo("GyroHandlerL3GD20H: Angular velocities (deg/s):\n"); sif::printInfo("X: %f\n", angVelocX); sif::printInfo("Y: %f\n", angVelocY); sif::printInfo("Z: %f\n", angVelocZ); @@ -231,7 +231,7 @@ ReturnValue_t GyroHandlerL3GD20H::interpretDeviceReply(DeviceCommandId_t id, uint32_t GyroHandlerL3GD20H::getTransitionDelayMs(Mode_t from, Mode_t to) { - return 10000; + return this->transitionDelayMs; } void GyroHandlerL3GD20H::setGoNormalModeAtStartup() { @@ -240,14 +240,10 @@ void GyroHandlerL3GD20H::setGoNormalModeAtStartup() { ReturnValue_t GyroHandlerL3GD20H::initializeLocalDataPool( localpool::DataPool &localDataPoolMap, LocalDataPoolManager &poolManager) { - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, - new PoolEntry({0.0})); - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, - new PoolEntry({0.0})); - localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, - new PoolEntry({0.0})); - localDataPoolMap.emplace(L3GD20H::TEMPERATURE, - new PoolEntry({0.0})); + localDataPoolMap.emplace(L3GD20H::ANG_VELOC_X, new PoolEntry({0.0})); + localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Y, new PoolEntry({0.0})); + localDataPoolMap.emplace(L3GD20H::ANG_VELOC_Z, new PoolEntry({0.0})); + localDataPoolMap.emplace(L3GD20H::TEMPERATURE, new PoolEntry({0.0})); return HasReturnvaluesIF::RETURN_OK; } diff --git a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h index 020c5a32..bc1d9c1c 100644 --- a/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h +++ b/hal/src/fsfw_hal/devicehandlers/GyroL3GD20Handler.h @@ -7,10 +7,6 @@ #include #include -#ifndef FSFW_HAL_L3GD20_GYRO_DEBUG -#define FSFW_HAL_L3GD20_GYRO_DEBUG 0 -#endif /* FSFW_HAL_L3GD20_GYRO_DEBUG */ - /** * @brief Device Handler for the L3GD20H gyroscope sensor * (https://www.st.com/en/mems-and-sensors/l3gd20h.html) @@ -23,9 +19,12 @@ class GyroHandlerL3GD20H: public DeviceHandlerBase { public: GyroHandlerL3GD20H(object_id_t objectId, object_id_t deviceCommunication, - CookieIF* comCookie); + CookieIF* comCookie, uint8_t switchId, uint32_t transitionDelayMs = 10000); virtual ~GyroHandlerL3GD20H(); + /** + * @brief Configure device handler to go to normal mode immediately + */ void setGoNormalModeAtStartup(); protected: @@ -51,6 +50,8 @@ protected: LocalDataPoolManager &poolManager) override; private: + uint8_t switchId = 0; + uint32_t transitionDelayMs = 0; GyroPrimaryDataset dataset; enum class InternalState { From 1732359f72766216acd7f8a719a51606cac66c33 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Wed, 15 Sep 2021 17:23:26 +0200 Subject: [PATCH 15/19] doc was wrong --- src/fsfw/pus/servicepackets/Service1Packets.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/fsfw/pus/servicepackets/Service1Packets.h b/src/fsfw/pus/servicepackets/Service1Packets.h index 2249b4b0..02ae339f 100644 --- a/src/fsfw/pus/servicepackets/Service1Packets.h +++ b/src/fsfw/pus/servicepackets/Service1Packets.h @@ -13,10 +13,10 @@ /** * @brief FailureReport class to serialize a failure report - * @brief Subservice 1, 3, 5, 7 + * @brief Subservice 2, 4, 6, 8 * @ingroup spacepackets */ -class FailureReport: public SerializeIF { //!< [EXPORT] : [SUBSERVICE] 1, 3, 5, 7 +class FailureReport: public SerializeIF { //!< [EXPORT] : [SUBSERVICE] 2, 4, 6, 8 public: FailureReport(uint8_t failureSubtype_, uint16_t packetId_, uint16_t packetSequenceControl_, uint8_t stepNumber_, @@ -108,10 +108,10 @@ private: }; /** - * @brief Subservices 2, 4, 6, 8 + * @brief Subservices 1, 3, 5, 7 * @ingroup spacepackets */ -class SuccessReport: public SerializeIF { //!< [EXPORT] : [SUBSERVICE] 2, 4, 6, 8 +class SuccessReport: public SerializeIF { //!< [EXPORT] : [SUBSERVICE] 1, 3, 5, 7 public: SuccessReport(uint8_t subtype_, uint16_t packetId_, uint16_t packetSequenceControl_,uint8_t stepNumber_) : From f2bc374f0ffd349c3c02552885433735eb632f92 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Thu, 23 Sep 2021 18:12:59 +0200 Subject: [PATCH 16/19] 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 17/19] 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 f6b03dee6ab7755df53d415fe75802a30fd2c435 Mon Sep 17 00:00:00 2001 From: Jakob Meier Date: Fri, 24 Sep 2021 12:11:12 +0200 Subject: [PATCH 18/19] 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 19/19] 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) {}