diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp index b0a5d1f0..52178683 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.cpp +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.cpp @@ -243,17 +243,28 @@ ReturnValue_t DeviceHandlerBase::initialize() { } void DeviceHandlerBase::decrementDeviceReplyMap() { + bool timedOut = false; for (std::pair& replyPair : deviceReplyMap) { - if (replyPair.second.delayCycles != 0) { + if (replyPair.second.countdown != nullptr && replyPair.second.active) { + if (replyPair.second.countdown->hasTimedOut()) { + timedOut = true; + } + } + if (replyPair.second.delayCycles != 0 && replyPair.second.countdown == nullptr) { replyPair.second.delayCycles--; if (replyPair.second.delayCycles == 0) { if (replyPair.second.periodic) { replyPair.second.delayCycles = replyPair.second.maxDelayCycles; } - replyToReply(replyPair.first, replyPair.second, TIMEOUT); - missedReply(replyPair.first); + timedOut = true; } } + if (timedOut) { + replyToReply(replyPair.first, replyPair.second, TIMEOUT); + missedReply(replyPair.first); + timedOut = false; + replyPair.second.active = false; + } } } @@ -416,20 +427,22 @@ ReturnValue_t DeviceHandlerBase::isModeCombinationValid(Mode_t mode, Submode_t s ReturnValue_t DeviceHandlerBase::insertInCommandAndReplyMap( DeviceCommandId_t deviceCommand, uint16_t maxDelayCycles, LocalPoolDataSetBase* replyDataSet, - size_t replyLen, bool periodic, bool hasDifferentReplyId, DeviceCommandId_t replyId) { + size_t replyLen, bool periodic, bool hasDifferentReplyId, DeviceCommandId_t replyId, + Countdown* countdown) { // No need to check, as we may try to insert multiple times. insertInCommandMap(deviceCommand, hasDifferentReplyId, replyId); if (hasDifferentReplyId) { - return insertInReplyMap(replyId, maxDelayCycles, replyDataSet, replyLen, periodic); + return insertInReplyMap(replyId, maxDelayCycles, replyDataSet, replyLen, periodic, countdown); } else { - return insertInReplyMap(deviceCommand, maxDelayCycles, replyDataSet, replyLen, periodic); + return insertInReplyMap(deviceCommand, maxDelayCycles, replyDataSet, replyLen, periodic, + countdown); } } ReturnValue_t DeviceHandlerBase::insertInReplyMap(DeviceCommandId_t replyId, uint16_t maxDelayCycles, LocalPoolDataSetBase* dataSet, size_t replyLen, - bool periodic) { + bool periodic, Countdown* countdown) { DeviceReplyInfo info; info.maxDelayCycles = maxDelayCycles; info.periodic = periodic; @@ -437,6 +450,10 @@ ReturnValue_t DeviceHandlerBase::insertInReplyMap(DeviceCommandId_t replyId, info.replyLen = replyLen; info.dataSet = dataSet; info.command = deviceCommandMap.end(); + info.countdown = countdown; + if (info.periodic) { + info.active = true; + } auto resultPair = deviceReplyMap.emplace(replyId, info); if (resultPair.second) { return RETURN_OK; @@ -472,7 +489,8 @@ size_t DeviceHandlerBase::getNextReplyLength(DeviceCommandId_t commandId) { } DeviceReplyIter iter = deviceReplyMap.find(replyId); if (iter != deviceReplyMap.end()) { - if (iter->second.delayCycles != 0) { + if ((iter->second.delayCycles != 0 && iter->second.countdown == nullptr) || + (iter->second.active && iter->second.countdown != nullptr)) { return iter->second.replyLen; } } @@ -816,17 +834,18 @@ void DeviceHandlerBase::handleReply(const uint8_t* receivedData, DeviceCommandId DeviceReplyInfo* info = &(iter->second); - if (info->delayCycles != 0) { + if ((info->delayCycles != 0 && info->countdown == nullptr) || + (info->active && info->countdown != nullptr)) { result = interpretDeviceReply(foundId, receivedData); if (result == IGNORE_REPLY_DATA) { return; } - if (info->periodic) { - info->delayCycles = info->maxDelayCycles; - } else { - info->delayCycles = 0; + if (info->active && info->countdown != nullptr) { + disableTimeoutControlledReply(info); + } else if (info->delayCycles != 0) { + disableDelayCyclesControlledReply(info); } if (result != RETURN_OK) { @@ -845,6 +864,24 @@ void DeviceHandlerBase::handleReply(const uint8_t* receivedData, DeviceCommandId } } +void DeviceHandlerBase::disableTimeoutControlledReply(DeviceReplyInfo* info) { + if (info->periodic) { + info->countdown->resetTimer(); + } else { + info->active = false; + info->countdown->timeOut(); + } +} + +void DeviceHandlerBase::disableDelayCyclesControlledReply(DeviceReplyInfo* info) { + if (info->periodic) { + info->delayCycles = info->maxDelayCycles; + } else { + info->delayCycles = 0; + info->active = false; + } +} + ReturnValue_t DeviceHandlerBase::getStorageData(store_address_t storageAddress, uint8_t** data, size_t* len) { size_t lenTmp; @@ -970,6 +1007,10 @@ ReturnValue_t DeviceHandlerBase::enableReplyInReplyMap(DeviceCommandMap::iterato info->delayCycles = info->maxDelayCycles; info->command = command; command->second.expectedReplies = expectedReplies; + if (info->countdown != nullptr) { + info->countdown->resetTimer(); + } + info->active = true; return RETURN_OK; } else { return NO_REPLY_EXPECTED; @@ -1204,7 +1245,8 @@ void DeviceHandlerBase::setParentQueue(MessageQueueId_t parentQueueId) { bool DeviceHandlerBase::isAwaitingReply() { std::map::iterator iter; for (iter = deviceReplyMap.begin(); iter != deviceReplyMap.end(); ++iter) { - if (iter->second.delayCycles != 0) { + if ((iter->second.delayCycles != 0 && iter->second.countdown == nullptr) || + (iter->second.active && iter->second.countdown != nullptr)) { return true; } } @@ -1360,6 +1402,9 @@ uint8_t DeviceHandlerBase::getReplyDelayCycles(DeviceCommandId_t deviceCommand) if (iter == deviceReplyMap.end()) { return 0; } + else if (iter->second.countdown != nullptr) { + return 0; + } return iter->second.delayCycles; } diff --git a/src/fsfw/devicehandlers/DeviceHandlerBase.h b/src/fsfw/devicehandlers/DeviceHandlerBase.h index d8c3363e..4e457c33 100644 --- a/src/fsfw/devicehandlers/DeviceHandlerBase.h +++ b/src/fsfw/devicehandlers/DeviceHandlerBase.h @@ -451,6 +451,9 @@ class DeviceHandlerBase : public DeviceHandlerIF, * by the device repeatedly without request) or not. Default is aperiodic (0). * Please note that periodic replies are disabled by default. You can enable them with * #updatePeriodicReply + * @param countdown Instead of using maxDelayCycles to timeout a device reply it is also possible + * to provide a pointer to a Countdown object which will signal the timeout + * when expired * @return - @c RETURN_OK when the command was successfully inserted, * - @c RETURN_FAILED else. */ @@ -458,7 +461,8 @@ class DeviceHandlerBase : public DeviceHandlerIF, LocalPoolDataSetBase *replyDataSet = nullptr, size_t replyLen = 0, bool periodic = false, bool hasDifferentReplyId = false, - DeviceCommandId_t replyId = 0); + DeviceCommandId_t replyId = 0, + Countdown *countdown = nullptr); /** * @brief This is a helper method to insert replies in the reply map. * @param deviceCommand Identifier of the reply to add. @@ -468,12 +472,15 @@ class DeviceHandlerBase : public DeviceHandlerIF, * by the device repeatedly without request) or not. Default is aperiodic (0). * Please note that periodic replies are disabled by default. You can enable them with * #updatePeriodicReply + * @param countdown Instead of using maxDelayCycles to timeout a device reply it is also possible + * to provide a pointer to a Countdown object which will signal the timeout + * when expired * @return - @c RETURN_OK when the command was successfully inserted, * - @c RETURN_FAILED else. */ ReturnValue_t insertInReplyMap(DeviceCommandId_t deviceCommand, uint16_t maxDelayCycles, LocalPoolDataSetBase *dataSet = nullptr, size_t replyLen = 0, - bool periodic = false); + bool periodic = false, Countdown *countdown = nullptr); /** * @brief A simple command to add a command to the commandList. @@ -792,6 +799,11 @@ class DeviceHandlerBase : public DeviceHandlerIF, LocalPoolDataSetBase *dataSet = nullptr; //! The command that expects this reply. DeviceCommandMap::iterator command; + //! Instead of using delayCycles to specify the maximum time to wait for the device reply, it + //! is also possible specify a countdown + Countdown* countdown = nullptr; + //! will be set to true when reply is enabled + bool active = false; }; using DeviceReplyMap = std::map; @@ -1253,6 +1265,17 @@ class DeviceHandlerBase : public DeviceHandlerIF, */ void doGetRead(void); + /** + * @brief Handles disabling of replies which use a timeout to detect missed replies. + */ + void disableTimeoutControlledReply(DeviceReplyInfo* info); + + /** + * @brief Handles disabling of replies which use a number of maximum delay cycles to detect + * missed replies. + */ + void disableDelayCyclesControlledReply(DeviceReplyInfo* info); + /** * Retrive data from the #IPCStore. * diff --git a/src/fsfw/timemanager/Countdown.cpp b/src/fsfw/timemanager/Countdown.cpp index a8ba78cb..327995f8 100644 --- a/src/fsfw/timemanager/Countdown.cpp +++ b/src/fsfw/timemanager/Countdown.cpp @@ -1,7 +1,12 @@ #include "fsfw/timemanager/Countdown.h" -Countdown::Countdown(uint32_t initialTimeout) : timeout(initialTimeout) { - setTimeout(initialTimeout); +Countdown::Countdown(uint32_t initialTimeout, bool startImmediately) : timeout(initialTimeout) { + if (startImmediately) { + setTimeout(initialTimeout); + } + else { + timeout = initialTimeout; + } } Countdown::~Countdown() {} diff --git a/src/fsfw/timemanager/Countdown.h b/src/fsfw/timemanager/Countdown.h index 44be2b1a..26534789 100644 --- a/src/fsfw/timemanager/Countdown.h +++ b/src/fsfw/timemanager/Countdown.h @@ -26,8 +26,9 @@ class Countdown { * Otherwise a call to hasTimedOut might return True. * * @param initialTimeout Countdown duration in milliseconds + * @param startImmediately Set to false if countdown should not be started immediately */ - Countdown(uint32_t initialTimeout = 0); + Countdown(uint32_t initialTimeout = 0, bool startImmediately = true); ~Countdown(); /** * Call to set a new countdown duration.