From 3e0819f954d3eb6b6fb090f7b0d157d867f4ed6c Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sun, 6 Sep 2020 11:48:32 +0200 Subject: [PATCH] some bugfixes --- fsfw.mk | 4 + osal/host/PeriodicTask.cpp | 4 + osal/host/QueueMapManager.cpp | 14 ++- osal/windows/TcWinUdpPollingTask.cpp | 147 +++++++++++++++++++++++++++ osal/windows/TcWinUdpPollingTask.h | 67 ++++++++++++ osal/windows/TmTcWinUdpBridge.cpp | 4 +- 6 files changed, 234 insertions(+), 6 deletions(-) create mode 100644 osal/windows/TcWinUdpPollingTask.cpp create mode 100644 osal/windows/TcWinUdpPollingTask.h diff --git a/fsfw.mk b/fsfw.mk index a548b6a8..b8e9ff98 100644 --- a/fsfw.mk +++ b/fsfw.mk @@ -37,6 +37,10 @@ else ifeq ($(OS_FSFW),freeRTOS) CXXSRC += $(wildcard $(FRAMEWORK_PATH)/osal/FreeRTOS/*.cpp) else ifeq ($(OS_FSFW),host) CXXSRC += $(wildcard $(FRAMEWORK_PATH)/osal/host/*.cpp) +ifeq ($(OS),Windows_NT) +CXXSRC += $(wildcard $(FRAMEWORK_PATH)/osal/windows/*.cpp) +endif + else $(error invalid OS_FSFW specified, valid OS_FSFW are rtems, linux, freeRTOS, host) endif diff --git a/osal/host/PeriodicTask.cpp b/osal/host/PeriodicTask.cpp index 3157ffb7..f7807a17 100644 --- a/osal/host/PeriodicTask.cpp +++ b/osal/host/PeriodicTask.cpp @@ -96,6 +96,10 @@ void PeriodicTask::taskFunctionality() { }; auto nextStartTime{ currentStartTime }; + for (const auto& object: objectList) { + object->initializeAfterTaskCreation(); + } + /* Enter the loop that defines the task behavior. */ for (;;) { if(terminateThread.load()) { diff --git a/osal/host/QueueMapManager.cpp b/osal/host/QueueMapManager.cpp index 1b2094e9..c3d94584 100644 --- a/osal/host/QueueMapManager.cpp +++ b/osal/host/QueueMapManager.cpp @@ -44,9 +44,17 @@ MessageQueueIF* QueueMapManager::getMessageQueue( return queueIter->second; } else { - sif::warning << "QueueMapManager::getQueueHandle: The ID" << - messageQueueId << " does not exists in the map" << std::endl; - return nullptr; + if(messageQueueId == MessageQueueIF::NO_QUEUE) { + sif::error << "QueueMapManager::getQueueHandle: Configuration" + << " error, NO_QUEUE was passed to this function!" + << std::endl; + } + else { + sif::warning << "QueueMapManager::getQueueHandle: The ID " + << messageQueueId << " does not exists in the map." + << std::endl; + } + return nullptr; } } diff --git a/osal/windows/TcWinUdpPollingTask.cpp b/osal/windows/TcWinUdpPollingTask.cpp new file mode 100644 index 00000000..661d596a --- /dev/null +++ b/osal/windows/TcWinUdpPollingTask.cpp @@ -0,0 +1,147 @@ +#include "TcWinUdpPollingTask.h" +#include "../../globalfunctions/arrayprinter.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" + +#include +#include + +TcWinUdpPollingTask::TcWinUdpPollingTask(object_id_t objectId, + object_id_t tmtcUnixUdpBridge, size_t frameSize, + double timeoutSeconds): SystemObject(objectId), + tmtcBridgeId(tmtcUnixUdpBridge) { + if(frameSize > 0) { + this->frameSize = frameSize; + } + else { + this->frameSize = DEFAULT_MAX_FRAME_SIZE; + } + + // Set up reception buffer with specified frame size. + // For now, it is assumed that only one frame is held in the buffer! + receptionBuffer.reserve(this->frameSize); + receptionBuffer.resize(this->frameSize); + + if(timeoutSeconds == -1) { + receptionTimeout = DEFAULT_TIMEOUT; + } + else { + receptionTimeout = timevalOperations::toTimeval(timeoutSeconds); + } +} + +TcWinUdpPollingTask::~TcWinUdpPollingTask() {} + +ReturnValue_t TcWinUdpPollingTask::performOperation(uint8_t opCode) { + // Poll for new UDP datagrams in permanent loop. + while(1) { + //! Sender Address is cached here. + struct sockaddr_in senderAddress; + int senderAddressSize = sizeof(senderAddress); + ssize_t bytesReceived = recvfrom(serverUdpSocket, + reinterpret_cast(receptionBuffer.data()), frameSize, + receptionFlags, reinterpret_cast(&senderAddress), + &senderAddressSize); + if(bytesReceived == SOCKET_ERROR) { + // handle error + sif::error << "TcWinUdpPollingTask::performOperation: Reception" + " error." << std::endl; + handleReadError(); + continue; + } + sif::debug << "TcSocketPollingTask::performOperation: " << bytesReceived + << " bytes received" << std::endl; + + ReturnValue_t result = handleSuccessfullTcRead(bytesReceived); + if(result != HasReturnvaluesIF::RETURN_FAILED) { + + } + tmtcBridge->registerCommConnect(); + tmtcBridge->checkAndSetClientAddress(senderAddress); + } + return HasReturnvaluesIF::RETURN_OK; +} + + +ReturnValue_t TcWinUdpPollingTask::handleSuccessfullTcRead(size_t bytesRead) { + store_address_t storeId; + ReturnValue_t result = tcStore->addData(&storeId, + receptionBuffer.data(), bytesRead); + // arrayprinter::print(receptionBuffer.data(), bytesRead); + if (result != HasReturnvaluesIF::RETURN_OK) { + sif::error << "TcSerialPollingTask::transferPusToSoftwareBus: Data " + "storage failed" << std::endl; + sif::error << "Packet size: " << bytesRead << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + + TmTcMessage message(storeId); + + result = MessageQueueSenderIF::sendMessage(targetTcDestination, &message); + if (result != HasReturnvaluesIF::RETURN_OK) { + sif::error << "Serial Polling: Sending message to queue failed" + << std::endl; + tcStore->deleteData(storeId); + } + return result; +} + +ReturnValue_t TcWinUdpPollingTask::initialize() { + tcStore = objectManager->get(objects::TC_STORE); + if (tcStore == nullptr) { + sif::error << "TcSerialPollingTask::initialize: TC Store uninitialized!" + << std::endl; + return ObjectManagerIF::CHILD_INIT_FAILED; + } + + tmtcBridge = objectManager->get(tmtcBridgeId); + if(tmtcBridge == nullptr) { + sif::error << "TcSocketPollingTask::TcSocketPollingTask: Invalid" + " TMTC bridge object!" << std::endl; + return ObjectManagerIF::CHILD_INIT_FAILED; + } + + serverUdpSocket = tmtcBridge->serverSocket; + //sif::info << "TcWinUdpPollingTask::initialize: Server UDP socket " + // << serverUdpSocket << std::endl; + + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t TcWinUdpPollingTask::initializeAfterTaskCreation() { + // Initialize the destination after task creation. This ensures + // that the destination has already been set in the TMTC bridge. + targetTcDestination = tmtcBridge->getRequestQueue(); + return HasReturnvaluesIF::RETURN_OK; +} + +void TcWinUdpPollingTask::setTimeout(double timeoutSeconds) { + DWORD timeoutMs = timeoutSeconds * 1000.0; + int result = setsockopt(serverUdpSocket, SOL_SOCKET, SO_RCVTIMEO, + reinterpret_cast(&timeoutMs), sizeof(DWORD)); + if(result == -1) { + sif::error << "TcSocketPollingTask::TcSocketPollingTask: Setting " + "receive timeout failed with " << strerror(errno) << std::endl; + } +} + +void TcWinUdpPollingTask::handleReadError() { + int error = WSAGetLastError(); + switch(error) { + case(WSANOTINITIALISED): { + sif::info << "TmTcWinUdpBridge::handleReadError: WSANOTINITIALISED: " + << "WSAStartup(...) call " << "necessary" << std::endl; + break; + } + case(WSAEFAULT): { + sif::info << "TmTcWinUdpBridge::handleReadError: WSADEFAULT: " + << "Bad address " << std::endl; + Sleep(2000); + break; + } + default: { + sif::info << "TmTcWinUdpBridge::handleReadError: Error code: " + << error << std::endl; + break; + } + } +} diff --git a/osal/windows/TcWinUdpPollingTask.h b/osal/windows/TcWinUdpPollingTask.h new file mode 100644 index 00000000..50d39d25 --- /dev/null +++ b/osal/windows/TcWinUdpPollingTask.h @@ -0,0 +1,67 @@ +#ifndef FSFW_OSAL_WINDOWS_TCSOCKETPOLLINGTASK_H_ +#define FSFW_OSAL_WINDOWS_TCSOCKETPOLLINGTASK_H_ + +#include "TmTcWinUdpBridge.h" +#include "../../objectmanager/SystemObject.h" +#include "../../tasks/ExecutableObjectIF.h" +#include "../../storagemanager/StorageManagerIF.h" + +#include + +/** + * @brief This class can be used to implement the polling of a Unix socket, + * using UDP for now. + * @details + * The task will be blocked while the specified number of bytes has not been + * received, so TC reception is handled inside a separate task. + * This class caches the IP address of the sender. It is assumed there + * is only one sender for now. + */ +class TcWinUdpPollingTask: public SystemObject, + public ExecutableObjectIF { + friend class TmTcWinUdpBridge; +public: + static constexpr size_t DEFAULT_MAX_FRAME_SIZE = 2048; + //! 0.5 default milliseconds timeout for now. + static constexpr timeval DEFAULT_TIMEOUT = {.tv_sec = 0, .tv_usec = 500}; + + TcWinUdpPollingTask(object_id_t objectId, object_id_t tmtcUnixUdpBridge, + size_t frameSize = 0, double timeoutSeconds = -1); + virtual~ TcWinUdpPollingTask(); + + /** + * Turn on optional timeout for UDP polling. In the default mode, + * the receive function will block until a packet is received. + * @param timeoutSeconds + */ + void setTimeout(double timeoutSeconds); + + virtual ReturnValue_t performOperation(uint8_t opCode) override; + virtual ReturnValue_t initialize() override; + virtual ReturnValue_t initializeAfterTaskCreation() override; + +protected: + StorageManagerIF* tcStore = nullptr; + +private: + //! TMTC bridge is cached. + object_id_t tmtcBridgeId = objects::NO_OBJECT; + TmTcWinUdpBridge* tmtcBridge = nullptr; + MessageQueueId_t targetTcDestination = MessageQueueIF::NO_QUEUE; + //! Reception flags: https://linux.die.net/man/2/recvfrom. + int receptionFlags = 0; + + //! Server socket, which is member of TMTC bridge and is assigned in + //! constructor + SOCKET serverUdpSocket = 0; + + std::vector receptionBuffer; + + size_t frameSize = 0; + timeval receptionTimeout; + + ReturnValue_t handleSuccessfullTcRead(size_t bytesRead); + void handleReadError(); +}; + +#endif /* FRAMEWORK_OSAL_LINUX_TCSOCKETPOLLINGTASK_H_ */ diff --git a/osal/windows/TmTcWinUdpBridge.cpp b/osal/windows/TmTcWinUdpBridge.cpp index c5902e88..2ea6ae97 100644 --- a/osal/windows/TmTcWinUdpBridge.cpp +++ b/osal/windows/TmTcWinUdpBridge.cpp @@ -31,7 +31,7 @@ TmTcWinUdpBridge::TmTcWinUdpBridge(object_id_t objectId, // Set up UDP socket: https://man7.org/linux/man-pages/man7/ip.7.html //clientSocket = socket(AF_INET, SOCK_DGRAM, 0); serverSocket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP); - if(serverSocket != 0) { + if(serverSocket == INVALID_SOCKET) { sif::error << "TmTcWinUdpBridge::TmTcWinUdpBridge: Could not open" " UDP socket!" << std::endl; handleSocketError(); @@ -62,7 +62,6 @@ TmTcWinUdpBridge::TmTcWinUdpBridge(object_id_t objectId, "local port " << setServerPort << " to server socket!" << std::endl; handleBindError(); - return; } } @@ -125,4 +124,3 @@ void TmTcWinUdpBridge::handleSendError() { } } -#endif