diff --git a/objectmanager/ObjectManager.cpp b/objectmanager/ObjectManager.cpp index cfdf6562..69542560 100644 --- a/objectmanager/ObjectManager.cpp +++ b/objectmanager/ObjectManager.cpp @@ -1,6 +1,7 @@ #include #include #include +#include ObjectManager::ObjectManager( void (*setProducer)() ): produceObjects(setProducer) { diff --git a/serialize/SerialBufferAdapter2.h b/serialize/SerialBufferAdapter2.h new file mode 100755 index 00000000..b897eb08 --- /dev/null +++ b/serialize/SerialBufferAdapter2.h @@ -0,0 +1,149 @@ +#ifndef SERIALBUFFERADAPTER2_H_ +#define SERIALBUFFERADAPTER2_H_ + +#include +#include +#include + +#include +extern std::ostream error; +extern std::ostream debug; +/** + * This adapter provides an interface for SerializeIF to serialize or deserialize + * buffers with no length header but a known size. + * + * Additionally, the buffer length can be serialized too and will be put in front of the serialized buffer. + * + * Can be used with SerialLinkedListAdapter by declaring a SerializeElement with + * SerialElement> serialBufferElement. + * Right now, the SerialBufferAdapter must always be initialized with the buffer and size ! + * + * \ingroup serialize + */ +template +class SerialBufferAdapter2: public SerializeIF { +public: + /** + * Constructor for constant uint8_t buffer. Length field can be serialized optionally. + * Type of length can be supplied as template type. + * @param buffer + * @param bufferLength + * @param serializeLength + */ + SerialBufferAdapter2(void * buffer_, count_t bufferLength_, bool serializeLength_ = false): + bufferLength(bufferLength_), serializeLength(serializeLength_) { + determineLengthInBytes(sizeof(BUFFER_TYPE)); + buffer = reinterpret_cast(buffer_); + constBuffer = NULL; + } + + SerialBufferAdapter2(const void * buffer_, count_t bufferLength_, bool serializeLength_ = false): + bufferLength(bufferLength_), serializeLength(serializeLength_) { + determineLengthInBytes(sizeof(BUFFER_TYPE)); + constBuffer = reinterpret_cast(buffer_); + buffer = NULL; + } + + ReturnValue_t serialize(uint8_t ** buffer, uint32_t* size, + const uint32_t max_size, bool bigEndian) const { + uint32_t serializedLength = bufferLength; + if (serializeLength) { + serializedLength += AutoSerializeAdapter::getSerializedSize( + &bufferLength); + } + if (*size + serializedLength > max_size) { + return BUFFER_TOO_SHORT; + } else { + if (serializeLength) { + AutoSerializeAdapter::serialize(&bufferLength, buffer, size, + max_size, bigEndian); + } + memcpy(*buffer, this->buffer, bufferLength); + *size += bufferLength; + (*buffer) += bufferLength; + return HasReturnvaluesIF::RETURN_OK; + } + } + + uint32_t getSerializedSize() const { + if (serializeLength) { + return bufferLength + AutoSerializeAdapter::getSerializedSize(&bufferLength); + } else { + return bufferLength; + } + } + + ReturnValue_t deSerialize(const uint8_t** buffer, + int32_t* size, bool bigEndian) { + //TODO Ignores Endian flag! + if (buffer != NULL) { + if(serializeLength){ + // Suggestion (would require removing rest of the block inside this if clause !): + //ReturnValue_t result = AutoSerializeAdapter::deSerialize(&bufferLength,buffer,size,bigEndian); + //if (result != HasReturnvaluesIF::RETURN_OK) { + // return result; + //} + count_t serializedSize = AutoSerializeAdapter::getSerializedSize( + &bufferLength); + if((*size - bufferLength - serializedSize) >= 0){ + *buffer += serializedSize; + *size -= serializedSize; + }else{ + return STREAM_TOO_SHORT; + } + } + //No Else If, go on with buffer + if (*size - bufferLength >= 0) { + *size -= bufferLength; + memcpy(this->buffer, *buffer, bufferLength); + (*buffer) += bufferLength; + return HasReturnvaluesIF::RETURN_OK; + } else { + return STREAM_TOO_SHORT; + } + } else { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + + + BUFFER_TYPE * getBuffer() { + return reinterpret_cast(buffer); + } + + void setBuffer(void * buffer_, count_t bufferLength_, bool serializeLength_ = false) { + buffer = buffer_; + bufferLength = bufferLength_; + serializeLength = serializeLength_; + determineLengthInBytes(sizeof(BUFFER_TYPE)); + } + + void setConstBuffer(const void * buffer_, count_t bufferLength_, bool serializeLength_ = false) { + constBuffer = buffer_; + bufferLength = bufferLength_; + serializeLength = serializeLength_; + determineLengthInBytes(sizeof(BUFFER_TYPE)); + } +private: + uint8_t * buffer; + const uint8_t * constBuffer; + count_t bufferLength; + bool serializeLength; + + void determineLengthInBytes(uint8_t typeSize) { + switch(typeSize) { + case(1): break; + case(2): + bufferLength *= 2; break; + case(4): + bufferLength *= 4; break; + case(8): + bufferLength *= 8; break; + default: + error << "Serial Buffer Adapter 2: Invalid type size, assuming regular uint8_t." << std::endl; + error << "Detected type size: " << (int) typeSize << std::endl; + } + } +}; + +#endif /* SERIALBUFFERADAPTER2_H_ */ diff --git a/tmtcservices/TmTcBridge.cpp b/tmtcservices/TmTcBridge.cpp new file mode 100644 index 00000000..3913e424 --- /dev/null +++ b/tmtcservices/TmTcBridge.cpp @@ -0,0 +1,177 @@ +/** + * @file TmTcBridge.cpp + * + * @date 26.12.2019 + * @author R. Mueller + */ + +#include + +#include +#include +#include +extern std::ostream error; +extern std::ostream debug; +extern std::ostream info; +TmTcBridge::TmTcBridge(object_id_t objectId_, object_id_t ccsdsPacketDistributor_): + SystemObject(objectId_),tcStore(NULL), tmStore(NULL), + ccsdsPacketDistributor(ccsdsPacketDistributor_), communicationLinkUp(false), + tmStored(false),recvBuffer(NULL), size(0) { + TmTcReceptionQueue = QueueFactory::instance()-> + createMessageQueue(TMTC_RECEPTION_QUEUE_DEPTH); + +} + +TmTcBridge::~TmTcBridge() { +} + +ReturnValue_t TmTcBridge::initialize() { + tcStore = objectManager->get(objects::TC_STORE); + if (tcStore == NULL) { + return RETURN_FAILED; + } + tmStore = objectManager->get(objects::TM_STORE); + if (tmStore == NULL) { + return RETURN_FAILED; + } + AcceptsTelecommandsIF* tcDistributor = + objectManager->get(ccsdsPacketDistributor); + if (tcDistributor == NULL) { + return RETURN_FAILED; + } + TmTcReceptionQueue->setDefaultDestination(tcDistributor->getRequestQueue()); + return RETURN_OK; +} + +ReturnValue_t TmTcBridge::performOperation(uint8_t operationCode) { + ReturnValue_t result; + result = handleTc(); + if(result != RETURN_OK) { + error << "TMTC Bridge: Error handling TCs" << std::endl; + } + result = handleTm(); + if (result != RETURN_OK) { + error << "TMTC Bridge: Error handling TMs" << std::endl; + } + return result; +} + +ReturnValue_t TmTcBridge::handleTc() { + ReturnValue_t result = receiveTc(&recvBuffer, &size); + return result; +} + +ReturnValue_t TmTcBridge::handleTm() { + ReturnValue_t result = readTmQueue(); + if(result != RETURN_OK) { + error << "TMTC Bridge: Reading TM Queue failed" << std::endl; + return RETURN_FAILED; + } + + if(tmStored && communicationLinkUp) { + result = sendStoredTm(); + } + return result; + +} + +ReturnValue_t TmTcBridge::readTmQueue() { + TmTcMessage message; + const uint8_t* data = NULL; + size_t size = 0; + for (ReturnValue_t result = TmTcReceptionQueue->receiveMessage(&message); + result == RETURN_OK; result = TmTcReceptionQueue->receiveMessage(&message)) + { + if(communicationLinkUp == false) { + result = storeDownlinkData(&message); + return result; + } + + result = tmStore->getData(message.getStorageId(), &data, &size); + if (result != HasReturnvaluesIF::RETURN_OK) { + continue; + } + + result = sendTm(data, size); + if (result != RETURN_OK) { + error << "TMTC Bridge: Could not send TM packet"<< std::endl; + tmStore->deleteData(message.getStorageId()); + return result; + + } + tmStore->deleteData(message.getStorageId()); + } + return RETURN_OK; +} + +ReturnValue_t TmTcBridge::storeDownlinkData(TmTcMessage *message) { + sif::info << "TMTC Bridge: Comm Link down. " + "Saving packet ID to be sent later " << std::endl; + store_address_t storeId; + + if(fifo.full()) { + sif::info << "TMTC Bridge: TM downlink max. number of stored packet IDs reached." + " Overwriting old data" << std::endl; + fifo.retrieve(&storeId); + tmStore->deleteData(storeId); + } + storeId = message->getStorageId(); + fifo.insert(storeId); + tmStored = true; + return RETURN_OK; +} + +ReturnValue_t TmTcBridge::sendStoredTm() { + uint8_t counter = 0; + ReturnValue_t result = RETURN_OK; + while(!fifo.empty() && counter < MAX_STORED_DATA_SENT_PER_CYCLE) { + sif::info << "UDP Server: Sending stored TM data. There are " + << (int) fifo.size() << " left to send" << std::endl; + store_address_t storeId; + const uint8_t* data = NULL; + size_t size = 0; + fifo.retrieve(&storeId); + result = tmStore->getData(storeId, &data, &size); + sendTm(data,size); + if(result != RETURN_OK) { + error << "UDP Server: Could not send stored downlink data" << std::endl; + result = RETURN_FAILED; + } + counter ++; + + if(fifo.empty()) { + tmStored = false; + } + tmStore->deleteData(storeId); + } + return result; +} + +void TmTcBridge::registerCommConnect() { + if(!communicationLinkUp) { + sif::info << "TMTC Bridge: Registered Comm Link Connect" << std::endl; + communicationLinkUp = true; + } +} + +void TmTcBridge::registerCommDisconnect() { + sif::info << "TMTC Bridge: Registered Comm Link Disconnect" << std::endl; + if(communicationLinkUp) { + communicationLinkUp = false; + } +} + +MessageQueueId_t TmTcBridge::getReportReceptionQueue(uint8_t virtualChannel) { + return TmTcReceptionQueue->getId(); +} + +void TmTcBridge::printData(uint8_t * data, uint32_t dataLen) { + sif::info << "TMTC Bridge: Printing data: ["; + for(uint32_t i=0;i +#include +#include +#include +#include + +#include +#include + +class TmTcBridge : public AcceptsTelemetryIF, + public ExecutableObjectIF, + public HasReturnvaluesIF, + public SystemObject { +public: + TmTcBridge(object_id_t objectId_, object_id_t ccsdsPacketDistributor_); + virtual ~TmTcBridge(); + + /** + * Initializes basic FSFW components for the TMTC Bridge + * @return + */ + virtual ReturnValue_t initialize(); + + /** + * @brief The performOperation method is executed in a task. + * @details There are no restrictions for calls within this method, so any + * other member of the class can be used. + * @return Currently, the return value is ignored. + */ + virtual ReturnValue_t performOperation(uint8_t operationCode = 0); + + /** + * Return TMTC Reception Queue + * @param virtualChannel + * @return + */ + virtual MessageQueueId_t getReportReceptionQueue(uint8_t virtualChannel = 0); + + void registerCommConnect(); + void registerCommDisconnect(); +protected: + MessageQueueIF* TmTcReceptionQueue; //!< Used to send and receive TMTC messages. TmTcMessage is used to transport messages between tasks. + StorageManagerIF* tcStore; + StorageManagerIF* tmStore; + object_id_t ccsdsPacketDistributor; + bool communicationLinkUp; //!< Used to specify whether communication link is up + bool tmStored; + + /** + * Handle TC reception. Default implementation provided + * @return + */ + virtual ReturnValue_t handleTc(); + + /** + * Implemented by child class. Perform receiving of Telecommand, for example by implementing + * specific drivers or wrappers, e.g. UART Communication or lwIP stack + * @param recvBuffer [out] Received data + * @param size [out] Size of received data + * @return + */ + virtual ReturnValue_t receiveTc(uint8_t ** recvBuffer, uint32_t * size) = 0; + + /** + * Handle Telemetry. Default implementation provided. + * Calls sendTm() + * @return + */ + virtual ReturnValue_t handleTm(); + + /** + * Read the TM Queue and send TM if necessary. Default implementation provided + * @return + */ + virtual ReturnValue_t readTmQueue(); + + /** + * Implemented by child class. Perform sending of Telemetry by implementing + * communication drivers or wrappers, e.g. UART communication or lwIP stack. + * @param data + * @param dataLen + * @return + */ + virtual ReturnValue_t sendTm(const uint8_t * data, uint32_t dataLen) = 0; + + /** + * Store data to be sent later if communication link is not up. + * @param message + * @return + */ + ReturnValue_t storeDownlinkData(TmTcMessage * message); + + /** + * Send stored data if communication link is active + * @return + */ + ReturnValue_t sendStoredTm(); + + /** + * Print data as hexidecimal array + * @param data + * @param dataLen + */ + void printData(uint8_t * data, uint32_t dataLen); + +private: + static const uint8_t TMTC_RECEPTION_QUEUE_DEPTH = 20; + static const uint8_t MAX_STORED_DATA_SENT_PER_CYCLE = 10; + static const uint8_t MAX_DOWNLINK_PACKETS_STORED = 15; + + FIFO fifo; + uint8_t * recvBuffer; + uint32_t size; +}; + + +#endif /* FRAMEWORK_TMTCSERVICES_TMTCBRIDGE_H_ */