diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp index 43660616..5665b101 100644 --- a/devicehandlers/DeviceHandlerBase.cpp +++ b/devicehandlers/DeviceHandlerBase.cpp @@ -226,16 +226,15 @@ ReturnValue_t DeviceHandlerBase::initialize() { } void DeviceHandlerBase::decrementDeviceReplyMap() { - for (std::map::iterator iter = - deviceReplyMap.begin(); iter != deviceReplyMap.end(); iter++) { - if (iter->second.delayCycles != 0) { - iter->second.delayCycles--; - if (iter->second.delayCycles == 0) { - if (iter->second.periodic) { - iter->second.delayCycles = iter->second.maxDelayCycles; + for (std::pair& replyPair: deviceReplyMap) { + if (replyPair.second.delayCycles != 0) { + replyPair.second.delayCycles--; + if (replyPair.second.delayCycles == 0) { + if (replyPair.second.periodic) { + replyPair.second.delayCycles = replyPair.second.maxDelayCycles; } - replyToReply(iter, TIMEOUT); - missedReply(iter->first); + replyToReply(replyPair.first, replyPair.second, TIMEOUT); + missedReply(replyPair.first); } } } @@ -584,17 +583,28 @@ void DeviceHandlerBase::replyToCommand(ReturnValue_t status, } } -void DeviceHandlerBase::replyToReply(DeviceReplyMap::iterator iter, +void DeviceHandlerBase::replyToReply(const DeviceCommandId_t command, DeviceReplyInfo& replyInfo, ReturnValue_t status) { // No need to check if iter exists, as this is checked by callers. // If someone else uses the method, add check. - if (iter->second.command == deviceCommandMap.end()) { + if (replyInfo.command == deviceCommandMap.end()) { //Is most likely periodic reply. Silent return. return; } + DeviceCommandInfo* info = &replyInfo.command->second; + if (info == nullptr){ + 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 if more replies are expected. If so, do nothing. - DeviceCommandInfo* info = &(iter->second.command->second); - if (--info->expectedReplies == 0) { + if (info->expectedReplies == 0) { // Check if it was transition or internal command. // Don't send any replies in that case. if (info->sendReplyTo != NO_COMMANDER) { @@ -602,7 +612,7 @@ void DeviceHandlerBase::replyToReply(DeviceReplyMap::iterator iter, if(status == HasReturnvaluesIF::RETURN_OK) { success = true; } - actionHelper.finish(success, info->sendReplyTo, iter->first, status); + actionHelper.finish(success, info->sendReplyTo, command, status); } info->isExecuting = false; } @@ -801,7 +811,7 @@ void DeviceHandlerBase::handleReply(const uint8_t* receivedData, replyRawReplyIfnotWiretapped(receivedData, foundLen); triggerEvent(DEVICE_INTERPRETING_REPLY_FAILED, result, foundId); } - replyToReply(iter, result); + replyToReply(iter->first, iter->second, result); } else { /* Other completion failure messages are created by timeout. diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h index 496c08ff..53bd1e65 100644 --- a/devicehandlers/DeviceHandlerBase.h +++ b/devicehandlers/DeviceHandlerBase.h @@ -667,7 +667,7 @@ protected: static const ReturnValue_t CHILD_TIMEOUT = MAKE_RETURN_CODE(0xE0); static const ReturnValue_t SWITCH_FAILED = MAKE_RETURN_CODE(0xE1); - static const MessageQueueId_t NO_COMMANDER = 0; + static const MessageQueueId_t NO_COMMANDER = MessageQueueIF::NO_QUEUE; //! Pointer to the raw packet that will be sent. uint8_t *rawPacket = nullptr; @@ -1195,7 +1195,8 @@ private: * @foundLen the length of the packet */ void handleReply(const uint8_t *data, DeviceCommandId_t id, uint32_t foundLen); - void replyToReply(DeviceReplyMap::iterator iter, ReturnValue_t status); + void replyToReply(const DeviceCommandId_t command, DeviceReplyInfo& replyInfo, + ReturnValue_t status); /** * Build and send a command to the device. diff --git a/osal/FreeRTOS/Clock.cpp b/osal/FreeRTOS/Clock.cpp index c15971fe..66207d75 100644 --- a/osal/FreeRTOS/Clock.cpp +++ b/osal/FreeRTOS/Clock.cpp @@ -134,71 +134,3 @@ ReturnValue_t Clock::convertTimevalToJD2000(timeval time, double* JD2000) { / 3600.; return HasReturnvaluesIF::RETURN_OK; } - -ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval* tt) { - //SHOULDDO: works not for dates in the past (might have less leap seconds) - if (timeMutex == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - - uint16_t leapSeconds; - ReturnValue_t result = getLeapSeconds(&leapSeconds); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - timeval leapSeconds_timeval = { 0, 0 }; - leapSeconds_timeval.tv_sec = leapSeconds; - - //initial offset between UTC and TAI - timeval UTCtoTAI1972 = { 10, 0 }; - - timeval TAItoTT = { 32, 184000 }; - - *tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT; - - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) { - if (checkOrCreateClockMutex() != HasReturnvaluesIF::RETURN_OK) { - return HasReturnvaluesIF::RETURN_FAILED; - } - ReturnValue_t result = timeMutex->lockMutex(MutexIF::TimeoutType::BLOCKING); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - - leapSeconds = leapSeconds_; - - result = timeMutex->unlockMutex(); - return result; -} - -ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) { - if (timeMutex == NULL) { - return HasReturnvaluesIF::RETURN_FAILED; - } - ReturnValue_t result = timeMutex->lockMutex(MutexIF::TimeoutType::BLOCKING); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - - *leapSeconds_ = leapSeconds; - - result = timeMutex->unlockMutex(); - return result; -} - -ReturnValue_t Clock::checkOrCreateClockMutex() { - if (timeMutex == NULL) { - MutexFactory* mutexFactory = MutexFactory::instance(); - if (mutexFactory == NULL) { - return HasReturnvaluesIF::RETURN_FAILED; - } - timeMutex = mutexFactory->createMutex(); - if (timeMutex == NULL) { - return HasReturnvaluesIF::RETURN_FAILED; - } - } - return HasReturnvaluesIF::RETURN_OK; -} diff --git a/osal/host/Clock.cpp b/osal/host/Clock.cpp index ed5aab62..1018de57 100644 --- a/osal/host/Clock.cpp +++ b/osal/host/Clock.cpp @@ -171,71 +171,3 @@ ReturnValue_t Clock::convertTimevalToJD2000(timeval time, double* JD2000) { / 3600.; return HasReturnvaluesIF::RETURN_OK; } - -ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval* tt) { - //SHOULDDO: works not for dates in the past (might have less leap seconds) - if (timeMutex == NULL) { - return HasReturnvaluesIF::RETURN_FAILED; - } - - uint16_t leapSeconds; - ReturnValue_t result = getLeapSeconds(&leapSeconds); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - timeval leapSeconds_timeval = { 0, 0 }; - leapSeconds_timeval.tv_sec = leapSeconds; - - //initial offset between UTC and TAI - timeval UTCtoTAI1972 = { 10, 0 }; - - timeval TAItoTT = { 32, 184000 }; - - *tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT; - - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) { - if(checkOrCreateClockMutex()!=HasReturnvaluesIF::RETURN_OK){ - return HasReturnvaluesIF::RETURN_FAILED; - } - ReturnValue_t result = timeMutex->lockMutex(); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - - leapSeconds = leapSeconds_; - - result = timeMutex->unlockMutex(); - return result; -} - -ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) { - if(timeMutex == nullptr){ - return HasReturnvaluesIF::RETURN_FAILED; - } - ReturnValue_t result = timeMutex->lockMutex(); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - - *leapSeconds_ = leapSeconds; - - result = timeMutex->unlockMutex(); - return result; -} - -ReturnValue_t Clock::checkOrCreateClockMutex(){ - if(timeMutex == nullptr){ - MutexFactory* mutexFactory = MutexFactory::instance(); - if (mutexFactory == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - timeMutex = mutexFactory->createMutex(); - if (timeMutex == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - } - return HasReturnvaluesIF::RETURN_OK; -} diff --git a/osal/linux/Clock.cpp b/osal/linux/Clock.cpp index 35cbfae0..960d9c0d 100644 --- a/osal/linux/Clock.cpp +++ b/osal/linux/Clock.cpp @@ -153,71 +153,3 @@ ReturnValue_t Clock::convertTimevalToJD2000(timeval time, double* JD2000) { / 3600.; return HasReturnvaluesIF::RETURN_OK; } - -ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval* tt) { - //SHOULDDO: works not for dates in the past (might have less leap seconds) - if (timeMutex == NULL) { - return HasReturnvaluesIF::RETURN_FAILED; - } - - uint16_t leapSeconds; - ReturnValue_t result = getLeapSeconds(&leapSeconds); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - timeval leapSeconds_timeval = { 0, 0 }; - leapSeconds_timeval.tv_sec = leapSeconds; - - //initial offset between UTC and TAI - timeval UTCtoTAI1972 = { 10, 0 }; - - timeval TAItoTT = { 32, 184000 }; - - *tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT; - - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) { - if(checkOrCreateClockMutex()!=HasReturnvaluesIF::RETURN_OK){ - return HasReturnvaluesIF::RETURN_FAILED; - } - ReturnValue_t result = timeMutex->lockMutex(MutexIF::TimeoutType::BLOCKING); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - - leapSeconds = leapSeconds_; - - result = timeMutex->unlockMutex(); - return result; -} - -ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) { - if(timeMutex==NULL){ - return HasReturnvaluesIF::RETURN_FAILED; - } - ReturnValue_t result = timeMutex->lockMutex(MutexIF::TimeoutType::BLOCKING); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - - *leapSeconds_ = leapSeconds; - - result = timeMutex->unlockMutex(); - return result; -} - -ReturnValue_t Clock::checkOrCreateClockMutex(){ - if(timeMutex == nullptr){ - MutexFactory* mutexFactory = MutexFactory::instance(); - if (mutexFactory == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - timeMutex = mutexFactory->createMutex(); - if (timeMutex == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - } - return HasReturnvaluesIF::RETURN_OK; -} diff --git a/osal/rtems/Clock.cpp b/osal/rtems/Clock.cpp index b80786f7..ae720c36 100644 --- a/osal/rtems/Clock.cpp +++ b/osal/rtems/Clock.cpp @@ -154,65 +154,3 @@ ReturnValue_t Clock::convertTimevalToJD2000(timeval time, double* JD2000) { / 3600.; return HasReturnvaluesIF::RETURN_OK; } - -ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval* tt) { - //SHOULDDO: works not for dates in the past (might have less leap seconds) - if (timeMutex == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - - uint16_t leapSeconds; - ReturnValue_t result = getLeapSeconds(&leapSeconds); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - timeval leapSeconds_timeval = { 0, 0 }; - leapSeconds_timeval.tv_sec = leapSeconds; - - //initial offset between UTC and TAI - timeval UTCtoTAI1972 = { 10, 0 }; - - timeval TAItoTT = { 32, 184000 }; - - *tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT; - - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) { - if(checkOrCreateClockMutex()!=HasReturnvaluesIF::RETURN_OK){ - return HasReturnvaluesIF::RETURN_FAILED; - } - MutexGuard helper(timeMutex); - - - leapSeconds = leapSeconds_; - - - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) { - if(timeMutex==nullptr){ - return HasReturnvaluesIF::RETURN_FAILED; - } - MutexGuard helper(timeMutex); - - *leapSeconds_ = leapSeconds; - - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t Clock::checkOrCreateClockMutex(){ - if(timeMutex==nullptr){ - MutexFactory* mutexFactory = MutexFactory::instance(); - if (mutexFactory == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - timeMutex = mutexFactory->createMutex(); - if (timeMutex == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - } - return HasReturnvaluesIF::RETURN_OK; -} diff --git a/returnvalues/FwClassIds.h b/returnvalues/FwClassIds.h index 60cb33ac..af32f9a7 100644 --- a/returnvalues/FwClassIds.h +++ b/returnvalues/FwClassIds.h @@ -75,6 +75,7 @@ enum: uint8_t { HAL_SPI, //HSPI HAL_UART, //HURT HAL_I2C, //HI2C + HAL_GPIO, //HGIO FW_CLASS_ID_COUNT // [EXPORT] : [END] }; diff --git a/timemanager/CMakeLists.txt b/timemanager/CMakeLists.txt index 3367775f..70dd41fa 100644 --- a/timemanager/CMakeLists.txt +++ b/timemanager/CMakeLists.txt @@ -5,4 +5,5 @@ target_sources(${LIB_FSFW_NAME} Stopwatch.cpp TimeMessage.cpp TimeStamper.cpp + ClockCommon.cpp ) diff --git a/timemanager/Clock.h b/timemanager/Clock.h index 6400d284..6a76c86d 100644 --- a/timemanager/Clock.h +++ b/timemanager/Clock.h @@ -41,14 +41,14 @@ public: * @return -@c RETURN_OK on success. Otherwise, the OS failure code * is returned. */ - static ReturnValue_t setClock(const TimeOfDay_t* time); + static ReturnValue_t setClock(const TimeOfDay_t *time); /** * This system call sets the system time. * To set the time, it uses a timeval struct. * @param time The struct with the time settings to set. * @return -@c RETURN_OK on success. Otherwise, the OS failure code is returned. */ - static ReturnValue_t setClock(const timeval* time); + static ReturnValue_t setClock(const timeval *time); /** * This system call returns the current system clock in timeval format. * The timval format has the fields @c tv_sec with seconds and @c tv_usec with @@ -56,7 +56,7 @@ public: * @param time A pointer to a timeval struct where the current time is stored. * @return @c RETURN_OK on success. Otherwise, the OS failure code is returned. */ - static ReturnValue_t getClock_timeval(timeval* time); + static ReturnValue_t getClock_timeval(timeval *time); /** * Get the time since boot in a timeval struct @@ -66,7 +66,7 @@ public: * * @deprecated, I do not think this should be able to fail, use timeval getUptime() */ - static ReturnValue_t getUptime(timeval* uptime); + static ReturnValue_t getUptime(timeval *uptime); static timeval getUptime(); @@ -79,7 +79,7 @@ public: * @param ms uptime in ms * @return RETURN_OK on success. Otherwise, the OS failure code is returned. */ - static ReturnValue_t getUptime(uint32_t* uptimeMs); + static ReturnValue_t getUptime(uint32_t *uptimeMs); /** * Returns the time in microseconds since an OS-defined epoch. @@ -89,7 +89,7 @@ public: * - @c RETURN_OK on success. * - Otherwise, the OS failure code is returned. */ - static ReturnValue_t getClock_usecs(uint64_t* time); + static ReturnValue_t getClock_usecs(uint64_t *time); /** * Returns the time in a TimeOfDay_t struct. * @param time A pointer to a TimeOfDay_t struct. @@ -97,7 +97,7 @@ public: * - @c RETURN_OK on success. * - Otherwise, the OS failure code is returned. */ - static ReturnValue_t getDateAndTime(TimeOfDay_t* time); + static ReturnValue_t getDateAndTime(TimeOfDay_t *time); /** * Converts a time of day struct to POSIX seconds. @@ -107,8 +107,8 @@ public: * - @c RETURN_OK on success. * - Otherwise, the OS failure code is returned. */ - static ReturnValue_t convertTimeOfDayToTimeval(const TimeOfDay_t* from, - timeval* to); + static ReturnValue_t convertTimeOfDayToTimeval(const TimeOfDay_t *from, + timeval *to); /** * Converts a time represented as seconds and subseconds since unix @@ -118,12 +118,14 @@ public: * @param[out] JD2000 days since J2000 * @return @c RETURN_OK */ - static ReturnValue_t convertTimevalToJD2000(timeval time, double* JD2000); + static ReturnValue_t convertTimevalToJD2000(timeval time, double *JD2000); /** * Calculates and adds the offset between UTC and TT * * Depends on the leap seconds to be set correctly. + * Therefore, it does not work for historic + * dates as only the current leap seconds are known. * * @param utc timeval, corresponding to UTC time * @param[out] tt timeval, corresponding to Terrestial Time @@ -131,7 +133,7 @@ public: * - @c RETURN_OK on success * - @c RETURN_FAILED if leapSeconds are not set */ - static ReturnValue_t convertUTCToTT(timeval utc, timeval* tt); + static ReturnValue_t convertUTCToTT(timeval utc, timeval *tt); /** * Set the Leap Seconds since 1972 @@ -139,22 +141,22 @@ public: * @param leapSeconds_ * @return * - @c RETURN_OK on success. - * - Otherwise, the OS failure code is returned. */ static ReturnValue_t setLeapSeconds(const uint16_t leapSeconds_); /** * Get the Leap Seconds since 1972 * - * Must be set before! + * Setter must be called before * * @param[out] leapSeconds_ * @return * - @c RETURN_OK on success. - * - Otherwise, the OS failure code is returned. + * - @c RETURN_FAILED on error */ static ReturnValue_t getLeapSeconds(uint16_t *leapSeconds_); +private: /** * Function to check and create the Mutex for the clock * @return @@ -163,10 +165,8 @@ public: */ static ReturnValue_t checkOrCreateClockMutex(); -private: - static MutexIF* timeMutex; + static MutexIF *timeMutex; static uint16_t leapSeconds; }; - #endif /* FSFW_TIMEMANAGER_CLOCK_H_ */ diff --git a/timemanager/ClockCommon.cpp b/timemanager/ClockCommon.cpp new file mode 100644 index 00000000..e56d4953 --- /dev/null +++ b/timemanager/ClockCommon.cpp @@ -0,0 +1,57 @@ +#include "Clock.h" +#include "../ipc/MutexGuard.h" + +ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval *tt) { + uint16_t leapSeconds; + ReturnValue_t result = getLeapSeconds(&leapSeconds); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + timeval leapSeconds_timeval = { 0, 0 }; + leapSeconds_timeval.tv_sec = leapSeconds; + + //initial offset between UTC and TAI + timeval UTCtoTAI1972 = { 10, 0 }; + + timeval TAItoTT = { 32, 184000 }; + + *tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT; + + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) { + if (checkOrCreateClockMutex() != HasReturnvaluesIF::RETURN_OK) { + return HasReturnvaluesIF::RETURN_FAILED; + } + MutexGuard helper(timeMutex); + + leapSeconds = leapSeconds_; + + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::getLeapSeconds(uint16_t *leapSeconds_) { + if (timeMutex == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + MutexGuard helper(timeMutex); + + *leapSeconds_ = leapSeconds; + + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::checkOrCreateClockMutex() { + if (timeMutex == nullptr) { + MutexFactory *mutexFactory = MutexFactory::instance(); + if (mutexFactory == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + timeMutex = mutexFactory->createMutex(); + if (timeMutex == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + return HasReturnvaluesIF::RETURN_OK; +} diff --git a/tmtcpacket/pus/tc/TcPacketStoredPus.cpp b/tmtcpacket/pus/tc/TcPacketStoredPus.cpp index f098ce2b..426aafdb 100644 --- a/tmtcpacket/pus/tc/TcPacketStoredPus.cpp +++ b/tmtcpacket/pus/tc/TcPacketStoredPus.cpp @@ -1,5 +1,7 @@ #include "TcPacketStoredPus.h" +#include "../../../serviceinterface/ServiceInterface.h" + #include TcPacketStoredPus::TcPacketStoredPus(uint16_t apid, uint8_t service,