diff --git a/tmtcservices/TmTcBridge.cpp b/tmtcservices/TmTcBridge.cpp new file mode 100644 index 000000000..f9a7d3bc5 --- /dev/null +++ b/tmtcservices/TmTcBridge.cpp @@ -0,0 +1,192 @@ +#include + +#include +#include +#include +#include + +TmTcBridge::TmTcBridge(object_id_t objectId_, + object_id_t ccsdsPacketDistributor_): SystemObject(objectId_), + ccsdsPacketDistributor(ccsdsPacketDistributor_) +{ + TmTcReceptionQueue = QueueFactory::instance()-> + createMessageQueue(TMTC_RECEPTION_QUEUE_DEPTH); +} + +TmTcBridge::~TmTcBridge() {} + +ReturnValue_t TmTcBridge::setNumberOfSentPacketsPerCycle( + uint8_t sentPacketsPerCycle) { + if(sentPacketsPerCycle <= LIMIT_STORED_DATA_SENT_PER_CYCLE) { + this->sentPacketsPerCycle = sentPacketsPerCycle; + return RETURN_OK; + } + else { + sif::warning << "TmTcBridge: Number of packets sent per cycle " + "exceeds limits. Keeping default value." << std::endl; + return RETURN_FAILED; + } +} + +ReturnValue_t TmTcBridge::setMaxNumberOfPacketsStored( + uint8_t maxNumberOfPacketsStored) { + if(maxNumberOfPacketsStored <= LIMIT_DOWNLINK_PACKETS_STORED) { + this->maxNumberOfPacketsStored = maxNumberOfPacketsStored; + return RETURN_OK; + } + else { + sif::warning << "TmTcBridge: Number of packets stored " + "exceeds limits. Keeping default value." << std::endl; + return RETURN_FAILED; + } +} + +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) { + sif::error << "TMTC Bridge: Error handling TCs" << std::endl; + } + result = handleTm(); + if (result != RETURN_OK) { + sif::error << "TMTC Bridge: Error handling TMs" << std::endl; + } + return result; +} + +ReturnValue_t TmTcBridge::handleTc() { + uint8_t * recvBuffer = nullptr; + size_t recvLen = 0; + ReturnValue_t result = receiveTc(&recvBuffer, &recvLen); + return result; +} + +ReturnValue_t TmTcBridge::handleTm() { + ReturnValue_t result = handleTmQueue(); + if(result != RETURN_OK) { + sif::error << "TMTC Bridge: Reading TM Queue failed" << std::endl; + return RETURN_FAILED; + } + + if(tmStored and communicationLinkUp) { + result = handleStoredTm(); + } + return result; + +} + +ReturnValue_t TmTcBridge::handleTmQueue() { + TmTcMessage message; + const uint8_t* data = nullptr; + 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) { + sif::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) { + //debug << "TMTC Bridge: Comm Link down. " + // "Saving packet ID to be sent later\r\n" << std::flush; + store_address_t storeId = 0; + + if(tmFifo.full()) { + sif::error << "TMTC Bridge: TM downlink max. number of stored packet IDs " + "reached! Overwriting old data" << std::endl; + tmFifo.retrieve(&storeId); + tmStore->deleteData(storeId); + } + storeId = message->getStorageId(); + tmFifo.insert(storeId); + tmStored = true; + return RETURN_OK; +} + +ReturnValue_t TmTcBridge::handleStoredTm() { + uint8_t counter = 0; + ReturnValue_t result = RETURN_OK; + while(not tmFifo.empty() and counter < sentPacketsPerCycle) { + //info << "TMTC Bridge: Sending stored TM data. There are " + // << (int) fifo.size() << " left to send\r\n" << std::flush; + store_address_t storeId; + const uint8_t* data = nullptr; + size_t size = 0; + tmFifo.retrieve(&storeId); + result = tmStore->getData(storeId, &data, &size); + + sendTm(data,size); + + if(result != RETURN_OK) { + sif::error << "TMTC Bridge: Could not send stored downlink data" + << std::endl; + result = RETURN_FAILED; + } + counter ++; + + if(tmFifo.empty()) { + tmStored = false; + } + tmStore->deleteData(storeId); + } + return result; +} + +void TmTcBridge::registerCommConnect() { + if(not communicationLinkUp) { + //info << "TMTC Bridge: Registered Comm Link Connect" << std::endl; + communicationLinkUp = true; + } +} + +void TmTcBridge::registerCommDisconnect() { + //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, size_t dataLen) { + arrayprinter::print(data, dataLen); +} diff --git a/tmtcservices/TmTcBridge.h b/tmtcservices/TmTcBridge.h new file mode 100644 index 000000000..3e0432d8a --- /dev/null +++ b/tmtcservices/TmTcBridge.h @@ -0,0 +1,152 @@ +#ifndef FRAMEWORK_TMTCSERVICES_TMTCBRIDGE_H_ +#define FRAMEWORK_TMTCSERVICES_TMTCBRIDGE_H_ + +#include +#include +#include +#include +#include + +#include +#include + +class TmTcBridge : public AcceptsTelemetryIF, + public ExecutableObjectIF, + public HasReturnvaluesIF, + public SystemObject { +public: + static constexpr uint8_t TMTC_RECEPTION_QUEUE_DEPTH = 20; + static constexpr uint8_t LIMIT_STORED_DATA_SENT_PER_CYCLE = 15; + static constexpr uint8_t LIMIT_DOWNLINK_PACKETS_STORED = 20; + + static constexpr uint8_t DEFAULT_STORED_DATA_SENT_PER_CYCLE = 5; + static constexpr uint8_t DEFAULT_DOWNLINK_PACKETS_STORED = 10; + + TmTcBridge(object_id_t objectId_, object_id_t ccsdsPacketDistributor_); + virtual ~TmTcBridge(); + + /** + * Set number of packets sent per performOperation().Please note that this + * value must be smaller than MAX_STORED_DATA_SENT_PER_CYCLE + * @param sentPacketsPerCycle + * @return -@c RETURN_OK if value was set successfully + * -@c RETURN_FAILED otherwise, stored value stays the same + */ + ReturnValue_t setNumberOfSentPacketsPerCycle(uint8_t sentPacketsPerCycle); + + /** + * Set number of packets sent per performOperation().Please note that this + * value must be smaller than MAX_DOWNLINK_PACKETS_STORED + * @param sentPacketsPerCycle + * @return -@c RETURN_OK if value was set successfully + * -@c RETURN_FAILED otherwise, stored value stays the same + */ + ReturnValue_t setMaxNumberOfPacketsStored(uint8_t maxNumberOfPacketsStored); + + virtual void registerCommConnect(); + virtual void registerCommDisconnect(); + + /** + * Initializes necessary FSFW components for the TMTC Bridge + * @return + */ + virtual ReturnValue_t initialize() override; + + /** + * @brief Handles TMTC reception + */ + virtual ReturnValue_t performOperation(uint8_t operationCode = 0) override; + + /** + * Return TMTC Reception Queue + * @param virtualChannel + * @return + */ + MessageQueueId_t getReportReceptionQueue( + uint8_t virtualChannel = 0) override; +protected: + //! Used to send and receive TMTC messages. + //! TmTcMessage is used to transport messages between tasks. + MessageQueueIF* TmTcReceptionQueue = nullptr; + StorageManagerIF* tcStore = nullptr; + StorageManagerIF* tmStore = nullptr; + object_id_t ccsdsPacketDistributor = 0; + //! Used to specify whether communication link is up + bool communicationLinkUp = false; + bool tmStored = false; + + /** + * @brief Handle TC reception + * @details + * Default implementation provided, but is empty. + * Child handler should override this in most cases orsend TC to the + * TC distributor directly with the address of the reception queue by + * calling getReportRecptionQueue() + * @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 an ethernet stack + * @param recvBuffer [out] Received data + * @param size [out] Size of received data + * @return + */ + virtual ReturnValue_t receiveTc(uint8_t ** recvBuffer, size_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 handleTmQueue(); + + /** + * Send stored data if communication link is active + * @return + */ + virtual ReturnValue_t handleStoredTm(); + + /** + * 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, size_t dataLen) = 0; + + /** + * Store data to be sent later if communication link is not up. + * @param message + * @return + */ + virtual ReturnValue_t storeDownlinkData(TmTcMessage * message); + + + /** + * Print data as hexidecimal array + * @param data + * @param dataLen + */ + void printData(uint8_t * data, size_t dataLen); + + /** + * This fifo can be used to store downlink data + * which can not be sent at the moment. + */ + FIFO tmFifo; + uint8_t sentPacketsPerCycle = DEFAULT_STORED_DATA_SENT_PER_CYCLE; + uint8_t maxNumberOfPacketsStored = DEFAULT_DOWNLINK_PACKETS_STORED; +}; + + +#endif /* FRAMEWORK_TMTCSERVICES_TMTCBRIDGE_H_ */