diff --git a/container/FixedOrderedMultimap.h b/container/FixedOrderedMultimap.h index 717575d7..96bc0073 100644 --- a/container/FixedOrderedMultimap.h +++ b/container/FixedOrderedMultimap.h @@ -1,10 +1,33 @@ -#ifndef FRAMEWORK_CONTAINER_FIXEDORDEREDMULTIMAP_H_ -#define FRAMEWORK_CONTAINER_FIXEDORDEREDMULTIMAP_H_ +#ifndef FSFW_CONTAINER_FIXEDORDEREDMULTIMAP_H_ +#define FSFW_CONTAINER_FIXEDORDEREDMULTIMAP_H_ #include "ArrayList.h" #include -#include + /** + * @brief An associative container which allows multiple entries of the same key. + * @details + * Same keys are ordered by KEY_COMPARE function which is std::less > by default. + * + * It uses the ArrayList, so technically this is not a real map, it is an array of pairs + * of type key_t, T. It is ordered by key_t as FixedMap but allows same keys. Thus it has a linear + * complexity O(n). As long as the number of entries remains low, this + * should not be an issue. + * The number of insertion and deletion operation should be minimized + * as those incur extensive memory move operations (the underlying container + * is not node based). + * + * Its of fixed size so no allocations are performed after the construction. + * + * The maximum size is given as first parameter of the constructor. + * + * It provides an iterator to do list iterations. + * + * The type T must have a copy constructor if it is not trivial copy-able. + * + * @warning Iterators return a non-const key_t in the pair. + * @warning A User is not allowed to change the key, otherwise the map is corrupted. + * * \ingroup container */ template> @@ -14,152 +37,170 @@ public: static const ReturnValue_t MAP_FULL = MAKE_RETURN_CODE(0x01); static const ReturnValue_t KEY_DOES_NOT_EXIST = MAKE_RETURN_CODE(0x02); -private: - typedef KEY_COMPARE compare; - compare myComp; - ArrayList, uint32_t> theMap; - uint32_t _size; + /*** + * Constructor which needs a size_t for the maximum allowed size + * + * Can not be resized during runtime + * + * Allocates memory at construction + * @param maxSize size_t of Maximum allowed size + */ + FixedOrderedMultimap(size_t maxSize):theMap(maxSize), _size(0){ + } - uint32_t findFirstIndex(key_t key, uint32_t startAt = 0) const { - if (startAt >= _size) { - return startAt + 1; - } - uint32_t i = startAt; - for (i = startAt; i < _size; ++i) { - if (theMap[i].first == key) { - return i; - } - } - return i; - } - - uint32_t findNicePlace(key_t key) const { - uint32_t i = 0; - for (i = 0; i < _size; ++i) { - if (myComp(key, theMap[i].first)) { - return i; - } - } - return i; - } - - void removeFromPosition(uint32_t position) { - if (_size <= position) { - return; - } - memmove(static_cast(&theMap[position]), static_cast(&theMap[position + 1]), - (_size - position - 1) * sizeof(std::pair)); - --_size; - } -public: - FixedOrderedMultimap(uint32_t maxSize) : - theMap(maxSize), _size(0) { - } + /*** + * Virtual destructor frees Memory by deleting its member + */ virtual ~FixedOrderedMultimap() { } - class Iterator: public ArrayList, uint32_t>::Iterator { + /*** + * Special iterator for FixedOrderedMultimap + */ + class Iterator: public ArrayList, size_t>::Iterator { public: Iterator() : - ArrayList, uint32_t>::Iterator() { + ArrayList, size_t>::Iterator() { } Iterator(std::pair *pair) : - ArrayList, uint32_t>::Iterator(pair) { + ArrayList, size_t>::Iterator(pair) { } }; + /*** + * Returns an iterator pointing to the first element + * @return Iterator pointing to first element + */ Iterator begin() const { return Iterator(&theMap[0]); } + /** + * Returns an iterator pointing to one element past the end + * @return Iterator pointing to one element past the end + */ Iterator end() const { return Iterator(&theMap[_size]); } - uint32_t size() const { + /*** + * Returns the current size of the map (not maximum size!) + * @return Current size + */ + size_t size() const{ return _size; } - ReturnValue_t insert(key_t key, T value, Iterator *storedValue = nullptr) { - if (_size == theMap.maxSize()) { - return MAP_FULL; - } - uint32_t position = findNicePlace(key); - memmove(static_cast(&theMap[position + 1]),static_cast(&theMap[position]), - (_size - position) * sizeof(std::pair)); - theMap[position].first = key; - theMap[position].second = value; - ++_size; - if (storedValue != nullptr) { - *storedValue = Iterator(&theMap[position]); - } - return HasReturnvaluesIF::RETURN_OK; + /** + * Clears the map, does not deallocate any memory + */ + void clear(){ + _size = 0; } - ReturnValue_t insert(std::pair pair) { - return insert(pair.first, pair.second); + /** + * Returns the maximum size of the map + * @return Maximum size of the map + */ + size_t maxSize() const{ + return theMap.maxSize(); } - ReturnValue_t exists(key_t key) const { - ReturnValue_t result = KEY_DOES_NOT_EXIST; - if (findFirstIndex(key) < _size) { - result = HasReturnvaluesIF::RETURN_OK; - } - return result; - } + /*** + * Used to insert a key and value separately. + * + * @param[in] key Key of the new element + * @param[in] value Value of the new element + * @param[in/out] (optional) storedValue On success this points to the new value, otherwise a nullptr + * @return RETURN_OK if insert was successful, MAP_FULL if no space is available + */ + ReturnValue_t insert(key_t key, T value, Iterator *storedValue = nullptr); - ReturnValue_t erase(Iterator *iter) { - uint32_t i; - if ((i = findFirstIndex((*iter).value->first)) >= _size) { - return KEY_DOES_NOT_EXIST; - } - removeFromPosition(i); - if (*iter != begin()) { - (*iter)--; - } else { - *iter = begin(); - } - return HasReturnvaluesIF::RETURN_OK; - } + /*** + * Used to insert new pair instead of single values + * + * @param pair Pair to be inserted + * @return RETURN_OK if insert was successful, MAP_FULL if no space is available + */ + ReturnValue_t insert(std::pair pair); - ReturnValue_t erase(key_t key) { - uint32_t i; - if ((i = findFirstIndex(key)) >= _size) { - return KEY_DOES_NOT_EXIST; - } - do { - removeFromPosition(i); - i = findFirstIndex(key, i); - } while (i < _size); - return HasReturnvaluesIF::RETURN_OK; - } + /*** + * Can be used to check if a certain key is in the map + * @param key Key to be checked + * @return RETURN_OK if the key exists KEY_DOES_NOT_EXIST otherwise + */ + ReturnValue_t exists(key_t key) const; - Iterator find(key_t key) const { + /*** + * Used to delete the element in the iterator + * + * The iterator will point to the element before or begin(), + * but never to one element in front of the map. + * + * @warning The iterator needs to be valid and dereferenceable + * @param[in/out] iter Pointer to iterator to the element that needs to be ereased + * @return RETURN_OK if erased, KEY_DOES_NOT_EXIST if the there is no element like this + */ + ReturnValue_t erase(Iterator *iter); + + /*** + * Used to erase by key + * @param key Key to be erased + * @return RETURN_OK if erased, KEY_DOES_NOT_EXIST if the there is no element like this + */ + ReturnValue_t erase(key_t key); + + /*** + * Find returns the first appearance of the key + * + * If the key does not exist, it points to end() + * + * @param key Key to search for + * @return Iterator pointing to the first entry of key + */ + Iterator find(key_t key) const{ ReturnValue_t result = exists(key); if (result != HasReturnvaluesIF::RETURN_OK) { return end(); } return Iterator(&theMap[findFirstIndex(key)]); + }; + + /*** + * Finds first entry of the given key and returns a + * pointer to the value + * + * @param key Key to search for + * @param value Found value + * @return RETURN_OK if it points to the value, + * KEY_DOES_NOT_EXIST if the key is not in the map + */ + ReturnValue_t find(key_t key, T **value) const; + + friend bool operator==(const typename FixedOrderedMultimap::Iterator& lhs, + const typename FixedOrderedMultimap::Iterator& rhs) { + return (lhs.value == rhs.value); } - ReturnValue_t find(key_t key, T **value) const { - ReturnValue_t result = exists(key); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - *value = &theMap[findFirstIndex(key)].second; - return HasReturnvaluesIF::RETURN_OK; + friend bool operator!=(const typename FixedOrderedMultimap::Iterator& lhs, + const typename FixedOrderedMultimap::Iterator& rhs) { + return not (lhs.value == rhs.value); } - void clear() { - _size = 0; - } +private: + typedef KEY_COMPARE compare; + compare myComp; + ArrayList, size_t> theMap; + size_t _size; - uint32_t maxSize() const { - return theMap.maxSize(); - } + size_t findFirstIndex(key_t key, size_t startAt = 0) const; + size_t findNicePlace(key_t key) const; + + void removeFromPosition(size_t position); }; -#endif /* FRAMEWORK_CONTAINER_FIXEDORDEREDMULTIMAP_H_ */ +#include "FixedOrderedMultimap.tpp" + +#endif /* FSFW_CONTAINER_FIXEDORDEREDMULTIMAP_H_ */ diff --git a/container/FixedOrderedMultimap.tpp b/container/FixedOrderedMultimap.tpp new file mode 100644 index 00000000..4aa85e97 --- /dev/null +++ b/container/FixedOrderedMultimap.tpp @@ -0,0 +1,109 @@ +#ifndef FRAMEWORK_CONTAINER_FIXEDORDEREDMULTIMAP_TPP_ +#define FRAMEWORK_CONTAINER_FIXEDORDEREDMULTIMAP_TPP_ + + +template +inline ReturnValue_t FixedOrderedMultimap::insert(key_t key, T value, Iterator *storedValue) { + if (_size == theMap.maxSize()) { + return MAP_FULL; + } + size_t position = findNicePlace(key); + memmove(static_cast(&theMap[position + 1]),static_cast(&theMap[position]), + (_size - position) * sizeof(std::pair)); + theMap[position].first = key; + theMap[position].second = value; + ++_size; + if (storedValue != nullptr) { + *storedValue = Iterator(&theMap[position]); + } + return HasReturnvaluesIF::RETURN_OK; +} +template +inline ReturnValue_t FixedOrderedMultimap::insert(std::pair pair) { + return insert(pair.first, pair.second); +} + +template +inline ReturnValue_t FixedOrderedMultimap::exists(key_t key) const { + ReturnValue_t result = KEY_DOES_NOT_EXIST; + if (findFirstIndex(key) < _size) { + result = HasReturnvaluesIF::RETURN_OK; + } + return result; +} + +template +inline ReturnValue_t FixedOrderedMultimap::erase(Iterator *iter) { + size_t i; + if ((i = findFirstIndex((*iter).value->first)) >= _size) { + return KEY_DOES_NOT_EXIST; + } + removeFromPosition(i); + if (*iter != begin()) { + (*iter)--; + } else { + *iter = begin(); + } + return HasReturnvaluesIF::RETURN_OK; +} + +template +inline ReturnValue_t FixedOrderedMultimap::erase(key_t key) { + size_t i; + if ((i = findFirstIndex(key)) >= _size) { + return KEY_DOES_NOT_EXIST; + } + do { + removeFromPosition(i); + i = findFirstIndex(key, i); + } while (i < _size); + return HasReturnvaluesIF::RETURN_OK; +} + +template +inline ReturnValue_t FixedOrderedMultimap::find(key_t key, T **value) const { + ReturnValue_t result = exists(key); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + *value = &theMap[findFirstIndex(key)].second; + return HasReturnvaluesIF::RETURN_OK; +} + +template +inline size_t FixedOrderedMultimap::findFirstIndex(key_t key, size_t startAt) const { + if (startAt >= _size) { + return startAt + 1; + } + size_t i = startAt; + for (i = startAt; i < _size; ++i) { + if (theMap[i].first == key) { + return i; + } + } + return i; +} + +template +inline size_t FixedOrderedMultimap::findNicePlace(key_t key) const { + size_t i = 0; + for (i = 0; i < _size; ++i) { + if (myComp(key, theMap[i].first)) { + return i; + } + } + return i; +} + +template +inline void FixedOrderedMultimap::removeFromPosition(size_t position) { + if (_size <= position) { + return; + } + memmove(static_cast(&theMap[position]), static_cast(&theMap[position + 1]), + (_size - position - 1) * sizeof(std::pair)); + --_size; +} + + +#endif /* FRAMEWORK_CONTAINER_FIXEDORDEREDMULTIMAP_TPP_ */ diff --git a/container/SharedRingBuffer.cpp b/container/SharedRingBuffer.cpp index 800e75d3..6ddb3d3e 100644 --- a/container/SharedRingBuffer.cpp +++ b/container/SharedRingBuffer.cpp @@ -9,6 +9,7 @@ SharedRingBuffer::SharedRingBuffer(object_id_t objectId, const size_t size, mutex = MutexFactory::instance()->createMutex(); } + SharedRingBuffer::SharedRingBuffer(object_id_t objectId, uint8_t *buffer, const size_t size, bool overwriteOld, size_t maxExcessBytes): SystemObject(objectId), SimpleRingBuffer(buffer, size, overwriteOld, @@ -16,6 +17,11 @@ SharedRingBuffer::SharedRingBuffer(object_id_t objectId, uint8_t *buffer, mutex = MutexFactory::instance()->createMutex(); } + +void SharedRingBuffer::setToUseReceiveSizeFIFO(size_t fifoDepth) { + this->fifoDepth = fifoDepth; +} + ReturnValue_t SharedRingBuffer::lockRingBufferMutex( MutexIF::TimeoutType timeoutType, dur_millis_t timeout) { return mutex->lockMutex(timeoutType, timeout); @@ -25,6 +31,25 @@ ReturnValue_t SharedRingBuffer::unlockRingBufferMutex() { return mutex->unlockMutex(); } + + MutexIF* SharedRingBuffer::getMutexHandle() const { return mutex; } + +ReturnValue_t SharedRingBuffer::initialize() { + if(fifoDepth > 0) { + receiveSizesFIFO = new DynamicFIFO(fifoDepth); + } + return SystemObject::initialize(); +} + +DynamicFIFO* SharedRingBuffer::getReceiveSizesFIFO() { + if(receiveSizesFIFO == nullptr) { + // Configuration error. + sif::warning << "SharedRingBuffer::getReceiveSizesFIFO: Ring buffer" + << " was not configured to have sizes FIFO, returning nullptr!" + << std::endl; + } + return receiveSizesFIFO; +} diff --git a/container/SharedRingBuffer.h b/container/SharedRingBuffer.h index 80c068b3..64d7ee29 100644 --- a/container/SharedRingBuffer.h +++ b/container/SharedRingBuffer.h @@ -2,6 +2,7 @@ #define FSFW_CONTAINER_SHAREDRINGBUFFER_H_ #include "SimpleRingBuffer.h" +#include "DynamicFIFO.h" #include "../ipc/MutexIF.h" #include "../objectmanager/SystemObject.h" #include "../timemanager/Clock.h" @@ -26,6 +27,16 @@ public: SharedRingBuffer(object_id_t objectId, const size_t size, bool overwriteOld, size_t maxExcessBytes); + /** + * @brief This function can be used to add an optional FIFO to the class + * @details + * This FIFO will be allocated in the initialize function (and will + * have a fixed maximum size after that). It can be used to store + * values like packet sizes, for example for a shared ring buffer + * used by producer/consumer tasks. + */ + void setToUseReceiveSizeFIFO(size_t fifoDepth); + /** * This constructor takes an external buffer with the specified size. * @param buffer @@ -59,8 +70,21 @@ public: * @return */ MutexIF* getMutexHandle() const; + + ReturnValue_t initialize() override; + + /** + * If the shared ring buffer was configured to have a sizes FIFO, a handle + * to that FIFO can be retrieved with this function. + * Do not forget to protect access with a lock if required! + * @return + */ + DynamicFIFO* getReceiveSizesFIFO(); private: MutexIF* mutex = nullptr; + + size_t fifoDepth = 0; + DynamicFIFO* receiveSizesFIFO = nullptr; }; diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp index 227d5b6d..e4bb6d71 100644 --- a/devicehandlers/DeviceHandlerBase.cpp +++ b/devicehandlers/DeviceHandlerBase.cpp @@ -1251,7 +1251,7 @@ void DeviceHandlerBase::buildInternalCommand(void) { if (result == BUSY) { //so we can track misconfigurations sif::debug << std::hex << getObjectId() - << ": DHB::buildInternalCommand: Busy" << std::endl; + << ": DHB::buildInternalCommand: Busy" << std::dec << std::endl; result = NOTHING_TO_SEND; //no need to report this } } diff --git a/ipc/MessageQueueIF.h b/ipc/MessageQueueIF.h index b0347db9..1c06521c 100644 --- a/ipc/MessageQueueIF.h +++ b/ipc/MessageQueueIF.h @@ -1,61 +1,77 @@ #ifndef FSFW_IPC_MESSAGEQUEUEIF_H_ #define FSFW_IPC_MESSAGEQUEUEIF_H_ -// COULDDO: We could support blocking calls - #include "messageQueueDefinitions.h" -#include "MessageQueueMessage.h" +#include "MessageQueueMessageIF.h" #include "../returnvalues/HasReturnvaluesIF.h" +#include + + +// COULDDO: We could support blocking calls +// semaphores are being implemented, which makes this idea even more iteresting. + +/** + * @defgroup message_queue Message Queue + * @brief Message Queue related software components + */ class MessageQueueIF { public: static const MessageQueueId_t NO_QUEUE = 0; static const uint8_t INTERFACE_ID = CLASS_ID::MESSAGE_QUEUE_IF; - /** - * No new messages on the queue - */ + //! No new messages on the queue static const ReturnValue_t EMPTY = MAKE_RETURN_CODE(1); - /** - * No space left for more messages - */ + //! No space left for more messages static const ReturnValue_t FULL = MAKE_RETURN_CODE(2); - /** - * Returned if a reply method was called without partner - */ + //! Returned if a reply method was called without partner static const ReturnValue_t NO_REPLY_PARTNER = MAKE_RETURN_CODE(3); + //! Returned if the target destination is invalid. + static constexpr ReturnValue_t DESTINVATION_INVALID = MAKE_RETURN_CODE(4); virtual ~MessageQueueIF() {} /** * @brief This operation sends a message to the last communication partner. - * @details This operation simplifies answering an incoming message by using the stored - * lastParnter information as destination. If there was no message received yet - * (i.e. lastPartner is zero), an error code is returned. - * @param message A pointer to a previously created message, which is sent. - * \return RETURN_OK if ok - * \return NO_REPLY_PARTNER Should return NO_REPLY_PARTNER if partner was found + * @details + * This operation simplifies answering an incoming message by using the + * stored lastParnter information as destination. If there was no message + * received yet (i.e. lastPartner is zero), an error code is returned. + * @param message + * A pointer to a previously created message, which is sent. + * @return + * -@c RETURN_OK if ok + * -@c NO_REPLY_PARTNER Should return NO_REPLY_PARTNER if partner was found. */ - virtual ReturnValue_t reply( MessageQueueMessage* message ) = 0; + virtual ReturnValue_t reply(MessageQueueMessageIF* message) = 0; /** - * @brief This function reads available messages from the message queue and returns the sender. - * @details It works identically to the other receiveMessage call, but in addition returns the - * sender's queue id. - * @param message A pointer to a message in which the received data is stored. - * @param receivedFrom A pointer to a queue id in which the sender's id is stored. + * @brief This function reads available messages from the message queue + * and returns the sender. + * @details + * It works identically to the other receiveMessage call, but in addition + * returns the sender's queue id. + * @param message + * A pointer to a message in which the received data is stored. + * @param receivedFrom + * A pointer to a queue id in which the sender's id is stored. */ - virtual ReturnValue_t receiveMessage(MessageQueueMessage* message, + virtual ReturnValue_t receiveMessage(MessageQueueMessageIF* message, MessageQueueId_t *receivedFrom) = 0; /** * @brief This function reads available messages from the message queue. - * @details If data is available it is stored in the passed message pointer. The message's - * original content is overwritten and the sendFrom information is stored in the - * lastPartner attribute. Else, the lastPartner information remains untouched, the - * message's content is cleared and the function returns immediately. - * @param message A pointer to a message in which the received data is stored. + * @details + * If data is available it is stored in the passed message pointer. + * The message's original content is overwritten and the sendFrom + * information is stored in theblastPartner attribute. Else, the lastPartner + * information remains untouched, the message's content is cleared and the + * function returns immediately. + * @param message + * A pointer to a message in which the received data is stored. + * @return -@c RETURN_OK on success + * -@c MessageQueueIF::EMPTY if queue is empty */ - virtual ReturnValue_t receiveMessage(MessageQueueMessage* message) = 0; + virtual ReturnValue_t receiveMessage(MessageQueueMessageIF* message) = 0; /** * Deletes all pending messages in the queue. * @param count The number of flushed messages. @@ -63,57 +79,89 @@ public: */ virtual ReturnValue_t flush(uint32_t* count) = 0; /** - * @brief This method returns the message queue id of the last communication partner. + * @brief This method returns the message queue + * id of the last communication partner. */ virtual MessageQueueId_t getLastPartner() const = 0; /** - * @brief This method returns the message queue id of this class's message queue. + * @brief This method returns the message queue + * id of this class's message queue. */ virtual MessageQueueId_t getId() const = 0; /** - * \brief With the sendMessage call, a queue message is sent to a receiving queue. - * \details This method takes the message provided, adds the sentFrom information and passes - * it on to the destination provided with an operating system call. The OS's return - * value is returned. - * \param sendTo This parameter specifies the message queue id to send the message to. - * \param message This is a pointer to a previously created message, which is sent. - * \param sentFrom The sentFrom information can be set to inject the sender's queue id into the message. - * This variable is set to zero by default. - * \param ignoreFault If set to true, the internal software fault counter is not incremented if queue is full (if implemented). + * @brief With the sendMessage call, a queue message + * is sent to a receiving queue. + * @details + * This method takes the message provided, adds the sentFrom information + * and passes it on to the destination provided with an operating system + * call. The OS's returnvalue is returned. + * @param sendTo + * This parameter specifies the message queue id to send the message to. + * @param message + * This is a pointer to a previously created message, which is sent. + * @param sentFrom + * The sentFrom information can be set to inject the sender's queue id + * into the message. This variable is set to zero by default. + * @param ignoreFault + * If set to true, the internal software fault counter is not incremented + * if queue is full (if implemented). + * @return -@c RETURN_OK on success + * -@c MessageQueueIF::FULL if queue is full */ - virtual ReturnValue_t sendMessageFrom( MessageQueueId_t sendTo, MessageQueueMessage* message, MessageQueueId_t sentFrom, bool ignoreFault = false ) = 0; - /** - * @brief This operation sends a message to the given destination. - * @details It directly uses the sendMessage call of the MessageQueueSender parent, but passes its - * queue id as "sentFrom" parameter. - * @param sendTo This parameter specifies the message queue id of the destination message queue. - * @param message A pointer to a previously created message, which is sent. - * @param ignoreFault If set to true, the internal software fault counter is not incremented if queue is full. - */ - virtual ReturnValue_t sendMessage( MessageQueueId_t sendTo, MessageQueueMessage* message, bool ignoreFault = false ) = 0; + virtual ReturnValue_t sendMessageFrom( MessageQueueId_t sendTo, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom, + bool ignoreFault = false ) = 0; /** - * \brief The sendToDefaultFrom method sends a queue message to the default destination. - * \details In all other aspects, it works identical to the sendMessage method. - * \param message This is a pointer to a previously created message, which is sent. - * \param sentFrom The sentFrom information can be set to inject the sender's queue id into the message. - * This variable is set to zero by default. + * @brief This operation sends a message to the given destination. + * @details + * It directly uses the sendMessage call of the MessageQueueSender parent, + * but passes its queue id as "sentFrom" parameter. + * @param sendTo + * This parameter specifies the message queue id of the destination + * message queue. + * @param message + * A pointer to a previously created message, which is sent. + * @param ignoreFault + * If set to true, the internal software fault counter is not incremented + * if queue is full. */ - virtual ReturnValue_t sendToDefaultFrom( MessageQueueMessage* message, MessageQueueId_t sentFrom, bool ignoreFault = false ) = 0; + virtual ReturnValue_t sendMessage( MessageQueueId_t sendTo, + MessageQueueMessageIF* message, bool ignoreFault = false ) = 0; + + /** + * @brief The sendToDefaultFrom method sends a queue message + * to the default destination. + * @details + * In all other aspects, it works identical to the sendMessage method. + * @param message + * This is a pointer to a previously created message, which is sent. + * @param sentFrom + * The sentFrom information can be set to inject the sender's queue id + * into the message. This variable is set to zero by default. + * @return -@c RETURN_OK on success + * -@c MessageQueueIF::FULL if queue is full + */ + virtual ReturnValue_t sendToDefaultFrom( MessageQueueMessageIF* message, + MessageQueueId_t sentFrom, bool ignoreFault = false ) = 0; /** * @brief This operation sends a message to the default destination. - * @details As in the sendMessage method, this function uses the sendToDefault call of the - * Implementation class and adds its queue id as "sentFrom" information. + * @details + * As in the sendMessage method, this function uses the sendToDefault + * call of the Implementation class and adds its queue id as + * "sentFrom" information. * @param message A pointer to a previously created message, which is sent. + * @return -@c RETURN_OK on success + * -@c MessageQueueIF::FULL if queue is full */ - virtual ReturnValue_t sendToDefault( MessageQueueMessage* message ) = 0; + virtual ReturnValue_t sendToDefault( MessageQueueMessageIF* message ) = 0; /** - * \brief This method is a simple setter for the default destination. + * @brief This method is a simple setter for the default destination. */ virtual void setDefaultDestination(MessageQueueId_t defaultDestination) = 0; /** - * \brief This method is a simple getter for the default destination. + * @brief This method is a simple getter for the default destination. */ virtual MessageQueueId_t getDefaultDestination() const = 0; @@ -122,4 +170,4 @@ public: -#endif /* FRAMEWORK_IPC_MESSAGEQUEUEIF_H_ */ +#endif /* FSFW_IPC_MESSAGEQUEUEIF_H_ */ diff --git a/ipc/MessageQueueSenderIF.h b/ipc/MessageQueueSenderIF.h index 7eea5146..683c2908 100644 --- a/ipc/MessageQueueSenderIF.h +++ b/ipc/MessageQueueSenderIF.h @@ -1,8 +1,8 @@ #ifndef FSFW_IPC_MESSAGEQUEUESENDERIF_H_ #define FSFW_IPC_MESSAGEQUEUESENDERIF_H_ -#include "../ipc/MessageQueueIF.h" -#include "../ipc/MessageQueueMessageIF.h" +#include "MessageQueueIF.h" +#include "MessageQueueMessageIF.h" #include "../objectmanager/ObjectManagerIF.h" class MessageQueueSenderIF { @@ -15,7 +15,7 @@ public: * Not sure whether this is actually a good idea. */ static ReturnValue_t sendMessage(MessageQueueId_t sendTo, - MessageQueueMessage* message, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom = MessageQueueIF::NO_QUEUE, bool ignoreFault = false); private: diff --git a/ipc/QueueFactory.h b/ipc/QueueFactory.h index 4f4d72c7..9853d256 100644 --- a/ipc/QueueFactory.h +++ b/ipc/QueueFactory.h @@ -1,8 +1,10 @@ -#ifndef FRAMEWORK_IPC_QUEUEFACTORY_H_ -#define FRAMEWORK_IPC_QUEUEFACTORY_H_ +#ifndef FSFW_IPC_QUEUEFACTORY_H_ +#define FSFW_IPC_QUEUEFACTORY_H_ #include "MessageQueueIF.h" -#include +#include "MessageQueueMessage.h" +#include + /** * Creates message queues. * This class is a "singleton" interface, i.e. it provides an @@ -30,4 +32,4 @@ private: static QueueFactory* factoryInstance; }; -#endif /* FRAMEWORK_IPC_QUEUEFACTORY_H_ */ +#endif /* FSFW_IPC_QUEUEFACTORY_H_ */ diff --git a/osal/FreeRTOS/MessageQueue.cpp b/osal/FreeRTOS/MessageQueue.cpp index 6c8e1de9..2dfe5ab6 100644 --- a/osal/FreeRTOS/MessageQueue.cpp +++ b/osal/FreeRTOS/MessageQueue.cpp @@ -1,42 +1,76 @@ #include "MessageQueue.h" - #include "../../objectmanager/ObjectManagerIF.h" #include "../../serviceinterface/ServiceInterfaceStream.h" -// TODO I guess we should have a way of checking if we are in an ISR and then use the "fromISR" versions of all calls - -MessageQueue::MessageQueue(size_t message_depth, size_t max_message_size) : -defaultDestination(0),lastPartner(0) { - handle = xQueueCreate(message_depth, max_message_size); - if (handle == NULL) { - sif::error << "MessageQueue creation failed" << std::endl; +// TODO I guess we should have a way of checking if we are in an ISR and then +// use the "fromISR" versions of all calls +// As a first step towards this, introduces system context variable which needs +// to be switched manually +// Haven't found function to find system context. +MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize): + maxMessageSize(maxMessageSize) { + handle = xQueueCreate(messageDepth, maxMessageSize); + if (handle == nullptr) { + sif::error << "MessageQueue::MessageQueue Creation failed" << std::endl; } } MessageQueue::~MessageQueue() { - if (handle != NULL) { + if (handle != nullptr) { vQueueDelete(handle); } } +void MessageQueue::switchSystemContext(CallContext callContext) { + this->callContext = callContext; +} + ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo, - MessageQueueMessage* message, bool ignoreFault) { + MessageQueueMessageIF* message, bool ignoreFault) { return sendMessageFrom(sendTo, message, this->getId(), ignoreFault); } -ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessage* message) { +ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) { return sendToDefaultFrom(message, this->getId()); } -ReturnValue_t MessageQueue::reply(MessageQueueMessage* message) { - if (this->lastPartner != 0) { +ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessageIF* message, + MessageQueueId_t sentFrom, bool ignoreFault) { + return sendMessageFrom(defaultDestination,message,sentFrom,ignoreFault); +} + +ReturnValue_t MessageQueue::reply(MessageQueueMessageIF* message) { + if (this->lastPartner != MessageQueueIF::NO_QUEUE) { return sendMessageFrom(this->lastPartner, message, this->getId()); } else { return NO_REPLY_PARTNER; } } -ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message, +ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom, + bool ignoreFault) { + return sendMessageFromMessageQueue(sendTo, message, sentFrom, ignoreFault, + callContext); +} + + +ReturnValue_t MessageQueue::handleSendResult(BaseType_t result, bool ignoreFault) { + if (result != pdPASS) { + if (not ignoreFault) { + InternalErrorReporterIF* internalErrorReporter = objectManager-> + get( + objects::INTERNAL_ERROR_REPORTER); + if (internalErrorReporter != nullptr) { + internalErrorReporter->queueMessageNotSent(); + } + } + return MessageQueueIF::FULL; + } + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message, MessageQueueId_t* receivedFrom) { ReturnValue_t status = this->receiveMessage(message); if(status == HasReturnvaluesIF::RETURN_OK) { @@ -45,8 +79,9 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message, return status; } -ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message) { - BaseType_t result = xQueueReceive(handle,reinterpret_cast(message->getBuffer()), 0); +ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) { + BaseType_t result = xQueueReceive(handle,reinterpret_cast( + message->getBuffer()), 0); if (result == pdPASS){ this->lastPartner = message->getSender(); return HasReturnvaluesIF::RETURN_OK; @@ -67,51 +102,55 @@ ReturnValue_t MessageQueue::flush(uint32_t* count) { } MessageQueueId_t MessageQueue::getId() const { - return (MessageQueueId_t) handle; + return reinterpret_cast(handle); } void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) { + defaultDestinationSet = true; this->defaultDestination = defaultDestination; } -ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, - MessageQueueMessage* message, MessageQueueId_t sentFrom, - bool ignoreFault) { - return sendMessageFromMessageQueue(sendTo,message,sentFrom,ignoreFault); -} - -ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessage* message, - MessageQueueId_t sentFrom, bool ignoreFault) { - return sendMessageFrom(defaultDestination,message,sentFrom,ignoreFault); -} - MessageQueueId_t MessageQueue::getDefaultDestination() const { return defaultDestination; } bool MessageQueue::isDefaultDestinationSet() const { - return 0; + return defaultDestinationSet; } + +// static core function to send messages. ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, - MessageQueueMessage *message, MessageQueueId_t sentFrom, - bool ignoreFault) { - message->setSender(sentFrom); + MessageQueueMessageIF* message, MessageQueueId_t sentFrom, + bool ignoreFault, CallContext callContext) { + BaseType_t result = pdFALSE; + QueueHandle_t destination = nullptr; - BaseType_t result = xQueueSendToBack(reinterpret_cast(sendTo), - reinterpret_cast(message->getBuffer()), 0); - if (result != pdPASS) { - if (!ignoreFault) { - InternalErrorReporterIF* internalErrorReporter = - objectManager->get( - objects::INTERNAL_ERROR_REPORTER); - if (internalErrorReporter != NULL) { - internalErrorReporter->queueMessageNotSent(); - } - } - return MessageQueueIF::FULL; + if(sendTo == MessageQueueIF::NO_QUEUE or sendTo == 0x00) { + return MessageQueueIF::DESTINVATION_INVALID; + } + else { + destination = reinterpret_cast(sendTo); } - return HasReturnvaluesIF::RETURN_OK; + message->setSender(sentFrom); + + + if(callContext == CallContext::TASK) { + result = xQueueSendToBack(destination, + static_cast(message->getBuffer()), 0); + } + else { + /* If the call context is from an interrupt, + * request a context switch if a higher priority task + * was blocked by the interrupt. */ + BaseType_t xHigherPriorityTaskWoken = pdFALSE; + result = xQueueSendFromISR(reinterpret_cast(sendTo), + static_cast(message->getBuffer()), + &xHigherPriorityTaskWoken); + if(xHigherPriorityTaskWoken == pdTRUE) { + TaskManagement::requestContextSwitch(callContext); + } + } + return handleSendResult(result, ignoreFault); } - diff --git a/osal/FreeRTOS/MessageQueue.h b/osal/FreeRTOS/MessageQueue.h index aa3d54c1..b99bf7c8 100644 --- a/osal/FreeRTOS/MessageQueue.h +++ b/osal/FreeRTOS/MessageQueue.h @@ -1,159 +1,150 @@ -#ifndef MESSAGEQUEUE_H_ -#define MESSAGEQUEUE_H_ +#ifndef FSFW_OSAL_FREERTOS_MESSAGEQUEUE_H_ +#define FSFW_OSAL_FREERTOS_MESSAGEQUEUE_H_ #include "../../internalError/InternalErrorReporterIF.h" #include "../../ipc/MessageQueueIF.h" -#include "../../ipc/MessageQueueMessage.h" +#include "../../ipc/MessageQueueMessageIF.h" +#include "../../osal/FreeRTOS/TaskManagement.h" -#include -#include +#include +#include +#include -//TODO this class assumes that MessageQueueId_t is the same size as void* (the FreeRTOS handle type), compiler will catch this but it might be nice to have something checking or even an always working solution +// TODO: this class assumes that MessageQueueId_t is the same size as void* +// (the FreeRTOS handle type), compiler will catch this but it might be nice +// to have something checking or even an always working solution // https://scaryreasoner.wordpress.com/2009/02/28/checking-sizeof-at-compile-time/ /** - * @brief This class manages sending and receiving of message queue messages. + * @brief This class manages sending and receiving of + * message queue messages. + * @details + * Message queues are used to pass asynchronous messages between processes. + * They work like post boxes, where all incoming messages are stored in FIFO + * order. This class creates a new receiving queue and provides methods to fetch + * received messages. Being a child of MessageQueueSender, this class also + * provides methods to send a message to a user-defined or a default destination. + * In addition it also provides a reply method to answer to the queue it + * received its last message from. * - * @details Message queues are used to pass asynchronous messages between processes. - * They work like post boxes, where all incoming messages are stored in FIFO - * order. This class creates a new receiving queue and provides methods to fetch - * received messages. Being a child of MessageQueueSender, this class also provides - * methods to send a message to a user-defined or a default destination. In addition - * it also provides a reply method to answer to the queue it received its last message - * from. - * The MessageQueue should be used as "post box" for a single owning object. So all - * message queue communication is "n-to-one". - * For creating the queue, as well as sending and receiving messages, the class makes - * use of the operating system calls provided. - * \ingroup message_queue + * The MessageQueue should be used as "post box" for a single owning object. + * So all message queue communication is "n-to-one". + * For creating the queue, as well as sending and receiving messages, the class + * makes use of the operating system calls provided. + * + * Please keep in mind that FreeRTOS offers different calls for message queue + * operations if called from an ISR. + * For now, the system context needs to be switched manually. + * @ingroup osal + * @ingroup message_queue */ class MessageQueue : public MessageQueueIF { friend class MessageQueueSenderIF; public: /** * @brief The constructor initializes and configures the message queue. - * @details By making use of the according operating system call, a message queue is created - * and initialized. The message depth - the maximum number of messages to be - * buffered - may be set with the help of a parameter, whereas the message size is - * automatically set to the maximum message queue message size. The operating system - * sets the message queue id, or i case of failure, it is set to zero. - * @param message_depth The number of messages to be buffered before passing an error to the - * sender. Default is three. - * @param max_message_size With this parameter, the maximum message size can be adjusted. - * This should be left default. + * @details + * By making use of the according operating system call, a message queue + * is created and initialized. The message depth - the maximum number of + * messages to be buffered - may be set with the help of a parameter, + * whereas the message size is automatically set to the maximum message + * queue message size. The operating system sets the message queue id, or + * in case of failure, it is set to zero. + * @param message_depth + * The number of messages to be buffered before passing an error to the + * sender. Default is three. + * @param max_message_size + * With this parameter, the maximum message size can be adjusted. + * This should be left default. */ - MessageQueue( size_t message_depth = 3, size_t max_message_size = MessageQueueMessage::MAX_MESSAGE_SIZE ); + MessageQueue( size_t messageDepth = 3, + size_t maxMessageSize = MessageQueueMessage::MAX_MESSAGE_SIZE ); + + /** Copying message queues forbidden */ + MessageQueue(const MessageQueue&) = delete; + MessageQueue& operator=(const MessageQueue&) = delete; + /** * @brief The destructor deletes the formerly created message queue. - * @details This is accomplished by using the delete call provided by the operating system. + * @details This is accomplished by using the delete call provided + * by the operating system. */ virtual ~MessageQueue(); + /** - * @brief This operation sends a message to the given destination. - * @details It directly uses the sendMessage call of the MessageQueueSender parent, but passes its - * queue id as "sentFrom" parameter. - * @param sendTo This parameter specifies the message queue id of the destination message queue. - * @param message A pointer to a previously created message, which is sent. - * @param ignoreFault If set to true, the internal software fault counter is not incremented if queue is full. + * This function is used to switch the call context. This has to be called + * if a message is sent or received from an ISR! + * @param callContext */ + void switchSystemContext(CallContext callContext); + + /** MessageQueueIF implementation */ ReturnValue_t sendMessage(MessageQueueId_t sendTo, - MessageQueueMessage* message, bool ignoreFault = false ); - /** - * @brief This operation sends a message to the default destination. - * @details As in the sendMessage method, this function uses the sendToDefault call of the - * MessageQueueSender parent class and adds its queue id as "sentFrom" information. - * @param message A pointer to a previously created message, which is sent. - */ - ReturnValue_t sendToDefault( MessageQueueMessage* message ); - /** - * @brief This operation sends a message to the last communication partner. - * @details This operation simplifies answering an incoming message by using the stored - * lastParnter information as destination. If there was no message received yet - * (i.e. lastPartner is zero), an error code is returned. - * @param message A pointer to a previously created message, which is sent. - */ - ReturnValue_t reply( MessageQueueMessage* message ); + MessageQueueMessageIF* message, bool ignoreFault = false) override; - /** - * @brief This function reads available messages from the message queue and returns the sender. - * @details It works identically to the other receiveMessage call, but in addition returns the - * sender's queue id. - * @param message A pointer to a message in which the received data is stored. - * @param receivedFrom A pointer to a queue id in which the sender's id is stored. - */ - ReturnValue_t receiveMessage(MessageQueueMessage* message, - MessageQueueId_t *receivedFrom); + ReturnValue_t sendToDefault(MessageQueueMessageIF* message) override; - /** - * @brief This function reads available messages from the message queue. - * @details If data is available it is stored in the passed message pointer. The message's - * original content is overwritten and the sendFrom information is stored in the - * lastPartner attribute. Else, the lastPartner information remains untouched, the - * message's content is cleared and the function returns immediately. - * @param message A pointer to a message in which the received data is stored. - */ - ReturnValue_t receiveMessage(MessageQueueMessage* message); - /** - * Deletes all pending messages in the queue. - * @param count The number of flushed messages. - * @return RETURN_OK on success. - */ - ReturnValue_t flush(uint32_t* count); - /** - * @brief This method returns the message queue id of the last communication partner. - */ - MessageQueueId_t getLastPartner() const; - /** - * @brief This method returns the message queue id of this class's message queue. - */ - MessageQueueId_t getId() const; - /** - * \brief With the sendMessage call, a queue message is sent to a receiving queue. - * \details This method takes the message provided, adds the sentFrom information and passes - * it on to the destination provided with an operating system call. The OS's return - * value is returned. - * \param sendTo This parameter specifies the message queue id to send the message to. - * \param message This is a pointer to a previously created message, which is sent. - * \param sentFrom The sentFrom information can be set to inject the sender's queue id into the message. - * This variable is set to zero by default. - * \param ignoreFault If set to true, the internal software fault counter is not incremented if queue is full. - */ - virtual ReturnValue_t sendMessageFrom( MessageQueueId_t sendTo, MessageQueueMessage* message, MessageQueueId_t sentFrom = NO_QUEUE, bool ignoreFault = false ); - /** - * \brief The sendToDefault method sends a queue message to the default destination. - * \details In all other aspects, it works identical to the sendMessage method. - * \param message This is a pointer to a previously created message, which is sent. - * \param sentFrom The sentFrom information can be set to inject the sender's queue id into the message. - * This variable is set to zero by default. - */ - virtual ReturnValue_t sendToDefaultFrom( MessageQueueMessage* message, MessageQueueId_t sentFrom = NO_QUEUE, bool ignoreFault = false ); - /** - * \brief This method is a simple setter for the default destination. - */ - void setDefaultDestination(MessageQueueId_t defaultDestination); - /** - * \brief This method is a simple getter for the default destination. - */ - MessageQueueId_t getDefaultDestination() const; + ReturnValue_t reply(MessageQueueMessageIF* message) override; + virtual ReturnValue_t sendMessageFrom(MessageQueueId_t sendTo, + MessageQueueMessageIF* message, + MessageQueueId_t sentFrom = NO_QUEUE, + bool ignoreFault = false) override; + + virtual ReturnValue_t sendToDefaultFrom( MessageQueueMessageIF* message, + MessageQueueId_t sentFrom = NO_QUEUE, + bool ignoreFault = false) override; + + ReturnValue_t receiveMessage(MessageQueueMessageIF* message, + MessageQueueId_t *receivedFrom) override; + + ReturnValue_t receiveMessage(MessageQueueMessageIF* message) override; + + ReturnValue_t flush(uint32_t* count) override; + + MessageQueueId_t getLastPartner() const override; + + MessageQueueId_t getId() const override; + + void setDefaultDestination(MessageQueueId_t defaultDestination) override; + + MessageQueueId_t getDefaultDestination() const override; + + bool isDefaultDestinationSet() const override; - bool isDefaultDestinationSet() const; protected: /** - * Implementation to be called from any send Call within MessageQueue and MessageQueueSenderIF - * \details This method takes the message provided, adds the sentFrom information and passes - * it on to the destination provided with an operating system call. The OS's return - * value is returned. - * \param sendTo This parameter specifies the message queue id to send the message to. - * \param message This is a pointer to a previously created message, which is sent. - * \param sentFrom The sentFrom information can be set to inject the sender's queue id into the message. - * This variable is set to zero by default. - * \param ignoreFault If set to true, the internal software fault counter is not incremented if queue is full. + * @brief Implementation to be called from any send Call within + * MessageQueue and MessageQueueSenderIF. + * @details + * This method takes the message provided, adds the sentFrom information and + * passes it on to the destination provided with an operating system call. + * The OS's return value is returned. + * @param sendTo + * This parameter specifies the message queue id to send the message to. + * @param message + * This is a pointer to a previously created message, which is sent. + * @param sentFrom + * The sentFrom information can be set to inject the sender's queue id into + * the message. This variable is set to zero by default. + * @param ignoreFault + * If set to true, the internal software fault counter is not incremented + * if queue is full. + * @param context Specify whether call is made from task or from an ISR. */ - static ReturnValue_t sendMessageFromMessageQueue(MessageQueueId_t sendTo,MessageQueueMessage* message, MessageQueueId_t sentFrom = NO_QUEUE,bool ignoreFault=false); + static ReturnValue_t sendMessageFromMessageQueue(MessageQueueId_t sendTo, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom = NO_QUEUE, + bool ignoreFault=false, CallContext callContext = CallContext::TASK); + + static ReturnValue_t handleSendResult(BaseType_t result, bool ignoreFault); + private: + bool defaultDestinationSet = false; QueueHandle_t handle; - MessageQueueId_t defaultDestination; - MessageQueueId_t lastPartner; + MessageQueueId_t defaultDestination = MessageQueueIF::NO_QUEUE; + MessageQueueId_t lastPartner = MessageQueueIF::NO_QUEUE; + const size_t maxMessageSize; + //! Stores the current system context + CallContext callContext = CallContext::TASK; }; -#endif /* MESSAGEQUEUE_H_ */ +#endif /* FSFW_OSAL_FREERTOS_MESSAGEQUEUE_H_ */ diff --git a/osal/FreeRTOS/QueueFactory.cpp b/osal/FreeRTOS/QueueFactory.cpp index 153d9b51..ed29e10c 100644 --- a/osal/FreeRTOS/QueueFactory.cpp +++ b/osal/FreeRTOS/QueueFactory.cpp @@ -1,14 +1,14 @@ +#include "MessageQueue.h" + #include "../../ipc/MessageQueueSenderIF.h" #include "../../ipc/QueueFactory.h" -#include "MessageQueue.h" - QueueFactory* QueueFactory::factoryInstance = nullptr; ReturnValue_t MessageQueueSenderIF::sendMessage(MessageQueueId_t sendTo, - MessageQueueMessage* message, MessageQueueId_t sentFrom, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom, bool ignoreFault) { return MessageQueue::sendMessageFromMessageQueue(sendTo,message, sentFrom,ignoreFault); diff --git a/osal/linux/MessageQueue.cpp b/osal/linux/MessageQueue.cpp index cb5ffd04..bc14374a 100644 --- a/osal/linux/MessageQueue.cpp +++ b/osal/linux/MessageQueue.cpp @@ -9,9 +9,11 @@ #include + MessageQueue::MessageQueue(uint32_t messageDepth, size_t maxMessageSize): id(MessageQueueIF::NO_QUEUE),lastPartner(MessageQueueIF::NO_QUEUE), - defaultDestination(MessageQueueIF::NO_QUEUE) { + defaultDestination(MessageQueueIF::NO_QUEUE), + maxMessageSize(maxMessageSize) { //debug << "MessageQueue::MessageQueue: Creating a queue" << std::endl; mq_attr attributes; this->id = 0; @@ -46,7 +48,7 @@ MessageQueue::~MessageQueue() { status = mq_unlink(name); if(status != 0){ sif::error << "MessageQueue::Destructor: mq_unlink Failed with status: " - << strerror(errno) <> defaultMqMaxMsg and defaultMqMaxMsg < messageDepth) { - // See: https://www.man7.org/linux/man-pages/man3/mq_open.3.html - // This happens if the msg_max value is not large enough - // It is ignored if the executable is run in privileged mode. - // Run the unlockRealtime script or grant the mode manually by using: - // sudo setcap 'CAP_SYS_RESOURCE=+ep' + /* + See: https://www.man7.org/linux/man-pages/man3/mq_open.3.html + This happens if the msg_max value is not large enough + It is ignored if the executable is run in privileged mode. + Run the unlockRealtime script or grant the mode manually by using: + sudo setcap 'CAP_SYS_RESOURCE=+ep' - // Persistent solution for session: - // echo | sudo tee /proc/sys/fs/mqueue/msg_max + Persistent solution for session: + echo | sudo tee /proc/sys/fs/mqueue/msg_max - // Permanent solution: - // sudo nano /etc/sysctl.conf - // Append at end: fs/mqueue/msg_max = - // Apply changes with: sudo sysctl -p + Permanent solution: + sudo nano /etc/sysctl.conf + Append at end: fs/mqueue/msg_max = + Apply changes with: sudo sysctl -p + */ sif::error << "MessageQueue::MessageQueue: Default MQ size " << defaultMqMaxMsg << " is too small for requested size " << messageDepth << std::endl; + sif::error << "This error can be fixed by setting the maximum " + "allowed message size higher!" << std::endl; + } break; } @@ -118,15 +125,15 @@ ReturnValue_t MessageQueue::handleError(mq_attr* attributes, } ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo, - MessageQueueMessage* message, bool ignoreFault) { + MessageQueueMessageIF* message, bool ignoreFault) { return sendMessageFrom(sendTo, message, this->getId(), false); } -ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessage* message) { +ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) { return sendToDefaultFrom(message, this->getId()); } -ReturnValue_t MessageQueue::reply(MessageQueueMessage* message) { +ReturnValue_t MessageQueue::reply(MessageQueueMessageIF* message) { if (this->lastPartner != 0) { return sendMessageFrom(this->lastPartner, message, this->getId()); } else { @@ -134,21 +141,34 @@ ReturnValue_t MessageQueue::reply(MessageQueueMessage* message) { } } -ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message, +ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message, MessageQueueId_t* receivedFrom) { ReturnValue_t status = this->receiveMessage(message); *receivedFrom = this->lastPartner; return status; } -ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message) { +ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) { + if(message == nullptr) { + sif::error << "MessageQueue::receiveMessage: Message is " + "nullptr!" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + + if(message->getMaximumMessageSize() < maxMessageSize) { + sif::error << "MessageQueue::receiveMessage: Message size " + << message->getMaximumMessageSize() + << " too small to receive data!" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + unsigned int messagePriority = 0; int status = mq_receive(id,reinterpret_cast(message->getBuffer()), - message->MAX_MESSAGE_SIZE,&messagePriority); + message->getMaximumMessageSize(),&messagePriority); if (status > 0) { this->lastPartner = message->getSender(); //Check size of incoming message. - if (message->messageSize < message->getMinimumMessageSize()) { + if (message->getMessageSize() < message->getMinimumMessageSize()) { return HasReturnvaluesIF::RETURN_FAILED; } return HasReturnvaluesIF::RETURN_OK; @@ -158,7 +178,7 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessage* message) { } else { //No message was received. Keep lastPartner anyway, I might send //something later. But still, delete packet content. - memset(message->getData(), 0, message->MAX_DATA_SIZE); + memset(message->getData(), 0, message->getMaximumMessageSize()); switch(errno){ case EAGAIN: //O_NONBLOCK or MQ_NONBLOCK was set and there are no messages @@ -258,18 +278,19 @@ void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) { this->defaultDestination = defaultDestination; } -ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, - MessageQueueMessage* message, MessageQueueId_t sentFrom, - bool ignoreFault) { - return sendMessageFromMessageQueue(sendTo,message,sentFrom,ignoreFault); - -} - -ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessage* message, +ReturnValue_t MessageQueue::sendToDefaultFrom(MessageQueueMessageIF* message, MessageQueueId_t sentFrom, bool ignoreFault) { return sendMessageFrom(defaultDestination, message, sentFrom, ignoreFault); } + +ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom, + bool ignoreFault) { + return sendMessageFromMessageQueue(sendTo,message, sentFrom,ignoreFault); + +} + MessageQueueId_t MessageQueue::getDefaultDestination() const { return this->defaultDestination; } @@ -281,11 +302,18 @@ bool MessageQueue::isDefaultDestinationSet() const { uint16_t MessageQueue::queueCounter = 0; ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, - MessageQueueMessage *message, MessageQueueId_t sentFrom, + MessageQueueMessageIF *message, MessageQueueId_t sentFrom, bool ignoreFault) { + if(message == nullptr) { + sif::error << "MessageQueue::sendMessageFromMessageQueue: Message is " + "nullptr!" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + message->setSender(sentFrom); int result = mq_send(sendTo, - reinterpret_cast(message->getBuffer()), message->messageSize,0); + reinterpret_cast(message->getBuffer()), + message->getMessageSize(),0); //TODO: Check if we're in ISR. if (result != 0) { @@ -303,13 +331,16 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, //MQ_NONBLOCK flag was set in its attributes, and the //specified queue is full. return MessageQueueIF::FULL; - case EBADF: + case EBADF: { //mq_des doesn't represent a valid message queue descriptor, //or mq_des wasn't opened for writing. - sif::error << "MessageQueue::sendMessage: Configuration error " - << strerror(errno) << " in mq_send mqSendTo: " << sendTo - << " sent from " << sentFrom << std::endl; - /*NO BREAK*/ + sif::error << "MessageQueue::sendMessage: Configuration error, MQ" + << " destination invalid." << std::endl; + sif::error << strerror(errno) << " in " + <<"mq_send to: " << sendTo << " sent from " + << sentFrom << std::endl; + return DESTINVATION_INVALID; + } case EINTR: //The call was interrupted by a signal. case EINVAL: diff --git a/osal/linux/MessageQueue.h b/osal/linux/MessageQueue.h index 31cdca11..239bbbdb 100644 --- a/osal/linux/MessageQueue.h +++ b/osal/linux/MessageQueue.h @@ -1,5 +1,5 @@ -#ifndef MESSAGEQUEUE_H_ -#define MESSAGEQUEUE_H_ +#ifndef FSFW_OSAL_LINUX_MESSAGEQUEUE_H_ +#define FSFW_OSAL_LINUX_MESSAGEQUEUE_H_ #include "../../internalError/InternalErrorReporterIF.h" #include "../../ipc/MessageQueueIF.h" @@ -56,14 +56,14 @@ public: * @param ignoreFault If set to true, the internal software fault counter is not incremented if queue is full. */ virtual ReturnValue_t sendMessage(MessageQueueId_t sendTo, - MessageQueueMessage* message, bool ignoreFault = false ); + MessageQueueMessageIF* message, bool ignoreFault = false ); /** * @brief This operation sends a message to the default destination. * @details As in the sendMessage method, this function uses the sendToDefault call of the * MessageQueueSender parent class and adds its queue id as "sentFrom" information. * @param message A pointer to a previously created message, which is sent. */ - virtual ReturnValue_t sendToDefault( MessageQueueMessage* message ); + virtual ReturnValue_t sendToDefault( MessageQueueMessageIF* message ); /** * @brief This operation sends a message to the last communication partner. * @details This operation simplifies answering an incoming message by using the stored @@ -71,7 +71,7 @@ public: * (i.e. lastPartner is zero), an error code is returned. * @param message A pointer to a previously created message, which is sent. */ - ReturnValue_t reply( MessageQueueMessage* message ); + ReturnValue_t reply( MessageQueueMessageIF* message ); /** * @brief This function reads available messages from the message queue and returns the sender. @@ -80,7 +80,7 @@ public: * @param message A pointer to a message in which the received data is stored. * @param receivedFrom A pointer to a queue id in which the sender's id is stored. */ - ReturnValue_t receiveMessage(MessageQueueMessage* message, + ReturnValue_t receiveMessage(MessageQueueMessageIF* message, MessageQueueId_t *receivedFrom); /** @@ -91,7 +91,7 @@ public: * message's content is cleared and the function returns immediately. * @param message A pointer to a message in which the received data is stored. */ - ReturnValue_t receiveMessage(MessageQueueMessage* message); + ReturnValue_t receiveMessage(MessageQueueMessageIF* message); /** * Deletes all pending messages in the queue. * @param count The number of flushed messages. @@ -114,7 +114,9 @@ public: * This variable is set to zero by default. * \param ignoreFault If set to true, the internal software fault counter is not incremented if queue is full. */ - virtual ReturnValue_t sendMessageFrom( MessageQueueId_t sendTo, MessageQueueMessage* message, MessageQueueId_t sentFrom, bool ignoreFault = false ); + virtual ReturnValue_t sendMessageFrom( MessageQueueId_t sendTo, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom, + bool ignoreFault = false ); /** * \brief The sendToDefault method sends a queue message to the default destination. * \details In all other aspects, it works identical to the sendMessage method. @@ -122,7 +124,8 @@ public: * \param sentFrom The sentFrom information can be set to inject the sender's queue id into the message. * This variable is set to zero by default. */ - virtual ReturnValue_t sendToDefaultFrom( MessageQueueMessage* message, MessageQueueId_t sentFrom = NO_QUEUE, bool ignoreFault = false ); + virtual ReturnValue_t sendToDefaultFrom( MessageQueueMessageIF* message, + MessageQueueId_t sentFrom = NO_QUEUE, bool ignoreFault = false ); /** * \brief This method is a simple setter for the default destination. */ @@ -145,7 +148,9 @@ protected: * This variable is set to zero by default. * \param ignoreFault If set to true, the internal software fault counter is not incremented if queue is full. */ - static ReturnValue_t sendMessageFromMessageQueue(MessageQueueId_t sendTo,MessageQueueMessage* message, MessageQueueId_t sentFrom = NO_QUEUE,bool ignoreFault=false); + static ReturnValue_t sendMessageFromMessageQueue(MessageQueueId_t sendTo, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom = NO_QUEUE, + bool ignoreFault=false); private: /** * @brief The class stores the queue id it got assigned from the operating system in this attribute. @@ -171,11 +176,12 @@ private: /** * The name of the message queue, stored for unlinking */ - char name[5]; + char name[16]; static uint16_t queueCounter; + const size_t maxMessageSize; ReturnValue_t handleError(mq_attr* attributes, uint32_t messageDepth); }; -#endif /* MESSAGEQUEUE_H_ */ +#endif /* FSFW_OSAL_LINUX_MESSAGEQUEUE_H_ */ diff --git a/osal/linux/QueueFactory.cpp b/osal/linux/QueueFactory.cpp index 0860950c..44def48a 100644 --- a/osal/linux/QueueFactory.cpp +++ b/osal/linux/QueueFactory.cpp @@ -15,7 +15,7 @@ QueueFactory* QueueFactory::factoryInstance = nullptr; ReturnValue_t MessageQueueSenderIF::sendMessage(MessageQueueId_t sendTo, - MessageQueueMessage* message, MessageQueueId_t sentFrom, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom, bool ignoreFault) { return MessageQueue::sendMessageFromMessageQueue(sendTo,message, sentFrom,ignoreFault); diff --git a/tcdistribution/CCSDSDistributor.cpp b/tcdistribution/CCSDSDistributor.cpp index 1ebb3185..ddd63308 100644 --- a/tcdistribution/CCSDSDistributor.cpp +++ b/tcdistribution/CCSDSDistributor.cpp @@ -1,31 +1,39 @@ -#include "../serviceinterface/ServiceInterfaceStream.h" #include "CCSDSDistributor.h" + +#include "../serviceinterface/ServiceInterfaceStream.h" #include "../tmtcpacket/SpacePacketBase.h" -CCSDSDistributor::CCSDSDistributor( uint16_t setDefaultApid, object_id_t setObjectId ) : - TcDistributor( setObjectId ), default_apid( setDefaultApid ), tcStore(NULL) { +CCSDSDistributor::CCSDSDistributor(uint16_t setDefaultApid, + object_id_t setObjectId): + TcDistributor(setObjectId), defaultApid( setDefaultApid ) { } -CCSDSDistributor::~CCSDSDistributor() { +CCSDSDistributor::~CCSDSDistributor() {} -} - -iterator_t CCSDSDistributor::selectDestination() { -// debug << "CCSDSDistributor::selectDestination received: " << this->currentMessage.getStorageId().pool_index << ", " << this->currentMessage.getStorageId().packet_index << std::endl; - const uint8_t* p_packet = NULL; +TcDistributor::TcMqMapIter CCSDSDistributor::selectDestination() { +// sif::debug << "CCSDSDistributor::selectDestination received: " << +// this->currentMessage.getStorageId().pool_index << ", " << +// this->currentMessage.getStorageId().packet_index << std::endl; + const uint8_t* packet = nullptr; size_t size = 0; - //TODO check returncode? - this->tcStore->getData( this->currentMessage.getStorageId(), &p_packet, &size ); - SpacePacketBase current_packet( p_packet ); -// info << "CCSDSDistributor::selectDestination has packet with APID " << std::hex << current_packet.getAPID() << std::dec << std::endl; - iterator_t position = this->queueMap.find( current_packet.getAPID() ); + ReturnValue_t result = this->tcStore->getData(currentMessage.getStorageId(), + &packet, &size ); + if(result != HasReturnvaluesIF::RETURN_OK) { + sif::error << "CCSDSDistributor::selectDestination: Getting data from" + " store failed!" << std::endl; + } + SpacePacketBase currentPacket(packet); + +// sif:: info << "CCSDSDistributor::selectDestination has packet with APID " +// << std::hex << currentPacket.getAPID() << std::dec << std::endl; + TcMqMapIter position = this->queueMap.find(currentPacket.getAPID()); if ( position != this->queueMap.end() ) { return position; } else { - //The APID was not found. Forward packet to main SW-APID anyway to create acceptance failure report. - return this->queueMap.find( this->default_apid ); + //The APID was not found. Forward packet to main SW-APID anyway to + // create acceptance failure report. + return this->queueMap.find( this->defaultApid ); } - } MessageQueueId_t CCSDSDistributor::getRequestQueue() { @@ -35,9 +43,9 @@ MessageQueueId_t CCSDSDistributor::getRequestQueue() { ReturnValue_t CCSDSDistributor::registerApplication( AcceptsTelecommandsIF* application) { ReturnValue_t returnValue = RETURN_OK; - bool errorCode = true; - errorCode = this->queueMap.insert( std::pair( application->getIdentifier(), application->getRequestQueue() ) ).second; - if( errorCode == false ) { + auto insertPair = this->queueMap.emplace(application->getIdentifier(), + application->getRequestQueue()); + if(not insertPair.second) { returnValue = RETURN_FAILED; } return returnValue; @@ -46,9 +54,8 @@ ReturnValue_t CCSDSDistributor::registerApplication( ReturnValue_t CCSDSDistributor::registerApplication(uint16_t apid, MessageQueueId_t id) { ReturnValue_t returnValue = RETURN_OK; - bool errorCode = true; - errorCode = this->queueMap.insert( std::pair( apid, id ) ).second; - if( errorCode == false ) { + auto insertPair = this->queueMap.emplace(apid, id); + if(not insertPair.second) { returnValue = RETURN_FAILED; } return returnValue; @@ -62,7 +69,11 @@ uint16_t CCSDSDistributor::getIdentifier() { ReturnValue_t CCSDSDistributor::initialize() { ReturnValue_t status = this->TcDistributor::initialize(); this->tcStore = objectManager->get( objects::TC_STORE ); - if (this->tcStore == NULL) status = RETURN_FAILED; + if (this->tcStore == nullptr) { + sif::error << "CCSDSDistributor::initialize: Could not initialize" + " TC store!" << std::endl; + status = RETURN_FAILED; + } return status; } diff --git a/tcdistribution/CCSDSDistributor.h b/tcdistribution/CCSDSDistributor.h index bff72092..e8d54c9c 100644 --- a/tcdistribution/CCSDSDistributor.h +++ b/tcdistribution/CCSDSDistributor.h @@ -1,58 +1,71 @@ -#ifndef CCSDSDISTRIBUTOR_H_ -#define CCSDSDISTRIBUTOR_H_ +#ifndef FRAMEWORK_TCDISTRIBUTION_CCSDSDISTRIBUTOR_H_ +#define FRAMEWORK_TCDISTRIBUTION_CCSDSDISTRIBUTOR_H_ #include "../objectmanager/ObjectManagerIF.h" #include "../storagemanager/StorageManagerIF.h" -#include "CCSDSDistributorIF.h" -#include "TcDistributor.h" +#include "../tcdistribution/CCSDSDistributorIF.h" +#include "../tcdistribution/TcDistributor.h" #include "../tmtcservices/AcceptsTelecommandsIF.h" + /** - * An instantiation of the CCSDSDistributorIF. - * It receives Space Packets, and selects a destination depending on the APID of the telecommands. + * @brief An instantiation of the CCSDSDistributorIF. + * @details + * It receives Space Packets, and selects a destination depending on the + * APID of the telecommands. * The Secondary Header (with Service/Subservice) is ignored. - * \ingroup tc_distribution + * @ingroup tc_distribution */ -class CCSDSDistributor : public TcDistributor, public CCSDSDistributorIF, public AcceptsTelecommandsIF { -protected: - /** - * This implementation checks if an Application with fitting APID has registered and forwards the - * packet to the according message queue. - * If the packet is not found, it returns the queue to \c default_apid, where a Acceptance Failure - * message should be generated. - * @return Iterator to map entry of found APID or iterator to default APID. - */ - iterator_t selectDestination(); - /** - * The default APID, where packets with unknown APID are sent to. - */ - uint16_t default_apid; - /** - * A reference to the TC storage must be maintained, as this class handles pure Space Packets and there - * exists no SpacePacketStored class. - */ - StorageManagerIF* tcStore; - /** - * The callback here handles the generation of acceptance success/failure messages. - */ - ReturnValue_t callbackAfterSending( ReturnValue_t queueStatus ); +class CCSDSDistributor : public TcDistributor, + public CCSDSDistributorIF, + public AcceptsTelecommandsIF { public: /** - * The constructor sets the default APID and calls the TcDistributor ctor with a certain object id. - * \c tcStore is set in the \c initialize method. - * @param set_default_apid The default APID, where packets with unknown destination are sent to. + * @brief The constructor sets the default APID and calls the + * TcDistributor ctor with a certain object id. + * @details + * @c tcStore is set in the @c initialize method. + * @param setDefaultApid The default APID, where packets with unknown + * destination are sent to. */ - CCSDSDistributor( uint16_t setDefaultApid, object_id_t setObjectId ); + CCSDSDistributor(uint16_t setDefaultApid, object_id_t setObjectId); /** * The destructor is empty. */ - ~CCSDSDistributor(); - MessageQueueId_t getRequestQueue(); - ReturnValue_t registerApplication( uint16_t apid, MessageQueueId_t id ); - ReturnValue_t registerApplication( AcceptsTelecommandsIF* application ); - uint16_t getIdentifier(); - ReturnValue_t initialize(); + virtual ~CCSDSDistributor(); + + MessageQueueId_t getRequestQueue() override; + ReturnValue_t registerApplication( uint16_t apid, + MessageQueueId_t id) override; + ReturnValue_t registerApplication( + AcceptsTelecommandsIF* application) override; + uint16_t getIdentifier() override; + ReturnValue_t initialize() override; + +protected: + /** + * This implementation checks if an application with fitting APID has + * registered and forwards the packet to the according message queue. + * If the packet is not found, it returns the queue to @c defaultApid, + * where a Acceptance Failure message should be generated. + * @return Iterator to map entry of found APID or iterator to default APID. + */ + TcMqMapIter selectDestination() override; + /** + * The callback here handles the generation of acceptance + * success/failure messages. + */ + ReturnValue_t callbackAfterSending( ReturnValue_t queueStatus ) override; + + /** + * The default APID, where packets with unknown APID are sent to. + */ + uint16_t defaultApid; + /** + * A reference to the TC storage must be maintained, as this class handles + * pure Space Packets and there exists no SpacePacketStored class. + */ + StorageManagerIF* tcStore = nullptr; + }; - - -#endif /* CCSDSDISTRIBUTOR_H_ */ +#endif /* FRAMEWORK_TCDISTRIBUTION_CCSDSDISTRIBUTOR_H_ */ diff --git a/tcdistribution/CCSDSDistributorIF.h b/tcdistribution/CCSDSDistributorIF.h index be3c2811..6334a35a 100644 --- a/tcdistribution/CCSDSDistributorIF.h +++ b/tcdistribution/CCSDSDistributorIF.h @@ -1,34 +1,38 @@ -#ifndef CCSDSDISTRIBUTORIF_H_ -#define CCSDSDISTRIBUTORIF_H_ +#ifndef FSFW_TCDISTRIBUTION_CCSDSDISTRIBUTORIF_H_ +#define FSFW_TCDISTRIBUTION_CCSDSDISTRIBUTORIF_H_ #include "../tmtcservices/AcceptsTelecommandsIF.h" #include "../ipc/MessageQueueSenderIF.h" /** * This is the Interface to a CCSDS Distributor. - * On a CCSDS Distributor, Applications (in terms of CCSDS) may register themselves, - * either by passing a pointer to themselves (and implementing the CCSDSApplicationIF, - * or by explicitly passing an APID and a MessageQueueId to route the TC's to. - * \ingroup tc_distribution + * On a CCSDS Distributor, Applications (in terms of CCSDS) may register + * themselves, either by passing a pointer to themselves (and implementing the + * CCSDSApplicationIF), or by explicitly passing an APID and a MessageQueueId + * to route the TC's to. + * @ingroup tc_distribution */ class CCSDSDistributorIF { public: /** - * With this call, a class implementing the CCSDSApplicationIF can register at the - * distributor. + * With this call, a class implementing the CCSDSApplicationIF can register + * at the distributor. * @param application A pointer to the Application to register. - * @return - \c RETURN_OK on success, - * - \c RETURN_FAILED on failure. + * @return - @c RETURN_OK on success, + * - @c RETURN_FAILED on failure. */ - virtual ReturnValue_t registerApplication( AcceptsTelecommandsIF* application ) = 0; + virtual ReturnValue_t registerApplication( + AcceptsTelecommandsIF* application) = 0; /** * With this call, other Applications can register to the CCSDS distributor. * This is done by passing an APID and a MessageQueueId to the method. * @param apid The APID to register. - * @param id The MessageQueueId of the message queue to send the TC Packets to. - * @return - \c RETURN_OK on success, - * - \c RETURN_FAILED on failure. + * @param id The MessageQueueId of the message queue to send the + * TC Packets to. + * @return - @c RETURN_OK on success, + * - @c RETURN_FAILED on failure. */ - virtual ReturnValue_t registerApplication( uint16_t apid, MessageQueueId_t id ) = 0; + virtual ReturnValue_t registerApplication( uint16_t apid, + MessageQueueId_t id) = 0; /** * The empty virtual destructor. */ @@ -37,4 +41,4 @@ public: }; -#endif /* CCSDSDISTRIBUTORIF_H_ */ +#endif /* FSFW_TCDISTRIBUTION_CCSDSDISTRIBUTORIF_H_ */ diff --git a/tcdistribution/PUSDistributor.cpp b/tcdistribution/PUSDistributor.cpp index f77eb99d..d964202f 100644 --- a/tcdistribution/PUSDistributor.cpp +++ b/tcdistribution/PUSDistributor.cpp @@ -1,61 +1,71 @@ -#include "../serviceinterface/ServiceInterfaceStream.h" #include "CCSDSDistributorIF.h" #include "PUSDistributor.h" + +#include "../serviceinterface/ServiceInterfaceStream.h" #include "../tmtcpacket/pus/TcPacketStored.h" #include "../tmtcservices/PusVerificationReport.h" -PUSDistributor::PUSDistributor(uint16_t setApid, object_id_t setObjectId, object_id_t setPacketSource) : - TcDistributor(setObjectId), checker(setApid), verifyChannel(), currentPacket(), tcStatus( - RETURN_FAILED), packetSource(setPacketSource) { +PUSDistributor::PUSDistributor(uint16_t setApid, object_id_t setObjectId, + object_id_t setPacketSource) : + TcDistributor(setObjectId), checker(setApid), verifyChannel(), + tcStatus(RETURN_FAILED), packetSource(setPacketSource) {} + +PUSDistributor::~PUSDistributor() {} + +PUSDistributor::TcMqMapIter PUSDistributor::selectDestination() { + // sif:: debug << "PUSDistributor::handlePacket received: " + // << this->current_packet_id.store_index << ", " + // << this->current_packet_id.packet_index << std::endl; + TcMqMapIter queueMapIt = this->queueMap.end(); + if(this->currentPacket == nullptr) { + return queueMapIt; + } + this->currentPacket->setStoreAddress(this->currentMessage.getStorageId()); + if (currentPacket->getWholeData() != nullptr) { + tcStatus = checker.checkPacket(currentPacket); +#ifdef DEBUG + if(tcStatus != HasReturnvaluesIF::RETURN_OK) { + sif::debug << "PUSDistributor::handlePacket: Packet format " + << "invalid, code "<< static_cast(tcStatus) + << std::endl; + } +#endif + uint32_t queue_id = currentPacket->getService(); + queueMapIt = this->queueMap.find(queue_id); + } + else { + tcStatus = PACKET_LOST; + } + + if (queueMapIt == this->queueMap.end()) { + tcStatus = DESTINATION_NOT_FOUND; +#ifdef DEBUG + sif::debug << "PUSDistributor::handlePacket: Destination not found, " + << "code "<< static_cast(tcStatus) << std::endl; +#endif + } + + if (tcStatus != RETURN_OK) { + return this->queueMap.end(); + } + else { + return queueMapIt; + } } -PUSDistributor::~PUSDistributor() { - -} - -iterator_t PUSDistributor::selectDestination() { -// debug << "PUSDistributor::handlePacket received: " << this->current_packet_id.store_index << ", " << this->current_packet_id.packet_index << std::endl; - iterator_t queueMapIt = this->queueMap.end(); - this->currentPacket.setStoreAddress(this->currentMessage.getStorageId()); - if (currentPacket.getWholeData() != NULL) { - tcStatus = checker.checkPacket(¤tPacket); -// info << "PUSDistributor::handlePacket: packetCheck returned with " << (int)tc_status << std::endl; - uint32_t queue_id = currentPacket.getService(); - queueMapIt = this->queueMap.find(queue_id); - } else { - tcStatus = PACKET_LOST; - } - if (queueMapIt == this->queueMap.end()) { - tcStatus = DESTINATION_NOT_FOUND; - } - - if (tcStatus != RETURN_OK) { - sif::debug << "PUSDistributor::handlePacket: error with " << (int) tcStatus - << std::endl; - return this->queueMap.end(); - } else { - return queueMapIt; - } - -} - -//uint16_t PUSDistributor::createDestination( uint8_t service_id, uint8_t subservice_id ) { -// return ( service_id << 8 ) + subservice_id; -//} ReturnValue_t PUSDistributor::registerService(AcceptsTelecommandsIF* service) { - ReturnValue_t returnValue = RETURN_OK; - bool errorCode = true; uint16_t serviceId = service->getIdentifier(); + // sif::info << "Service ID: " << (int)serviceId << std::endl; MessageQueueId_t queue = service->getRequestQueue(); - errorCode = this->queueMap.insert( - std::pair(serviceId, queue)).second; - if (errorCode == false) { - //TODO Return Code - returnValue = MessageQueueIF::NO_QUEUE; + auto returnPair = queueMap.emplace(serviceId, queue); + if (not returnPair.second) { + sif::error << "PUSDistributor::registerService: Service ID already" + " exists in map." << std::endl; + return SERVICE_ID_ALREADY_EXISTS; } - return returnValue; + return HasReturnvaluesIF::RETURN_OK; } MessageQueueId_t PUSDistributor::getRequestQueue() { @@ -68,13 +78,14 @@ ReturnValue_t PUSDistributor::callbackAfterSending(ReturnValue_t queueStatus) { } if (tcStatus != RETURN_OK) { this->verifyChannel.sendFailureReport(TC_VERIFY::ACCEPTANCE_FAILURE, - ¤tPacket, tcStatus); - //A failed packet is deleted immediately after reporting, otherwise it will block memory. - currentPacket.deletePacket(); + currentPacket, tcStatus); + // A failed packet is deleted immediately after reporting, + // otherwise it will block memory. + currentPacket->deletePacket(); return RETURN_FAILED; } else { this->verifyChannel.sendSuccessReport(TC_VERIFY::ACCEPTANCE_SUCCESS, - ¤tPacket); + currentPacket); return RETURN_OK; } } @@ -84,11 +95,19 @@ uint16_t PUSDistributor::getIdentifier() { } ReturnValue_t PUSDistributor::initialize() { + currentPacket = new TcPacketStored(); + if(currentPacket == nullptr) { + // Should not happen, memory allocation failed! + return ObjectManagerIF::CHILD_INIT_FAILED; + } + CCSDSDistributorIF* ccsdsDistributor = objectManager->get(packetSource); - if (ccsdsDistributor == NULL) { - return RETURN_FAILED; - } else { - return ccsdsDistributor->registerApplication(this); + if (ccsdsDistributor == nullptr) { + sif::error << "PUSDistributor::initialize: Packet source invalid." + << " Make sure it exists and implements CCSDSDistributorIF!" + << std::endl; + return RETURN_FAILED; } + return ccsdsDistributor->registerApplication(this); } diff --git a/tcdistribution/PUSDistributor.h b/tcdistribution/PUSDistributor.h index e9d4be8c..be3804ef 100644 --- a/tcdistribution/PUSDistributor.h +++ b/tcdistribution/PUSDistributor.h @@ -1,67 +1,79 @@ -#ifndef PUSDISTRIBUTOR_H_ -#define PUSDISTRIBUTOR_H_ +#ifndef FSFW_TCDISTRIBUTION_PUSDISTRIBUTOR_H_ +#define FSFW_TCDISTRIBUTION_PUSDISTRIBUTOR_H_ -#include "../returnvalues/HasReturnvaluesIF.h" #include "PUSDistributorIF.h" #include "TcDistributor.h" #include "TcPacketCheck.h" + +#include "../returnvalues/HasReturnvaluesIF.h" #include "../tmtcservices/AcceptsTelecommandsIF.h" #include "../tmtcservices/VerificationReporter.h" /** - * This class accepts PUS Telecommands and forwards them to Application services. - * In addition, the class performs a formal packet check and sends acceptance success - * or failure messages. - * \ingroup tc_distribution + * This class accepts PUS Telecommands and forwards them to Application + * services. In addition, the class performs a formal packet check and + * sends acceptance success or failure messages. + * @ingroup tc_distribution */ class PUSDistributor: public TcDistributor, public PUSDistributorIF, public AcceptsTelecommandsIF { public: /** - * The ctor passes \c set_apid to the checker class and calls the TcDistribution ctor with a certain object id. + * The ctor passes @c set_apid to the checker class and calls the + * TcDistribution ctor with a certain object id. * @param setApid The APID of this receiving Application. * @param setObjectId Object ID of the distributor itself - * @param setPacketSource Object ID of the source of TC packets. Must implement CCSDSDistributorIF. + * @param setPacketSource Object ID of the source of TC packets. + * Must implement CCSDSDistributorIF. */ - PUSDistributor(uint16_t setApid, object_id_t setObjectId, object_id_t setPacketSource); + PUSDistributor(uint16_t setApid, object_id_t setObjectId, + object_id_t setPacketSource); /** * The destructor is empty. */ virtual ~PUSDistributor(); - ReturnValue_t registerService(AcceptsTelecommandsIF* service); - MessageQueueId_t getRequestQueue(); - uint16_t getIdentifier(); - ReturnValue_t initialize(); + ReturnValue_t registerService(AcceptsTelecommandsIF* service) override; + MessageQueueId_t getRequestQueue() override; + ReturnValue_t initialize() override; + uint16_t getIdentifier() override; + protected: /** * This attribute contains the class, that performs a formal packet check. */ TcPacketCheck checker; /** - * With this class, verification messages are sent to the TC Verification service. + * With this class, verification messages are sent to the + * TC Verification service. */ VerificationReporter verifyChannel; /** * The currently handled packet is stored here. */ - TcPacketStored currentPacket; + TcPacketStored* currentPacket = nullptr; /** - * With this variable, the current check status is stored to generate acceptance messages later. + * With this variable, the current check status is stored to generate + * acceptance messages later. */ ReturnValue_t tcStatus; const object_id_t packetSource; + /** - * This method reads the packet service, checks if such a service is registered and forwards the packet to the destination. - * It also initiates the formal packet check and sending of verification messages. - * @return Iterator to map entry of found service id or iterator to \c map.end(). + * This method reads the packet service, checks if such a service is + * registered and forwards the packet to the destination. + * It also initiates the formal packet check and sending of verification + * messages. + * @return Iterator to map entry of found service id + * or iterator to @c map.end(). */ - iterator_t selectDestination(); + TcMqMapIter selectDestination() override; /** - * The callback here handles the generation of acceptance success/failure messages. + * The callback here handles the generation of acceptance + * success/failure messages. */ - ReturnValue_t callbackAfterSending(ReturnValue_t queueStatus); + ReturnValue_t callbackAfterSending(ReturnValue_t queueStatus) override; }; -#endif /* PUSDISTRIBUTOR_H_ */ +#endif /* FSFW_TCDISTRIBUTION_PUSDISTRIBUTOR_H_ */ diff --git a/tcdistribution/PUSDistributorIF.h b/tcdistribution/PUSDistributorIF.h index 5e27b35c..0125c08f 100644 --- a/tcdistribution/PUSDistributorIF.h +++ b/tcdistribution/PUSDistributorIF.h @@ -1,11 +1,12 @@ -#ifndef PUSDISTRIBUTORIF_H_ -#define PUSDISTRIBUTORIF_H_ +#ifndef FSFW_TCDISTRIBUTION_PUSDISTRIBUTORIF_H_ +#define FSFW_TCDISTRIBUTION_PUSDISTRIBUTORIF_H_ #include "../tmtcservices/AcceptsTelecommandsIF.h" #include "../ipc/MessageQueueSenderIF.h" + /** * This interface allows PUS Services to register themselves at a PUS Distributor. - * \ingroup tc_distribution + * @ingroup tc_distribution */ class PUSDistributorIF { public: @@ -17,10 +18,10 @@ public: /** * With this method, Services can register themselves at the PUS Distributor. * @param service A pointer to the registering Service. - * @return - \c RETURN_OK on success, - * - \c RETURN_FAILED on failure. + * @return - @c RETURN_OK on success, + * - @c RETURN_FAILED on failure. */ virtual ReturnValue_t registerService( AcceptsTelecommandsIF* service ) = 0; }; -#endif /* PUSDISTRIBUTORIF_H_ */ +#endif /* FSFW_TCDISTRIBUTION_PUSDISTRIBUTORIF_H_ */ diff --git a/tcdistribution/TcDistributor.cpp b/tcdistribution/TcDistributor.cpp index 49a996d9..06e1817f 100644 --- a/tcdistribution/TcDistributor.cpp +++ b/tcdistribution/TcDistributor.cpp @@ -1,12 +1,13 @@ -#include "../serviceinterface/ServiceInterfaceStream.h" -#include "../serviceinterface/ServiceInterfaceStream.h" #include "TcDistributor.h" + +#include "../serviceinterface/ServiceInterfaceStream.h" #include "../tmtcservices/TmTcMessage.h" #include "../ipc/QueueFactory.h" -TcDistributor::TcDistributor(object_id_t set_object_id) : - SystemObject(set_object_id), tcQueue(NULL) { - tcQueue = QueueFactory::instance()->createMessageQueue(DISTRIBUTER_MAX_PACKETS); +TcDistributor::TcDistributor(object_id_t objectId) : + SystemObject(objectId) { + tcQueue = QueueFactory::instance()-> + createMessageQueue(DISTRIBUTER_MAX_PACKETS); } TcDistributor::~TcDistributor() { @@ -15,7 +16,6 @@ TcDistributor::~TcDistributor() { ReturnValue_t TcDistributor::performOperation(uint8_t opCode) { ReturnValue_t status = RETURN_OK; -// debug << "TcDistributor: performing Operation." << std::endl; for (status = tcQueue->receiveMessage(¤tMessage); status == RETURN_OK; status = tcQueue->receiveMessage(¤tMessage)) { status = handlePacket(); @@ -29,7 +29,7 @@ ReturnValue_t TcDistributor::performOperation(uint8_t opCode) { ReturnValue_t TcDistributor::handlePacket() { - iterator_t queueMapIt = this->selectDestination(); + TcMqMapIter queueMapIt = this->selectDestination(); ReturnValue_t returnValue = RETURN_FAILED; if (queueMapIt != this->queueMap.end()) { returnValue = this->tcQueue->sendMessage(queueMapIt->second, @@ -39,14 +39,14 @@ ReturnValue_t TcDistributor::handlePacket() { } void TcDistributor::print() { - sif::debug << "Distributor content is: " << std::endl << "ID\t| message queue id" - << std::endl; - for (iterator_t it = this->queueMap.begin(); it != this->queueMap.end(); - it++) { - sif::debug << it->first << "\t| 0x" << std::hex << it->second << std::dec - << std::endl; + sif::debug << "Distributor content is: " << std::endl + << "ID\t| Message Queue ID" << std::endl; + sif::debug << std::setfill('0') << std::setw(8) << std::hex; + for (const auto& queueMapIter: queueMap) { + sif::debug << queueMapIter.first << "\t| 0x" << queueMapIter.second + << std::endl; } - sif::debug << std::dec; + sif::debug << std::setfill(' ') << std::dec; } diff --git a/tcdistribution/TcDistributor.h b/tcdistribution/TcDistributor.h index fed1cb3e..5d0ca45d 100644 --- a/tcdistribution/TcDistributor.h +++ b/tcdistribution/TcDistributor.h @@ -1,5 +1,6 @@ -#ifndef TCDISTRIBUTOR_H_ -#define TCDISTRIBUTOR_H_ +#ifndef FSFW_TMTCSERVICES_TCDISTRIBUTOR_H_ +#define FSFW_TMTCSERVICES_TCDISTRIBUTOR_H_ + #include "../objectmanager/ObjectManagerIF.h" #include "../objectmanager/SystemObject.h" #include "../returnvalues/HasReturnvaluesIF.h" @@ -9,16 +10,12 @@ #include "../ipc/MessageQueueIF.h" #include - /** - * \defgroup tc_distribution Telecommand Distribution - * All classes associated with Routing and Distribution of Telecommands belong to this group. + * @defgroup tc_distribution Telecommand Distribution + * All classes associated with Routing and Distribution of Telecommands + * belong to this group. */ -/** - * This typedef simplifies writing down the \c map iterator. - */ -typedef std::map::iterator iterator_t; /** * This is the base class to implement distributors for Space Packets. @@ -28,62 +25,19 @@ typedef std::map::iterator iterator_t; * message queue ids to some identifier. The process of unpacking the * destination information from the packet is handled by the child class * implementations. - * \ingroup tc_distribution + * @ingroup tc_distribution */ -class TcDistributor : public SystemObject, public ExecutableObjectIF, public HasReturnvaluesIF { -private: - /** - * This constant sets the maximum number of packets distributed per call. - */ - static const uint8_t DISTRIBUTER_MAX_PACKETS = 128; -protected: - /** - * This is the receiving queue for incoming Telecommands. - * The child classes must make its queue id public. - */ - MessageQueueIF* tcQueue; - /** - * The last received incoming packet information is stored in this - * member. - * As different child classes unpack the incoming packet differently - * (i.e. as a CCSDS Space Packet or as a PUS Telecommand Packet), it - * is not tried to unpack the packet information within this class. - */ - TmTcMessage currentMessage; - /** - * The map that links certain packet information to a destination. - * The packet information may be the APID of the packet or the service - * identifier. Filling of the map is under control of the different child - * classes. - */ - std::map queueMap; - /** - * This method shall unpack the routing information from the incoming - * packet and select the map entry which represents the packet's target. - * @return An iterator to the map element to forward to or queuMap.end(). - */ - virtual iterator_t selectDestination() = 0; - /** - * The handlePacket method calls the child class's selectDestination method - * and forwards the packet to its destination, if found. - * @return The message queue return value or \c RETURN_FAILED, in case no - * destination was found. - */ - ReturnValue_t handlePacket(); - /** - * This method gives the child class a chance to perform some kind of operation - * after the parent tried to forward the message. - * A typically application would be sending success/failure messages. - * The default implementation just returns \c RETURN_OK. - * @param queueStatus The status of the message queue after an attempt to send the TC. - * @return - \c RETURN_OK on success - * - \c RETURN_FAILED on failure - */ - virtual ReturnValue_t callbackAfterSending( ReturnValue_t queueStatus ); +class TcDistributor : public SystemObject, + public ExecutableObjectIF, + public HasReturnvaluesIF { public: - static const uint8_t INTERFACE_ID = CLASS_ID::PACKET_DISTRIBUTION; - static const ReturnValue_t PACKET_LOST = MAKE_RETURN_CODE( 1 ); - static const ReturnValue_t DESTINATION_NOT_FOUND = MAKE_RETURN_CODE( 2 ); + using TcMessageQueueMap = std::map; + using TcMqMapIter = std::map::iterator; + + static constexpr uint8_t INTERFACE_ID = CLASS_ID::PACKET_DISTRIBUTION; + static constexpr ReturnValue_t PACKET_LOST = MAKE_RETURN_CODE( 1 ); + static constexpr ReturnValue_t DESTINATION_NOT_FOUND = MAKE_RETURN_CODE( 2 ); + static constexpr ReturnValue_t SERVICE_ID_ALREADY_EXISTS = MAKE_RETURN_CODE(3); /** * Within the default constructor, the SystemObject id is set and the * message queue is initialized. @@ -91,7 +45,7 @@ public: * @param set_object_id This id is assigned to the distributor * implementation. */ - TcDistributor( object_id_t set_object_id ); + TcDistributor(object_id_t objectId); /** * The destructor is empty, the message queues are not in the vicinity of * this class. @@ -110,7 +64,59 @@ public: * queueMap. */ void print(); + +protected: + /** + * This is the receiving queue for incoming Telecommands. + * The child classes must make its queue id public. + */ + MessageQueueIF* tcQueue = nullptr; + /** + * The last received incoming packet information is stored in this + * member. + * As different child classes unpack the incoming packet differently + * (i.e. as a CCSDS Space Packet or as a PUS Telecommand Packet), it + * is not tried to unpack the packet information within this class. + */ + TmTcMessage currentMessage; + /** + * The map that links certain packet information to a destination. + * The packet information may be the APID of the packet or the service + * identifier. Filling of the map is under control of the different child + * classes. + */ + TcMessageQueueMap queueMap; + /** + * This method shall unpack the routing information from the incoming + * packet and select the map entry which represents the packet's target. + * @return An iterator to the map element to forward to or queuMap.end(). + */ + virtual TcMqMapIter selectDestination() = 0; + /** + * The handlePacket method calls the child class's selectDestination method + * and forwards the packet to its destination, if found. + * @return The message queue return value or @c RETURN_FAILED, in case no + * destination was found. + */ + ReturnValue_t handlePacket(); + /** + * This method gives the child class a chance to perform some kind of + * operation after the parent tried to forward the message. + * A typically application would be sending success/failure messages. + * The default implementation just returns @c RETURN_OK. + * @param queueStatus The status of the message queue after an attempt + * to send the TC. + * @return - @c RETURN_OK on success + * - @c RETURN_FAILED on failure + */ + virtual ReturnValue_t callbackAfterSending( ReturnValue_t queueStatus ); + +private: + /** + * This constant sets the maximum number of packets distributed per call. + */ + static constexpr uint8_t DISTRIBUTER_MAX_PACKETS = 128; }; -#endif /* TCDISTRIBUTOR_H_ */ +#endif /* FSFW_TMTCSERVICES_TCDISTRIBUTOR_H_ */ diff --git a/tcdistribution/TcPacketCheck.cpp b/tcdistribution/TcPacketCheck.cpp index b5881083..38ed04aa 100644 --- a/tcdistribution/TcPacketCheck.cpp +++ b/tcdistribution/TcPacketCheck.cpp @@ -1,31 +1,33 @@ +#include "TcPacketCheck.h" + #include "../globalfunctions/CRC.h" #include "../serviceinterface/ServiceInterfaceStream.h" #include "../storagemanager/StorageManagerIF.h" -#include "TcPacketCheck.h" #include "../tmtcservices/VerificationCodes.h" -TcPacketCheck::TcPacketCheck( uint16_t set_apid ) : apid(set_apid) { +TcPacketCheck::TcPacketCheck( uint16_t setApid ) : apid(setApid) { } -ReturnValue_t TcPacketCheck::checkPacket( TcPacketStored* current_packet ) { - uint16_t calculated_crc = CRC::crc16ccitt( current_packet->getWholeData(), current_packet->getFullSize() ); +ReturnValue_t TcPacketCheck::checkPacket( TcPacketStored* currentPacket ) { + uint16_t calculated_crc = CRC::crc16ccitt( currentPacket->getWholeData(), + currentPacket->getFullSize() ); if ( calculated_crc != 0 ) { return INCORRECT_CHECKSUM; } - bool condition = !(current_packet->hasSecondaryHeader()) || - current_packet->getPacketVersionNumber() != CCSDS_VERSION_NUMBER || - !(current_packet->isTelecommand()); + bool condition = (not currentPacket->hasSecondaryHeader()) or + (currentPacket->getPacketVersionNumber() != CCSDS_VERSION_NUMBER) or + (not currentPacket->isTelecommand()); if ( condition ) { return INCORRECT_PRIMARY_HEADER; } - if ( current_packet->getAPID() != this->apid ) + if ( currentPacket->getAPID() != this->apid ) return ILLEGAL_APID; - if ( !current_packet->isSizeCorrect() ) { + if ( not currentPacket->isSizeCorrect() ) { return INCOMPLETE_PACKET; } - condition = (current_packet->getSecondaryHeaderFlag() != CCSDS_SECONDARY_HEADER_FLAG) || - (current_packet->getPusVersionNumber() != PUS_VERSION_NUMBER); + condition = (currentPacket->getSecondaryHeaderFlag() != CCSDS_SECONDARY_HEADER_FLAG) || + (currentPacket->getPusVersionNumber() != PUS_VERSION_NUMBER); if ( condition ) { return INCORRECT_SECONDARY_HEADER; } diff --git a/tcdistribution/TcPacketCheck.h b/tcdistribution/TcPacketCheck.h index 4ba269f5..703bb1bb 100644 --- a/tcdistribution/TcPacketCheck.h +++ b/tcdistribution/TcPacketCheck.h @@ -1,28 +1,29 @@ -#ifndef TCPACKETCHECK_H_ -#define TCPACKETCHECK_H_ +#ifndef FSFW_TCDISTRIBUTION_TCPACKETCHECK_H_ +#define FSFW_TCDISTRIBUTION_TCPACKETCHECK_H_ #include "../returnvalues/HasReturnvaluesIF.h" #include "../tmtcpacket/pus/TcPacketStored.h" #include "../tmtcservices/PusVerificationReport.h" + /** * This class performs a formal packet check for incoming PUS Telecommand Packets. * Currently, it only checks if the APID and CRC are correct. - * \ingroup tc_distribution + * @ingroup tc_distribution */ class TcPacketCheck : public HasReturnvaluesIF { protected: /** * Describes the version number a packet must have to pass. */ - static const uint8_t CCSDS_VERSION_NUMBER = 0; + static constexpr uint8_t CCSDS_VERSION_NUMBER = 0; /** * Describes the secondary header a packet must have to pass. */ - static const uint8_t CCSDS_SECONDARY_HEADER_FLAG = 0; + static constexpr uint8_t CCSDS_SECONDARY_HEADER_FLAG = 0; /** * Describes the TC Packet PUS Version Number a packet must have to pass. */ - static const uint8_t PUS_VERSION_NUMBER = 1; + static constexpr uint8_t PUS_VERSION_NUMBER = 1; /** * The packet id each correct packet should have. * It is composed of the APID and some static fields. @@ -41,19 +42,19 @@ public: * The constructor only sets the APID attribute. * @param set_apid The APID to set. */ - TcPacketCheck( uint16_t set_apid ); + TcPacketCheck( uint16_t setApid ); /** * This is the actual method to formally check a certain Telecommand Packet. * The packet's Application Data can not be checked here. * @param current_packet The packt to check - * @return - \c RETURN_OK on success. - * - \c INCORRECT_CHECKSUM if checksum is invalid. - * - \c ILLEGAL_APID if APID does not match. + * @return - @c RETURN_OK on success. + * - @c INCORRECT_CHECKSUM if checksum is invalid. + * - @c ILLEGAL_APID if APID does not match. */ - ReturnValue_t checkPacket( TcPacketStored* current_packet ); + ReturnValue_t checkPacket( TcPacketStored* currentPacket ); uint16_t getApid() const; }; -#endif /* TCPACKETCHECK_H_ */ +#endif /* FSFW_TCDISTRIBUTION_TCPACKETCHECK_H_ */ diff --git a/timemanager/TimeStamperIF.h b/timemanager/TimeStamperIF.h index bdc5e7e3..96011088 100644 --- a/timemanager/TimeStamperIF.h +++ b/timemanager/TimeStamperIF.h @@ -14,8 +14,12 @@ public: static const uint8_t INTERFACE_ID = CLASS_ID::TIME_STAMPER_IF; static const ReturnValue_t BAD_TIMESTAMP = MAKE_RETURN_CODE(1); - static const uint8_t MISSION_TIMESTAMP_SIZE = 8; //!< This is a mission-specific constant and determines the total size reserved for timestamps. - virtual ReturnValue_t addTimeStamp(uint8_t* buffer, const uint8_t maxSize) = 0; + //! This is a mission-specific constant and determines the total + //! size reserved for timestamps. + //! TODO: Default define in FSFWConfig ? + static const uint8_t MISSION_TIMESTAMP_SIZE = 8; + virtual ReturnValue_t addTimeStamp(uint8_t* buffer, + const uint8_t maxSize) = 0; virtual ~TimeStamperIF() {} }; diff --git a/tmtcpacket/SpacePacketBase.cpp b/tmtcpacket/SpacePacketBase.cpp index a37e024b..e13af8d0 100644 --- a/tmtcpacket/SpacePacketBase.cpp +++ b/tmtcpacket/SpacePacketBase.cpp @@ -1,6 +1,6 @@ -#include "../serviceinterface/ServiceInterfaceStream.h" #include "SpacePacketBase.h" -#include +#include "../serviceinterface/ServiceInterfaceStream.h" +#include SpacePacketBase::SpacePacketBase( const uint8_t* set_address ) { this->data = (SpacePacketPointer*) set_address; @@ -14,7 +14,8 @@ uint8_t SpacePacketBase::getPacketVersionNumber( void ) { return (this->data->header.packet_id_h & 0b11100000) >> 5; } -void SpacePacketBase::initSpacePacketHeader(bool isTelecommand, bool hasSecondaryHeader, uint16_t apid, uint16_t sequenceCount) { +void SpacePacketBase::initSpacePacketHeader(bool isTelecommand, + bool hasSecondaryHeader, uint16_t apid, uint16_t sequenceCount) { //reset header to zero: memset(data,0, sizeof(this->data->header) ); //Set TC/TM bit. @@ -81,7 +82,7 @@ void SpacePacketBase::setPacketDataLength( uint16_t new_length) { this->data->header.packet_length_l = ( new_length & 0x00FF ); } -uint32_t SpacePacketBase::getFullSize() { +size_t SpacePacketBase::getFullSize() { //+1 is done because size in packet data length field is: size of data field -1 return this->getPacketDataLength() + sizeof(this->data->header) + 1; } diff --git a/tmtcpacket/SpacePacketBase.h b/tmtcpacket/SpacePacketBase.h index ef3ad79d..19cbd074 100644 --- a/tmtcpacket/SpacePacketBase.h +++ b/tmtcpacket/SpacePacketBase.h @@ -1,10 +1,11 @@ -#ifndef SPACEPACKETBASE_H_ -#define SPACEPACKETBASE_H_ +#ifndef FSFW_TMTCPACKET_SPACEPACKETBASE_H_ +#define FSFW_TMTCPACKET_SPACEPACKETBASE_H_ #include "ccsds_header.h" +#include /** - * \defgroup tmtcpackets Space Packets + * @defgroup tmtcpackets Space Packets * This is the group, where all classes associated with Telecommand and * Telemetry packets belong to. * The class hierarchy resembles the dependency between the different standards @@ -81,7 +82,8 @@ public: */ bool isTelecommand( void ); - void initSpacePacketHeader(bool isTelecommand, bool hasSecondaryHeader, uint16_t apid, uint16_t sequenceCount = 0); + void initSpacePacketHeader(bool isTelecommand, bool hasSecondaryHeader, + uint16_t apid, uint16_t sequenceCount = 0); /** * The CCSDS header provides a secondary header flag (the fifth-highest bit), * which is checked with this method. @@ -167,10 +169,10 @@ public: * This method returns the full raw packet size. * @return The full size of the packet in bytes. */ - uint32_t getFullSize(); + size_t getFullSize(); uint32_t getApidAndSequenceCount() const; }; -#endif /* SPACEPACKETBASE_H_ */ +#endif /* FSFW_TMTCPACKET_SPACEPACKETBASE_H_ */ diff --git a/tmtcpacket/pus/PacketTimestampInterpreterIF.h b/tmtcpacket/pus/PacketTimestampInterpreterIF.h index dd0c0328..40e7a2ea 100644 --- a/tmtcpacket/pus/PacketTimestampInterpreterIF.h +++ b/tmtcpacket/pus/PacketTimestampInterpreterIF.h @@ -7,7 +7,8 @@ class TmPacketMinimal; class PacketTimestampInterpreterIF { public: virtual ~PacketTimestampInterpreterIF() {} - virtual ReturnValue_t getPacketTime(TmPacketMinimal* packet, timeval* timestamp) const = 0; + virtual ReturnValue_t getPacketTime(TmPacketMinimal* packet, + timeval* timestamp) const = 0; virtual ReturnValue_t getPacketTimeRaw(TmPacketMinimal* packet, const uint8_t** timePtr, uint32_t* size) const = 0; }; diff --git a/tmtcpacket/pus/TcPacketBase.cpp b/tmtcpacket/pus/TcPacketBase.cpp index 3815eb20..eaa8416a 100644 --- a/tmtcpacket/pus/TcPacketBase.cpp +++ b/tmtcpacket/pus/TcPacketBase.cpp @@ -1,11 +1,14 @@ -#include "../../globalfunctions/CRC.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" #include "TcPacketBase.h" -#include -TcPacketBase::TcPacketBase(const uint8_t* set_data) : - SpacePacketBase(set_data) { - tcData = (TcPacketPointer*) set_data; +#include "../../globalfunctions/CRC.h" +#include "../../globalfunctions/arrayprinter.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" + +#include + +TcPacketBase::TcPacketBase(const uint8_t* setData) : + SpacePacketBase(setData) { + tcData = reinterpret_cast(const_cast(setData)); } TcPacketBase::~TcPacketBase() { @@ -13,28 +16,28 @@ TcPacketBase::~TcPacketBase() { } uint8_t TcPacketBase::getService() { - return tcData->data_field.service_type; + return tcData->dataField.service_type; } uint8_t TcPacketBase::getSubService() { - return tcData->data_field.service_subtype; + return tcData->dataField.service_subtype; } uint8_t TcPacketBase::getAcknowledgeFlags() { - return tcData->data_field.version_type_ack & 0b00001111; + return tcData->dataField.version_type_ack & 0b00001111; } const uint8_t* TcPacketBase::getApplicationData() const { - return &tcData->data; + return &tcData->appData; } uint16_t TcPacketBase::getApplicationDataSize() { - return getPacketDataLength() - sizeof(tcData->data_field) - CRC_SIZE + 1; + return getPacketDataLength() - sizeof(tcData->dataField) - CRC_SIZE + 1; } uint16_t TcPacketBase::getErrorControl() { uint16_t size = getApplicationDataSize() + CRC_SIZE; - uint8_t* p_to_buffer = &tcData->data; + uint8_t* p_to_buffer = &tcData->appData; return (p_to_buffer[size - 2] << 8) + p_to_buffer[size - 1]; } @@ -42,41 +45,42 @@ void TcPacketBase::setErrorControl() { uint32_t full_size = getFullSize(); uint16_t crc = CRC::crc16ccitt(getWholeData(), full_size - CRC_SIZE); uint32_t size = getApplicationDataSize(); - (&tcData->data)[size] = (crc & 0XFF00) >> 8; // CRCH - (&tcData->data)[size + 1] = (crc) & 0X00FF; // CRCL + (&tcData->appData)[size] = (crc & 0XFF00) >> 8; // CRCH + (&tcData->appData)[size + 1] = (crc) & 0X00FF; // CRCL } -void TcPacketBase::setData(const uint8_t* p_Data) { - SpacePacketBase::setData(p_Data); - tcData = (TcPacketPointer*) p_Data; +void TcPacketBase::setData(const uint8_t* pData) { + SpacePacketBase::setData(pData); + tcData = (TcPacketPointer*) pData; } uint8_t TcPacketBase::getSecondaryHeaderFlag() { - return (tcData->data_field.version_type_ack & 0b10000000) >> 7; + return (tcData->dataField.version_type_ack & 0b10000000) >> 7; } uint8_t TcPacketBase::getPusVersionNumber() { - return (tcData->data_field.version_type_ack & 0b01110000) >> 4; + return (tcData->dataField.version_type_ack & 0b01110000) >> 4; } void TcPacketBase::print() { - uint8_t * wholeData = getWholeData(); - sif::debug << "TcPacket contains: " << std::endl; - for (uint8_t count = 0; count < getFullSize(); ++count) { - sif::debug << std::hex << (uint16_t) wholeData[count] << " "; - } - sif::debug << std::dec << std::endl; + sif::debug << "TcPacketBase::print: " << std::endl; + arrayprinter::print(getWholeData(), getFullSize()); } void TcPacketBase::initializeTcPacket(uint16_t apid, uint16_t sequenceCount, - uint8_t ack, uint8_t service, uint8_t subservice) { + uint8_t ack, uint8_t service, uint8_t subservice) { initSpacePacketHeader(true, true, apid, sequenceCount); - memset(&tcData->data_field, 0, sizeof(tcData->data_field)); - setPacketDataLength(sizeof(tcData->data_field) + CRC_SIZE); + std::memset(&tcData->dataField, 0, sizeof(tcData->dataField)); + setPacketDataLength(sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1); //Data Field Header: - //Set CCSDS_secondary_header_flag to 0, version number to 001 and ack to 0000 - tcData->data_field.version_type_ack = 0b00010000; - tcData->data_field.version_type_ack |= (ack & 0x0F); - tcData->data_field.service_type = service; - tcData->data_field.service_subtype = subservice; + //Set CCSDS_secondary_header_flag to 0 and version number to 001 + tcData->dataField.version_type_ack = 0b00010000; + tcData->dataField.version_type_ack |= (ack & 0x0F); + tcData->dataField.service_type = service; + tcData->dataField.service_subtype = subservice; +} + +size_t TcPacketBase::calculateFullPacketLength(size_t appDataLen) { + return sizeof(CCSDSPrimaryHeader) + sizeof(PUSTcDataFieldHeader) + + appDataLen + TcPacketBase::CRC_SIZE; } diff --git a/tmtcpacket/pus/TcPacketBase.h b/tmtcpacket/pus/TcPacketBase.h index dd63d2be..582a2214 100644 --- a/tmtcpacket/pus/TcPacketBase.h +++ b/tmtcpacket/pus/TcPacketBase.h @@ -1,7 +1,9 @@ -#ifndef TCPACKETBASE_H_ -#define TCPACKETBASE_H_ +#ifndef TMTCPACKET_PUS_TCPACKETBASE_H_ +#define TMTCPACKET_PUS_TCPACKETBASE_H_ #include "../../tmtcpacket/SpacePacketBase.h" +#include + /** * This struct defines a byte-wise structured PUS TC Data Field Header. @@ -23,14 +25,14 @@ struct PUSTcDataFieldHeader { */ struct TcPacketPointer { CCSDSPrimaryHeader primary; - PUSTcDataFieldHeader data_field; - uint8_t data; + PUSTcDataFieldHeader dataField; + uint8_t appData; }; /** * This class is the basic data handler for any ECSS PUS Telecommand packet. * - * In addition to \SpacePacketBase, the class provides methods to handle + * In addition to #SpacePacketBase, the class provides methods to handle * the standardized entries of the PUS TC Packet Data Field Header. * It does not contain the packet data itself but a pointer to the * data must be set on instantiation. An invalid pointer may cause @@ -39,67 +41,38 @@ struct TcPacketPointer { * @ingroup tmtcpackets */ class TcPacketBase : public SpacePacketBase { -protected: - /** - * A pointer to a structure which defines the data structure of - * the packet's data. - * - * To be hardware-safe, all elements are of byte size. - */ - TcPacketPointer* tcData; public: - static const uint16_t TC_PACKET_MIN_SIZE = (sizeof(CCSDSPrimaryHeader) + sizeof(PUSTcDataFieldHeader) + 2); - /** - * With this constant for the acknowledge field responses on all levels are expected. - */ - static const uint8_t ACK_ALL = 0b1111; - /** - * With this constant for the acknowledge field a response on acceptance is expected. - */ - static const uint8_t ACK_ACCEPTANCE = 0b0001; - /** - * With this constant for the acknowledge field a response on start of execution is expected. - */ - static const uint8_t ACK_START = 0b0010; - /** - * With this constant for the acknowledge field responses on execution steps are expected. - */ - static const uint8_t ACK_STEP = 0b0100; - /** - * With this constant for the acknowledge field a response on completion is expected. - */ - static const uint8_t ACK_COMPLETION = 0b1000; - /** - * With this constant for the acknowledge field no responses are expected. - */ - static const uint8_t ACK_NONE = 0b000; + static const uint16_t TC_PACKET_MIN_SIZE = (sizeof(CCSDSPrimaryHeader) + + sizeof(PUSTcDataFieldHeader) + 2); + + enum AckField { + //! No acknowledgements are expected. + ACK_NONE = 0b0000, + //! Acknowledgements on acceptance are expected. + ACK_ACCEPTANCE = 0b0001, + //! Acknowledgements on start are expected. + ACK_START = 0b0010, + //! Acknowledgements on step are expected. + ACK_STEP = 0b0100, + //! Acknowledfgement on completion are expected. + ACK_COMPLETION = 0b1000 + }; + + static constexpr uint8_t ACK_ALL = ACK_ACCEPTANCE | ACK_START | ACK_STEP | + ACK_COMPLETION; + /** * This is the default constructor. * It sets its internal data pointer to the address passed and also * forwards the data pointer to the parent SpacePacketBase class. - * @param set_address The position where the packet data lies. + * @param setData The position where the packet data lies. */ - TcPacketBase( const uint8_t* set_data ); + TcPacketBase( const uint8_t* setData ); /** * This is the empty default destructor. */ virtual ~TcPacketBase(); - /** - * Initializes the Tc Packet header. - * @param apid APID used. - * @param service PUS Service - * @param subservice PUS Subservice - * @param packetSubcounter Additional subcounter used. - */ - /** - * Initializes the Tc Packet header. - * @param apid APID used. - * @param sequenceCount Sequence Count in the primary header. - * @param ack Which acknowledeges are expected from the receiver. - * @param service PUS Service - * @param subservice PUS Subservice - */ - void initializeTcPacket(uint16_t apid, uint16_t sequenceCount, uint8_t ack, uint8_t service, uint8_t subservice); + /** * This command returns the CCSDS Secondary Header Flag. * It shall always be zero for PUS Packets. This is the @@ -166,22 +139,49 @@ public: * current content of the packet. */ void setErrorControl(); - /** - * With this method, the packet data pointer can be redirected to another - * location. - * - * This call overwrites the parent's setData method to set both its - * \c tc_data pointer and the parent's \c data pointer. - * - * @param p_data A pointer to another PUS Telecommand Packet. - */ - void setData( const uint8_t* p_data ); + /** * This is a debugging helper method that prints the whole packet content * to the screen. */ void print(); + /** + * Calculate full packet length from application data length. + * @param appDataLen + * @return + */ + static size_t calculateFullPacketLength(size_t appDataLen); + +protected: + /** + * A pointer to a structure which defines the data structure of + * the packet's data. + * + * To be hardware-safe, all elements are of byte size. + */ + TcPacketPointer* tcData; + + /** + * Initializes the Tc Packet header. + * @param apid APID used. + * @param sequenceCount Sequence Count in the primary header. + * @param ack Which acknowledeges are expected from the receiver. + * @param service PUS Service + * @param subservice PUS Subservice + */ + void initializeTcPacket(uint16_t apid, uint16_t sequenceCount, uint8_t ack, + uint8_t service, uint8_t subservice); + + /** + * With this method, the packet data pointer can be redirected to another + * location. + * This call overwrites the parent's setData method to set both its + * @c tc_data pointer and the parent's @c data pointer. + * + * @param p_data A pointer to another PUS Telecommand Packet. + */ + void setData( const uint8_t* pData ); }; -#endif /* TCPACKETBASE_H_ */ +#endif /* TMTCPACKET_PUS_TCPACKETBASE_H_ */ diff --git a/tmtcpacket/pus/TcPacketStored.cpp b/tmtcpacket/pus/TcPacketStored.cpp index 216f89c9..fffc86ae 100644 --- a/tmtcpacket/pus/TcPacketStored.cpp +++ b/tmtcpacket/pus/TcPacketStored.cpp @@ -1,37 +1,50 @@ +#include "TcPacketStored.h" #include "../../objectmanager/ObjectManagerIF.h" #include "../../serviceinterface/ServiceInterfaceStream.h" -#include "TcPacketStored.h" -#include + +#include + +StorageManagerIF* TcPacketStored::store = nullptr; TcPacketStored::TcPacketStored(store_address_t setAddress) : - TcPacketBase(NULL), storeAddress(setAddress) { - this->setStoreAddress(this->storeAddress); + TcPacketBase(nullptr), storeAddress(setAddress) { + setStoreAddress(storeAddress); } -TcPacketStored::TcPacketStored(uint16_t apid, uint8_t ack, uint8_t service, - uint8_t subservice, uint8_t sequence_count, const uint8_t* data, - uint32_t size) : - TcPacketBase(NULL) { +TcPacketStored::TcPacketStored(uint16_t apid, uint8_t service, + uint8_t subservice, uint8_t sequenceCount, const uint8_t* data, + size_t size, uint8_t ack) : + TcPacketBase(nullptr) { this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS; - if (!this->checkAndSetStore()) { + if (not this->checkAndSetStore()) { return; } - uint8_t* p_data = NULL; + uint8_t* pData = nullptr; ReturnValue_t returnValue = this->store->getFreeElement(&this->storeAddress, - (TC_PACKET_MIN_SIZE + size), &p_data); + (TC_PACKET_MIN_SIZE + size), &pData); if (returnValue != this->store->RETURN_OK) { + sif::warning << "TcPacketStored: Could not get free element from store!" + << std::endl; return; } - this->setData(p_data); - initializeTcPacket(apid, sequence_count, ack, service, subservice); - memcpy(&tcData->data, data, size); + this->setData(pData); + initializeTcPacket(apid, sequenceCount, ack, service, subservice); + memcpy(&tcData->appData, data, size); this->setPacketDataLength( size + sizeof(PUSTcDataFieldHeader) + CRC_SIZE - 1); this->setErrorControl(); } -TcPacketStored::TcPacketStored() : - TcPacketBase(NULL) { +ReturnValue_t TcPacketStored::getData(const uint8_t ** dataPtr, + size_t* dataSize) { + auto result = this->store->getData(storeAddress, dataPtr, dataSize); + if(result != HasReturnvaluesIF::RETURN_OK) { + sif::warning << "TcPacketStored: Could not get data!" << std::endl; + } + return result; +} + +TcPacketStored::TcPacketStored(): TcPacketBase(nullptr) { this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS; this->checkAndSetStore(); @@ -40,14 +53,14 @@ TcPacketStored::TcPacketStored() : ReturnValue_t TcPacketStored::deletePacket() { ReturnValue_t result = this->store->deleteData(this->storeAddress); this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS; - this->setData( NULL); + this->setData(nullptr); return result; } bool TcPacketStored::checkAndSetStore() { - if (this->store == NULL) { + if (this->store == nullptr) { this->store = objectManager->get(objects::TC_STORE); - if (this->store == NULL) { + if (this->store == nullptr) { sif::error << "TcPacketStored::TcPacketStored: TC Store not found!" << std::endl; return false; @@ -58,17 +71,17 @@ bool TcPacketStored::checkAndSetStore() { void TcPacketStored::setStoreAddress(store_address_t setAddress) { this->storeAddress = setAddress; - const uint8_t* temp_data = NULL; + const uint8_t* tempData = nullptr; size_t temp_size; ReturnValue_t status = StorageManagerIF::RETURN_FAILED; if (this->checkAndSetStore()) { - status = this->store->getData(this->storeAddress, &temp_data, + status = this->store->getData(this->storeAddress, &tempData, &temp_size); } if (status == StorageManagerIF::RETURN_OK) { - this->setData(temp_data); + this->setData(tempData); } else { - this->setData(NULL); + this->setData(nullptr); this->storeAddress.raw = StorageManagerIF::INVALID_ADDRESS; } } @@ -78,7 +91,7 @@ store_address_t TcPacketStored::getStoreAddress() { } bool TcPacketStored::isSizeCorrect() { - const uint8_t* temp_data = NULL; + const uint8_t* temp_data = nullptr; size_t temp_size; ReturnValue_t status = this->store->getData(this->storeAddress, &temp_data, &temp_size); @@ -90,8 +103,6 @@ bool TcPacketStored::isSizeCorrect() { return false; } -StorageManagerIF* TcPacketStored::store = NULL; - TcPacketStored::TcPacketStored(const uint8_t* data, uint32_t size) : TcPacketBase(data) { if (getFullSize() != size) { @@ -100,7 +111,7 @@ TcPacketStored::TcPacketStored(const uint8_t* data, uint32_t size) : if (this->checkAndSetStore()) { ReturnValue_t status = store->addData(&storeAddress, data, size); if (status != HasReturnvaluesIF::RETURN_OK) { - this->setData(NULL); + this->setData(nullptr); } } } diff --git a/tmtcpacket/pus/TcPacketStored.h b/tmtcpacket/pus/TcPacketStored.h index 2d86f89b..1666107b 100644 --- a/tmtcpacket/pus/TcPacketStored.h +++ b/tmtcpacket/pus/TcPacketStored.h @@ -1,8 +1,8 @@ -#ifndef TCPACKETSTORED_H_ -#define TCPACKETSTORED_H_ +#ifndef TMTCPACKET_PUS_TCPACKETSTORED_H_ +#define TMTCPACKET_PUS_TCPACKETSTORED_H_ -#include "../../storagemanager/StorageManagerIF.h" #include "TcPacketBase.h" +#include "../../storagemanager/StorageManagerIF.h" /** * This class generates a ECSS PUS Telecommand packet within a given @@ -15,26 +15,6 @@ * @ingroup tmtcpackets */ class TcPacketStored : public TcPacketBase { -private: - /** - * This is a pointer to the store all instances of the class use. - * If the store is not yet set (i.e. \c store is NULL), every constructor - * call tries to set it and throws an error message in case of failures. - * The default store is objects::TC_STORE. - */ - static StorageManagerIF* store; - /** - * The address where the packet data of the object instance is stored. - */ - store_address_t storeAddress; - /** - * A helper method to check if a store is assigned to the class. - * If not, the method tries to retrieve the store from the global - * ObjectManager. - * @return @li \c true if the store is linked or could be created. - * @li \c false otherwise. - */ - bool checkAndSetStore(); public: /** * This is a default constructor which does not set the data pointer. @@ -53,18 +33,20 @@ public: * a new PUS Telecommand Packet is created there. * Packet Application Data passed in data is copied into the packet. * @param apid Sets the packet's APID field. - * @param ack Set's the packet's Ack field, - * which specifies number and size of verification packets returned - * for this command. * @param service Sets the packet's Service ID field. - * This specifies the destination service. + * This specifies the destination service. * @param subservice Sets the packet's Service Subtype field. - * This specifies the destination sub-service. - * @param sequence_count Sets the packet's Source Sequence Count field. + * This specifies the destination sub-service. + * @param sequence_count Sets the packet's Source Sequence Count field. * @param data The data to be copied to the Application Data Field. * @param size The amount of data to be copied. + * @param ack Set's the packet's Ack field, which specifies + * number of verification packets returned + * for this command. */ - TcPacketStored( uint16_t apid, uint8_t ack, uint8_t service, uint8_t subservice, uint8_t sequence_count = 0, const uint8_t* data = NULL, uint32_t size = 0 ); + TcPacketStored(uint16_t apid, uint8_t service, uint8_t subservice, + uint8_t sequence_count = 0, const uint8_t* data = nullptr, + size_t size = 0, uint8_t ack = TcPacketBase::ACK_ALL); /** * Another constructor to create a TcPacket from a raw packet stream. * Takes the data and adds it unchecked to the TcStore. @@ -72,10 +54,19 @@ public: * @param Size size of the packet. */ TcPacketStored( const uint8_t* data, uint32_t size); + + /** + * Getter function for the raw data. + * @param dataPtr [out] Pointer to the data pointer to set + * @param dataSize [out] Address of size to set. + * @return -@c RETURN_OK if data was retrieved successfully. + */ + ReturnValue_t getData(const uint8_t ** dataPtr, + size_t* dataSize); /** * This is a getter for the current store address of the packet. - * @return The current store address. The (raw) value is \c StorageManagerIF::INVALID_ADDRESS if - * the packet is not linked. + * @return The current store address. The (raw) value is + * @c StorageManagerIF::INVALID_ADDRESS if the packet is not linked. */ store_address_t getStoreAddress(); /** @@ -99,7 +90,28 @@ public: * store or size is incorrect. */ bool isSizeCorrect(); + +private: + /** + * This is a pointer to the store all instances of the class use. + * If the store is not yet set (i.e. @c store is NULL), every constructor + * call tries to set it and throws an error message in case of failures. + * The default store is objects::TC_STORE. + */ + static StorageManagerIF* store; + /** + * The address where the packet data of the object instance is stored. + */ + store_address_t storeAddress; + /** + * A helper method to check if a store is assigned to the class. + * If not, the method tries to retrieve the store from the global + * ObjectManager. + * @return @li @c true if the store is linked or could be created. + * @li @c false otherwise. + */ + bool checkAndSetStore(); }; -#endif /* TCPACKETSTORED_H_ */ +#endif /* TMTCPACKET_PUS_TCPACKETSTORED_H_ */ diff --git a/tmtcpacket/pus/TmPacketBase.cpp b/tmtcpacket/pus/TmPacketBase.cpp index 3c1a8ca4..f3bc3f00 100644 --- a/tmtcpacket/pus/TmPacketBase.cpp +++ b/tmtcpacket/pus/TmPacketBase.cpp @@ -1,13 +1,19 @@ +#include "TmPacketBase.h" + #include "../../globalfunctions/CRC.h" +#include "../../globalfunctions/arrayprinter.h" #include "../../objectmanager/ObjectManagerIF.h" #include "../../serviceinterface/ServiceInterfaceStream.h" -#include "TmPacketBase.h" #include "../../timemanager/CCSDSTime.h" -#include -TmPacketBase::TmPacketBase(uint8_t* set_data) : - SpacePacketBase(set_data) { - tm_data = (TmPacketPointer*) set_data; +#include + +TimeStamperIF* TmPacketBase::timeStamper = nullptr; +object_id_t TmPacketBase::timeStamperId = 0; + +TmPacketBase::TmPacketBase(uint8_t* setData) : + SpacePacketBase(setData) { + tmData = reinterpret_cast(setData); } TmPacketBase::~TmPacketBase() { @@ -15,25 +21,25 @@ TmPacketBase::~TmPacketBase() { } uint8_t TmPacketBase::getService() { - return tm_data->data_field.service_type; + return tmData->data_field.service_type; } uint8_t TmPacketBase::getSubService() { - return tm_data->data_field.service_subtype; + return tmData->data_field.service_subtype; } uint8_t* TmPacketBase::getSourceData() { - return &tm_data->data; + return &tmData->data; } uint16_t TmPacketBase::getSourceDataSize() { - return getPacketDataLength() - sizeof(tm_data->data_field) + return getPacketDataLength() - sizeof(tmData->data_field) - CRC_SIZE + 1; } uint16_t TmPacketBase::getErrorControl() { uint32_t size = getSourceDataSize() + CRC_SIZE; - uint8_t* p_to_buffer = &tm_data->data; + uint8_t* p_to_buffer = &tmData->data; return (p_to_buffer[size - 2] << 8) + p_to_buffer[size - 1]; } @@ -47,16 +53,12 @@ void TmPacketBase::setErrorControl() { void TmPacketBase::setData(const uint8_t* p_Data) { SpacePacketBase::setData(p_Data); - tm_data = (TmPacketPointer*) p_Data; + tmData = (TmPacketPointer*) p_Data; } void TmPacketBase::print() { - /*uint8_t * wholeData = getWholeData(); - debug << "TmPacket contains: " << std::endl; - for (uint8_t count = 0; count < getFullSize(); ++count ) { - debug << std::hex << (uint16_t)wholeData[count] << " "; - } - debug << std::dec << std::endl;*/ + sif::debug << "TmPacketBase::print: " << std::endl; + arrayprinter::print(getWholeData(), getFullSize()); } bool TmPacketBase::checkAndSetStamper() { @@ -73,32 +75,36 @@ bool TmPacketBase::checkAndSetStamper() { ReturnValue_t TmPacketBase::getPacketTime(timeval* timestamp) const { uint32_t tempSize = 0; - return CCSDSTime::convertFromCcsds(timestamp, tm_data->data_field.time, - &tempSize, sizeof(tm_data->data_field.time)); + return CCSDSTime::convertFromCcsds(timestamp, tmData->data_field.time, + &tempSize, sizeof(tmData->data_field.time)); } uint8_t* TmPacketBase::getPacketTimeRaw() const{ - return tm_data->data_field.time; + return tmData->data_field.time; } -void TmPacketBase::initializeTmPacket(uint16_t apid, uint8_t service, uint8_t subservice, uint8_t packetSubcounter) { +void TmPacketBase::initializeTmPacket(uint16_t apid, uint8_t service, + uint8_t subservice, uint8_t packetSubcounter) { //Set primary header: initSpacePacketHeader(false, true, apid); //Set data Field Header: //First, set to zero. - memset(&tm_data->data_field, 0, sizeof(tm_data->data_field)); - //Set CCSDS_secondary header flag to 0, version number to 001 and ack to 0000 + memset(&tmData->data_field, 0, sizeof(tmData->data_field)); + // NOTE: In PUS-C, the PUS Version is 2 and specified for the first 4 bits. - // The other 4 bits of the first byte are the spacecraft time reference status - // To change to PUS-C, set 0b00100000 - tm_data->data_field.version_type_ack = 0b00010000; - tm_data->data_field.service_type = service; - tm_data->data_field.service_subtype = subservice; - tm_data->data_field.subcounter = packetSubcounter; + // The other 4 bits of the first byte are the spacecraft time reference + // status. To change to PUS-C, set 0b00100000. + // Set CCSDS_secondary header flag to 0, version number to 001 and ack + // to 0000 + tmData->data_field.version_type_ack = 0b00010000; + tmData->data_field.service_type = service; + tmData->data_field.service_subtype = subservice; + tmData->data_field.subcounter = packetSubcounter; //Timestamp packet if (checkAndSetStamper()) { - timeStamper->addTimeStamp(tm_data->data_field.time, sizeof(tm_data->data_field.time)); + timeStamper->addTimeStamp(tmData->data_field.time, + sizeof(tmData->data_field.time)); } } @@ -106,9 +112,6 @@ void TmPacketBase::setSourceDataSize(uint16_t size) { setPacketDataLength(size + sizeof(PUSTmDataFieldHeader) + CRC_SIZE - 1); } -uint32_t TmPacketBase::getTimestampSize() const { - return sizeof(tm_data->data_field.time); +size_t TmPacketBase::getTimestampSize() const { + return sizeof(tmData->data_field.time); } - -TimeStamperIF* TmPacketBase::timeStamper = NULL; -object_id_t TmPacketBase::timeStamperId = 0; diff --git a/tmtcpacket/pus/TmPacketBase.h b/tmtcpacket/pus/TmPacketBase.h index 85ae4dac..6efc0165 100644 --- a/tmtcpacket/pus/TmPacketBase.h +++ b/tmtcpacket/pus/TmPacketBase.h @@ -1,8 +1,8 @@ -#ifndef TMPACKETBASE_H_ -#define TMPACKETBASE_H_ +#ifndef TMTCPACKET_PUS_TMPACKETBASE_H_ +#define TMTCPACKET_PUS_TMPACKETBASE_H_ +#include "../SpacePacketBase.h" #include "../../timemanager/TimeStamperIF.h" -#include "../../tmtcpacket/SpacePacketBase.h" #include "../../timemanager/Clock.h" #include "../../objectmanager/SystemObjectIF.h" @@ -14,7 +14,7 @@ void setStaticFrameworkObjectIds(); * This struct defines a byte-wise structured PUS TM Data Field Header. * Any optional fields in the header must be added or removed here. * Currently, no Destination field is present, but an eigth-byte representation - * for a time tag [TBD]. + * for a time tag. * @ingroup tmtcpackets */ struct PUSTmDataFieldHeader { @@ -40,7 +40,7 @@ struct TmPacketPointer { /** * This class is the basic data handler for any ECSS PUS Telemetry packet. * - * In addition to \SpacePacketBase, the class provides methods to handle + * In addition to #SpacePacketBase, the class provides methods to handle * the standardized entries of the PUS TM Packet Data Field Header. * It does not contain the packet data itself but a pointer to the * data must be set on instantiation. An invalid pointer may cause @@ -54,29 +54,27 @@ public: /** * This constant defines the minimum size of a valid PUS Telemetry Packet. */ - static const uint32_t TM_PACKET_MIN_SIZE = (sizeof(CCSDSPrimaryHeader) + sizeof(PUSTmDataFieldHeader) + 2); //!< Minimum size of a valid PUS Telemetry Packet. - static const uint32_t MISSION_TM_PACKET_MAX_SIZE = 2048; //!< Maximum size of a TM Packet in this mission. - static const uint8_t VERSION_NUMBER_BYTE_PUS_A = 0b00010000; //!< First byte of secondary header for PUS-A packets. + static const uint32_t TM_PACKET_MIN_SIZE = (sizeof(CCSDSPrimaryHeader) + + sizeof(PUSTmDataFieldHeader) + 2); + //! Maximum size of a TM Packet in this mission. + //! TODO: Make this dependant on a config variable. + static const uint32_t MISSION_TM_PACKET_MAX_SIZE = 2048; + //! First byte of secondary header for PUS-A packets. + //! TODO: Maybe also support PUS-C via config? + static const uint8_t VERSION_NUMBER_BYTE_PUS_A = 0b00010000; + /** * This is the default constructor. * It sets its internal data pointer to the address passed and also * forwards the data pointer to the parent SpacePacketBase class. * @param set_address The position where the packet data lies. */ - TmPacketBase( uint8_t* set_data ); + TmPacketBase( uint8_t* setData ); /** * This is the empty default destructor. */ virtual ~TmPacketBase(); - /** - * Initializes the Tm Packet header. - * Does set the timestamp (to now), but not the error control field. - * @param apid APID used. - * @param service PUS Service - * @param subservice PUS Subservice - * @param packetSubcounter Additional subcounter used. - */ - void initializeTmPacket(uint16_t apid, uint8_t service, uint8_t subservice, uint8_t packetSubcounter); + /** * This is a getter for the packet's PUS Service ID, which is the second * byte of the Data Field Header. @@ -106,11 +104,14 @@ public: */ uint16_t getSourceDataSize(); - /** - * In case data was filled manually (almost never the case). - * @param size Size of source data (without CRC and data filed header!). - */ - void setSourceDataSize(uint16_t size); + /** + * With this method, the Error Control Field is updated to match the + * current content of the packet. This method is not protected because + * a recalculation by the user might be necessary when manipulating fields + * like the sequence count. + */ + void setErrorControl(); + /** * This getter returns the Error Control Field of the packet. * @@ -120,28 +121,15 @@ public: * @return The PUS Error Control */ uint16_t getErrorControl(); - /** - * With this method, the Error Control Field is updated to match the - * current content of the packet. - */ - void setErrorControl(); - /** - * With this method, the packet data pointer can be redirected to another - * location. - * - * This call overwrites the parent's setData method to set both its - * \c tc_data pointer and the parent's \c data pointer. - * - * @param p_data A pointer to another PUS Telemetry Packet. - */ - void setData( const uint8_t* p_Data ); + /** * This is a debugging helper method that prints the whole packet content * to the screen. */ void print(); /** - * Interprets the "time"-field in the secondary header and returns it in timeval format. + * Interprets the "time"-field in the secondary header and returns it in + * timeval format. * @return Converted timestamp of packet. */ ReturnValue_t getPacketTime(timeval* timestamp) const; @@ -151,7 +139,7 @@ public: */ uint8_t* getPacketTimeRaw() const; - uint32_t getTimestampSize() const; + size_t getTimestampSize() const; protected: /** @@ -160,20 +148,49 @@ protected: * * To be hardware-safe, all elements are of byte size. */ - TmPacketPointer* tm_data; + TmPacketPointer* tmData; /** * The timeStamper is responsible for adding a timestamp to the packet. * It is initialized lazy. */ static TimeStamperIF* timeStamper; + //! The ID to use when looking for a time stamper. + static object_id_t timeStamperId; - static object_id_t timeStamperId; //!< The ID to use when looking for a time stamper. - /** - * Checks if a time stamper is available and tries to set it if not. - * @return Returns false if setting failed. - */ - bool checkAndSetStamper(); + /** + * Initializes the Tm Packet header. + * Does set the timestamp (to now), but not the error control field. + * @param apid APID used. + * @param service PUS Service + * @param subservice PUS Subservice + * @param packetSubcounter Additional subcounter used. + */ + void initializeTmPacket(uint16_t apid, uint8_t service, uint8_t subservice, + uint8_t packetSubcounter); + + /** + * With this method, the packet data pointer can be redirected to another + * location. + * + * This call overwrites the parent's setData method to set both its + * @c tc_data pointer and the parent's @c data pointer. + * + * @param p_data A pointer to another PUS Telemetry Packet. + */ + void setData( const uint8_t* pData ); + + /** + * In case data was filled manually (almost never the case). + * @param size Size of source data (without CRC and data filed header!). + */ + void setSourceDataSize(uint16_t size); + + /** + * Checks if a time stamper is available and tries to set it if not. + * @return Returns false if setting failed. + */ + bool checkAndSetStamper(); }; -#endif /* TMPACKETBASE_H_ */ +#endif /* TMTCPACKET_PUS_TMPACKETBASE_H_ */ diff --git a/tmtcpacket/pus/TmPacketStored.cpp b/tmtcpacket/pus/TmPacketStored.cpp index 4e4c9434..0fb789aa 100644 --- a/tmtcpacket/pus/TmPacketStored.cpp +++ b/tmtcpacket/pus/TmPacketStored.cpp @@ -1,11 +1,16 @@ +#include "TmPacketStored.h" + #include "../../objectmanager/ObjectManagerIF.h" #include "../../serviceinterface/ServiceInterfaceStream.h" -#include "TmPacketStored.h" #include "../../tmtcservices/TmTcMessage.h" -#include + +#include + +StorageManagerIF *TmPacketStored::store = nullptr; +InternalErrorReporterIF *TmPacketStored::internalErrorReporter = nullptr; TmPacketStored::TmPacketStored(store_address_t setAddress) : - TmPacketBase(NULL), storeAddress(setAddress) { + TmPacketBase(nullptr), storeAddress(setAddress) { setStoreAddress(storeAddress); } @@ -14,10 +19,10 @@ TmPacketStored::TmPacketStored(uint16_t apid, uint8_t service, uint32_t size, const uint8_t *headerData, uint32_t headerSize) : TmPacketBase(NULL) { storeAddress.raw = StorageManagerIF::INVALID_ADDRESS; - if (!checkAndSetStore()) { + if (not checkAndSetStore()) { return; } - uint8_t *pData = NULL; + uint8_t *pData = nullptr; ReturnValue_t returnValue = store->getFreeElement(&storeAddress, (TmPacketBase::TM_PACKET_MIN_SIZE + size + headerSize), &pData); @@ -38,7 +43,7 @@ TmPacketStored::TmPacketStored(uint16_t apid, uint8_t service, SerializeIF *header) : TmPacketBase(NULL) { storeAddress.raw = StorageManagerIF::INVALID_ADDRESS; - if (!checkAndSetStore()) { + if (not checkAndSetStore()) { return; } size_t sourceDataSize = 0; @@ -77,29 +82,29 @@ store_address_t TmPacketStored::getStoreAddress() { void TmPacketStored::deletePacket() { store->deleteData(storeAddress); storeAddress.raw = StorageManagerIF::INVALID_ADDRESS; - setData(NULL); + setData(nullptr); } void TmPacketStored::setStoreAddress(store_address_t setAddress) { storeAddress = setAddress; - const uint8_t* temp_data = NULL; - size_t temp_size; - if (!checkAndSetStore()) { + const uint8_t* tempData = nullptr; + size_t tempSize; + if (not checkAndSetStore()) { return; } - ReturnValue_t status = store->getData(storeAddress, &temp_data, &temp_size); + ReturnValue_t status = store->getData(storeAddress, &tempData, &tempSize); if (status == StorageManagerIF::RETURN_OK) { - setData(temp_data); + setData(tempData); } else { - setData(NULL); + setData(nullptr); storeAddress.raw = StorageManagerIF::INVALID_ADDRESS; } } bool TmPacketStored::checkAndSetStore() { - if (store == NULL) { + if (store == nullptr) { store = objectManager->get(objects::TM_STORE); - if (store == NULL) { + if (store == nullptr) { sif::error << "TmPacketStored::TmPacketStored: TM Store not found!" << std::endl; return false; @@ -108,12 +113,9 @@ bool TmPacketStored::checkAndSetStore() { return true; } -StorageManagerIF *TmPacketStored::store = NULL; -InternalErrorReporterIF *TmPacketStored::internalErrorReporter = NULL; - ReturnValue_t TmPacketStored::sendPacket(MessageQueueId_t destination, MessageQueueId_t sentFrom, bool doErrorReporting) { - if (getWholeData() == NULL) { + if (getWholeData() == nullptr) { //SHOULDDO: More decent code. return HasReturnvaluesIF::RETURN_FAILED; } @@ -133,11 +135,11 @@ ReturnValue_t TmPacketStored::sendPacket(MessageQueueId_t destination, } void TmPacketStored::checkAndReportLostTm() { - if (internalErrorReporter == NULL) { + if (internalErrorReporter == nullptr) { internalErrorReporter = objectManager->get( objects::INTERNAL_ERROR_REPORTER); } - if (internalErrorReporter != NULL) { + if (internalErrorReporter != nullptr) { internalErrorReporter->lostTm(); } } diff --git a/tmtcpacket/pus/TmPacketStored.h b/tmtcpacket/pus/TmPacketStored.h index 8962d343..6ddbf131 100644 --- a/tmtcpacket/pus/TmPacketStored.h +++ b/tmtcpacket/pus/TmPacketStored.h @@ -1,9 +1,10 @@ -#ifndef TMPACKETSTORED_H_ -#define TMPACKETSTORED_H_ +#ifndef TMTCPACKET_PUS_TMPACKETSTORED_H_ +#define TMTCPACKET_PUS_TMPACKETSTORED_H_ + +#include "TmPacketBase.h" #include "../../serialize/SerializeIF.h" #include "../../storagemanager/StorageManagerIF.h" -#include "TmPacketBase.h" #include "../../internalError/InternalErrorReporterIF.h" #include "../../ipc/MessageQueueSenderIF.h" @@ -18,31 +19,6 @@ * @ingroup tmtcpackets */ class TmPacketStored : public TmPacketBase { -private: - /** - * This is a pointer to the store all instances of the class use. - * If the store is not yet set (i.e. \c store is NULL), every constructor - * call tries to set it and throws an error message in case of failures. - * The default store is objects::TM_STORE. - */ - static StorageManagerIF* store; - - static InternalErrorReporterIF *internalErrorReporter; - - /** - * The address where the packet data of the object instance is stored. - */ - store_address_t storeAddress; - /** - * A helper method to check if a store is assigned to the class. - * If not, the method tries to retrieve the store from the global - * ObjectManager. - * @return @li \c true if the store is linked or could be created. - * @li \c false otherwise. - */ - bool checkAndSetStore(); - - void checkAndReportLostTm(); public: /** * This is a default constructor which does not set the data pointer. @@ -52,28 +28,38 @@ public: /** * With this constructor, new space is allocated in the packet store and * a new PUS Telemetry Packet is created there. - * Packet Application Data passed in data is copied into the packet. The Application data is - * passed in two parts, first a header, then a data field. This allows building a Telemetry - * Packet from two separate data sources. + * Packet Application Data passed in data is copied into the packet. + * The Application data is passed in two parts, first a header, then a + * data field. This allows building a Telemetry Packet from two separate + * data sources. * @param apid Sets the packet's APID field. * @param service Sets the packet's Service ID field. * This specifies the source service. * @param subservice Sets the packet's Service Subtype field. * This specifies the source sub-service. * @param packet_counter Sets the Packet counter field of this packet - * @param data The payload data to be copied to the Application Data Field + * @param data The payload data to be copied to the + * Application Data Field * @param size The amount of data to be copied. - * @param headerData The header Data of the Application field; will be copied in front of data + * @param headerData The header Data of the Application field, + * will be copied in front of data * @param headerSize The size of the headerDataF */ - TmPacketStored( uint16_t apid, uint8_t service, uint8_t subservice, uint8_t packet_counter = 0, const uint8_t* data = NULL, uint32_t size = 0, const uint8_t* headerData = NULL, uint32_t headerSize = 0); + TmPacketStored( uint16_t apid, uint8_t service, uint8_t subservice, + uint8_t packet_counter = 0, const uint8_t* data = nullptr, + uint32_t size = 0, const uint8_t* headerData = nullptr, + uint32_t headerSize = 0); /** - * Another ctor to directly pass structured content and header data to the packet to avoid additional buffers. + * Another ctor to directly pass structured content and header data to the + * packet to avoid additional buffers. */ - TmPacketStored( uint16_t apid, uint8_t service, uint8_t subservice, uint8_t packet_counter, SerializeIF* content, SerializeIF* header = NULL); + TmPacketStored( uint16_t apid, uint8_t service, uint8_t subservice, + uint8_t packet_counter, SerializeIF* content, + SerializeIF* header = nullptr); /** * This is a getter for the current store address of the packet. - * @return The current store address. The (raw) value is \c StorageManagerIF::INVALID_ADDRESS if + * @return The current store address. The (raw) value is + * @c StorageManagerIF::INVALID_ADDRESS if * the packet is not linked. */ store_address_t getStoreAddress(); @@ -89,8 +75,34 @@ public: */ void setStoreAddress( store_address_t setAddress ); - ReturnValue_t sendPacket( MessageQueueId_t destination, MessageQueueId_t sentFrom, bool doErrorReporting = true ); + ReturnValue_t sendPacket( MessageQueueId_t destination, + MessageQueueId_t sentFrom, bool doErrorReporting = true ); +private: + /** + * This is a pointer to the store all instances of the class use. + * If the store is not yet set (i.e. @c store is NULL), every constructor + * call tries to set it and throws an error message in case of failures. + * The default store is objects::TM_STORE. + */ + static StorageManagerIF* store; + + static InternalErrorReporterIF *internalErrorReporter; + + /** + * The address where the packet data of the object instance is stored. + */ + store_address_t storeAddress; + /** + * A helper method to check if a store is assigned to the class. + * If not, the method tries to retrieve the store from the global + * ObjectManager. + * @return @li @c true if the store is linked or could be created. + * @li @c false otherwise. + */ + bool checkAndSetStore(); + + void checkAndReportLostTm(); }; -#endif /* TMPACKETSTORED_H_ */ +#endif /* TMTCPACKET_PUS_TMPACKETSTORED_H_ */