From 111f9dce7dc4babf632ce8353def90729555b504 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sun, 16 Aug 2020 23:06:32 +0200 Subject: [PATCH 01/20] added host osal --- osal/host/Clock.cpp | 227 +++++++++++++++++++++++++++++++ osal/host/FixedTimeslotTask.cpp | 191 ++++++++++++++++++++++++++ osal/host/FixedTimeslotTask.h | 130 ++++++++++++++++++ osal/host/MessageQueue.cpp | 155 +++++++++++++++++++++ osal/host/MessageQueue.h | 230 ++++++++++++++++++++++++++++++++ osal/host/Mutex.cpp | 40 ++++++ osal/host/Mutex.h | 28 ++++ osal/host/MutexFactory.cpp | 28 ++++ osal/host/PeriodicTask.cpp | 176 ++++++++++++++++++++++++ osal/host/PeriodicTask.h | 123 +++++++++++++++++ osal/host/QueueFactory.cpp | 41 ++++++ osal/host/QueueMapManager.cpp | 51 +++++++ osal/host/QueueMapManager.h | 47 +++++++ osal/host/SemaphoreFactory.cpp | 42 ++++++ osal/host/TaskFactory.cpp | 55 ++++++++ 15 files changed, 1564 insertions(+) create mode 100644 osal/host/Clock.cpp create mode 100644 osal/host/FixedTimeslotTask.cpp create mode 100644 osal/host/FixedTimeslotTask.h create mode 100644 osal/host/MessageQueue.cpp create mode 100644 osal/host/MessageQueue.h create mode 100644 osal/host/Mutex.cpp create mode 100644 osal/host/Mutex.h create mode 100644 osal/host/MutexFactory.cpp create mode 100644 osal/host/PeriodicTask.cpp create mode 100644 osal/host/PeriodicTask.h create mode 100644 osal/host/QueueFactory.cpp create mode 100644 osal/host/QueueMapManager.cpp create mode 100644 osal/host/QueueMapManager.h create mode 100644 osal/host/SemaphoreFactory.cpp create mode 100644 osal/host/TaskFactory.cpp diff --git a/osal/host/Clock.cpp b/osal/host/Clock.cpp new file mode 100644 index 00000000..57f2572b --- /dev/null +++ b/osal/host/Clock.cpp @@ -0,0 +1,227 @@ +#include +#include + +#include +#if defined(WIN32) +#include +#elif defined(LINUX) +#include +#endif + +uint16_t Clock::leapSeconds = 0; +MutexIF* Clock::timeMutex = NULL; + +using SystemClock = std::chrono::system_clock; + +uint32_t Clock::getTicksPerSecond(void){ + sif::warning << "Clock::getTicksPerSecond: not implemented yet" << std::endl; + return 0; + //return CLOCKS_PER_SEC; + //uint32_t ticks = sysconf(_SC_CLK_TCK); + //return ticks; +} + +ReturnValue_t Clock::setClock(const TimeOfDay_t* time) { + // do some magic with chrono + sif::warning << "Clock::setClock: not implemented yet" << std::endl; + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::setClock(const timeval* time) { + // do some magic with chrono +#if defined(WIN32) + return HasReturnvaluesIF::RETURN_OK; +#elif defined(LINUX) + return HasReturnvaluesIF::RETURN_OK; +#else + +#endif + sif::warning << "Clock::getUptime: Not implemented for found OS" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; +} + +ReturnValue_t Clock::getClock_timeval(timeval* time) { +#if defined(WIN32) + auto now = std::chrono::system_clock::now(); + auto secondsChrono = std::chrono::time_point_cast(now); + auto epoch = now.time_since_epoch(); + time->tv_sec = std::chrono::duration_cast(epoch).count(); + auto fraction = now - secondsChrono; + time->tv_usec = std::chrono::duration_cast( + fraction).count(); + return HasReturnvaluesIF::RETURN_OK; +#elif defined(LINUX) + timespec timeUnix; + int status = clock_gettime(CLOCK_REALTIME,&timeUnix); + if(status!=0){ + return HasReturnvaluesIF::RETURN_FAILED; + } + time->tv_sec = timeUnix.tv_sec; + time->tv_usec = timeUnix.tv_nsec / 1000.0; + return HasReturnvaluesIF::RETURN_OK; +#else + sif::warning << "Clock::getUptime: Not implemented for found OS" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; +#endif + +} + +ReturnValue_t Clock::getClock_usecs(uint64_t* time) { + // do some magic with chrono + sif::warning << "Clock::gerClock_usecs: not implemented yet" << std::endl; + return HasReturnvaluesIF::RETURN_OK; +} + +timeval Clock::getUptime() { + timeval timeval; +#if defined(WIN32) + auto uptime = std::chrono::milliseconds(GetTickCount64()); + auto secondsChrono = std::chrono::duration_cast(uptime); + timeval.tv_sec = secondsChrono.count(); + auto fraction = uptime - secondsChrono; + timeval.tv_usec = std::chrono::duration_cast( + fraction).count(); +#elif defined(LINUX) + double uptimeSeconds; + if (std::ifstream("/proc/uptime", std::ios::in) >> uptimeSeconds) + { + // value is rounded down automatically + timeval.tv_sec = uptimeSeconds; + timeval.tv_usec = uptimeSeconds *(double) 1e6 - (timeval.tv_sec *1e6); + } +#else + sif::warning << "Clock::getUptime: Not implemented for found OS" << std::endl; +#endif + return timeval; +} + +ReturnValue_t Clock::getUptime(timeval* uptime) { + *uptime = getUptime(); + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::getUptime(uint32_t* uptimeMs) { + timeval uptime = getUptime(); + *uptimeMs = uptime.tv_sec * 1000 + uptime.tv_usec / 1000; + return HasReturnvaluesIF::RETURN_OK; +} + + +ReturnValue_t Clock::getDateAndTime(TimeOfDay_t* time) { + // do some magic with chrono (C++20!) + // Right now, the library doesn't have the new features yet. + // so we work around that for now. + auto now = SystemClock::now(); + auto seconds = std::chrono::time_point_cast(now); + auto fraction = now - seconds; + time_t tt = SystemClock::to_time_t(now); + struct tm* timeInfo; + timeInfo = gmtime(&tt); + time->year = timeInfo->tm_year + 1900; + time->month = timeInfo->tm_mon+1; + time->day = timeInfo->tm_mday; + time->hour = timeInfo->tm_hour; + time->minute = timeInfo->tm_min; + time->second = timeInfo->tm_sec; + auto usecond = std::chrono::duration_cast(fraction); + time->usecond = usecond.count(); + + //sif::warning << "Clock::getDateAndTime: not implemented yet" << std::endl; + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::convertTimeOfDayToTimeval(const TimeOfDay_t* from, + timeval* to) { + struct tm time_tm; + + time_tm.tm_year = from->year - 1900; + time_tm.tm_mon = from->month - 1; + time_tm.tm_mday = from->day; + + time_tm.tm_hour = from->hour; + time_tm.tm_min = from->minute; + time_tm.tm_sec = from->second; + + time_t seconds = mktime(&time_tm); + + to->tv_sec = seconds; + to->tv_usec = from->usecond; + //Fails in 2038.. + return HasReturnvaluesIF::RETURN_OK; + sif::warning << "Clock::convertTimeBla: not implemented yet" << std::endl; + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::convertTimevalToJD2000(timeval time, double* JD2000) { + *JD2000 = (time.tv_sec - 946728000. + time.tv_usec / 1000000.) / 24. + / 3600.; + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval* tt) { + //SHOULDDO: works not for dates in the past (might have less leap seconds) + if (timeMutex == NULL) { + return HasReturnvaluesIF::RETURN_FAILED; + } + + uint16_t leapSeconds; + ReturnValue_t result = getLeapSeconds(&leapSeconds); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + timeval leapSeconds_timeval = { 0, 0 }; + leapSeconds_timeval.tv_sec = leapSeconds; + + //initial offset between UTC and TAI + timeval UTCtoTAI1972 = { 10, 0 }; + + timeval TAItoTT = { 32, 184000 }; + + *tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT; + + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) { + if(checkOrCreateClockMutex()!=HasReturnvaluesIF::RETURN_OK){ + return HasReturnvaluesIF::RETURN_FAILED; + } + ReturnValue_t result = timeMutex->lockMutex(MutexIF::BLOCKING); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + leapSeconds = leapSeconds_; + + result = timeMutex->unlockMutex(); + return result; +} + +ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) { + if(timeMutex == nullptr){ + return HasReturnvaluesIF::RETURN_FAILED; + } + ReturnValue_t result = timeMutex->lockMutex(MutexIF::BLOCKING); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + *leapSeconds_ = leapSeconds; + + result = timeMutex->unlockMutex(); + return result; +} + +ReturnValue_t Clock::checkOrCreateClockMutex(){ + if(timeMutex == nullptr){ + MutexFactory* mutexFactory = MutexFactory::instance(); + if (mutexFactory == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + timeMutex = mutexFactory->createMutex(); + if (timeMutex == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + return HasReturnvaluesIF::RETURN_OK; +} diff --git a/osal/host/FixedTimeslotTask.cpp b/osal/host/FixedTimeslotTask.cpp new file mode 100644 index 00000000..dac399f6 --- /dev/null +++ b/osal/host/FixedTimeslotTask.cpp @@ -0,0 +1,191 @@ +#include + +#include +#include +#include + +#include +#include + +#include +#include + +#if defined(WIN32) +#include +#elif defined(LINUX) +#include +#endif + +FixedTimeslotTask::FixedTimeslotTask(const char *name, TaskPriority setPriority, + TaskStackSize setStack, TaskPeriod setPeriod, + void (*setDeadlineMissedFunc)()) : + started(false), pollingSeqTable(setPeriod*1000), taskName(name), + period(setPeriod), deadlineMissedFunc(setDeadlineMissedFunc) { + // It is propably possible to set task priorities by using the native + // task handles for Windows / Linux + mainThread = std::thread(&FixedTimeslotTask::taskEntryPoint, this, this); +#if defined(WIN32) + /* List of possible priority classes: + * https://docs.microsoft.com/en-us/windows/win32/api/processthreadsapi/ + * nf-processthreadsapi-setpriorityclass + * And respective thread priority numbers: + * https://docs.microsoft.com/en-us/windows/ + * win32/procthread/scheduling-priorities */ + int result = SetPriorityClass( + reinterpret_cast(mainThread.native_handle()), + ABOVE_NORMAL_PRIORITY_CLASS); + if(result != 0) { + sif::error << "FixedTimeslotTask: Windows SetPriorityClass failed with code " + << GetLastError() << std::endl; + } + result = SetThreadPriority( + reinterpret_cast(mainThread.native_handle()), + THREAD_PRIORITY_NORMAL); + if(result != 0) { + sif::error << "FixedTimeslotTask: Windows SetPriorityClass failed with code " + << GetLastError() << std::endl; + } +#elif defined(LINUX) + // we can just copy and paste the code from linux here. +#endif +} + +FixedTimeslotTask::~FixedTimeslotTask(void) { + //Do not delete objects, we were responsible for ptrs only. + terminateThread = true; + if(mainThread.joinable()) { + mainThread.join(); + } + delete this; +} + +void FixedTimeslotTask::taskEntryPoint(void* argument) { + FixedTimeslotTask *originalTask(reinterpret_cast(argument)); + + if (not originalTask->started) { + // we have to suspend/block here until the task is started. + // if semaphores are implemented, use them here. + std::unique_lock lock(initMutex); + initCondition.wait(lock); + } + + this->taskFunctionality(); + sif::debug << "FixedTimeslotTask::taskEntryPoint: " + "Returned from taskFunctionality." << std::endl; +} + +ReturnValue_t FixedTimeslotTask::startTask() { + started = true; + + // Notify task to start. + std::lock_guard lock(initMutex); + initCondition.notify_one(); + + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t FixedTimeslotTask::sleepFor(uint32_t ms) { + std::this_thread::sleep_for(std::chrono::milliseconds(ms)); + return HasReturnvaluesIF::RETURN_OK; +} + +void FixedTimeslotTask::taskFunctionality() { + // A local iterator for the Polling Sequence Table is created to + // find the start time for the first entry. + FixedSlotSequence::SlotListIter slotListIter = pollingSeqTable.current; + // Get start time for first entry. + chron_ms interval(slotListIter->pollingTimeMs); + auto currentStartTime { + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + }; + if(interval.count() > 0) { + delayForInterval(¤tStartTime, interval); + } + + /* Enter the loop that defines the task behavior. */ + for (;;) { + if(terminateThread.load()) { + break; + } + //The component for this slot is executed and the next one is chosen. + this->pollingSeqTable.executeAndAdvance(); + if (not pollingSeqTable.slotFollowsImmediately()) { + // we need to wait before executing the current slot + //this gives us the time to wait: + interval = chron_ms(this->pollingSeqTable.getIntervalToPreviousSlotMs()); + delayForInterval(¤tStartTime, interval); + //TODO deadline missed check + } + } +} + +ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId, + uint32_t slotTimeMs, int8_t executionStep) { + if (objectManager->get(componentId) != nullptr) { + pollingSeqTable.addSlot(componentId, slotTimeMs, executionStep, this); + return HasReturnvaluesIF::RETURN_OK; + } + + sif::error << "Component " << std::hex << componentId << + " not found, not adding it to pst" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; +} + +ReturnValue_t FixedTimeslotTask::checkSequence() const { + return pollingSeqTable.checkSequence(); +} + +uint32_t FixedTimeslotTask::getPeriodMs() const { + return period * 1000; +} + +bool FixedTimeslotTask::delayForInterval(chron_ms * previousWakeTimeMs, + const chron_ms interval) { + bool shouldDelay = false; + //Get current wakeup time + auto currentStartTime = + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()); + /* Generate the tick time at which the task wants to wake. */ + auto nextTimeToWake_ms = (*previousWakeTimeMs) + interval; + + if (currentStartTime < *previousWakeTimeMs) { + /* The tick count has overflowed since this function was + lasted called. In this case the only time we should ever + actually delay is if the wake time has also overflowed, + and the wake time is greater than the tick time. When this + is the case it is as if neither time had overflowed. */ + if ((nextTimeToWake_ms < *previousWakeTimeMs) + && (nextTimeToWake_ms > currentStartTime)) { + shouldDelay = true; + } + } else { + /* The tick time has not overflowed. In this case we will + delay if either the wake time has overflowed, and/or the + tick time is less than the wake time. */ + if ((nextTimeToWake_ms < *previousWakeTimeMs) + || (nextTimeToWake_ms > currentStartTime)) { + shouldDelay = true; + } + } + + /* Update the wake time ready for the next call. */ + + (*previousWakeTimeMs) = nextTimeToWake_ms; + + if (shouldDelay) { + auto sleepTime = std::chrono::duration_cast( + nextTimeToWake_ms - currentStartTime); + std::this_thread::sleep_for(sleepTime); + return true; + } + //We are shifting the time in case the deadline was missed like rtems + (*previousWakeTimeMs) = currentStartTime; + return false; + +} + + + + diff --git a/osal/host/FixedTimeslotTask.h b/osal/host/FixedTimeslotTask.h new file mode 100644 index 00000000..b12cc6d3 --- /dev/null +++ b/osal/host/FixedTimeslotTask.h @@ -0,0 +1,130 @@ +#ifndef FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ +#define FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +class ExecutableObjectIF; + +/** + * @brief This class represents a task for periodic activities with multiple + * steps and strict timeslot requirements for these steps. + * @details + * @ingroup task_handling + */ +class FixedTimeslotTask: public FixedTimeslotTaskIF { +public: + /** + * @brief Standard constructor of the class. + * @details + * The class is initialized without allocated objects. These need to be + * added with #addComponent. + * @param priority + * @param stack_size + * @param setPeriod + * @param setDeadlineMissedFunc + * The function pointer to the deadline missed function that shall be + * assigned. + */ + FixedTimeslotTask(const char *name, TaskPriority setPriority, + TaskStackSize setStack, TaskPeriod setPeriod, + void (*setDeadlineMissedFunc)()); + /** + * @brief Currently, the executed object's lifetime is not coupled with + * the task object's lifetime, so the destructor is empty. + */ + virtual ~FixedTimeslotTask(void); + + /** + * @brief The method to start the task. + * @details The method starts the task with the respective system call. + * Entry point is the taskEntryPoint method described below. + * The address of the task object is passed as an argument + * to the system call. + */ + ReturnValue_t startTask(void); + + /** + * Add timeslot to the polling sequence table. + * @param componentId + * @param slotTimeMs + * @param executionStep + * @return + */ + ReturnValue_t addSlot(object_id_t componentId, + uint32_t slotTimeMs, int8_t executionStep); + + ReturnValue_t checkSequence() const; + + uint32_t getPeriodMs() const; + + ReturnValue_t sleepFor(uint32_t ms); + +protected: + using chron_ms = std::chrono::milliseconds; + + bool started; + //!< Typedef for the List of objects. + typedef std::vector ObjectList; + std::thread mainThread; + std::atomic terminateThread = false; + + //! Polling sequence table which contains the object to execute + //! and information like the timeslots and the passed execution step. + FixedSlotSequence pollingSeqTable; + + std::condition_variable initCondition; + std::mutex initMutex; + std::string taskName; + /** + * @brief The period of the task. + * @details + * The period determines the frequency of the task's execution. + * It is expressed in clock ticks. + */ + TaskPeriod period; + + /** + * @brief The pointer to the deadline-missed function. + * @details + * This pointer stores the function that is executed if the task's deadline + * is missed. So, each may react individually on a timing failure. + * The pointer may be NULL, then nothing happens on missing the deadline. + * The deadline is equal to the next execution of the periodic task. + */ + void (*deadlineMissedFunc)(void); + /** + * @brief This is the function executed in the new task's context. + * @details + * It converts the argument back to the thread object type and copies the + * class instance to the task context. + * The taskFunctionality method is called afterwards. + * @param A pointer to the task object itself is passed as argument. + */ + + void taskEntryPoint(void* argument); + /** + * @brief The function containing the actual functionality of the task. + * @details + * The method sets and starts the task's period, then enters a loop that is + * repeated as long as the isRunning attribute is true. Within the loop, + * all performOperation methods of the added objects are called. Afterwards + * the checkAndRestartPeriod system call blocks the task until the next + * period. On missing the deadline, the deadlineMissedFunction is executed. + */ + void taskFunctionality(void); + + bool delayForInterval(chron_ms * previousWakeTimeMs, + const chron_ms interval); +}; + + + +#endif /* FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ */ diff --git a/osal/host/MessageQueue.cpp b/osal/host/MessageQueue.cpp new file mode 100644 index 00000000..7af5fbd9 --- /dev/null +++ b/osal/host/MessageQueue.cpp @@ -0,0 +1,155 @@ +#include +#include +#include +#include +#include + +MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize): + messageSize(maxMessageSize), messageDepth(messageDepth) { + queueLock = MutexFactory::instance()->createMutex(); + auto result = QueueMapManager::instance()->addMessageQueue(this, &mqId); + if(result != HasReturnvaluesIF::RETURN_OK) { + sif::error << "MessageQueue: Could not be created" << std::endl; + } +} + +MessageQueue::~MessageQueue() { + MutexFactory::instance()->deleteMutex(queueLock); +} + +ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo, + MessageQueueMessageIF* message, bool ignoreFault) { + return sendMessageFrom(sendTo, message, this->getId(), ignoreFault); +} + +ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) { + return sendToDefaultFrom(message, this->getId()); +} + +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 != 0) { + return sendMessageFrom(this->lastPartner, message, this->getId()); + } else { + return MessageQueueIF::NO_REPLY_PARTNER; + } +} + +ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom, + bool ignoreFault) { + return sendMessageFromMessageQueue(sendTo, message, sentFrom, + ignoreFault); +} + +ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message, + MessageQueueId_t* receivedFrom) { + ReturnValue_t status = this->receiveMessage(message); + if(status == HasReturnvaluesIF::RETURN_OK) { + *receivedFrom = this->lastPartner; + } + return status; +} + +ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) { + if(messageQueue.empty()) { + return MessageQueueIF::EMPTY; + } + // not sure this will work.. + //*message = std::move(messageQueue.front()); + MutexHelper mutexLock(queueLock, 20); + MessageQueueMessage* currentMessage = &messageQueue.front(); + std::copy(currentMessage->getBuffer(), + currentMessage->getBuffer() + messageSize, message->getBuffer()); + messageQueue.pop(); + // The last partner is the first uint32_t field in the message + this->lastPartner = message->getSender(); + return HasReturnvaluesIF::RETURN_OK; +} + +MessageQueueId_t MessageQueue::getLastPartner() const { + return lastPartner; +} + +ReturnValue_t MessageQueue::flush(uint32_t* count) { + *count = messageQueue.size(); + // Clears the queue. + messageQueue = std::queue(); + return HasReturnvaluesIF::RETURN_OK; +} + +MessageQueueId_t MessageQueue::getId() const { + return mqId; +} + +void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) { + defaultDestinationSet = true; + this->defaultDestination = defaultDestination; +} + +MessageQueueId_t MessageQueue::getDefaultDestination() const { + return defaultDestination; +} + +bool MessageQueue::isDefaultDestinationSet() const { + return defaultDestinationSet; +} + + +// static core function to send messages. +ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom, + bool ignoreFault) { + if(message->getMessageSize() > message->getMaximumMessageSize()) { + // Actually, this should never happen or an error will be emitted + // in MessageQueueMessage. + // But I will still return a failure here. + return HasReturnvaluesIF::RETURN_FAILED; + } + MessageQueue* targetQueue = dynamic_cast( + QueueMapManager::instance()->getMessageQueue(sendTo)); + if(targetQueue == nullptr) { + if(not ignoreFault) { + InternalErrorReporterIF* internalErrorReporter = + objectManager->get( + objects::INTERNAL_ERROR_REPORTER); + if (internalErrorReporter != nullptr) { + internalErrorReporter->queueMessageNotSent(); + } + } + // TODO: Better returnvalue + return HasReturnvaluesIF::RETURN_FAILED; + } + + if(targetQueue->messageQueue.size() < targetQueue->messageDepth) { + MutexHelper mutexLock(targetQueue->queueLock, 20); + // not ideal, works for now though. + MessageQueueMessage* mqmMessage = + dynamic_cast(message); + if(message != nullptr) { + targetQueue->messageQueue.push(*mqmMessage); + } + else { + sif::error << "MessageQueue::sendMessageFromMessageQueue: Message" + "is not MessageQueueMessage!" << std::endl; + } + + } + else { + return MessageQueueIF::FULL; + } + message->setSender(sentFrom); + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t MessageQueue::lockQueue(dur_millis_t lockTimeout) { + return queueLock->lockMutex(lockTimeout); +} + +ReturnValue_t MessageQueue::unlockQueue() { + return queueLock->unlockMutex(); +} diff --git a/osal/host/MessageQueue.h b/osal/host/MessageQueue.h new file mode 100644 index 00000000..7fc77f7a --- /dev/null +++ b/osal/host/MessageQueue.h @@ -0,0 +1,230 @@ +#ifndef FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ +#define FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ + +#include +#include +#include +#include +#include + +#include +#include + +/** + * @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. + * + * 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 + * 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 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. + */ + 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. + */ + ReturnValue_t sendMessage(MessageQueueId_t sendTo, + MessageQueueMessageIF* message, bool ignoreFault = false) override; + /** + * @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(MessageQueueMessageIF* message) override; + /** + * @brief This operation sends a message to the last communication partner. + * @details This operation simplifies answering an incoming message by using + * the stored lastPartner 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(MessageQueueMessageIF* message) override; + + /** + * @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, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom = NO_QUEUE, + bool ignoreFault = false) override; + + /** + * @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( MessageQueueMessageIF* message, + MessageQueueId_t sentFrom = NO_QUEUE, + 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(MessageQueueMessageIF* message, + MessageQueueId_t *receivedFrom) 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(MessageQueueMessageIF* message) override; + /** + * 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) override; + /** + * @brief This method returns the message queue id of the last + * communication partner. + */ + MessageQueueId_t getLastPartner() const override; + /** + * @brief This method returns the message queue id of this class's + * message queue. + */ + MessageQueueId_t getId() const override; + + /** + * @brief This method is a simple setter for the default destination. + */ + void setDefaultDestination(MessageQueueId_t defaultDestination) override; + /** + * @brief This method is a simple getter for the default destination. + */ + MessageQueueId_t getDefaultDestination() const override; + + bool isDefaultDestinationSet() const override; + + ReturnValue_t lockQueue(dur_millis_t lockTimeout); + ReturnValue_t unlockQueue(); +protected: + /** + * @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, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom = NO_QUEUE, + bool ignoreFault=false); + + //static ReturnValue_t handleSendResult(BaseType_t result, bool ignoreFault); + +private: + std::queue messageQueue; + /** + * @brief The class stores the queue id it got assigned. + * If initialization fails, the queue id is set to zero. + */ + MessageQueueId_t mqId = 0; + size_t messageSize = 0; + size_t messageDepth = 0; + + MutexIF* queueLock; + + bool defaultDestinationSet = false; + MessageQueueId_t defaultDestination = 0; + MessageQueueId_t lastPartner = 0; +}; + +#endif /* FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ */ diff --git a/osal/host/Mutex.cpp b/osal/host/Mutex.cpp new file mode 100644 index 00000000..03948c4c --- /dev/null +++ b/osal/host/Mutex.cpp @@ -0,0 +1,40 @@ +#include +#include + +const uint32_t MutexIF::POLLING = 0; +const uint32_t MutexIF::BLOCKING = 0xffffffff; + +ReturnValue_t Mutex::lockMutex(uint32_t timeoutMs) { + if(timeoutMs == MutexIF::BLOCKING) { + mutex.lock(); + locked = true; + return HasReturnvaluesIF::RETURN_OK; + } + else if(timeoutMs == MutexIF::POLLING) { + if(mutex.try_lock()) { + locked = true; + return HasReturnvaluesIF::RETURN_OK; + } + } + else if(timeoutMs > MutexIF::POLLING){ + auto chronoMs = std::chrono::milliseconds(timeoutMs); + if(mutex.try_lock_for(chronoMs)) { + locked = true; + return HasReturnvaluesIF::RETURN_OK; + } + } + return MutexIF::MUTEX_TIMEOUT; +} + +ReturnValue_t Mutex::unlockMutex() { + if(not locked) { + return MutexIF::CURR_THREAD_DOES_NOT_OWN_MUTEX; + } + mutex.unlock(); + locked = false; + return HasReturnvaluesIF::RETURN_OK; +} + +std::timed_mutex* Mutex::getMutexHandle() { + return &mutex; +} diff --git a/osal/host/Mutex.h b/osal/host/Mutex.h new file mode 100644 index 00000000..d882c457 --- /dev/null +++ b/osal/host/Mutex.h @@ -0,0 +1,28 @@ +#ifndef FRAMEWORK_OSAL_FREERTOS_MUTEX_H_ +#define FRAMEWORK_OSAL_FREERTOS_MUTEX_H_ + +#include + +#include + +/** + * @brief OS component to implement MUTual EXclusion + * + * @details + * Mutexes are binary semaphores which include a priority inheritance mechanism. + * Documentation: https://www.freertos.org/Real-time-embedded-RTOS-mutexes.html + * @ingroup osal + */ +class Mutex : public MutexIF { +public: + Mutex() = default; + ReturnValue_t lockMutex(uint32_t timeoutMs = MutexIF::BLOCKING) override; + ReturnValue_t unlockMutex() override; + + std::timed_mutex* getMutexHandle(); +private: + bool locked = false; + std::timed_mutex mutex; +}; + +#endif /* FRAMEWORK_FREERTOS_MUTEX_H_ */ diff --git a/osal/host/MutexFactory.cpp b/osal/host/MutexFactory.cpp new file mode 100644 index 00000000..25d9fa46 --- /dev/null +++ b/osal/host/MutexFactory.cpp @@ -0,0 +1,28 @@ +#include +#include + +//TODO: Different variant than the lazy loading in QueueFactory. +//What's better and why? -> one is on heap the other on bss/data +//MutexFactory* MutexFactory::factoryInstance = new MutexFactory(); +MutexFactory* MutexFactory::factoryInstance = nullptr; + +MutexFactory::MutexFactory() { +} + +MutexFactory::~MutexFactory() { +} + +MutexFactory* MutexFactory::instance() { + if (factoryInstance == nullptr){ + factoryInstance = new MutexFactory(); + } + return MutexFactory::factoryInstance; +} + +MutexIF* MutexFactory::createMutex() { + return new Mutex(); +} + +void MutexFactory::deleteMutex(MutexIF* mutex) { + delete mutex; +} diff --git a/osal/host/PeriodicTask.cpp b/osal/host/PeriodicTask.cpp new file mode 100644 index 00000000..1a5024ab --- /dev/null +++ b/osal/host/PeriodicTask.cpp @@ -0,0 +1,176 @@ +#include +#include +#include + +#include +#include + +#include +#include + +#if defined(WIN32) +#include +#elif defined(LINUX) +#include +#endif + +PeriodicTask::PeriodicTask(const char *name, TaskPriority setPriority, + TaskStackSize setStack, TaskPeriod setPeriod, + void (*setDeadlineMissedFunc)()) : + started(false), taskName(name), period(setPeriod), + deadlineMissedFunc(setDeadlineMissedFunc) { + // It is propably possible to set task priorities by using the native + // task handles for Windows / Linux + mainThread = std::thread(&PeriodicTask::taskEntryPoint, this, this); +#if defined(WIN32) + /* List of possible priority classes: + * https://docs.microsoft.com/en-us/windows/win32/api/processthreadsapi/ + * nf-processthreadsapi-setpriorityclass + * And respective thread priority numbers: + * https://docs.microsoft.com/en-us/windows/ + * win32/procthread/scheduling-priorities */ + int result = SetPriorityClass( + reinterpret_cast(mainThread.native_handle()), + ABOVE_NORMAL_PRIORITY_CLASS); + if(result != 0) { + sif::error << "PeriodicTask: Windows SetPriorityClass failed with code " + << GetLastError() << std::endl; + } + result = SetThreadPriority( + reinterpret_cast(mainThread.native_handle()), + THREAD_PRIORITY_NORMAL); + if(result != 0) { + sif::error << "PeriodicTask: Windows SetPriorityClass failed with code " + << GetLastError() << std::endl; + } +#elif defined(LINUX) + // we can just copy and paste the code from linux here. +#endif +} + +PeriodicTask::~PeriodicTask(void) { + //Do not delete objects, we were responsible for ptrs only. + terminateThread = true; + if(mainThread.joinable()) { + mainThread.join(); + } + delete this; +} + +void PeriodicTask::taskEntryPoint(void* argument) { + PeriodicTask *originalTask(reinterpret_cast(argument)); + + + if (not originalTask->started) { + // we have to suspend/block here until the task is started. + // if semaphores are implemented, use them here. + std::unique_lock lock(initMutex); + initCondition.wait(lock); + } + + this->taskFunctionality(); + sif::debug << "PeriodicTask::taskEntryPoint: " + "Returned from taskFunctionality." << std::endl; +} + +ReturnValue_t PeriodicTask::startTask() { + started = true; + + // Notify task to start. + std::lock_guard lock(initMutex); + initCondition.notify_one(); + + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t PeriodicTask::sleepFor(uint32_t ms) { + std::this_thread::sleep_for(std::chrono::milliseconds(ms)); + return HasReturnvaluesIF::RETURN_OK; +} + +void PeriodicTask::taskFunctionality() { + std::chrono::milliseconds periodChrono(static_cast(period*1000)); + auto currentStartTime { + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + }; + auto nextStartTime{ currentStartTime }; + + /* Enter the loop that defines the task behavior. */ + for (;;) { + if(terminateThread.load()) { + break; + } + for (ObjectList::iterator it = objectList.begin(); + it != objectList.end(); ++it) { + (*it)->performOperation(); + } + if(not delayForInterval(¤tStartTime, periodChrono)) { + sif::warning << "PeriodicTask: " << taskName << + " missed deadline!\n" << std::flush; + if(deadlineMissedFunc != nullptr) { + this->deadlineMissedFunc(); + } + } + } +} + +ReturnValue_t PeriodicTask::addComponent(object_id_t object) { + ExecutableObjectIF* newObject = objectManager->get( + object); + if (newObject == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + objectList.push_back(newObject); + return HasReturnvaluesIF::RETURN_OK; +} + +uint32_t PeriodicTask::getPeriodMs() const { + return period * 1000; +} + +bool PeriodicTask::delayForInterval(chron_ms* previousWakeTimeMs, + const chron_ms interval) { + bool shouldDelay = false; + //Get current wakeup time + auto currentStartTime = + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()); + /* Generate the tick time at which the task wants to wake. */ + auto nextTimeToWake_ms = (*previousWakeTimeMs) + interval; + + if (currentStartTime < *previousWakeTimeMs) { + /* The tick count has overflowed since this function was + lasted called. In this case the only time we should ever + actually delay is if the wake time has also overflowed, + and the wake time is greater than the tick time. When this + is the case it is as if neither time had overflowed. */ + if ((nextTimeToWake_ms < *previousWakeTimeMs) + && (nextTimeToWake_ms > currentStartTime)) { + shouldDelay = true; + } + } else { + /* The tick time has not overflowed. In this case we will + delay if either the wake time has overflowed, and/or the + tick time is less than the wake time. */ + if ((nextTimeToWake_ms < *previousWakeTimeMs) + || (nextTimeToWake_ms > currentStartTime)) { + shouldDelay = true; + } + } + + /* Update the wake time ready for the next call. */ + + (*previousWakeTimeMs) = nextTimeToWake_ms; + + if (shouldDelay) { + auto sleepTime = std::chrono::duration_cast( + nextTimeToWake_ms - currentStartTime); + std::this_thread::sleep_for(sleepTime); + return true; + } + //We are shifting the time in case the deadline was missed like rtems + (*previousWakeTimeMs) = currentStartTime; + return false; + +} diff --git a/osal/host/PeriodicTask.h b/osal/host/PeriodicTask.h new file mode 100644 index 00000000..d97bf089 --- /dev/null +++ b/osal/host/PeriodicTask.h @@ -0,0 +1,123 @@ +#ifndef FRAMEWORK_OSAL_HOST_PERIODICTASK_H_ +#define FRAMEWORK_OSAL_HOST_PERIODICTASK_H_ + +#include +#include +#include + +#include +#include +#include +#include + +class ExecutableObjectIF; + +/** + * @brief This class represents a specialized task for + * periodic activities of multiple objects. + * @details + * + * @ingroup task_handling + */ +class PeriodicTask: public PeriodicTaskIF { +public: + /** + * @brief Standard constructor of the class. + * @details + * The class is initialized without allocated objects. These need to be + * added with #addComponent. + * @param priority + * @param stack_size + * @param setPeriod + * @param setDeadlineMissedFunc + * The function pointer to the deadline missed function that shall be + * assigned. + */ + PeriodicTask(const char *name, TaskPriority setPriority, TaskStackSize setStack, + TaskPeriod setPeriod,void (*setDeadlineMissedFunc)()); + /** + * @brief Currently, the executed object's lifetime is not coupled with + * the task object's lifetime, so the destructor is empty. + */ + virtual ~PeriodicTask(void); + + /** + * @brief The method to start the task. + * @details The method starts the task with the respective system call. + * Entry point is the taskEntryPoint method described below. + * The address of the task object is passed as an argument + * to the system call. + */ + ReturnValue_t startTask(void); + /** + * Adds an object to the list of objects to be executed. + * The objects are executed in the order added. + * @param object Id of the object to add. + * @return + * -@c RETURN_OK on success + * -@c RETURN_FAILED if the object could not be added. + */ + ReturnValue_t addComponent(object_id_t object); + + uint32_t getPeriodMs() const; + + ReturnValue_t sleepFor(uint32_t ms); + +protected: + using chron_ms = std::chrono::milliseconds; + bool started; + //!< Typedef for the List of objects. + typedef std::vector ObjectList; + std::thread mainThread; + std::atomic terminateThread = false; + + /** + * @brief This attribute holds a list of objects to be executed. + */ + ObjectList objectList; + + std::condition_variable initCondition; + std::mutex initMutex; + std::string taskName; + /** + * @brief The period of the task. + * @details + * The period determines the frequency of the task's execution. + * It is expressed in clock ticks. + */ + TaskPeriod period; + /** + * @brief The pointer to the deadline-missed function. + * @details + * This pointer stores the function that is executed if the task's deadline + * is missed. So, each may react individually on a timing failure. + * The pointer may be NULL, then nothing happens on missing the deadline. + * The deadline is equal to the next execution of the periodic task. + */ + void (*deadlineMissedFunc)(void); + /** + * @brief This is the function executed in the new task's context. + * @details + * It converts the argument back to the thread object type and copies the + * class instance to the task context. + * The taskFunctionality method is called afterwards. + * @param A pointer to the task object itself is passed as argument. + */ + + void taskEntryPoint(void* argument); + /** + * @brief The function containing the actual functionality of the task. + * @details + * The method sets and starts the task's period, then enters a loop that is + * repeated as long as the isRunning attribute is true. Within the loop, + * all performOperation methods of the added objects are called. Afterwards + * the checkAndRestartPeriod system call blocks the task until the next + * period. On missing the deadline, the deadlineMissedFunction is executed. + */ + void taskFunctionality(void); + + bool delayForInterval(chron_ms * previousWakeTimeMs, + const chron_ms interval); +}; + +#endif /* PERIODICTASK_H_ */ diff --git a/osal/host/QueueFactory.cpp b/osal/host/QueueFactory.cpp new file mode 100644 index 00000000..225bb8c7 --- /dev/null +++ b/osal/host/QueueFactory.cpp @@ -0,0 +1,41 @@ +#include +#include +#include +#include + +QueueFactory* QueueFactory::factoryInstance = nullptr; + + +ReturnValue_t MessageQueueSenderIF::sendMessage(MessageQueueId_t sendTo, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom, + bool ignoreFault) { + return MessageQueue::sendMessageFromMessageQueue(sendTo,message, + sentFrom,ignoreFault); + return HasReturnvaluesIF::RETURN_OK; +} + +QueueFactory* QueueFactory::instance() { + if (factoryInstance == nullptr) { + factoryInstance = new QueueFactory; + } + return factoryInstance; +} + +QueueFactory::QueueFactory() { +} + +QueueFactory::~QueueFactory() { +} + +MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, + size_t maxMessageSize) { + // A thread-safe queue can be implemented by using a combination + // of std::queue and std::mutex. This uses dynamic memory allocation + // which could be alleviated by using a custom allocator, external library + // (etl::queue) or simply using std::queue, we're on a host machine anyway. + return new MessageQueue(messageDepth, maxMessageSize); +} + +void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) { + delete queue; +} diff --git a/osal/host/QueueMapManager.cpp b/osal/host/QueueMapManager.cpp new file mode 100644 index 00000000..89b9a948 --- /dev/null +++ b/osal/host/QueueMapManager.cpp @@ -0,0 +1,51 @@ +#include +#include +#include + +QueueMapManager* QueueMapManager::mqManagerInstance = nullptr; + +QueueMapManager::QueueMapManager() { + mapLock = MutexFactory::instance()->createMutex(); +} + +QueueMapManager* QueueMapManager::instance() { + if (mqManagerInstance == nullptr){ + mqManagerInstance = new QueueMapManager(); + } + return QueueMapManager::mqManagerInstance; +} + +ReturnValue_t QueueMapManager::addMessageQueue( + MessageQueueIF* queueToInsert, MessageQueueId_t* id) { + // Not thread-safe, but it is assumed all message queues are created + // at software initialization now. If this is to be made thread-safe in + // the future, it propably would be sufficient to lock the increment + // operation here + uint32_t currentId = queueCounter++; + auto returnPair = queueMap.emplace(currentId, queueToInsert); + if(not returnPair.second) { + // this should never happen for the atomic variable. + sif::error << "QueueMapManager: This ID is already inside the map!" + << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + if (id != nullptr) { + *id = currentId; + } + return HasReturnvaluesIF::RETURN_OK; +} + +MessageQueueIF* QueueMapManager::getMessageQueue( + MessageQueueId_t messageQueueId) const { + MutexHelper(mapLock, 50); + auto queueIter = queueMap.find(messageQueueId); + if(queueIter != queueMap.end()) { + return queueIter->second; + } + else { + sif::warning << "QueueMapManager::getQueueHandle: The ID" << + messageQueueId << " does not exists in the map" << std::endl; + return nullptr; + } +} + diff --git a/osal/host/QueueMapManager.h b/osal/host/QueueMapManager.h new file mode 100644 index 00000000..a2a1b658 --- /dev/null +++ b/osal/host/QueueMapManager.h @@ -0,0 +1,47 @@ +#ifndef FRAMEWORK_OSAL_HOST_QUEUEMAP_H_ +#define FRAMEWORK_OSAL_HOST_QUEUEMAP_H_ + +#include +#include +#include +#include + +using QueueMap = std::unordered_map; + + +/** + * An internal map to map message queue IDs to message queues. + * This propably should be a singleton.. + */ +class QueueMapManager { +public: + //! Returns the single instance of SemaphoreFactory. + static QueueMapManager* instance(); + + /** + * Insert a message queue into the map and returns a message queue ID + * @param queue The message queue to insert. + * @param id The passed value will be set unless a nullptr is passed + * @return + */ + ReturnValue_t addMessageQueue(MessageQueueIF* queue, MessageQueueId_t* + id = nullptr); + /** + * Get the message queue handle by providing a message queue ID. + * @param messageQueueId + * @return + */ + MessageQueueIF* getMessageQueue(MessageQueueId_t messageQueueId) const; + +private: + //! External instantiation is forbidden. + QueueMapManager(); + uint32_t queueCounter = 1; + MutexIF* mapLock; + QueueMap queueMap; + static QueueMapManager* mqManagerInstance; +}; + + + +#endif /* FRAMEWORK_OSAL_HOST_QUEUEMAP_H_ */ diff --git a/osal/host/SemaphoreFactory.cpp b/osal/host/SemaphoreFactory.cpp new file mode 100644 index 00000000..0c077f68 --- /dev/null +++ b/osal/host/SemaphoreFactory.cpp @@ -0,0 +1,42 @@ +#include +#include +#include +#include + +const uint32_t SemaphoreIF::POLLING = 0; +const uint32_t SemaphoreIF::BLOCKING = 0xFFFFFFFF; + +SemaphoreFactory* SemaphoreFactory::factoryInstance = nullptr; + +SemaphoreFactory::SemaphoreFactory() { +} + +SemaphoreFactory::~SemaphoreFactory() { + delete factoryInstance; +} + +SemaphoreFactory* SemaphoreFactory::instance() { + if (factoryInstance == nullptr){ + factoryInstance = new SemaphoreFactory(); + } + return SemaphoreFactory::factoryInstance; +} + +SemaphoreIF* SemaphoreFactory::createBinarySemaphore(uint32_t arguments) { + // Just gonna wait for full C++20 for now. + sif::error << "SemaphoreFactory: Binary Semaphore not implemented yet." + " Returning nullptr!\n" << std::flush; + return nullptr; +} + +SemaphoreIF* SemaphoreFactory::createCountingSemaphore(const uint8_t maxCount, + uint8_t initCount, uint32_t arguments) { + // Just gonna wait for full C++20 for now. + sif::error << "SemaphoreFactory: Counting Semaphore not implemented yet." + " Returning nullptr!\n" << std::flush; + return nullptr; +} + +void SemaphoreFactory::deleteSemaphore(SemaphoreIF* semaphore) { + delete semaphore; +} diff --git a/osal/host/TaskFactory.cpp b/osal/host/TaskFactory.cpp new file mode 100644 index 00000000..9db8ac4d --- /dev/null +++ b/osal/host/TaskFactory.cpp @@ -0,0 +1,55 @@ +#include +#include +#include +#include +#include + +#include + +TaskFactory* TaskFactory::factoryInstance = new TaskFactory(); + +// Will propably not be used for hosted implementation +const size_t PeriodicTaskIF::MINIMUM_STACK_SIZE = 0; + +TaskFactory::TaskFactory() { +} + +TaskFactory::~TaskFactory() { +} + +TaskFactory* TaskFactory::instance() { + return TaskFactory::factoryInstance; +} + +PeriodicTaskIF* TaskFactory::createPeriodicTask(TaskName name_, + TaskPriority taskPriority_,TaskStackSize stackSize_, + TaskPeriod periodInSeconds_, + TaskDeadlineMissedFunction deadLineMissedFunction_) { + // This is going to be interesting. Time now learn the C++ threading library + // :-) + return new PeriodicTask(name_, taskPriority_, stackSize_, periodInSeconds_, + deadLineMissedFunction_); +} + +FixedTimeslotTaskIF* TaskFactory::createFixedTimeslotTask(TaskName name_, + TaskPriority taskPriority_,TaskStackSize stackSize_, + TaskPeriod periodInSeconds_, + TaskDeadlineMissedFunction deadLineMissedFunction_) { + // This is going to be interesting. Time now learn the C++ threading library + // :-) + return new FixedTimeslotTask(name_, taskPriority_, stackSize_, + periodInSeconds_, deadLineMissedFunction_); +} + +ReturnValue_t TaskFactory::deleteTask(PeriodicTaskIF* task) { + // This might block for some time! + delete task; + return HasReturnvaluesIF::RETURN_FAILED; +} + +ReturnValue_t TaskFactory::delayTask(uint32_t delayMs){ + std::this_thread::sleep_for(std::chrono::milliseconds(delayMs)); + return HasReturnvaluesIF::RETURN_OK; +} + + From f13e7b4255005a1857abe9734cd8e1df325a4b0a Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Tue, 25 Aug 2020 18:30:11 +0200 Subject: [PATCH 02/20] include replacements --- osal/host/Clock.cpp | 454 +++++++++++++++---------------- osal/host/FixedTimeslotTask.cpp | 382 +++++++++++++------------- osal/host/FixedTimeslotTask.h | 260 +++++++++--------- osal/host/MessageQueue.cpp | 310 ++++++++++----------- osal/host/MessageQueue.h | 460 ++++++++++++++++---------------- osal/host/Mutex.cpp | 80 +++--- osal/host/Mutex.h | 56 ++-- osal/host/MutexFactory.cpp | 56 ++-- osal/host/PeriodicTask.cpp | 352 ++++++++++++------------ osal/host/PeriodicTask.h | 246 ++++++++--------- osal/host/QueueFactory.cpp | 82 +++--- osal/host/QueueMapManager.cpp | 102 +++---- osal/host/QueueMapManager.h | 94 +++---- osal/host/SemaphoreFactory.cpp | 84 +++--- osal/host/TaskFactory.cpp | 110 ++++---- 15 files changed, 1564 insertions(+), 1564 deletions(-) diff --git a/osal/host/Clock.cpp b/osal/host/Clock.cpp index 57f2572b..52d0d5f0 100644 --- a/osal/host/Clock.cpp +++ b/osal/host/Clock.cpp @@ -1,227 +1,227 @@ -#include -#include - -#include -#if defined(WIN32) -#include -#elif defined(LINUX) -#include -#endif - -uint16_t Clock::leapSeconds = 0; -MutexIF* Clock::timeMutex = NULL; - -using SystemClock = std::chrono::system_clock; - -uint32_t Clock::getTicksPerSecond(void){ - sif::warning << "Clock::getTicksPerSecond: not implemented yet" << std::endl; - return 0; - //return CLOCKS_PER_SEC; - //uint32_t ticks = sysconf(_SC_CLK_TCK); - //return ticks; -} - -ReturnValue_t Clock::setClock(const TimeOfDay_t* time) { - // do some magic with chrono - sif::warning << "Clock::setClock: not implemented yet" << std::endl; - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t Clock::setClock(const timeval* time) { - // do some magic with chrono -#if defined(WIN32) - return HasReturnvaluesIF::RETURN_OK; -#elif defined(LINUX) - return HasReturnvaluesIF::RETURN_OK; -#else - -#endif - sif::warning << "Clock::getUptime: Not implemented for found OS" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; -} - -ReturnValue_t Clock::getClock_timeval(timeval* time) { -#if defined(WIN32) - auto now = std::chrono::system_clock::now(); - auto secondsChrono = std::chrono::time_point_cast(now); - auto epoch = now.time_since_epoch(); - time->tv_sec = std::chrono::duration_cast(epoch).count(); - auto fraction = now - secondsChrono; - time->tv_usec = std::chrono::duration_cast( - fraction).count(); - return HasReturnvaluesIF::RETURN_OK; -#elif defined(LINUX) - timespec timeUnix; - int status = clock_gettime(CLOCK_REALTIME,&timeUnix); - if(status!=0){ - return HasReturnvaluesIF::RETURN_FAILED; - } - time->tv_sec = timeUnix.tv_sec; - time->tv_usec = timeUnix.tv_nsec / 1000.0; - return HasReturnvaluesIF::RETURN_OK; -#else - sif::warning << "Clock::getUptime: Not implemented for found OS" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; -#endif - -} - -ReturnValue_t Clock::getClock_usecs(uint64_t* time) { - // do some magic with chrono - sif::warning << "Clock::gerClock_usecs: not implemented yet" << std::endl; - return HasReturnvaluesIF::RETURN_OK; -} - -timeval Clock::getUptime() { - timeval timeval; -#if defined(WIN32) - auto uptime = std::chrono::milliseconds(GetTickCount64()); - auto secondsChrono = std::chrono::duration_cast(uptime); - timeval.tv_sec = secondsChrono.count(); - auto fraction = uptime - secondsChrono; - timeval.tv_usec = std::chrono::duration_cast( - fraction).count(); -#elif defined(LINUX) - double uptimeSeconds; - if (std::ifstream("/proc/uptime", std::ios::in) >> uptimeSeconds) - { - // value is rounded down automatically - timeval.tv_sec = uptimeSeconds; - timeval.tv_usec = uptimeSeconds *(double) 1e6 - (timeval.tv_sec *1e6); - } -#else - sif::warning << "Clock::getUptime: Not implemented for found OS" << std::endl; -#endif - return timeval; -} - -ReturnValue_t Clock::getUptime(timeval* uptime) { - *uptime = getUptime(); - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t Clock::getUptime(uint32_t* uptimeMs) { - timeval uptime = getUptime(); - *uptimeMs = uptime.tv_sec * 1000 + uptime.tv_usec / 1000; - return HasReturnvaluesIF::RETURN_OK; -} - - -ReturnValue_t Clock::getDateAndTime(TimeOfDay_t* time) { - // do some magic with chrono (C++20!) - // Right now, the library doesn't have the new features yet. - // so we work around that for now. - auto now = SystemClock::now(); - auto seconds = std::chrono::time_point_cast(now); - auto fraction = now - seconds; - time_t tt = SystemClock::to_time_t(now); - struct tm* timeInfo; - timeInfo = gmtime(&tt); - time->year = timeInfo->tm_year + 1900; - time->month = timeInfo->tm_mon+1; - time->day = timeInfo->tm_mday; - time->hour = timeInfo->tm_hour; - time->minute = timeInfo->tm_min; - time->second = timeInfo->tm_sec; - auto usecond = std::chrono::duration_cast(fraction); - time->usecond = usecond.count(); - - //sif::warning << "Clock::getDateAndTime: not implemented yet" << std::endl; - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t Clock::convertTimeOfDayToTimeval(const TimeOfDay_t* from, - timeval* to) { - struct tm time_tm; - - time_tm.tm_year = from->year - 1900; - time_tm.tm_mon = from->month - 1; - time_tm.tm_mday = from->day; - - time_tm.tm_hour = from->hour; - time_tm.tm_min = from->minute; - time_tm.tm_sec = from->second; - - time_t seconds = mktime(&time_tm); - - to->tv_sec = seconds; - to->tv_usec = from->usecond; - //Fails in 2038.. - return HasReturnvaluesIF::RETURN_OK; - sif::warning << "Clock::convertTimeBla: not implemented yet" << std::endl; - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t Clock::convertTimevalToJD2000(timeval time, double* JD2000) { - *JD2000 = (time.tv_sec - 946728000. + time.tv_usec / 1000000.) / 24. - / 3600.; - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval* tt) { - //SHOULDDO: works not for dates in the past (might have less leap seconds) - if (timeMutex == NULL) { - return HasReturnvaluesIF::RETURN_FAILED; - } - - uint16_t leapSeconds; - ReturnValue_t result = getLeapSeconds(&leapSeconds); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - timeval leapSeconds_timeval = { 0, 0 }; - leapSeconds_timeval.tv_sec = leapSeconds; - - //initial offset between UTC and TAI - timeval UTCtoTAI1972 = { 10, 0 }; - - timeval TAItoTT = { 32, 184000 }; - - *tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT; - - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) { - if(checkOrCreateClockMutex()!=HasReturnvaluesIF::RETURN_OK){ - return HasReturnvaluesIF::RETURN_FAILED; - } - ReturnValue_t result = timeMutex->lockMutex(MutexIF::BLOCKING); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - - leapSeconds = leapSeconds_; - - result = timeMutex->unlockMutex(); - return result; -} - -ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) { - if(timeMutex == nullptr){ - return HasReturnvaluesIF::RETURN_FAILED; - } - ReturnValue_t result = timeMutex->lockMutex(MutexIF::BLOCKING); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - - *leapSeconds_ = leapSeconds; - - result = timeMutex->unlockMutex(); - return result; -} - -ReturnValue_t Clock::checkOrCreateClockMutex(){ - if(timeMutex == nullptr){ - MutexFactory* mutexFactory = MutexFactory::instance(); - if (mutexFactory == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - timeMutex = mutexFactory->createMutex(); - if (timeMutex == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - } - return HasReturnvaluesIF::RETURN_OK; -} +#include "../../serviceinterface/ServiceInterfaceStream.h" +#include "../../timemanager/Clock.h" + +#include +#if defined(WIN32) +#include +#elif defined(LINUX) +#include +#endif + +uint16_t Clock::leapSeconds = 0; +MutexIF* Clock::timeMutex = NULL; + +using SystemClock = std::chrono::system_clock; + +uint32_t Clock::getTicksPerSecond(void){ + sif::warning << "Clock::getTicksPerSecond: not implemented yet" << std::endl; + return 0; + //return CLOCKS_PER_SEC; + //uint32_t ticks = sysconf(_SC_CLK_TCK); + //return ticks; +} + +ReturnValue_t Clock::setClock(const TimeOfDay_t* time) { + // do some magic with chrono + sif::warning << "Clock::setClock: not implemented yet" << std::endl; + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::setClock(const timeval* time) { + // do some magic with chrono +#if defined(WIN32) + return HasReturnvaluesIF::RETURN_OK; +#elif defined(LINUX) + return HasReturnvaluesIF::RETURN_OK; +#else + +#endif + sif::warning << "Clock::getUptime: Not implemented for found OS" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; +} + +ReturnValue_t Clock::getClock_timeval(timeval* time) { +#if defined(WIN32) + auto now = std::chrono::system_clock::now(); + auto secondsChrono = std::chrono::time_point_cast(now); + auto epoch = now.time_since_epoch(); + time->tv_sec = std::chrono::duration_cast(epoch).count(); + auto fraction = now - secondsChrono; + time->tv_usec = std::chrono::duration_cast( + fraction).count(); + return HasReturnvaluesIF::RETURN_OK; +#elif defined(LINUX) + timespec timeUnix; + int status = clock_gettime(CLOCK_REALTIME,&timeUnix); + if(status!=0){ + return HasReturnvaluesIF::RETURN_FAILED; + } + time->tv_sec = timeUnix.tv_sec; + time->tv_usec = timeUnix.tv_nsec / 1000.0; + return HasReturnvaluesIF::RETURN_OK; +#else + sif::warning << "Clock::getUptime: Not implemented for found OS" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; +#endif + +} + +ReturnValue_t Clock::getClock_usecs(uint64_t* time) { + // do some magic with chrono + sif::warning << "Clock::gerClock_usecs: not implemented yet" << std::endl; + return HasReturnvaluesIF::RETURN_OK; +} + +timeval Clock::getUptime() { + timeval timeval; +#if defined(WIN32) + auto uptime = std::chrono::milliseconds(GetTickCount64()); + auto secondsChrono = std::chrono::duration_cast(uptime); + timeval.tv_sec = secondsChrono.count(); + auto fraction = uptime - secondsChrono; + timeval.tv_usec = std::chrono::duration_cast( + fraction).count(); +#elif defined(LINUX) + double uptimeSeconds; + if (std::ifstream("/proc/uptime", std::ios::in) >> uptimeSeconds) + { + // value is rounded down automatically + timeval.tv_sec = uptimeSeconds; + timeval.tv_usec = uptimeSeconds *(double) 1e6 - (timeval.tv_sec *1e6); + } +#else + sif::warning << "Clock::getUptime: Not implemented for found OS" << std::endl; +#endif + return timeval; +} + +ReturnValue_t Clock::getUptime(timeval* uptime) { + *uptime = getUptime(); + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::getUptime(uint32_t* uptimeMs) { + timeval uptime = getUptime(); + *uptimeMs = uptime.tv_sec * 1000 + uptime.tv_usec / 1000; + return HasReturnvaluesIF::RETURN_OK; +} + + +ReturnValue_t Clock::getDateAndTime(TimeOfDay_t* time) { + // do some magic with chrono (C++20!) + // Right now, the library doesn't have the new features yet. + // so we work around that for now. + auto now = SystemClock::now(); + auto seconds = std::chrono::time_point_cast(now); + auto fraction = now - seconds; + time_t tt = SystemClock::to_time_t(now); + struct tm* timeInfo; + timeInfo = gmtime(&tt); + time->year = timeInfo->tm_year + 1900; + time->month = timeInfo->tm_mon+1; + time->day = timeInfo->tm_mday; + time->hour = timeInfo->tm_hour; + time->minute = timeInfo->tm_min; + time->second = timeInfo->tm_sec; + auto usecond = std::chrono::duration_cast(fraction); + time->usecond = usecond.count(); + + //sif::warning << "Clock::getDateAndTime: not implemented yet" << std::endl; + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::convertTimeOfDayToTimeval(const TimeOfDay_t* from, + timeval* to) { + struct tm time_tm; + + time_tm.tm_year = from->year - 1900; + time_tm.tm_mon = from->month - 1; + time_tm.tm_mday = from->day; + + time_tm.tm_hour = from->hour; + time_tm.tm_min = from->minute; + time_tm.tm_sec = from->second; + + time_t seconds = mktime(&time_tm); + + to->tv_sec = seconds; + to->tv_usec = from->usecond; + //Fails in 2038.. + return HasReturnvaluesIF::RETURN_OK; + sif::warning << "Clock::convertTimeBla: not implemented yet" << std::endl; + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::convertTimevalToJD2000(timeval time, double* JD2000) { + *JD2000 = (time.tv_sec - 946728000. + time.tv_usec / 1000000.) / 24. + / 3600.; + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval* tt) { + //SHOULDDO: works not for dates in the past (might have less leap seconds) + if (timeMutex == NULL) { + return HasReturnvaluesIF::RETURN_FAILED; + } + + uint16_t leapSeconds; + ReturnValue_t result = getLeapSeconds(&leapSeconds); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + timeval leapSeconds_timeval = { 0, 0 }; + leapSeconds_timeval.tv_sec = leapSeconds; + + //initial offset between UTC and TAI + timeval UTCtoTAI1972 = { 10, 0 }; + + timeval TAItoTT = { 32, 184000 }; + + *tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT; + + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) { + if(checkOrCreateClockMutex()!=HasReturnvaluesIF::RETURN_OK){ + return HasReturnvaluesIF::RETURN_FAILED; + } + ReturnValue_t result = timeMutex->lockMutex(MutexIF::BLOCKING); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + leapSeconds = leapSeconds_; + + result = timeMutex->unlockMutex(); + return result; +} + +ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) { + if(timeMutex == nullptr){ + return HasReturnvaluesIF::RETURN_FAILED; + } + ReturnValue_t result = timeMutex->lockMutex(MutexIF::BLOCKING); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + *leapSeconds_ = leapSeconds; + + result = timeMutex->unlockMutex(); + return result; +} + +ReturnValue_t Clock::checkOrCreateClockMutex(){ + if(timeMutex == nullptr){ + MutexFactory* mutexFactory = MutexFactory::instance(); + if (mutexFactory == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + timeMutex = mutexFactory->createMutex(); + if (timeMutex == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + return HasReturnvaluesIF::RETURN_OK; +} diff --git a/osal/host/FixedTimeslotTask.cpp b/osal/host/FixedTimeslotTask.cpp index dac399f6..1999c9e6 100644 --- a/osal/host/FixedTimeslotTask.cpp +++ b/osal/host/FixedTimeslotTask.cpp @@ -1,191 +1,191 @@ -#include - -#include -#include -#include - -#include -#include - -#include -#include - -#if defined(WIN32) -#include -#elif defined(LINUX) -#include -#endif - -FixedTimeslotTask::FixedTimeslotTask(const char *name, TaskPriority setPriority, - TaskStackSize setStack, TaskPeriod setPeriod, - void (*setDeadlineMissedFunc)()) : - started(false), pollingSeqTable(setPeriod*1000), taskName(name), - period(setPeriod), deadlineMissedFunc(setDeadlineMissedFunc) { - // It is propably possible to set task priorities by using the native - // task handles for Windows / Linux - mainThread = std::thread(&FixedTimeslotTask::taskEntryPoint, this, this); -#if defined(WIN32) - /* List of possible priority classes: - * https://docs.microsoft.com/en-us/windows/win32/api/processthreadsapi/ - * nf-processthreadsapi-setpriorityclass - * And respective thread priority numbers: - * https://docs.microsoft.com/en-us/windows/ - * win32/procthread/scheduling-priorities */ - int result = SetPriorityClass( - reinterpret_cast(mainThread.native_handle()), - ABOVE_NORMAL_PRIORITY_CLASS); - if(result != 0) { - sif::error << "FixedTimeslotTask: Windows SetPriorityClass failed with code " - << GetLastError() << std::endl; - } - result = SetThreadPriority( - reinterpret_cast(mainThread.native_handle()), - THREAD_PRIORITY_NORMAL); - if(result != 0) { - sif::error << "FixedTimeslotTask: Windows SetPriorityClass failed with code " - << GetLastError() << std::endl; - } -#elif defined(LINUX) - // we can just copy and paste the code from linux here. -#endif -} - -FixedTimeslotTask::~FixedTimeslotTask(void) { - //Do not delete objects, we were responsible for ptrs only. - terminateThread = true; - if(mainThread.joinable()) { - mainThread.join(); - } - delete this; -} - -void FixedTimeslotTask::taskEntryPoint(void* argument) { - FixedTimeslotTask *originalTask(reinterpret_cast(argument)); - - if (not originalTask->started) { - // we have to suspend/block here until the task is started. - // if semaphores are implemented, use them here. - std::unique_lock lock(initMutex); - initCondition.wait(lock); - } - - this->taskFunctionality(); - sif::debug << "FixedTimeslotTask::taskEntryPoint: " - "Returned from taskFunctionality." << std::endl; -} - -ReturnValue_t FixedTimeslotTask::startTask() { - started = true; - - // Notify task to start. - std::lock_guard lock(initMutex); - initCondition.notify_one(); - - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t FixedTimeslotTask::sleepFor(uint32_t ms) { - std::this_thread::sleep_for(std::chrono::milliseconds(ms)); - return HasReturnvaluesIF::RETURN_OK; -} - -void FixedTimeslotTask::taskFunctionality() { - // A local iterator for the Polling Sequence Table is created to - // find the start time for the first entry. - FixedSlotSequence::SlotListIter slotListIter = pollingSeqTable.current; - // Get start time for first entry. - chron_ms interval(slotListIter->pollingTimeMs); - auto currentStartTime { - std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()) - }; - if(interval.count() > 0) { - delayForInterval(¤tStartTime, interval); - } - - /* Enter the loop that defines the task behavior. */ - for (;;) { - if(terminateThread.load()) { - break; - } - //The component for this slot is executed and the next one is chosen. - this->pollingSeqTable.executeAndAdvance(); - if (not pollingSeqTable.slotFollowsImmediately()) { - // we need to wait before executing the current slot - //this gives us the time to wait: - interval = chron_ms(this->pollingSeqTable.getIntervalToPreviousSlotMs()); - delayForInterval(¤tStartTime, interval); - //TODO deadline missed check - } - } -} - -ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId, - uint32_t slotTimeMs, int8_t executionStep) { - if (objectManager->get(componentId) != nullptr) { - pollingSeqTable.addSlot(componentId, slotTimeMs, executionStep, this); - return HasReturnvaluesIF::RETURN_OK; - } - - sif::error << "Component " << std::hex << componentId << - " not found, not adding it to pst" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; -} - -ReturnValue_t FixedTimeslotTask::checkSequence() const { - return pollingSeqTable.checkSequence(); -} - -uint32_t FixedTimeslotTask::getPeriodMs() const { - return period * 1000; -} - -bool FixedTimeslotTask::delayForInterval(chron_ms * previousWakeTimeMs, - const chron_ms interval) { - bool shouldDelay = false; - //Get current wakeup time - auto currentStartTime = - std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - /* Generate the tick time at which the task wants to wake. */ - auto nextTimeToWake_ms = (*previousWakeTimeMs) + interval; - - if (currentStartTime < *previousWakeTimeMs) { - /* The tick count has overflowed since this function was - lasted called. In this case the only time we should ever - actually delay is if the wake time has also overflowed, - and the wake time is greater than the tick time. When this - is the case it is as if neither time had overflowed. */ - if ((nextTimeToWake_ms < *previousWakeTimeMs) - && (nextTimeToWake_ms > currentStartTime)) { - shouldDelay = true; - } - } else { - /* The tick time has not overflowed. In this case we will - delay if either the wake time has overflowed, and/or the - tick time is less than the wake time. */ - if ((nextTimeToWake_ms < *previousWakeTimeMs) - || (nextTimeToWake_ms > currentStartTime)) { - shouldDelay = true; - } - } - - /* Update the wake time ready for the next call. */ - - (*previousWakeTimeMs) = nextTimeToWake_ms; - - if (shouldDelay) { - auto sleepTime = std::chrono::duration_cast( - nextTimeToWake_ms - currentStartTime); - std::this_thread::sleep_for(sleepTime); - return true; - } - //We are shifting the time in case the deadline was missed like rtems - (*previousWakeTimeMs) = currentStartTime; - return false; - -} - - - - +#include "../../osal/host/FixedTimeslotTask.h" + +#include "../../ipc/MutexFactory.h" +#include "../../osal/host/Mutex.h" +#include "../../osal/host/FixedTimeslotTask.h" + +#include "../../serviceinterface/ServiceInterfaceStream.h" +#include "../../tasks/ExecutableObjectIF.h" + +#include +#include + +#if defined(WIN32) +#include +#elif defined(LINUX) +#include +#endif + +FixedTimeslotTask::FixedTimeslotTask(const char *name, TaskPriority setPriority, + TaskStackSize setStack, TaskPeriod setPeriod, + void (*setDeadlineMissedFunc)()) : + started(false), pollingSeqTable(setPeriod*1000), taskName(name), + period(setPeriod), deadlineMissedFunc(setDeadlineMissedFunc) { + // It is propably possible to set task priorities by using the native + // task handles for Windows / Linux + mainThread = std::thread(&FixedTimeslotTask::taskEntryPoint, this, this); +#if defined(WIN32) + /* List of possible priority classes: + * https://docs.microsoft.com/en-us/windows/win32/api/processthreadsapi/ + * nf-processthreadsapi-setpriorityclass + * And respective thread priority numbers: + * https://docs.microsoft.com/en-us/windows/ + * win32/procthread/scheduling-priorities */ + int result = SetPriorityClass( + reinterpret_cast(mainThread.native_handle()), + ABOVE_NORMAL_PRIORITY_CLASS); + if(result != 0) { + sif::error << "FixedTimeslotTask: Windows SetPriorityClass failed with code " + << GetLastError() << std::endl; + } + result = SetThreadPriority( + reinterpret_cast(mainThread.native_handle()), + THREAD_PRIORITY_NORMAL); + if(result != 0) { + sif::error << "FixedTimeslotTask: Windows SetPriorityClass failed with code " + << GetLastError() << std::endl; + } +#elif defined(LINUX) + // we can just copy and paste the code from linux here. +#endif +} + +FixedTimeslotTask::~FixedTimeslotTask(void) { + //Do not delete objects, we were responsible for ptrs only. + terminateThread = true; + if(mainThread.joinable()) { + mainThread.join(); + } + delete this; +} + +void FixedTimeslotTask::taskEntryPoint(void* argument) { + FixedTimeslotTask *originalTask(reinterpret_cast(argument)); + + if (not originalTask->started) { + // we have to suspend/block here until the task is started. + // if semaphores are implemented, use them here. + std::unique_lock lock(initMutex); + initCondition.wait(lock); + } + + this->taskFunctionality(); + sif::debug << "FixedTimeslotTask::taskEntryPoint: " + "Returned from taskFunctionality." << std::endl; +} + +ReturnValue_t FixedTimeslotTask::startTask() { + started = true; + + // Notify task to start. + std::lock_guard lock(initMutex); + initCondition.notify_one(); + + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t FixedTimeslotTask::sleepFor(uint32_t ms) { + std::this_thread::sleep_for(std::chrono::milliseconds(ms)); + return HasReturnvaluesIF::RETURN_OK; +} + +void FixedTimeslotTask::taskFunctionality() { + // A local iterator for the Polling Sequence Table is created to + // find the start time for the first entry. + FixedSlotSequence::SlotListIter slotListIter = pollingSeqTable.current; + // Get start time for first entry. + chron_ms interval(slotListIter->pollingTimeMs); + auto currentStartTime { + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + }; + if(interval.count() > 0) { + delayForInterval(¤tStartTime, interval); + } + + /* Enter the loop that defines the task behavior. */ + for (;;) { + if(terminateThread.load()) { + break; + } + //The component for this slot is executed and the next one is chosen. + this->pollingSeqTable.executeAndAdvance(); + if (not pollingSeqTable.slotFollowsImmediately()) { + // we need to wait before executing the current slot + //this gives us the time to wait: + interval = chron_ms(this->pollingSeqTable.getIntervalToPreviousSlotMs()); + delayForInterval(¤tStartTime, interval); + //TODO deadline missed check + } + } +} + +ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId, + uint32_t slotTimeMs, int8_t executionStep) { + if (objectManager->get(componentId) != nullptr) { + pollingSeqTable.addSlot(componentId, slotTimeMs, executionStep, this); + return HasReturnvaluesIF::RETURN_OK; + } + + sif::error << "Component " << std::hex << componentId << + " not found, not adding it to pst" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; +} + +ReturnValue_t FixedTimeslotTask::checkSequence() const { + return pollingSeqTable.checkSequence(); +} + +uint32_t FixedTimeslotTask::getPeriodMs() const { + return period * 1000; +} + +bool FixedTimeslotTask::delayForInterval(chron_ms * previousWakeTimeMs, + const chron_ms interval) { + bool shouldDelay = false; + //Get current wakeup time + auto currentStartTime = + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()); + /* Generate the tick time at which the task wants to wake. */ + auto nextTimeToWake_ms = (*previousWakeTimeMs) + interval; + + if (currentStartTime < *previousWakeTimeMs) { + /* The tick count has overflowed since this function was + lasted called. In this case the only time we should ever + actually delay is if the wake time has also overflowed, + and the wake time is greater than the tick time. When this + is the case it is as if neither time had overflowed. */ + if ((nextTimeToWake_ms < *previousWakeTimeMs) + && (nextTimeToWake_ms > currentStartTime)) { + shouldDelay = true; + } + } else { + /* The tick time has not overflowed. In this case we will + delay if either the wake time has overflowed, and/or the + tick time is less than the wake time. */ + if ((nextTimeToWake_ms < *previousWakeTimeMs) + || (nextTimeToWake_ms > currentStartTime)) { + shouldDelay = true; + } + } + + /* Update the wake time ready for the next call. */ + + (*previousWakeTimeMs) = nextTimeToWake_ms; + + if (shouldDelay) { + auto sleepTime = std::chrono::duration_cast( + nextTimeToWake_ms - currentStartTime); + std::this_thread::sleep_for(sleepTime); + return true; + } + //We are shifting the time in case the deadline was missed like rtems + (*previousWakeTimeMs) = currentStartTime; + return false; + +} + + + + diff --git a/osal/host/FixedTimeslotTask.h b/osal/host/FixedTimeslotTask.h index b12cc6d3..bbee94a6 100644 --- a/osal/host/FixedTimeslotTask.h +++ b/osal/host/FixedTimeslotTask.h @@ -1,130 +1,130 @@ -#ifndef FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ -#define FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ - -#include -#include -#include -#include - -#include -#include -#include -#include - -class ExecutableObjectIF; - -/** - * @brief This class represents a task for periodic activities with multiple - * steps and strict timeslot requirements for these steps. - * @details - * @ingroup task_handling - */ -class FixedTimeslotTask: public FixedTimeslotTaskIF { -public: - /** - * @brief Standard constructor of the class. - * @details - * The class is initialized without allocated objects. These need to be - * added with #addComponent. - * @param priority - * @param stack_size - * @param setPeriod - * @param setDeadlineMissedFunc - * The function pointer to the deadline missed function that shall be - * assigned. - */ - FixedTimeslotTask(const char *name, TaskPriority setPriority, - TaskStackSize setStack, TaskPeriod setPeriod, - void (*setDeadlineMissedFunc)()); - /** - * @brief Currently, the executed object's lifetime is not coupled with - * the task object's lifetime, so the destructor is empty. - */ - virtual ~FixedTimeslotTask(void); - - /** - * @brief The method to start the task. - * @details The method starts the task with the respective system call. - * Entry point is the taskEntryPoint method described below. - * The address of the task object is passed as an argument - * to the system call. - */ - ReturnValue_t startTask(void); - - /** - * Add timeslot to the polling sequence table. - * @param componentId - * @param slotTimeMs - * @param executionStep - * @return - */ - ReturnValue_t addSlot(object_id_t componentId, - uint32_t slotTimeMs, int8_t executionStep); - - ReturnValue_t checkSequence() const; - - uint32_t getPeriodMs() const; - - ReturnValue_t sleepFor(uint32_t ms); - -protected: - using chron_ms = std::chrono::milliseconds; - - bool started; - //!< Typedef for the List of objects. - typedef std::vector ObjectList; - std::thread mainThread; - std::atomic terminateThread = false; - - //! Polling sequence table which contains the object to execute - //! and information like the timeslots and the passed execution step. - FixedSlotSequence pollingSeqTable; - - std::condition_variable initCondition; - std::mutex initMutex; - std::string taskName; - /** - * @brief The period of the task. - * @details - * The period determines the frequency of the task's execution. - * It is expressed in clock ticks. - */ - TaskPeriod period; - - /** - * @brief The pointer to the deadline-missed function. - * @details - * This pointer stores the function that is executed if the task's deadline - * is missed. So, each may react individually on a timing failure. - * The pointer may be NULL, then nothing happens on missing the deadline. - * The deadline is equal to the next execution of the periodic task. - */ - void (*deadlineMissedFunc)(void); - /** - * @brief This is the function executed in the new task's context. - * @details - * It converts the argument back to the thread object type and copies the - * class instance to the task context. - * The taskFunctionality method is called afterwards. - * @param A pointer to the task object itself is passed as argument. - */ - - void taskEntryPoint(void* argument); - /** - * @brief The function containing the actual functionality of the task. - * @details - * The method sets and starts the task's period, then enters a loop that is - * repeated as long as the isRunning attribute is true. Within the loop, - * all performOperation methods of the added objects are called. Afterwards - * the checkAndRestartPeriod system call blocks the task until the next - * period. On missing the deadline, the deadlineMissedFunction is executed. - */ - void taskFunctionality(void); - - bool delayForInterval(chron_ms * previousWakeTimeMs, - const chron_ms interval); -}; - - - -#endif /* FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ */ +#ifndef FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ +#define FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ + +#include "../../objectmanager/ObjectManagerIF.h" +#include "../../tasks/FixedSlotSequence.h" +#include "../../tasks/FixedTimeslotTaskIF.h" +#include "../../tasks/Typedef.h" + +#include +#include +#include +#include + +class ExecutableObjectIF; + +/** + * @brief This class represents a task for periodic activities with multiple + * steps and strict timeslot requirements for these steps. + * @details + * @ingroup task_handling + */ +class FixedTimeslotTask: public FixedTimeslotTaskIF { +public: + /** + * @brief Standard constructor of the class. + * @details + * The class is initialized without allocated objects. These need to be + * added with #addComponent. + * @param priority + * @param stack_size + * @param setPeriod + * @param setDeadlineMissedFunc + * The function pointer to the deadline missed function that shall be + * assigned. + */ + FixedTimeslotTask(const char *name, TaskPriority setPriority, + TaskStackSize setStack, TaskPeriod setPeriod, + void (*setDeadlineMissedFunc)()); + /** + * @brief Currently, the executed object's lifetime is not coupled with + * the task object's lifetime, so the destructor is empty. + */ + virtual ~FixedTimeslotTask(void); + + /** + * @brief The method to start the task. + * @details The method starts the task with the respective system call. + * Entry point is the taskEntryPoint method described below. + * The address of the task object is passed as an argument + * to the system call. + */ + ReturnValue_t startTask(void); + + /** + * Add timeslot to the polling sequence table. + * @param componentId + * @param slotTimeMs + * @param executionStep + * @return + */ + ReturnValue_t addSlot(object_id_t componentId, + uint32_t slotTimeMs, int8_t executionStep); + + ReturnValue_t checkSequence() const; + + uint32_t getPeriodMs() const; + + ReturnValue_t sleepFor(uint32_t ms); + +protected: + using chron_ms = std::chrono::milliseconds; + + bool started; + //!< Typedef for the List of objects. + typedef std::vector ObjectList; + std::thread mainThread; + std::atomic terminateThread = false; + + //! Polling sequence table which contains the object to execute + //! and information like the timeslots and the passed execution step. + FixedSlotSequence pollingSeqTable; + + std::condition_variable initCondition; + std::mutex initMutex; + std::string taskName; + /** + * @brief The period of the task. + * @details + * The period determines the frequency of the task's execution. + * It is expressed in clock ticks. + */ + TaskPeriod period; + + /** + * @brief The pointer to the deadline-missed function. + * @details + * This pointer stores the function that is executed if the task's deadline + * is missed. So, each may react individually on a timing failure. + * The pointer may be NULL, then nothing happens on missing the deadline. + * The deadline is equal to the next execution of the periodic task. + */ + void (*deadlineMissedFunc)(void); + /** + * @brief This is the function executed in the new task's context. + * @details + * It converts the argument back to the thread object type and copies the + * class instance to the task context. + * The taskFunctionality method is called afterwards. + * @param A pointer to the task object itself is passed as argument. + */ + + void taskEntryPoint(void* argument); + /** + * @brief The function containing the actual functionality of the task. + * @details + * The method sets and starts the task's period, then enters a loop that is + * repeated as long as the isRunning attribute is true. Within the loop, + * all performOperation methods of the added objects are called. Afterwards + * the checkAndRestartPeriod system call blocks the task until the next + * period. On missing the deadline, the deadlineMissedFunction is executed. + */ + void taskFunctionality(void); + + bool delayForInterval(chron_ms * previousWakeTimeMs, + const chron_ms interval); +}; + + + +#endif /* FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ */ diff --git a/osal/host/MessageQueue.cpp b/osal/host/MessageQueue.cpp index 7af5fbd9..6fd42849 100644 --- a/osal/host/MessageQueue.cpp +++ b/osal/host/MessageQueue.cpp @@ -1,155 +1,155 @@ -#include -#include -#include -#include -#include - -MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize): - messageSize(maxMessageSize), messageDepth(messageDepth) { - queueLock = MutexFactory::instance()->createMutex(); - auto result = QueueMapManager::instance()->addMessageQueue(this, &mqId); - if(result != HasReturnvaluesIF::RETURN_OK) { - sif::error << "MessageQueue: Could not be created" << std::endl; - } -} - -MessageQueue::~MessageQueue() { - MutexFactory::instance()->deleteMutex(queueLock); -} - -ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo, - MessageQueueMessageIF* message, bool ignoreFault) { - return sendMessageFrom(sendTo, message, this->getId(), ignoreFault); -} - -ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) { - return sendToDefaultFrom(message, this->getId()); -} - -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 != 0) { - return sendMessageFrom(this->lastPartner, message, this->getId()); - } else { - return MessageQueueIF::NO_REPLY_PARTNER; - } -} - -ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, - MessageQueueMessageIF* message, MessageQueueId_t sentFrom, - bool ignoreFault) { - return sendMessageFromMessageQueue(sendTo, message, sentFrom, - ignoreFault); -} - -ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message, - MessageQueueId_t* receivedFrom) { - ReturnValue_t status = this->receiveMessage(message); - if(status == HasReturnvaluesIF::RETURN_OK) { - *receivedFrom = this->lastPartner; - } - return status; -} - -ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) { - if(messageQueue.empty()) { - return MessageQueueIF::EMPTY; - } - // not sure this will work.. - //*message = std::move(messageQueue.front()); - MutexHelper mutexLock(queueLock, 20); - MessageQueueMessage* currentMessage = &messageQueue.front(); - std::copy(currentMessage->getBuffer(), - currentMessage->getBuffer() + messageSize, message->getBuffer()); - messageQueue.pop(); - // The last partner is the first uint32_t field in the message - this->lastPartner = message->getSender(); - return HasReturnvaluesIF::RETURN_OK; -} - -MessageQueueId_t MessageQueue::getLastPartner() const { - return lastPartner; -} - -ReturnValue_t MessageQueue::flush(uint32_t* count) { - *count = messageQueue.size(); - // Clears the queue. - messageQueue = std::queue(); - return HasReturnvaluesIF::RETURN_OK; -} - -MessageQueueId_t MessageQueue::getId() const { - return mqId; -} - -void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) { - defaultDestinationSet = true; - this->defaultDestination = defaultDestination; -} - -MessageQueueId_t MessageQueue::getDefaultDestination() const { - return defaultDestination; -} - -bool MessageQueue::isDefaultDestinationSet() const { - return defaultDestinationSet; -} - - -// static core function to send messages. -ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, - MessageQueueMessageIF* message, MessageQueueId_t sentFrom, - bool ignoreFault) { - if(message->getMessageSize() > message->getMaximumMessageSize()) { - // Actually, this should never happen or an error will be emitted - // in MessageQueueMessage. - // But I will still return a failure here. - return HasReturnvaluesIF::RETURN_FAILED; - } - MessageQueue* targetQueue = dynamic_cast( - QueueMapManager::instance()->getMessageQueue(sendTo)); - if(targetQueue == nullptr) { - if(not ignoreFault) { - InternalErrorReporterIF* internalErrorReporter = - objectManager->get( - objects::INTERNAL_ERROR_REPORTER); - if (internalErrorReporter != nullptr) { - internalErrorReporter->queueMessageNotSent(); - } - } - // TODO: Better returnvalue - return HasReturnvaluesIF::RETURN_FAILED; - } - - if(targetQueue->messageQueue.size() < targetQueue->messageDepth) { - MutexHelper mutexLock(targetQueue->queueLock, 20); - // not ideal, works for now though. - MessageQueueMessage* mqmMessage = - dynamic_cast(message); - if(message != nullptr) { - targetQueue->messageQueue.push(*mqmMessage); - } - else { - sif::error << "MessageQueue::sendMessageFromMessageQueue: Message" - "is not MessageQueueMessage!" << std::endl; - } - - } - else { - return MessageQueueIF::FULL; - } - message->setSender(sentFrom); - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t MessageQueue::lockQueue(dur_millis_t lockTimeout) { - return queueLock->lockMutex(lockTimeout); -} - -ReturnValue_t MessageQueue::unlockQueue() { - return queueLock->unlockMutex(); -} +#include "../../osal/host/MessageQueue.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" +#include "../../osal/host/QueueMapManager.h" +#include "../../ipc/MutexFactory.h" +#include "../../ipc/MutexHelper.h" + +MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize): + messageSize(maxMessageSize), messageDepth(messageDepth) { + queueLock = MutexFactory::instance()->createMutex(); + auto result = QueueMapManager::instance()->addMessageQueue(this, &mqId); + if(result != HasReturnvaluesIF::RETURN_OK) { + sif::error << "MessageQueue: Could not be created" << std::endl; + } +} + +MessageQueue::~MessageQueue() { + MutexFactory::instance()->deleteMutex(queueLock); +} + +ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo, + MessageQueueMessageIF* message, bool ignoreFault) { + return sendMessageFrom(sendTo, message, this->getId(), ignoreFault); +} + +ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) { + return sendToDefaultFrom(message, this->getId()); +} + +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 != 0) { + return sendMessageFrom(this->lastPartner, message, this->getId()); + } else { + return MessageQueueIF::NO_REPLY_PARTNER; + } +} + +ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom, + bool ignoreFault) { + return sendMessageFromMessageQueue(sendTo, message, sentFrom, + ignoreFault); +} + +ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message, + MessageQueueId_t* receivedFrom) { + ReturnValue_t status = this->receiveMessage(message); + if(status == HasReturnvaluesIF::RETURN_OK) { + *receivedFrom = this->lastPartner; + } + return status; +} + +ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) { + if(messageQueue.empty()) { + return MessageQueueIF::EMPTY; + } + // not sure this will work.. + //*message = std::move(messageQueue.front()); + MutexHelper mutexLock(queueLock, 20); + MessageQueueMessage* currentMessage = &messageQueue.front(); + std::copy(currentMessage->getBuffer(), + currentMessage->getBuffer() + messageSize, message->getBuffer()); + messageQueue.pop(); + // The last partner is the first uint32_t field in the message + this->lastPartner = message->getSender(); + return HasReturnvaluesIF::RETURN_OK; +} + +MessageQueueId_t MessageQueue::getLastPartner() const { + return lastPartner; +} + +ReturnValue_t MessageQueue::flush(uint32_t* count) { + *count = messageQueue.size(); + // Clears the queue. + messageQueue = std::queue(); + return HasReturnvaluesIF::RETURN_OK; +} + +MessageQueueId_t MessageQueue::getId() const { + return mqId; +} + +void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) { + defaultDestinationSet = true; + this->defaultDestination = defaultDestination; +} + +MessageQueueId_t MessageQueue::getDefaultDestination() const { + return defaultDestination; +} + +bool MessageQueue::isDefaultDestinationSet() const { + return defaultDestinationSet; +} + + +// static core function to send messages. +ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom, + bool ignoreFault) { + if(message->getMessageSize() > message->getMaximumMessageSize()) { + // Actually, this should never happen or an error will be emitted + // in MessageQueueMessage. + // But I will still return a failure here. + return HasReturnvaluesIF::RETURN_FAILED; + } + MessageQueue* targetQueue = dynamic_cast( + QueueMapManager::instance()->getMessageQueue(sendTo)); + if(targetQueue == nullptr) { + if(not ignoreFault) { + InternalErrorReporterIF* internalErrorReporter = + objectManager->get( + objects::INTERNAL_ERROR_REPORTER); + if (internalErrorReporter != nullptr) { + internalErrorReporter->queueMessageNotSent(); + } + } + // TODO: Better returnvalue + return HasReturnvaluesIF::RETURN_FAILED; + } + + if(targetQueue->messageQueue.size() < targetQueue->messageDepth) { + MutexHelper mutexLock(targetQueue->queueLock, 20); + // not ideal, works for now though. + MessageQueueMessage* mqmMessage = + dynamic_cast(message); + if(message != nullptr) { + targetQueue->messageQueue.push(*mqmMessage); + } + else { + sif::error << "MessageQueue::sendMessageFromMessageQueue: Message" + "is not MessageQueueMessage!" << std::endl; + } + + } + else { + return MessageQueueIF::FULL; + } + message->setSender(sentFrom); + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t MessageQueue::lockQueue(dur_millis_t lockTimeout) { + return queueLock->lockMutex(lockTimeout); +} + +ReturnValue_t MessageQueue::unlockQueue() { + return queueLock->unlockMutex(); +} diff --git a/osal/host/MessageQueue.h b/osal/host/MessageQueue.h index 7fc77f7a..d2da2402 100644 --- a/osal/host/MessageQueue.h +++ b/osal/host/MessageQueue.h @@ -1,230 +1,230 @@ -#ifndef FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ -#define FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ - -#include -#include -#include -#include -#include - -#include -#include - -/** - * @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. - * - * 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 - * 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 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. - */ - 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. - */ - ReturnValue_t sendMessage(MessageQueueId_t sendTo, - MessageQueueMessageIF* message, bool ignoreFault = false) override; - /** - * @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(MessageQueueMessageIF* message) override; - /** - * @brief This operation sends a message to the last communication partner. - * @details This operation simplifies answering an incoming message by using - * the stored lastPartner 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(MessageQueueMessageIF* message) override; - - /** - * @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, - MessageQueueMessageIF* message, MessageQueueId_t sentFrom = NO_QUEUE, - bool ignoreFault = false) override; - - /** - * @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( MessageQueueMessageIF* message, - MessageQueueId_t sentFrom = NO_QUEUE, - 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(MessageQueueMessageIF* message, - MessageQueueId_t *receivedFrom) 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(MessageQueueMessageIF* message) override; - /** - * 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) override; - /** - * @brief This method returns the message queue id of the last - * communication partner. - */ - MessageQueueId_t getLastPartner() const override; - /** - * @brief This method returns the message queue id of this class's - * message queue. - */ - MessageQueueId_t getId() const override; - - /** - * @brief This method is a simple setter for the default destination. - */ - void setDefaultDestination(MessageQueueId_t defaultDestination) override; - /** - * @brief This method is a simple getter for the default destination. - */ - MessageQueueId_t getDefaultDestination() const override; - - bool isDefaultDestinationSet() const override; - - ReturnValue_t lockQueue(dur_millis_t lockTimeout); - ReturnValue_t unlockQueue(); -protected: - /** - * @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, - MessageQueueMessageIF* message, MessageQueueId_t sentFrom = NO_QUEUE, - bool ignoreFault=false); - - //static ReturnValue_t handleSendResult(BaseType_t result, bool ignoreFault); - -private: - std::queue messageQueue; - /** - * @brief The class stores the queue id it got assigned. - * If initialization fails, the queue id is set to zero. - */ - MessageQueueId_t mqId = 0; - size_t messageSize = 0; - size_t messageDepth = 0; - - MutexIF* queueLock; - - bool defaultDestinationSet = false; - MessageQueueId_t defaultDestination = 0; - MessageQueueId_t lastPartner = 0; -}; - -#endif /* FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ */ +#ifndef FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ +#define FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ + +#include "../../internalError/InternalErrorReporterIF.h" +#include "../../ipc/MessageQueueIF.h" +#include "../../ipc/MessageQueueMessage.h" +#include "../../ipc/MutexIF.h" +#include "../../timemanager/Clock.h" + +#include +#include + +/** + * @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. + * + * 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 + * 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 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. + */ + 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. + */ + ReturnValue_t sendMessage(MessageQueueId_t sendTo, + MessageQueueMessageIF* message, bool ignoreFault = false) override; + /** + * @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(MessageQueueMessageIF* message) override; + /** + * @brief This operation sends a message to the last communication partner. + * @details This operation simplifies answering an incoming message by using + * the stored lastPartner 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(MessageQueueMessageIF* message) override; + + /** + * @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, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom = NO_QUEUE, + bool ignoreFault = false) override; + + /** + * @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( MessageQueueMessageIF* message, + MessageQueueId_t sentFrom = NO_QUEUE, + 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(MessageQueueMessageIF* message, + MessageQueueId_t *receivedFrom) 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(MessageQueueMessageIF* message) override; + /** + * 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) override; + /** + * @brief This method returns the message queue id of the last + * communication partner. + */ + MessageQueueId_t getLastPartner() const override; + /** + * @brief This method returns the message queue id of this class's + * message queue. + */ + MessageQueueId_t getId() const override; + + /** + * @brief This method is a simple setter for the default destination. + */ + void setDefaultDestination(MessageQueueId_t defaultDestination) override; + /** + * @brief This method is a simple getter for the default destination. + */ + MessageQueueId_t getDefaultDestination() const override; + + bool isDefaultDestinationSet() const override; + + ReturnValue_t lockQueue(dur_millis_t lockTimeout); + ReturnValue_t unlockQueue(); +protected: + /** + * @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, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom = NO_QUEUE, + bool ignoreFault=false); + + //static ReturnValue_t handleSendResult(BaseType_t result, bool ignoreFault); + +private: + std::queue messageQueue; + /** + * @brief The class stores the queue id it got assigned. + * If initialization fails, the queue id is set to zero. + */ + MessageQueueId_t mqId = 0; + size_t messageSize = 0; + size_t messageDepth = 0; + + MutexIF* queueLock; + + bool defaultDestinationSet = false; + MessageQueueId_t defaultDestination = 0; + MessageQueueId_t lastPartner = 0; +}; + +#endif /* FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ */ diff --git a/osal/host/Mutex.cpp b/osal/host/Mutex.cpp index 03948c4c..28768507 100644 --- a/osal/host/Mutex.cpp +++ b/osal/host/Mutex.cpp @@ -1,40 +1,40 @@ -#include -#include - -const uint32_t MutexIF::POLLING = 0; -const uint32_t MutexIF::BLOCKING = 0xffffffff; - -ReturnValue_t Mutex::lockMutex(uint32_t timeoutMs) { - if(timeoutMs == MutexIF::BLOCKING) { - mutex.lock(); - locked = true; - return HasReturnvaluesIF::RETURN_OK; - } - else if(timeoutMs == MutexIF::POLLING) { - if(mutex.try_lock()) { - locked = true; - return HasReturnvaluesIF::RETURN_OK; - } - } - else if(timeoutMs > MutexIF::POLLING){ - auto chronoMs = std::chrono::milliseconds(timeoutMs); - if(mutex.try_lock_for(chronoMs)) { - locked = true; - return HasReturnvaluesIF::RETURN_OK; - } - } - return MutexIF::MUTEX_TIMEOUT; -} - -ReturnValue_t Mutex::unlockMutex() { - if(not locked) { - return MutexIF::CURR_THREAD_DOES_NOT_OWN_MUTEX; - } - mutex.unlock(); - locked = false; - return HasReturnvaluesIF::RETURN_OK; -} - -std::timed_mutex* Mutex::getMutexHandle() { - return &mutex; -} +#include "../../osal/host/Mutex.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" + +const uint32_t MutexIF::POLLING = 0; +const uint32_t MutexIF::BLOCKING = 0xffffffff; + +ReturnValue_t Mutex::lockMutex(uint32_t timeoutMs) { + if(timeoutMs == MutexIF::BLOCKING) { + mutex.lock(); + locked = true; + return HasReturnvaluesIF::RETURN_OK; + } + else if(timeoutMs == MutexIF::POLLING) { + if(mutex.try_lock()) { + locked = true; + return HasReturnvaluesIF::RETURN_OK; + } + } + else if(timeoutMs > MutexIF::POLLING){ + auto chronoMs = std::chrono::milliseconds(timeoutMs); + if(mutex.try_lock_for(chronoMs)) { + locked = true; + return HasReturnvaluesIF::RETURN_OK; + } + } + return MutexIF::MUTEX_TIMEOUT; +} + +ReturnValue_t Mutex::unlockMutex() { + if(not locked) { + return MutexIF::CURR_THREAD_DOES_NOT_OWN_MUTEX; + } + mutex.unlock(); + locked = false; + return HasReturnvaluesIF::RETURN_OK; +} + +std::timed_mutex* Mutex::getMutexHandle() { + return &mutex; +} diff --git a/osal/host/Mutex.h b/osal/host/Mutex.h index d882c457..4d49bac7 100644 --- a/osal/host/Mutex.h +++ b/osal/host/Mutex.h @@ -1,28 +1,28 @@ -#ifndef FRAMEWORK_OSAL_FREERTOS_MUTEX_H_ -#define FRAMEWORK_OSAL_FREERTOS_MUTEX_H_ - -#include - -#include - -/** - * @brief OS component to implement MUTual EXclusion - * - * @details - * Mutexes are binary semaphores which include a priority inheritance mechanism. - * Documentation: https://www.freertos.org/Real-time-embedded-RTOS-mutexes.html - * @ingroup osal - */ -class Mutex : public MutexIF { -public: - Mutex() = default; - ReturnValue_t lockMutex(uint32_t timeoutMs = MutexIF::BLOCKING) override; - ReturnValue_t unlockMutex() override; - - std::timed_mutex* getMutexHandle(); -private: - bool locked = false; - std::timed_mutex mutex; -}; - -#endif /* FRAMEWORK_FREERTOS_MUTEX_H_ */ +#ifndef FRAMEWORK_OSAL_FREERTOS_MUTEX_H_ +#define FRAMEWORK_OSAL_FREERTOS_MUTEX_H_ + +#include "../../ipc/MutexIF.h" + +#include + +/** + * @brief OS component to implement MUTual EXclusion + * + * @details + * Mutexes are binary semaphores which include a priority inheritance mechanism. + * Documentation: https://www.freertos.org/Real-time-embedded-RTOS-mutexes.html + * @ingroup osal + */ +class Mutex : public MutexIF { +public: + Mutex() = default; + ReturnValue_t lockMutex(uint32_t timeoutMs = MutexIF::BLOCKING) override; + ReturnValue_t unlockMutex() override; + + std::timed_mutex* getMutexHandle(); +private: + bool locked = false; + std::timed_mutex mutex; +}; + +#endif /* FRAMEWORK_FREERTOS_MUTEX_H_ */ diff --git a/osal/host/MutexFactory.cpp b/osal/host/MutexFactory.cpp index 25d9fa46..284ce59f 100644 --- a/osal/host/MutexFactory.cpp +++ b/osal/host/MutexFactory.cpp @@ -1,28 +1,28 @@ -#include -#include - -//TODO: Different variant than the lazy loading in QueueFactory. -//What's better and why? -> one is on heap the other on bss/data -//MutexFactory* MutexFactory::factoryInstance = new MutexFactory(); -MutexFactory* MutexFactory::factoryInstance = nullptr; - -MutexFactory::MutexFactory() { -} - -MutexFactory::~MutexFactory() { -} - -MutexFactory* MutexFactory::instance() { - if (factoryInstance == nullptr){ - factoryInstance = new MutexFactory(); - } - return MutexFactory::factoryInstance; -} - -MutexIF* MutexFactory::createMutex() { - return new Mutex(); -} - -void MutexFactory::deleteMutex(MutexIF* mutex) { - delete mutex; -} +#include "../../ipc/MutexFactory.h" +#include "../../osal/host/Mutex.h" + +//TODO: Different variant than the lazy loading in QueueFactory. +//What's better and why? -> one is on heap the other on bss/data +//MutexFactory* MutexFactory::factoryInstance = new MutexFactory(); +MutexFactory* MutexFactory::factoryInstance = nullptr; + +MutexFactory::MutexFactory() { +} + +MutexFactory::~MutexFactory() { +} + +MutexFactory* MutexFactory::instance() { + if (factoryInstance == nullptr){ + factoryInstance = new MutexFactory(); + } + return MutexFactory::factoryInstance; +} + +MutexIF* MutexFactory::createMutex() { + return new Mutex(); +} + +void MutexFactory::deleteMutex(MutexIF* mutex) { + delete mutex; +} diff --git a/osal/host/PeriodicTask.cpp b/osal/host/PeriodicTask.cpp index 1a5024ab..c4cf9f56 100644 --- a/osal/host/PeriodicTask.cpp +++ b/osal/host/PeriodicTask.cpp @@ -1,176 +1,176 @@ -#include -#include -#include - -#include -#include - -#include -#include - -#if defined(WIN32) -#include -#elif defined(LINUX) -#include -#endif - -PeriodicTask::PeriodicTask(const char *name, TaskPriority setPriority, - TaskStackSize setStack, TaskPeriod setPeriod, - void (*setDeadlineMissedFunc)()) : - started(false), taskName(name), period(setPeriod), - deadlineMissedFunc(setDeadlineMissedFunc) { - // It is propably possible to set task priorities by using the native - // task handles for Windows / Linux - mainThread = std::thread(&PeriodicTask::taskEntryPoint, this, this); -#if defined(WIN32) - /* List of possible priority classes: - * https://docs.microsoft.com/en-us/windows/win32/api/processthreadsapi/ - * nf-processthreadsapi-setpriorityclass - * And respective thread priority numbers: - * https://docs.microsoft.com/en-us/windows/ - * win32/procthread/scheduling-priorities */ - int result = SetPriorityClass( - reinterpret_cast(mainThread.native_handle()), - ABOVE_NORMAL_PRIORITY_CLASS); - if(result != 0) { - sif::error << "PeriodicTask: Windows SetPriorityClass failed with code " - << GetLastError() << std::endl; - } - result = SetThreadPriority( - reinterpret_cast(mainThread.native_handle()), - THREAD_PRIORITY_NORMAL); - if(result != 0) { - sif::error << "PeriodicTask: Windows SetPriorityClass failed with code " - << GetLastError() << std::endl; - } -#elif defined(LINUX) - // we can just copy and paste the code from linux here. -#endif -} - -PeriodicTask::~PeriodicTask(void) { - //Do not delete objects, we were responsible for ptrs only. - terminateThread = true; - if(mainThread.joinable()) { - mainThread.join(); - } - delete this; -} - -void PeriodicTask::taskEntryPoint(void* argument) { - PeriodicTask *originalTask(reinterpret_cast(argument)); - - - if (not originalTask->started) { - // we have to suspend/block here until the task is started. - // if semaphores are implemented, use them here. - std::unique_lock lock(initMutex); - initCondition.wait(lock); - } - - this->taskFunctionality(); - sif::debug << "PeriodicTask::taskEntryPoint: " - "Returned from taskFunctionality." << std::endl; -} - -ReturnValue_t PeriodicTask::startTask() { - started = true; - - // Notify task to start. - std::lock_guard lock(initMutex); - initCondition.notify_one(); - - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t PeriodicTask::sleepFor(uint32_t ms) { - std::this_thread::sleep_for(std::chrono::milliseconds(ms)); - return HasReturnvaluesIF::RETURN_OK; -} - -void PeriodicTask::taskFunctionality() { - std::chrono::milliseconds periodChrono(static_cast(period*1000)); - auto currentStartTime { - std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()) - }; - auto nextStartTime{ currentStartTime }; - - /* Enter the loop that defines the task behavior. */ - for (;;) { - if(terminateThread.load()) { - break; - } - for (ObjectList::iterator it = objectList.begin(); - it != objectList.end(); ++it) { - (*it)->performOperation(); - } - if(not delayForInterval(¤tStartTime, periodChrono)) { - sif::warning << "PeriodicTask: " << taskName << - " missed deadline!\n" << std::flush; - if(deadlineMissedFunc != nullptr) { - this->deadlineMissedFunc(); - } - } - } -} - -ReturnValue_t PeriodicTask::addComponent(object_id_t object) { - ExecutableObjectIF* newObject = objectManager->get( - object); - if (newObject == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - objectList.push_back(newObject); - return HasReturnvaluesIF::RETURN_OK; -} - -uint32_t PeriodicTask::getPeriodMs() const { - return period * 1000; -} - -bool PeriodicTask::delayForInterval(chron_ms* previousWakeTimeMs, - const chron_ms interval) { - bool shouldDelay = false; - //Get current wakeup time - auto currentStartTime = - std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - /* Generate the tick time at which the task wants to wake. */ - auto nextTimeToWake_ms = (*previousWakeTimeMs) + interval; - - if (currentStartTime < *previousWakeTimeMs) { - /* The tick count has overflowed since this function was - lasted called. In this case the only time we should ever - actually delay is if the wake time has also overflowed, - and the wake time is greater than the tick time. When this - is the case it is as if neither time had overflowed. */ - if ((nextTimeToWake_ms < *previousWakeTimeMs) - && (nextTimeToWake_ms > currentStartTime)) { - shouldDelay = true; - } - } else { - /* The tick time has not overflowed. In this case we will - delay if either the wake time has overflowed, and/or the - tick time is less than the wake time. */ - if ((nextTimeToWake_ms < *previousWakeTimeMs) - || (nextTimeToWake_ms > currentStartTime)) { - shouldDelay = true; - } - } - - /* Update the wake time ready for the next call. */ - - (*previousWakeTimeMs) = nextTimeToWake_ms; - - if (shouldDelay) { - auto sleepTime = std::chrono::duration_cast( - nextTimeToWake_ms - currentStartTime); - std::this_thread::sleep_for(sleepTime); - return true; - } - //We are shifting the time in case the deadline was missed like rtems - (*previousWakeTimeMs) = currentStartTime; - return false; - -} +#include "../../ipc/MutexFactory.h" +#include "../../osal/host/Mutex.h" +#include "../../osal/host/PeriodicTask.h" + +#include "../../serviceinterface/ServiceInterfaceStream.h" +#include "../../tasks/ExecutableObjectIF.h" + +#include +#include + +#if defined(WIN32) +#include +#elif defined(LINUX) +#include +#endif + +PeriodicTask::PeriodicTask(const char *name, TaskPriority setPriority, + TaskStackSize setStack, TaskPeriod setPeriod, + void (*setDeadlineMissedFunc)()) : + started(false), taskName(name), period(setPeriod), + deadlineMissedFunc(setDeadlineMissedFunc) { + // It is propably possible to set task priorities by using the native + // task handles for Windows / Linux + mainThread = std::thread(&PeriodicTask::taskEntryPoint, this, this); +#if defined(WIN32) + /* List of possible priority classes: + * https://docs.microsoft.com/en-us/windows/win32/api/processthreadsapi/ + * nf-processthreadsapi-setpriorityclass + * And respective thread priority numbers: + * https://docs.microsoft.com/en-us/windows/ + * win32/procthread/scheduling-priorities */ + int result = SetPriorityClass( + reinterpret_cast(mainThread.native_handle()), + ABOVE_NORMAL_PRIORITY_CLASS); + if(result != 0) { + sif::error << "PeriodicTask: Windows SetPriorityClass failed with code " + << GetLastError() << std::endl; + } + result = SetThreadPriority( + reinterpret_cast(mainThread.native_handle()), + THREAD_PRIORITY_NORMAL); + if(result != 0) { + sif::error << "PeriodicTask: Windows SetPriorityClass failed with code " + << GetLastError() << std::endl; + } +#elif defined(LINUX) + // we can just copy and paste the code from linux here. +#endif +} + +PeriodicTask::~PeriodicTask(void) { + //Do not delete objects, we were responsible for ptrs only. + terminateThread = true; + if(mainThread.joinable()) { + mainThread.join(); + } + delete this; +} + +void PeriodicTask::taskEntryPoint(void* argument) { + PeriodicTask *originalTask(reinterpret_cast(argument)); + + + if (not originalTask->started) { + // we have to suspend/block here until the task is started. + // if semaphores are implemented, use them here. + std::unique_lock lock(initMutex); + initCondition.wait(lock); + } + + this->taskFunctionality(); + sif::debug << "PeriodicTask::taskEntryPoint: " + "Returned from taskFunctionality." << std::endl; +} + +ReturnValue_t PeriodicTask::startTask() { + started = true; + + // Notify task to start. + std::lock_guard lock(initMutex); + initCondition.notify_one(); + + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t PeriodicTask::sleepFor(uint32_t ms) { + std::this_thread::sleep_for(std::chrono::milliseconds(ms)); + return HasReturnvaluesIF::RETURN_OK; +} + +void PeriodicTask::taskFunctionality() { + std::chrono::milliseconds periodChrono(static_cast(period*1000)); + auto currentStartTime { + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + }; + auto nextStartTime{ currentStartTime }; + + /* Enter the loop that defines the task behavior. */ + for (;;) { + if(terminateThread.load()) { + break; + } + for (ObjectList::iterator it = objectList.begin(); + it != objectList.end(); ++it) { + (*it)->performOperation(); + } + if(not delayForInterval(¤tStartTime, periodChrono)) { + sif::warning << "PeriodicTask: " << taskName << + " missed deadline!\n" << std::flush; + if(deadlineMissedFunc != nullptr) { + this->deadlineMissedFunc(); + } + } + } +} + +ReturnValue_t PeriodicTask::addComponent(object_id_t object) { + ExecutableObjectIF* newObject = objectManager->get( + object); + if (newObject == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + objectList.push_back(newObject); + return HasReturnvaluesIF::RETURN_OK; +} + +uint32_t PeriodicTask::getPeriodMs() const { + return period * 1000; +} + +bool PeriodicTask::delayForInterval(chron_ms* previousWakeTimeMs, + const chron_ms interval) { + bool shouldDelay = false; + //Get current wakeup time + auto currentStartTime = + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()); + /* Generate the tick time at which the task wants to wake. */ + auto nextTimeToWake_ms = (*previousWakeTimeMs) + interval; + + if (currentStartTime < *previousWakeTimeMs) { + /* The tick count has overflowed since this function was + lasted called. In this case the only time we should ever + actually delay is if the wake time has also overflowed, + and the wake time is greater than the tick time. When this + is the case it is as if neither time had overflowed. */ + if ((nextTimeToWake_ms < *previousWakeTimeMs) + && (nextTimeToWake_ms > currentStartTime)) { + shouldDelay = true; + } + } else { + /* The tick time has not overflowed. In this case we will + delay if either the wake time has overflowed, and/or the + tick time is less than the wake time. */ + if ((nextTimeToWake_ms < *previousWakeTimeMs) + || (nextTimeToWake_ms > currentStartTime)) { + shouldDelay = true; + } + } + + /* Update the wake time ready for the next call. */ + + (*previousWakeTimeMs) = nextTimeToWake_ms; + + if (shouldDelay) { + auto sleepTime = std::chrono::duration_cast( + nextTimeToWake_ms - currentStartTime); + std::this_thread::sleep_for(sleepTime); + return true; + } + //We are shifting the time in case the deadline was missed like rtems + (*previousWakeTimeMs) = currentStartTime; + return false; + +} diff --git a/osal/host/PeriodicTask.h b/osal/host/PeriodicTask.h index d97bf089..7eb768cb 100644 --- a/osal/host/PeriodicTask.h +++ b/osal/host/PeriodicTask.h @@ -1,123 +1,123 @@ -#ifndef FRAMEWORK_OSAL_HOST_PERIODICTASK_H_ -#define FRAMEWORK_OSAL_HOST_PERIODICTASK_H_ - -#include -#include -#include - -#include -#include -#include -#include - -class ExecutableObjectIF; - -/** - * @brief This class represents a specialized task for - * periodic activities of multiple objects. - * @details - * - * @ingroup task_handling - */ -class PeriodicTask: public PeriodicTaskIF { -public: - /** - * @brief Standard constructor of the class. - * @details - * The class is initialized without allocated objects. These need to be - * added with #addComponent. - * @param priority - * @param stack_size - * @param setPeriod - * @param setDeadlineMissedFunc - * The function pointer to the deadline missed function that shall be - * assigned. - */ - PeriodicTask(const char *name, TaskPriority setPriority, TaskStackSize setStack, - TaskPeriod setPeriod,void (*setDeadlineMissedFunc)()); - /** - * @brief Currently, the executed object's lifetime is not coupled with - * the task object's lifetime, so the destructor is empty. - */ - virtual ~PeriodicTask(void); - - /** - * @brief The method to start the task. - * @details The method starts the task with the respective system call. - * Entry point is the taskEntryPoint method described below. - * The address of the task object is passed as an argument - * to the system call. - */ - ReturnValue_t startTask(void); - /** - * Adds an object to the list of objects to be executed. - * The objects are executed in the order added. - * @param object Id of the object to add. - * @return - * -@c RETURN_OK on success - * -@c RETURN_FAILED if the object could not be added. - */ - ReturnValue_t addComponent(object_id_t object); - - uint32_t getPeriodMs() const; - - ReturnValue_t sleepFor(uint32_t ms); - -protected: - using chron_ms = std::chrono::milliseconds; - bool started; - //!< Typedef for the List of objects. - typedef std::vector ObjectList; - std::thread mainThread; - std::atomic terminateThread = false; - - /** - * @brief This attribute holds a list of objects to be executed. - */ - ObjectList objectList; - - std::condition_variable initCondition; - std::mutex initMutex; - std::string taskName; - /** - * @brief The period of the task. - * @details - * The period determines the frequency of the task's execution. - * It is expressed in clock ticks. - */ - TaskPeriod period; - /** - * @brief The pointer to the deadline-missed function. - * @details - * This pointer stores the function that is executed if the task's deadline - * is missed. So, each may react individually on a timing failure. - * The pointer may be NULL, then nothing happens on missing the deadline. - * The deadline is equal to the next execution of the periodic task. - */ - void (*deadlineMissedFunc)(void); - /** - * @brief This is the function executed in the new task's context. - * @details - * It converts the argument back to the thread object type and copies the - * class instance to the task context. - * The taskFunctionality method is called afterwards. - * @param A pointer to the task object itself is passed as argument. - */ - - void taskEntryPoint(void* argument); - /** - * @brief The function containing the actual functionality of the task. - * @details - * The method sets and starts the task's period, then enters a loop that is - * repeated as long as the isRunning attribute is true. Within the loop, - * all performOperation methods of the added objects are called. Afterwards - * the checkAndRestartPeriod system call blocks the task until the next - * period. On missing the deadline, the deadlineMissedFunction is executed. - */ - void taskFunctionality(void); - - bool delayForInterval(chron_ms * previousWakeTimeMs, - const chron_ms interval); -}; - -#endif /* PERIODICTASK_H_ */ +#ifndef FRAMEWORK_OSAL_HOST_PERIODICTASK_H_ +#define FRAMEWORK_OSAL_HOST_PERIODICTASK_H_ + +#include "../../objectmanager/ObjectManagerIF.h" +#include "../../tasks/PeriodicTaskIF.h" +#include "../../tasks/Typedef.h" + +#include +#include +#include +#include + +class ExecutableObjectIF; + +/** + * @brief This class represents a specialized task for + * periodic activities of multiple objects. + * @details + * + * @ingroup task_handling + */ +class PeriodicTask: public PeriodicTaskIF { +public: + /** + * @brief Standard constructor of the class. + * @details + * The class is initialized without allocated objects. These need to be + * added with #addComponent. + * @param priority + * @param stack_size + * @param setPeriod + * @param setDeadlineMissedFunc + * The function pointer to the deadline missed function that shall be + * assigned. + */ + PeriodicTask(const char *name, TaskPriority setPriority, TaskStackSize setStack, + TaskPeriod setPeriod,void (*setDeadlineMissedFunc)()); + /** + * @brief Currently, the executed object's lifetime is not coupled with + * the task object's lifetime, so the destructor is empty. + */ + virtual ~PeriodicTask(void); + + /** + * @brief The method to start the task. + * @details The method starts the task with the respective system call. + * Entry point is the taskEntryPoint method described below. + * The address of the task object is passed as an argument + * to the system call. + */ + ReturnValue_t startTask(void); + /** + * Adds an object to the list of objects to be executed. + * The objects are executed in the order added. + * @param object Id of the object to add. + * @return + * -@c RETURN_OK on success + * -@c RETURN_FAILED if the object could not be added. + */ + ReturnValue_t addComponent(object_id_t object); + + uint32_t getPeriodMs() const; + + ReturnValue_t sleepFor(uint32_t ms); + +protected: + using chron_ms = std::chrono::milliseconds; + bool started; + //!< Typedef for the List of objects. + typedef std::vector ObjectList; + std::thread mainThread; + std::atomic terminateThread = false; + + /** + * @brief This attribute holds a list of objects to be executed. + */ + ObjectList objectList; + + std::condition_variable initCondition; + std::mutex initMutex; + std::string taskName; + /** + * @brief The period of the task. + * @details + * The period determines the frequency of the task's execution. + * It is expressed in clock ticks. + */ + TaskPeriod period; + /** + * @brief The pointer to the deadline-missed function. + * @details + * This pointer stores the function that is executed if the task's deadline + * is missed. So, each may react individually on a timing failure. + * The pointer may be NULL, then nothing happens on missing the deadline. + * The deadline is equal to the next execution of the periodic task. + */ + void (*deadlineMissedFunc)(void); + /** + * @brief This is the function executed in the new task's context. + * @details + * It converts the argument back to the thread object type and copies the + * class instance to the task context. + * The taskFunctionality method is called afterwards. + * @param A pointer to the task object itself is passed as argument. + */ + + void taskEntryPoint(void* argument); + /** + * @brief The function containing the actual functionality of the task. + * @details + * The method sets and starts the task's period, then enters a loop that is + * repeated as long as the isRunning attribute is true. Within the loop, + * all performOperation methods of the added objects are called. Afterwards + * the checkAndRestartPeriod system call blocks the task until the next + * period. On missing the deadline, the deadlineMissedFunction is executed. + */ + void taskFunctionality(void); + + bool delayForInterval(chron_ms * previousWakeTimeMs, + const chron_ms interval); +}; + +#endif /* PERIODICTASK_H_ */ diff --git a/osal/host/QueueFactory.cpp b/osal/host/QueueFactory.cpp index 225bb8c7..052671ca 100644 --- a/osal/host/QueueFactory.cpp +++ b/osal/host/QueueFactory.cpp @@ -1,41 +1,41 @@ -#include -#include -#include -#include - -QueueFactory* QueueFactory::factoryInstance = nullptr; - - -ReturnValue_t MessageQueueSenderIF::sendMessage(MessageQueueId_t sendTo, - MessageQueueMessageIF* message, MessageQueueId_t sentFrom, - bool ignoreFault) { - return MessageQueue::sendMessageFromMessageQueue(sendTo,message, - sentFrom,ignoreFault); - return HasReturnvaluesIF::RETURN_OK; -} - -QueueFactory* QueueFactory::instance() { - if (factoryInstance == nullptr) { - factoryInstance = new QueueFactory; - } - return factoryInstance; -} - -QueueFactory::QueueFactory() { -} - -QueueFactory::~QueueFactory() { -} - -MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, - size_t maxMessageSize) { - // A thread-safe queue can be implemented by using a combination - // of std::queue and std::mutex. This uses dynamic memory allocation - // which could be alleviated by using a custom allocator, external library - // (etl::queue) or simply using std::queue, we're on a host machine anyway. - return new MessageQueue(messageDepth, maxMessageSize); -} - -void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) { - delete queue; -} +#include "../../ipc/QueueFactory.h" +#include "../../osal/host/MessageQueue.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" +#include + +QueueFactory* QueueFactory::factoryInstance = nullptr; + + +ReturnValue_t MessageQueueSenderIF::sendMessage(MessageQueueId_t sendTo, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom, + bool ignoreFault) { + return MessageQueue::sendMessageFromMessageQueue(sendTo,message, + sentFrom,ignoreFault); + return HasReturnvaluesIF::RETURN_OK; +} + +QueueFactory* QueueFactory::instance() { + if (factoryInstance == nullptr) { + factoryInstance = new QueueFactory; + } + return factoryInstance; +} + +QueueFactory::QueueFactory() { +} + +QueueFactory::~QueueFactory() { +} + +MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, + size_t maxMessageSize) { + // A thread-safe queue can be implemented by using a combination + // of std::queue and std::mutex. This uses dynamic memory allocation + // which could be alleviated by using a custom allocator, external library + // (etl::queue) or simply using std::queue, we're on a host machine anyway. + return new MessageQueue(messageDepth, maxMessageSize); +} + +void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) { + delete queue; +} diff --git a/osal/host/QueueMapManager.cpp b/osal/host/QueueMapManager.cpp index 89b9a948..35db204e 100644 --- a/osal/host/QueueMapManager.cpp +++ b/osal/host/QueueMapManager.cpp @@ -1,51 +1,51 @@ -#include -#include -#include - -QueueMapManager* QueueMapManager::mqManagerInstance = nullptr; - -QueueMapManager::QueueMapManager() { - mapLock = MutexFactory::instance()->createMutex(); -} - -QueueMapManager* QueueMapManager::instance() { - if (mqManagerInstance == nullptr){ - mqManagerInstance = new QueueMapManager(); - } - return QueueMapManager::mqManagerInstance; -} - -ReturnValue_t QueueMapManager::addMessageQueue( - MessageQueueIF* queueToInsert, MessageQueueId_t* id) { - // Not thread-safe, but it is assumed all message queues are created - // at software initialization now. If this is to be made thread-safe in - // the future, it propably would be sufficient to lock the increment - // operation here - uint32_t currentId = queueCounter++; - auto returnPair = queueMap.emplace(currentId, queueToInsert); - if(not returnPair.second) { - // this should never happen for the atomic variable. - sif::error << "QueueMapManager: This ID is already inside the map!" - << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; - } - if (id != nullptr) { - *id = currentId; - } - return HasReturnvaluesIF::RETURN_OK; -} - -MessageQueueIF* QueueMapManager::getMessageQueue( - MessageQueueId_t messageQueueId) const { - MutexHelper(mapLock, 50); - auto queueIter = queueMap.find(messageQueueId); - if(queueIter != queueMap.end()) { - return queueIter->second; - } - else { - sif::warning << "QueueMapManager::getQueueHandle: The ID" << - messageQueueId << " does not exists in the map" << std::endl; - return nullptr; - } -} - +#include "../../ipc/MutexFactory.h" +#include "../../ipc/MutexHelper.h" +#include "../../osal/host/QueueMapManager.h" + +QueueMapManager* QueueMapManager::mqManagerInstance = nullptr; + +QueueMapManager::QueueMapManager() { + mapLock = MutexFactory::instance()->createMutex(); +} + +QueueMapManager* QueueMapManager::instance() { + if (mqManagerInstance == nullptr){ + mqManagerInstance = new QueueMapManager(); + } + return QueueMapManager::mqManagerInstance; +} + +ReturnValue_t QueueMapManager::addMessageQueue( + MessageQueueIF* queueToInsert, MessageQueueId_t* id) { + // Not thread-safe, but it is assumed all message queues are created + // at software initialization now. If this is to be made thread-safe in + // the future, it propably would be sufficient to lock the increment + // operation here + uint32_t currentId = queueCounter++; + auto returnPair = queueMap.emplace(currentId, queueToInsert); + if(not returnPair.second) { + // this should never happen for the atomic variable. + sif::error << "QueueMapManager: This ID is already inside the map!" + << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + if (id != nullptr) { + *id = currentId; + } + return HasReturnvaluesIF::RETURN_OK; +} + +MessageQueueIF* QueueMapManager::getMessageQueue( + MessageQueueId_t messageQueueId) const { + MutexHelper(mapLock, 50); + auto queueIter = queueMap.find(messageQueueId); + if(queueIter != queueMap.end()) { + return queueIter->second; + } + else { + sif::warning << "QueueMapManager::getQueueHandle: The ID" << + messageQueueId << " does not exists in the map" << std::endl; + return nullptr; + } +} + diff --git a/osal/host/QueueMapManager.h b/osal/host/QueueMapManager.h index a2a1b658..acf20389 100644 --- a/osal/host/QueueMapManager.h +++ b/osal/host/QueueMapManager.h @@ -1,47 +1,47 @@ -#ifndef FRAMEWORK_OSAL_HOST_QUEUEMAP_H_ -#define FRAMEWORK_OSAL_HOST_QUEUEMAP_H_ - -#include -#include -#include -#include - -using QueueMap = std::unordered_map; - - -/** - * An internal map to map message queue IDs to message queues. - * This propably should be a singleton.. - */ -class QueueMapManager { -public: - //! Returns the single instance of SemaphoreFactory. - static QueueMapManager* instance(); - - /** - * Insert a message queue into the map and returns a message queue ID - * @param queue The message queue to insert. - * @param id The passed value will be set unless a nullptr is passed - * @return - */ - ReturnValue_t addMessageQueue(MessageQueueIF* queue, MessageQueueId_t* - id = nullptr); - /** - * Get the message queue handle by providing a message queue ID. - * @param messageQueueId - * @return - */ - MessageQueueIF* getMessageQueue(MessageQueueId_t messageQueueId) const; - -private: - //! External instantiation is forbidden. - QueueMapManager(); - uint32_t queueCounter = 1; - MutexIF* mapLock; - QueueMap queueMap; - static QueueMapManager* mqManagerInstance; -}; - - - -#endif /* FRAMEWORK_OSAL_HOST_QUEUEMAP_H_ */ +#ifndef FRAMEWORK_OSAL_HOST_QUEUEMAP_H_ +#define FRAMEWORK_OSAL_HOST_QUEUEMAP_H_ + +#include "../../ipc/MessageQueueSenderIF.h" +#include "../../osal/host/MessageQueue.h" +#include +#include + +using QueueMap = std::unordered_map; + + +/** + * An internal map to map message queue IDs to message queues. + * This propably should be a singleton.. + */ +class QueueMapManager { +public: + //! Returns the single instance of SemaphoreFactory. + static QueueMapManager* instance(); + + /** + * Insert a message queue into the map and returns a message queue ID + * @param queue The message queue to insert. + * @param id The passed value will be set unless a nullptr is passed + * @return + */ + ReturnValue_t addMessageQueue(MessageQueueIF* queue, MessageQueueId_t* + id = nullptr); + /** + * Get the message queue handle by providing a message queue ID. + * @param messageQueueId + * @return + */ + MessageQueueIF* getMessageQueue(MessageQueueId_t messageQueueId) const; + +private: + //! External instantiation is forbidden. + QueueMapManager(); + uint32_t queueCounter = 1; + MutexIF* mapLock; + QueueMap queueMap; + static QueueMapManager* mqManagerInstance; +}; + + + +#endif /* FRAMEWORK_OSAL_HOST_QUEUEMAP_H_ */ diff --git a/osal/host/SemaphoreFactory.cpp b/osal/host/SemaphoreFactory.cpp index 0c077f68..58040428 100644 --- a/osal/host/SemaphoreFactory.cpp +++ b/osal/host/SemaphoreFactory.cpp @@ -1,42 +1,42 @@ -#include -#include -#include -#include - -const uint32_t SemaphoreIF::POLLING = 0; -const uint32_t SemaphoreIF::BLOCKING = 0xFFFFFFFF; - -SemaphoreFactory* SemaphoreFactory::factoryInstance = nullptr; - -SemaphoreFactory::SemaphoreFactory() { -} - -SemaphoreFactory::~SemaphoreFactory() { - delete factoryInstance; -} - -SemaphoreFactory* SemaphoreFactory::instance() { - if (factoryInstance == nullptr){ - factoryInstance = new SemaphoreFactory(); - } - return SemaphoreFactory::factoryInstance; -} - -SemaphoreIF* SemaphoreFactory::createBinarySemaphore(uint32_t arguments) { - // Just gonna wait for full C++20 for now. - sif::error << "SemaphoreFactory: Binary Semaphore not implemented yet." - " Returning nullptr!\n" << std::flush; - return nullptr; -} - -SemaphoreIF* SemaphoreFactory::createCountingSemaphore(const uint8_t maxCount, - uint8_t initCount, uint32_t arguments) { - // Just gonna wait for full C++20 for now. - sif::error << "SemaphoreFactory: Counting Semaphore not implemented yet." - " Returning nullptr!\n" << std::flush; - return nullptr; -} - -void SemaphoreFactory::deleteSemaphore(SemaphoreIF* semaphore) { - delete semaphore; -} +#include "../../tasks/SemaphoreFactory.h" +#include "../../osal/linux/BinarySemaphore.h" +#include "../../osal/linux/CountingSemaphore.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" + +const uint32_t SemaphoreIF::POLLING = 0; +const uint32_t SemaphoreIF::BLOCKING = 0xFFFFFFFF; + +SemaphoreFactory* SemaphoreFactory::factoryInstance = nullptr; + +SemaphoreFactory::SemaphoreFactory() { +} + +SemaphoreFactory::~SemaphoreFactory() { + delete factoryInstance; +} + +SemaphoreFactory* SemaphoreFactory::instance() { + if (factoryInstance == nullptr){ + factoryInstance = new SemaphoreFactory(); + } + return SemaphoreFactory::factoryInstance; +} + +SemaphoreIF* SemaphoreFactory::createBinarySemaphore(uint32_t arguments) { + // Just gonna wait for full C++20 for now. + sif::error << "SemaphoreFactory: Binary Semaphore not implemented yet." + " Returning nullptr!\n" << std::flush; + return nullptr; +} + +SemaphoreIF* SemaphoreFactory::createCountingSemaphore(const uint8_t maxCount, + uint8_t initCount, uint32_t arguments) { + // Just gonna wait for full C++20 for now. + sif::error << "SemaphoreFactory: Counting Semaphore not implemented yet." + " Returning nullptr!\n" << std::flush; + return nullptr; +} + +void SemaphoreFactory::deleteSemaphore(SemaphoreIF* semaphore) { + delete semaphore; +} diff --git a/osal/host/TaskFactory.cpp b/osal/host/TaskFactory.cpp index 9db8ac4d..3f527130 100644 --- a/osal/host/TaskFactory.cpp +++ b/osal/host/TaskFactory.cpp @@ -1,55 +1,55 @@ -#include -#include -#include -#include -#include - -#include - -TaskFactory* TaskFactory::factoryInstance = new TaskFactory(); - -// Will propably not be used for hosted implementation -const size_t PeriodicTaskIF::MINIMUM_STACK_SIZE = 0; - -TaskFactory::TaskFactory() { -} - -TaskFactory::~TaskFactory() { -} - -TaskFactory* TaskFactory::instance() { - return TaskFactory::factoryInstance; -} - -PeriodicTaskIF* TaskFactory::createPeriodicTask(TaskName name_, - TaskPriority taskPriority_,TaskStackSize stackSize_, - TaskPeriod periodInSeconds_, - TaskDeadlineMissedFunction deadLineMissedFunction_) { - // This is going to be interesting. Time now learn the C++ threading library - // :-) - return new PeriodicTask(name_, taskPriority_, stackSize_, periodInSeconds_, - deadLineMissedFunction_); -} - -FixedTimeslotTaskIF* TaskFactory::createFixedTimeslotTask(TaskName name_, - TaskPriority taskPriority_,TaskStackSize stackSize_, - TaskPeriod periodInSeconds_, - TaskDeadlineMissedFunction deadLineMissedFunction_) { - // This is going to be interesting. Time now learn the C++ threading library - // :-) - return new FixedTimeslotTask(name_, taskPriority_, stackSize_, - periodInSeconds_, deadLineMissedFunction_); -} - -ReturnValue_t TaskFactory::deleteTask(PeriodicTaskIF* task) { - // This might block for some time! - delete task; - return HasReturnvaluesIF::RETURN_FAILED; -} - -ReturnValue_t TaskFactory::delayTask(uint32_t delayMs){ - std::this_thread::sleep_for(std::chrono::milliseconds(delayMs)); - return HasReturnvaluesIF::RETURN_OK; -} - - +#include "../../osal/host/FixedTimeslotTask.h" +#include "../../osal/host/PeriodicTask.h" +#include "../../tasks/TaskFactory.h" +#include "../../returnvalues/HasReturnvaluesIF.h" +#include "../../tasks/PeriodicTaskIF.h" + +#include + +TaskFactory* TaskFactory::factoryInstance = new TaskFactory(); + +// Will propably not be used for hosted implementation +const size_t PeriodicTaskIF::MINIMUM_STACK_SIZE = 0; + +TaskFactory::TaskFactory() { +} + +TaskFactory::~TaskFactory() { +} + +TaskFactory* TaskFactory::instance() { + return TaskFactory::factoryInstance; +} + +PeriodicTaskIF* TaskFactory::createPeriodicTask(TaskName name_, + TaskPriority taskPriority_,TaskStackSize stackSize_, + TaskPeriod periodInSeconds_, + TaskDeadlineMissedFunction deadLineMissedFunction_) { + // This is going to be interesting. Time now learn the C++ threading library + // :-) + return new PeriodicTask(name_, taskPriority_, stackSize_, periodInSeconds_, + deadLineMissedFunction_); +} + +FixedTimeslotTaskIF* TaskFactory::createFixedTimeslotTask(TaskName name_, + TaskPriority taskPriority_,TaskStackSize stackSize_, + TaskPeriod periodInSeconds_, + TaskDeadlineMissedFunction deadLineMissedFunction_) { + // This is going to be interesting. Time now learn the C++ threading library + // :-) + return new FixedTimeslotTask(name_, taskPriority_, stackSize_, + periodInSeconds_, deadLineMissedFunction_); +} + +ReturnValue_t TaskFactory::deleteTask(PeriodicTaskIF* task) { + // This might block for some time! + delete task; + return HasReturnvaluesIF::RETURN_FAILED; +} + +ReturnValue_t TaskFactory::delayTask(uint32_t delayMs){ + std::this_thread::sleep_for(std::chrono::milliseconds(delayMs)); + return HasReturnvaluesIF::RETURN_OK; +} + + From 2748a8b93b9d6fa9663cea0bd57c94e46d7f0f15 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Wed, 26 Aug 2020 17:42:13 +0200 Subject: [PATCH 03/20] added doc to dle encoder --- globalfunctions/DleEncoder.cpp | 219 +++++++++++++++++++-------------- globalfunctions/DleEncoder.h | 104 ++++++++++++---- 2 files changed, 203 insertions(+), 120 deletions(-) diff --git a/globalfunctions/DleEncoder.cpp b/globalfunctions/DleEncoder.cpp index 088c9d80..21dbd2e6 100644 --- a/globalfunctions/DleEncoder.cpp +++ b/globalfunctions/DleEncoder.cpp @@ -1,95 +1,124 @@ -#include "DleEncoder.h" - -DleEncoder::DleEncoder() { -} - -DleEncoder::~DleEncoder() { -} - -ReturnValue_t DleEncoder::decode(const uint8_t *sourceStream, - uint32_t sourceStreamLen, uint32_t *readLen, uint8_t *destStream, - uint32_t maxDestStreamlen, uint32_t *decodedLen) { - uint32_t encodedIndex = 0, decodedIndex = 0; - uint8_t nextByte; - if (*sourceStream != STX) { - return RETURN_FAILED; - } - ++encodedIndex; - while ((encodedIndex < sourceStreamLen) && (decodedIndex < maxDestStreamlen) - && (sourceStream[encodedIndex] != ETX) - && (sourceStream[encodedIndex] != STX)) { - if (sourceStream[encodedIndex] == DLE) { - nextByte = sourceStream[encodedIndex + 1]; - if (nextByte == 0x10) { - destStream[decodedIndex] = nextByte; - } else { - if ((nextByte == 0x42) || (nextByte == 0x43) - || (nextByte == 0x4D)) { - destStream[decodedIndex] = nextByte - 0x40; - } else { - return RETURN_FAILED; - } - } - ++encodedIndex; - } else { - destStream[decodedIndex] = sourceStream[encodedIndex]; - } - ++encodedIndex; - ++decodedIndex; - } - if (sourceStream[encodedIndex] != ETX) { - return RETURN_FAILED; - } else { - *readLen = ++encodedIndex; - *decodedLen = decodedIndex; - return RETURN_OK; - } -} - -ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream, - uint32_t sourceLen, uint8_t* destStream, uint32_t maxDestLen, - uint32_t* encodedLen, bool addStxEtx) { - if (maxDestLen < 2) { - return RETURN_FAILED; - } - uint32_t encodedIndex = 0, sourceIndex = 0; - uint8_t nextByte; - if (addStxEtx) { - destStream[0] = STX; - ++encodedIndex; - } - while ((encodedIndex < maxDestLen) && (sourceIndex < sourceLen)) { - nextByte = sourceStream[sourceIndex]; - if ((nextByte == STX) || (nextByte == ETX) || (nextByte == 0x0D)) { - if (encodedIndex + 1 >= maxDestLen) { - return RETURN_FAILED; - } else { - destStream[encodedIndex] = DLE; - ++encodedIndex; - destStream[encodedIndex] = nextByte + 0x40; - } - } else if (nextByte == DLE) { - if (encodedIndex + 1 >= maxDestLen) { - return RETURN_FAILED; - } else { - destStream[encodedIndex] = DLE; - ++encodedIndex; - destStream[encodedIndex] = DLE; - } - } else { - destStream[encodedIndex] = nextByte; - } - ++encodedIndex; - ++sourceIndex; - } - if ((sourceIndex == sourceLen) && (encodedIndex < maxDestLen)) { - if (addStxEtx) { - destStream[encodedIndex] = ETX; - ++encodedIndex; - } - *encodedLen = encodedIndex; - return RETURN_OK; - } else { - return RETURN_FAILED; - } -} +#include "../globalfunctions/DleEncoder.h" + +DleEncoder::DleEncoder() {} + +DleEncoder::~DleEncoder() {} + +ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream, + size_t sourceLen, uint8_t* destStream, size_t maxDestLen, + size_t* encodedLen, bool addStxEtx) { + if (maxDestLen < 2) { + return STREAM_TOO_SHORT; + } + size_t encodedIndex = 0, sourceIndex = 0; + uint8_t nextByte; + if (addStxEtx) { + destStream[0] = STX_CHAR; + ++encodedIndex; + } + + while (encodedIndex < maxDestLen and sourceIndex < sourceLen) + { + nextByte = sourceStream[sourceIndex]; + // STX, ETX and CR characters in the stream need to be escaped with DLE + if (nextByte == STX_CHAR or nextByte == ETX_CHAR or nextByte == CARRIAGE_RETURN) { + if (encodedIndex + 1 >= maxDestLen) { + return STREAM_TOO_SHORT; + } + else { + destStream[encodedIndex] = DLE_CHAR; + ++encodedIndex; + /* Escaped byte will be actual byte + 0x40. This prevents + * STX, ETX, and carriage return characters from appearing + * in the encoded data stream at all, so when polling an + * encoded stream, the transmission can be stopped at ETX. + * 0x40 was chosen at random with special requirements: + * - Prevent going from one control char to another + * - Prevent overflow for common characters */ + destStream[encodedIndex] = nextByte + 0x40; + } + } + // DLE characters are simply escaped with DLE. + else if (nextByte == DLE_CHAR) { + if (encodedIndex + 1 >= maxDestLen) { + return STREAM_TOO_SHORT; + } + else { + destStream[encodedIndex] = DLE_CHAR; + ++encodedIndex; + destStream[encodedIndex] = DLE_CHAR; + } + } + else { + destStream[encodedIndex] = nextByte; + } + ++encodedIndex; + ++sourceIndex; + } + + if (sourceIndex == sourceLen and encodedIndex < maxDestLen) { + if (addStxEtx) { + destStream[encodedIndex] = ETX_CHAR; + ++encodedIndex; + } + *encodedLen = encodedIndex; + return RETURN_OK; + } + else { + return STREAM_TOO_SHORT; + } +} + +ReturnValue_t DleEncoder::decode(const uint8_t *sourceStream, + size_t sourceStreamLen, size_t *readLen, uint8_t *destStream, + size_t maxDestStreamlen, size_t *decodedLen) { + size_t encodedIndex = 0, decodedIndex = 0; + uint8_t nextByte; + if (*sourceStream != STX_CHAR) { + return DECODING_ERROR; + } + ++encodedIndex; + + while ((encodedIndex < sourceStreamLen) && (decodedIndex < maxDestStreamlen) + && (sourceStream[encodedIndex] != ETX_CHAR) + && (sourceStream[encodedIndex] != STX_CHAR)) { + if (sourceStream[encodedIndex] == DLE_CHAR) { + nextByte = sourceStream[encodedIndex + 1]; + // The next byte is a DLE character that was escaped by another + // DLE character, so we can write it to the destination stream. + if (nextByte == DLE_CHAR) { + destStream[decodedIndex] = nextByte; + } + else { + /* The next byte is a STX, DTX or 0x0D character which + * was escaped by a DLE character. The actual byte was + * also encoded by adding + 0x40 to preven having control chars, + * in the stream at all, so we convert it back. */ + if (nextByte == 0x42 or nextByte == 0x43 or nextByte == 0x4D) { + destStream[decodedIndex] = nextByte - 0x40; + } + else { + return DECODING_ERROR; + } + } + ++encodedIndex; + } + else { + destStream[decodedIndex] = sourceStream[encodedIndex]; + } + + ++encodedIndex; + ++decodedIndex; + } + + if (sourceStream[encodedIndex] != ETX_CHAR) { + *readLen = ++encodedIndex; + return DECODING_ERROR; + } + else { + *readLen = ++encodedIndex; + *decodedLen = decodedIndex; + return RETURN_OK; + } +} + diff --git a/globalfunctions/DleEncoder.h b/globalfunctions/DleEncoder.h index ff3a2727..3c327a55 100644 --- a/globalfunctions/DleEncoder.h +++ b/globalfunctions/DleEncoder.h @@ -1,25 +1,79 @@ -#ifndef DLEENCODER_H_ -#define DLEENCODER_H_ - -#include "../returnvalues/HasReturnvaluesIF.h" - -class DleEncoder: public HasReturnvaluesIF { -private: - DleEncoder(); - virtual ~DleEncoder(); - -public: - static const uint8_t STX = 0x02; - static const uint8_t ETX = 0x03; - static const uint8_t DLE = 0x10; - - static ReturnValue_t decode(const uint8_t *sourceStream, - uint32_t sourceStreamLen, uint32_t *readLen, uint8_t *destStream, - uint32_t maxDestStreamlen, uint32_t *decodedLen); - - static ReturnValue_t encode(const uint8_t *sourceStream, uint32_t sourceLen, - uint8_t *destStream, uint32_t maxDestLen, uint32_t *encodedLen, - bool addStxEtx = true); -}; - -#endif /* DLEENCODER_H_ */ +#ifndef FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_ +#define FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_ + +#include "../returnvalues/HasReturnvaluesIF.h" +#include + +/** + * @brief This DLE Encoder (Data Link Encoder) can be used to encode and + * decode arbitrary data with ASCII control characters + * @details + * List of control codes: + * https://en.wikipedia.org/wiki/C0_and_C1_control_codes + * + * This encoder can be used to achieve a basic transport layer when using + * char based transmission systems. + * The passed source strean is converted into a encoded stream by adding + * a STX marker at the start of the stream and an ETX marker at the end of + * the stream. Any STX, ETX, DLE and CR occurences in the source stream are + * escaped by a DLE character. The encoder also replaces escaped control chars + * by another char, so STX, ETX and CR should not appear anywhere in the actual + * encoded data stream. + * + * When using a strictly char based reception of packets enoded with DLE, + * STX can be used to notify a reader that actual data will start to arrive + * while ETX can be used to notify the reader that the data has ended. + */ +class DleEncoder: public HasReturnvaluesIF { +private: + DleEncoder(); + virtual ~DleEncoder(); + +public: + static constexpr uint8_t INTERFACE_ID = CLASS_ID::DLE_ENCODER; + static constexpr ReturnValue_t STREAM_TOO_SHORT = MAKE_RETURN_CODE(0x01); + static constexpr ReturnValue_t DECODING_ERROR = MAKE_RETURN_CODE(0x02); + + //! Start Of Text character. First character is encoded stream + static constexpr uint8_t STX_CHAR = 0x02; + //! End Of Text character. Last character in encoded stream + static constexpr uint8_t ETX_CHAR = 0x03; + //! Data Link Escape character. Used to escape STX, ETX and DLE occurences + //! in the source stream. + static constexpr uint8_t DLE_CHAR = 0x10; + static constexpr uint8_t CARRIAGE_RETURN = 0x0D; + + /** + * Encodes the give data stream by preceding it with the STX marker + * and ending it with an ETX marker. STX, ETX and DLE characters inside + * the stream are escaped by DLE characters and also replaced by adding + * 0x40 (which is reverted in the decoing process). + * @param sourceStream + * @param sourceLen + * @param destStream + * @param maxDestLen + * @param encodedLen + * @param addStxEtx + * Adding STX and ETX can be omitted, if they are added manually. + * @return + */ + static ReturnValue_t encode(const uint8_t *sourceStream, size_t sourceLen, + uint8_t *destStream, size_t maxDestLen, size_t *encodedLen, + bool addStxEtx = true); + + /** + * Converts an encoded stream back. + * @param sourceStream + * @param sourceStreamLen + * @param readLen + * @param destStream + * @param maxDestStreamlen + * @param decodedLen + * @return + */ + static ReturnValue_t decode(const uint8_t *sourceStream, + size_t sourceStreamLen, size_t *readLen, uint8_t *destStream, + size_t maxDestStreamlen, size_t *decodedLen); +}; + +#endif /* FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_ */ From a8f2cf910b025c22f93d1ca1f070285e6d48773f Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Wed, 26 Aug 2020 17:43:20 +0200 Subject: [PATCH 04/20] taken over dle encoder from upstream master --- globalfunctions/DleEncoder.cpp | 219 ++++++++++++++------------------- globalfunctions/DleEncoder.h | 104 ++++------------ 2 files changed, 120 insertions(+), 203 deletions(-) diff --git a/globalfunctions/DleEncoder.cpp b/globalfunctions/DleEncoder.cpp index 21dbd2e6..088c9d80 100644 --- a/globalfunctions/DleEncoder.cpp +++ b/globalfunctions/DleEncoder.cpp @@ -1,124 +1,95 @@ -#include "../globalfunctions/DleEncoder.h" - -DleEncoder::DleEncoder() {} - -DleEncoder::~DleEncoder() {} - -ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream, - size_t sourceLen, uint8_t* destStream, size_t maxDestLen, - size_t* encodedLen, bool addStxEtx) { - if (maxDestLen < 2) { - return STREAM_TOO_SHORT; - } - size_t encodedIndex = 0, sourceIndex = 0; - uint8_t nextByte; - if (addStxEtx) { - destStream[0] = STX_CHAR; - ++encodedIndex; - } - - while (encodedIndex < maxDestLen and sourceIndex < sourceLen) - { - nextByte = sourceStream[sourceIndex]; - // STX, ETX and CR characters in the stream need to be escaped with DLE - if (nextByte == STX_CHAR or nextByte == ETX_CHAR or nextByte == CARRIAGE_RETURN) { - if (encodedIndex + 1 >= maxDestLen) { - return STREAM_TOO_SHORT; - } - else { - destStream[encodedIndex] = DLE_CHAR; - ++encodedIndex; - /* Escaped byte will be actual byte + 0x40. This prevents - * STX, ETX, and carriage return characters from appearing - * in the encoded data stream at all, so when polling an - * encoded stream, the transmission can be stopped at ETX. - * 0x40 was chosen at random with special requirements: - * - Prevent going from one control char to another - * - Prevent overflow for common characters */ - destStream[encodedIndex] = nextByte + 0x40; - } - } - // DLE characters are simply escaped with DLE. - else if (nextByte == DLE_CHAR) { - if (encodedIndex + 1 >= maxDestLen) { - return STREAM_TOO_SHORT; - } - else { - destStream[encodedIndex] = DLE_CHAR; - ++encodedIndex; - destStream[encodedIndex] = DLE_CHAR; - } - } - else { - destStream[encodedIndex] = nextByte; - } - ++encodedIndex; - ++sourceIndex; - } - - if (sourceIndex == sourceLen and encodedIndex < maxDestLen) { - if (addStxEtx) { - destStream[encodedIndex] = ETX_CHAR; - ++encodedIndex; - } - *encodedLen = encodedIndex; - return RETURN_OK; - } - else { - return STREAM_TOO_SHORT; - } -} - -ReturnValue_t DleEncoder::decode(const uint8_t *sourceStream, - size_t sourceStreamLen, size_t *readLen, uint8_t *destStream, - size_t maxDestStreamlen, size_t *decodedLen) { - size_t encodedIndex = 0, decodedIndex = 0; - uint8_t nextByte; - if (*sourceStream != STX_CHAR) { - return DECODING_ERROR; - } - ++encodedIndex; - - while ((encodedIndex < sourceStreamLen) && (decodedIndex < maxDestStreamlen) - && (sourceStream[encodedIndex] != ETX_CHAR) - && (sourceStream[encodedIndex] != STX_CHAR)) { - if (sourceStream[encodedIndex] == DLE_CHAR) { - nextByte = sourceStream[encodedIndex + 1]; - // The next byte is a DLE character that was escaped by another - // DLE character, so we can write it to the destination stream. - if (nextByte == DLE_CHAR) { - destStream[decodedIndex] = nextByte; - } - else { - /* The next byte is a STX, DTX or 0x0D character which - * was escaped by a DLE character. The actual byte was - * also encoded by adding + 0x40 to preven having control chars, - * in the stream at all, so we convert it back. */ - if (nextByte == 0x42 or nextByte == 0x43 or nextByte == 0x4D) { - destStream[decodedIndex] = nextByte - 0x40; - } - else { - return DECODING_ERROR; - } - } - ++encodedIndex; - } - else { - destStream[decodedIndex] = sourceStream[encodedIndex]; - } - - ++encodedIndex; - ++decodedIndex; - } - - if (sourceStream[encodedIndex] != ETX_CHAR) { - *readLen = ++encodedIndex; - return DECODING_ERROR; - } - else { - *readLen = ++encodedIndex; - *decodedLen = decodedIndex; - return RETURN_OK; - } -} - +#include "DleEncoder.h" + +DleEncoder::DleEncoder() { +} + +DleEncoder::~DleEncoder() { +} + +ReturnValue_t DleEncoder::decode(const uint8_t *sourceStream, + uint32_t sourceStreamLen, uint32_t *readLen, uint8_t *destStream, + uint32_t maxDestStreamlen, uint32_t *decodedLen) { + uint32_t encodedIndex = 0, decodedIndex = 0; + uint8_t nextByte; + if (*sourceStream != STX) { + return RETURN_FAILED; + } + ++encodedIndex; + while ((encodedIndex < sourceStreamLen) && (decodedIndex < maxDestStreamlen) + && (sourceStream[encodedIndex] != ETX) + && (sourceStream[encodedIndex] != STX)) { + if (sourceStream[encodedIndex] == DLE) { + nextByte = sourceStream[encodedIndex + 1]; + if (nextByte == 0x10) { + destStream[decodedIndex] = nextByte; + } else { + if ((nextByte == 0x42) || (nextByte == 0x43) + || (nextByte == 0x4D)) { + destStream[decodedIndex] = nextByte - 0x40; + } else { + return RETURN_FAILED; + } + } + ++encodedIndex; + } else { + destStream[decodedIndex] = sourceStream[encodedIndex]; + } + ++encodedIndex; + ++decodedIndex; + } + if (sourceStream[encodedIndex] != ETX) { + return RETURN_FAILED; + } else { + *readLen = ++encodedIndex; + *decodedLen = decodedIndex; + return RETURN_OK; + } +} + +ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream, + uint32_t sourceLen, uint8_t* destStream, uint32_t maxDestLen, + uint32_t* encodedLen, bool addStxEtx) { + if (maxDestLen < 2) { + return RETURN_FAILED; + } + uint32_t encodedIndex = 0, sourceIndex = 0; + uint8_t nextByte; + if (addStxEtx) { + destStream[0] = STX; + ++encodedIndex; + } + while ((encodedIndex < maxDestLen) && (sourceIndex < sourceLen)) { + nextByte = sourceStream[sourceIndex]; + if ((nextByte == STX) || (nextByte == ETX) || (nextByte == 0x0D)) { + if (encodedIndex + 1 >= maxDestLen) { + return RETURN_FAILED; + } else { + destStream[encodedIndex] = DLE; + ++encodedIndex; + destStream[encodedIndex] = nextByte + 0x40; + } + } else if (nextByte == DLE) { + if (encodedIndex + 1 >= maxDestLen) { + return RETURN_FAILED; + } else { + destStream[encodedIndex] = DLE; + ++encodedIndex; + destStream[encodedIndex] = DLE; + } + } else { + destStream[encodedIndex] = nextByte; + } + ++encodedIndex; + ++sourceIndex; + } + if ((sourceIndex == sourceLen) && (encodedIndex < maxDestLen)) { + if (addStxEtx) { + destStream[encodedIndex] = ETX; + ++encodedIndex; + } + *encodedLen = encodedIndex; + return RETURN_OK; + } else { + return RETURN_FAILED; + } +} diff --git a/globalfunctions/DleEncoder.h b/globalfunctions/DleEncoder.h index 3c327a55..ff3a2727 100644 --- a/globalfunctions/DleEncoder.h +++ b/globalfunctions/DleEncoder.h @@ -1,79 +1,25 @@ -#ifndef FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_ -#define FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_ - -#include "../returnvalues/HasReturnvaluesIF.h" -#include - -/** - * @brief This DLE Encoder (Data Link Encoder) can be used to encode and - * decode arbitrary data with ASCII control characters - * @details - * List of control codes: - * https://en.wikipedia.org/wiki/C0_and_C1_control_codes - * - * This encoder can be used to achieve a basic transport layer when using - * char based transmission systems. - * The passed source strean is converted into a encoded stream by adding - * a STX marker at the start of the stream and an ETX marker at the end of - * the stream. Any STX, ETX, DLE and CR occurences in the source stream are - * escaped by a DLE character. The encoder also replaces escaped control chars - * by another char, so STX, ETX and CR should not appear anywhere in the actual - * encoded data stream. - * - * When using a strictly char based reception of packets enoded with DLE, - * STX can be used to notify a reader that actual data will start to arrive - * while ETX can be used to notify the reader that the data has ended. - */ -class DleEncoder: public HasReturnvaluesIF { -private: - DleEncoder(); - virtual ~DleEncoder(); - -public: - static constexpr uint8_t INTERFACE_ID = CLASS_ID::DLE_ENCODER; - static constexpr ReturnValue_t STREAM_TOO_SHORT = MAKE_RETURN_CODE(0x01); - static constexpr ReturnValue_t DECODING_ERROR = MAKE_RETURN_CODE(0x02); - - //! Start Of Text character. First character is encoded stream - static constexpr uint8_t STX_CHAR = 0x02; - //! End Of Text character. Last character in encoded stream - static constexpr uint8_t ETX_CHAR = 0x03; - //! Data Link Escape character. Used to escape STX, ETX and DLE occurences - //! in the source stream. - static constexpr uint8_t DLE_CHAR = 0x10; - static constexpr uint8_t CARRIAGE_RETURN = 0x0D; - - /** - * Encodes the give data stream by preceding it with the STX marker - * and ending it with an ETX marker. STX, ETX and DLE characters inside - * the stream are escaped by DLE characters and also replaced by adding - * 0x40 (which is reverted in the decoing process). - * @param sourceStream - * @param sourceLen - * @param destStream - * @param maxDestLen - * @param encodedLen - * @param addStxEtx - * Adding STX and ETX can be omitted, if they are added manually. - * @return - */ - static ReturnValue_t encode(const uint8_t *sourceStream, size_t sourceLen, - uint8_t *destStream, size_t maxDestLen, size_t *encodedLen, - bool addStxEtx = true); - - /** - * Converts an encoded stream back. - * @param sourceStream - * @param sourceStreamLen - * @param readLen - * @param destStream - * @param maxDestStreamlen - * @param decodedLen - * @return - */ - static ReturnValue_t decode(const uint8_t *sourceStream, - size_t sourceStreamLen, size_t *readLen, uint8_t *destStream, - size_t maxDestStreamlen, size_t *decodedLen); -}; - -#endif /* FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_ */ +#ifndef DLEENCODER_H_ +#define DLEENCODER_H_ + +#include "../returnvalues/HasReturnvaluesIF.h" + +class DleEncoder: public HasReturnvaluesIF { +private: + DleEncoder(); + virtual ~DleEncoder(); + +public: + static const uint8_t STX = 0x02; + static const uint8_t ETX = 0x03; + static const uint8_t DLE = 0x10; + + static ReturnValue_t decode(const uint8_t *sourceStream, + uint32_t sourceStreamLen, uint32_t *readLen, uint8_t *destStream, + uint32_t maxDestStreamlen, uint32_t *decodedLen); + + static ReturnValue_t encode(const uint8_t *sourceStream, uint32_t sourceLen, + uint8_t *destStream, uint32_t maxDestLen, uint32_t *encodedLen, + bool addStxEtx = true); +}; + +#endif /* DLEENCODER_H_ */ From df850116ffc924ff823e1238868994c594249094 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Wed, 26 Aug 2020 17:43:47 +0200 Subject: [PATCH 05/20] dle encoder doc added --- globalfunctions/DleEncoder.cpp | 219 +++++++++++++++++++-------------- globalfunctions/DleEncoder.h | 104 ++++++++++++---- 2 files changed, 203 insertions(+), 120 deletions(-) diff --git a/globalfunctions/DleEncoder.cpp b/globalfunctions/DleEncoder.cpp index 088c9d80..21dbd2e6 100644 --- a/globalfunctions/DleEncoder.cpp +++ b/globalfunctions/DleEncoder.cpp @@ -1,95 +1,124 @@ -#include "DleEncoder.h" - -DleEncoder::DleEncoder() { -} - -DleEncoder::~DleEncoder() { -} - -ReturnValue_t DleEncoder::decode(const uint8_t *sourceStream, - uint32_t sourceStreamLen, uint32_t *readLen, uint8_t *destStream, - uint32_t maxDestStreamlen, uint32_t *decodedLen) { - uint32_t encodedIndex = 0, decodedIndex = 0; - uint8_t nextByte; - if (*sourceStream != STX) { - return RETURN_FAILED; - } - ++encodedIndex; - while ((encodedIndex < sourceStreamLen) && (decodedIndex < maxDestStreamlen) - && (sourceStream[encodedIndex] != ETX) - && (sourceStream[encodedIndex] != STX)) { - if (sourceStream[encodedIndex] == DLE) { - nextByte = sourceStream[encodedIndex + 1]; - if (nextByte == 0x10) { - destStream[decodedIndex] = nextByte; - } else { - if ((nextByte == 0x42) || (nextByte == 0x43) - || (nextByte == 0x4D)) { - destStream[decodedIndex] = nextByte - 0x40; - } else { - return RETURN_FAILED; - } - } - ++encodedIndex; - } else { - destStream[decodedIndex] = sourceStream[encodedIndex]; - } - ++encodedIndex; - ++decodedIndex; - } - if (sourceStream[encodedIndex] != ETX) { - return RETURN_FAILED; - } else { - *readLen = ++encodedIndex; - *decodedLen = decodedIndex; - return RETURN_OK; - } -} - -ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream, - uint32_t sourceLen, uint8_t* destStream, uint32_t maxDestLen, - uint32_t* encodedLen, bool addStxEtx) { - if (maxDestLen < 2) { - return RETURN_FAILED; - } - uint32_t encodedIndex = 0, sourceIndex = 0; - uint8_t nextByte; - if (addStxEtx) { - destStream[0] = STX; - ++encodedIndex; - } - while ((encodedIndex < maxDestLen) && (sourceIndex < sourceLen)) { - nextByte = sourceStream[sourceIndex]; - if ((nextByte == STX) || (nextByte == ETX) || (nextByte == 0x0D)) { - if (encodedIndex + 1 >= maxDestLen) { - return RETURN_FAILED; - } else { - destStream[encodedIndex] = DLE; - ++encodedIndex; - destStream[encodedIndex] = nextByte + 0x40; - } - } else if (nextByte == DLE) { - if (encodedIndex + 1 >= maxDestLen) { - return RETURN_FAILED; - } else { - destStream[encodedIndex] = DLE; - ++encodedIndex; - destStream[encodedIndex] = DLE; - } - } else { - destStream[encodedIndex] = nextByte; - } - ++encodedIndex; - ++sourceIndex; - } - if ((sourceIndex == sourceLen) && (encodedIndex < maxDestLen)) { - if (addStxEtx) { - destStream[encodedIndex] = ETX; - ++encodedIndex; - } - *encodedLen = encodedIndex; - return RETURN_OK; - } else { - return RETURN_FAILED; - } -} +#include "../globalfunctions/DleEncoder.h" + +DleEncoder::DleEncoder() {} + +DleEncoder::~DleEncoder() {} + +ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream, + size_t sourceLen, uint8_t* destStream, size_t maxDestLen, + size_t* encodedLen, bool addStxEtx) { + if (maxDestLen < 2) { + return STREAM_TOO_SHORT; + } + size_t encodedIndex = 0, sourceIndex = 0; + uint8_t nextByte; + if (addStxEtx) { + destStream[0] = STX_CHAR; + ++encodedIndex; + } + + while (encodedIndex < maxDestLen and sourceIndex < sourceLen) + { + nextByte = sourceStream[sourceIndex]; + // STX, ETX and CR characters in the stream need to be escaped with DLE + if (nextByte == STX_CHAR or nextByte == ETX_CHAR or nextByte == CARRIAGE_RETURN) { + if (encodedIndex + 1 >= maxDestLen) { + return STREAM_TOO_SHORT; + } + else { + destStream[encodedIndex] = DLE_CHAR; + ++encodedIndex; + /* Escaped byte will be actual byte + 0x40. This prevents + * STX, ETX, and carriage return characters from appearing + * in the encoded data stream at all, so when polling an + * encoded stream, the transmission can be stopped at ETX. + * 0x40 was chosen at random with special requirements: + * - Prevent going from one control char to another + * - Prevent overflow for common characters */ + destStream[encodedIndex] = nextByte + 0x40; + } + } + // DLE characters are simply escaped with DLE. + else if (nextByte == DLE_CHAR) { + if (encodedIndex + 1 >= maxDestLen) { + return STREAM_TOO_SHORT; + } + else { + destStream[encodedIndex] = DLE_CHAR; + ++encodedIndex; + destStream[encodedIndex] = DLE_CHAR; + } + } + else { + destStream[encodedIndex] = nextByte; + } + ++encodedIndex; + ++sourceIndex; + } + + if (sourceIndex == sourceLen and encodedIndex < maxDestLen) { + if (addStxEtx) { + destStream[encodedIndex] = ETX_CHAR; + ++encodedIndex; + } + *encodedLen = encodedIndex; + return RETURN_OK; + } + else { + return STREAM_TOO_SHORT; + } +} + +ReturnValue_t DleEncoder::decode(const uint8_t *sourceStream, + size_t sourceStreamLen, size_t *readLen, uint8_t *destStream, + size_t maxDestStreamlen, size_t *decodedLen) { + size_t encodedIndex = 0, decodedIndex = 0; + uint8_t nextByte; + if (*sourceStream != STX_CHAR) { + return DECODING_ERROR; + } + ++encodedIndex; + + while ((encodedIndex < sourceStreamLen) && (decodedIndex < maxDestStreamlen) + && (sourceStream[encodedIndex] != ETX_CHAR) + && (sourceStream[encodedIndex] != STX_CHAR)) { + if (sourceStream[encodedIndex] == DLE_CHAR) { + nextByte = sourceStream[encodedIndex + 1]; + // The next byte is a DLE character that was escaped by another + // DLE character, so we can write it to the destination stream. + if (nextByte == DLE_CHAR) { + destStream[decodedIndex] = nextByte; + } + else { + /* The next byte is a STX, DTX or 0x0D character which + * was escaped by a DLE character. The actual byte was + * also encoded by adding + 0x40 to preven having control chars, + * in the stream at all, so we convert it back. */ + if (nextByte == 0x42 or nextByte == 0x43 or nextByte == 0x4D) { + destStream[decodedIndex] = nextByte - 0x40; + } + else { + return DECODING_ERROR; + } + } + ++encodedIndex; + } + else { + destStream[decodedIndex] = sourceStream[encodedIndex]; + } + + ++encodedIndex; + ++decodedIndex; + } + + if (sourceStream[encodedIndex] != ETX_CHAR) { + *readLen = ++encodedIndex; + return DECODING_ERROR; + } + else { + *readLen = ++encodedIndex; + *decodedLen = decodedIndex; + return RETURN_OK; + } +} + diff --git a/globalfunctions/DleEncoder.h b/globalfunctions/DleEncoder.h index ff3a2727..3c327a55 100644 --- a/globalfunctions/DleEncoder.h +++ b/globalfunctions/DleEncoder.h @@ -1,25 +1,79 @@ -#ifndef DLEENCODER_H_ -#define DLEENCODER_H_ - -#include "../returnvalues/HasReturnvaluesIF.h" - -class DleEncoder: public HasReturnvaluesIF { -private: - DleEncoder(); - virtual ~DleEncoder(); - -public: - static const uint8_t STX = 0x02; - static const uint8_t ETX = 0x03; - static const uint8_t DLE = 0x10; - - static ReturnValue_t decode(const uint8_t *sourceStream, - uint32_t sourceStreamLen, uint32_t *readLen, uint8_t *destStream, - uint32_t maxDestStreamlen, uint32_t *decodedLen); - - static ReturnValue_t encode(const uint8_t *sourceStream, uint32_t sourceLen, - uint8_t *destStream, uint32_t maxDestLen, uint32_t *encodedLen, - bool addStxEtx = true); -}; - -#endif /* DLEENCODER_H_ */ +#ifndef FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_ +#define FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_ + +#include "../returnvalues/HasReturnvaluesIF.h" +#include + +/** + * @brief This DLE Encoder (Data Link Encoder) can be used to encode and + * decode arbitrary data with ASCII control characters + * @details + * List of control codes: + * https://en.wikipedia.org/wiki/C0_and_C1_control_codes + * + * This encoder can be used to achieve a basic transport layer when using + * char based transmission systems. + * The passed source strean is converted into a encoded stream by adding + * a STX marker at the start of the stream and an ETX marker at the end of + * the stream. Any STX, ETX, DLE and CR occurences in the source stream are + * escaped by a DLE character. The encoder also replaces escaped control chars + * by another char, so STX, ETX and CR should not appear anywhere in the actual + * encoded data stream. + * + * When using a strictly char based reception of packets enoded with DLE, + * STX can be used to notify a reader that actual data will start to arrive + * while ETX can be used to notify the reader that the data has ended. + */ +class DleEncoder: public HasReturnvaluesIF { +private: + DleEncoder(); + virtual ~DleEncoder(); + +public: + static constexpr uint8_t INTERFACE_ID = CLASS_ID::DLE_ENCODER; + static constexpr ReturnValue_t STREAM_TOO_SHORT = MAKE_RETURN_CODE(0x01); + static constexpr ReturnValue_t DECODING_ERROR = MAKE_RETURN_CODE(0x02); + + //! Start Of Text character. First character is encoded stream + static constexpr uint8_t STX_CHAR = 0x02; + //! End Of Text character. Last character in encoded stream + static constexpr uint8_t ETX_CHAR = 0x03; + //! Data Link Escape character. Used to escape STX, ETX and DLE occurences + //! in the source stream. + static constexpr uint8_t DLE_CHAR = 0x10; + static constexpr uint8_t CARRIAGE_RETURN = 0x0D; + + /** + * Encodes the give data stream by preceding it with the STX marker + * and ending it with an ETX marker. STX, ETX and DLE characters inside + * the stream are escaped by DLE characters and also replaced by adding + * 0x40 (which is reverted in the decoing process). + * @param sourceStream + * @param sourceLen + * @param destStream + * @param maxDestLen + * @param encodedLen + * @param addStxEtx + * Adding STX and ETX can be omitted, if they are added manually. + * @return + */ + static ReturnValue_t encode(const uint8_t *sourceStream, size_t sourceLen, + uint8_t *destStream, size_t maxDestLen, size_t *encodedLen, + bool addStxEtx = true); + + /** + * Converts an encoded stream back. + * @param sourceStream + * @param sourceStreamLen + * @param readLen + * @param destStream + * @param maxDestStreamlen + * @param decodedLen + * @return + */ + static ReturnValue_t decode(const uint8_t *sourceStream, + size_t sourceStreamLen, size_t *readLen, uint8_t *destStream, + size_t maxDestStreamlen, size_t *decodedLen); +}; + +#endif /* FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_ */ From 281f13e27ba915bc987f7a4023a37a4a73305cf6 Mon Sep 17 00:00:00 2001 From: gaisser Date: Wed, 26 Aug 2020 19:58:18 +0200 Subject: [PATCH 06/20] Update 'globalfunctions/DleEncoder.h' Corrected some spelling mistakes --- globalfunctions/DleEncoder.h | 158 +++++++++++++++++------------------ 1 file changed, 79 insertions(+), 79 deletions(-) diff --git a/globalfunctions/DleEncoder.h b/globalfunctions/DleEncoder.h index 3c327a55..6d073f9a 100644 --- a/globalfunctions/DleEncoder.h +++ b/globalfunctions/DleEncoder.h @@ -1,79 +1,79 @@ -#ifndef FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_ -#define FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_ - -#include "../returnvalues/HasReturnvaluesIF.h" -#include - -/** - * @brief This DLE Encoder (Data Link Encoder) can be used to encode and - * decode arbitrary data with ASCII control characters - * @details - * List of control codes: - * https://en.wikipedia.org/wiki/C0_and_C1_control_codes - * - * This encoder can be used to achieve a basic transport layer when using - * char based transmission systems. - * The passed source strean is converted into a encoded stream by adding - * a STX marker at the start of the stream and an ETX marker at the end of - * the stream. Any STX, ETX, DLE and CR occurences in the source stream are - * escaped by a DLE character. The encoder also replaces escaped control chars - * by another char, so STX, ETX and CR should not appear anywhere in the actual - * encoded data stream. - * - * When using a strictly char based reception of packets enoded with DLE, - * STX can be used to notify a reader that actual data will start to arrive - * while ETX can be used to notify the reader that the data has ended. - */ -class DleEncoder: public HasReturnvaluesIF { -private: - DleEncoder(); - virtual ~DleEncoder(); - -public: - static constexpr uint8_t INTERFACE_ID = CLASS_ID::DLE_ENCODER; - static constexpr ReturnValue_t STREAM_TOO_SHORT = MAKE_RETURN_CODE(0x01); - static constexpr ReturnValue_t DECODING_ERROR = MAKE_RETURN_CODE(0x02); - - //! Start Of Text character. First character is encoded stream - static constexpr uint8_t STX_CHAR = 0x02; - //! End Of Text character. Last character in encoded stream - static constexpr uint8_t ETX_CHAR = 0x03; - //! Data Link Escape character. Used to escape STX, ETX and DLE occurences - //! in the source stream. - static constexpr uint8_t DLE_CHAR = 0x10; - static constexpr uint8_t CARRIAGE_RETURN = 0x0D; - - /** - * Encodes the give data stream by preceding it with the STX marker - * and ending it with an ETX marker. STX, ETX and DLE characters inside - * the stream are escaped by DLE characters and also replaced by adding - * 0x40 (which is reverted in the decoing process). - * @param sourceStream - * @param sourceLen - * @param destStream - * @param maxDestLen - * @param encodedLen - * @param addStxEtx - * Adding STX and ETX can be omitted, if they are added manually. - * @return - */ - static ReturnValue_t encode(const uint8_t *sourceStream, size_t sourceLen, - uint8_t *destStream, size_t maxDestLen, size_t *encodedLen, - bool addStxEtx = true); - - /** - * Converts an encoded stream back. - * @param sourceStream - * @param sourceStreamLen - * @param readLen - * @param destStream - * @param maxDestStreamlen - * @param decodedLen - * @return - */ - static ReturnValue_t decode(const uint8_t *sourceStream, - size_t sourceStreamLen, size_t *readLen, uint8_t *destStream, - size_t maxDestStreamlen, size_t *decodedLen); -}; - -#endif /* FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_ */ +#ifndef FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_ +#define FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_ + +#include "../returnvalues/HasReturnvaluesIF.h" +#include + +/** + * @brief This DLE Encoder (Data Link Encoder) can be used to encode and + * decode arbitrary data with ASCII control characters + * @details + * List of control codes: + * https://en.wikipedia.org/wiki/C0_and_C1_control_codes + * + * This encoder can be used to achieve a basic transport layer when using + * char based transmission systems. + * The passed source strean is converted into a encoded stream by adding + * a STX marker at the start of the stream and an ETX marker at the end of + * the stream. Any STX, ETX, DLE and CR occurrences in the source stream are + * escaped by a DLE character. The encoder also replaces escaped control chars + * by another char, so STX, ETX and CR should not appear anywhere in the actual + * encoded data stream. + * + * When using a strictly char based reception of packets encoded with DLE, + * STX can be used to notify a reader that actual data will start to arrive + * while ETX can be used to notify the reader that the data has ended. + */ +class DleEncoder: public HasReturnvaluesIF { +private: + DleEncoder(); + virtual ~DleEncoder(); + +public: + static constexpr uint8_t INTERFACE_ID = CLASS_ID::DLE_ENCODER; + static constexpr ReturnValue_t STREAM_TOO_SHORT = MAKE_RETURN_CODE(0x01); + static constexpr ReturnValue_t DECODING_ERROR = MAKE_RETURN_CODE(0x02); + + //! Start Of Text character. First character is encoded stream + static constexpr uint8_t STX_CHAR = 0x02; + //! End Of Text character. Last character in encoded stream + static constexpr uint8_t ETX_CHAR = 0x03; + //! Data Link Escape character. Used to escape STX, ETX and DLE occurrences + //! in the source stream. + static constexpr uint8_t DLE_CHAR = 0x10; + static constexpr uint8_t CARRIAGE_RETURN = 0x0D; + + /** + * Encodes the give data stream by preceding it with the STX marker + * and ending it with an ETX marker. STX, ETX and DLE characters inside + * the stream are escaped by DLE characters and also replaced by adding + * 0x40 (which is reverted in the decoding process). + * @param sourceStream + * @param sourceLen + * @param destStream + * @param maxDestLen + * @param encodedLen + * @param addStxEtx + * Adding STX and ETX can be omitted, if they are added manually. + * @return + */ + static ReturnValue_t encode(const uint8_t *sourceStream, size_t sourceLen, + uint8_t *destStream, size_t maxDestLen, size_t *encodedLen, + bool addStxEtx = true); + + /** + * Converts an encoded stream back. + * @param sourceStream + * @param sourceStreamLen + * @param readLen + * @param destStream + * @param maxDestStreamlen + * @param decodedLen + * @return + */ + static ReturnValue_t decode(const uint8_t *sourceStream, + size_t sourceStreamLen, size_t *readLen, uint8_t *destStream, + size_t maxDestStreamlen, size_t *decodedLen); +}; + +#endif /* FRAMEWORK_GLOBALFUNCTIONS_DLEENCODER_H_ */ From 76f145ddcfe2bd0ed6e201d70873cfe3927d80df Mon Sep 17 00:00:00 2001 From: gaisser Date: Wed, 26 Aug 2020 19:59:58 +0200 Subject: [PATCH 07/20] Update 'globalfunctions/DleEncoder.cpp' Just a missing char --- globalfunctions/DleEncoder.cpp | 248 ++++++++++++++++----------------- 1 file changed, 124 insertions(+), 124 deletions(-) diff --git a/globalfunctions/DleEncoder.cpp b/globalfunctions/DleEncoder.cpp index 21dbd2e6..8520389d 100644 --- a/globalfunctions/DleEncoder.cpp +++ b/globalfunctions/DleEncoder.cpp @@ -1,124 +1,124 @@ -#include "../globalfunctions/DleEncoder.h" - -DleEncoder::DleEncoder() {} - -DleEncoder::~DleEncoder() {} - -ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream, - size_t sourceLen, uint8_t* destStream, size_t maxDestLen, - size_t* encodedLen, bool addStxEtx) { - if (maxDestLen < 2) { - return STREAM_TOO_SHORT; - } - size_t encodedIndex = 0, sourceIndex = 0; - uint8_t nextByte; - if (addStxEtx) { - destStream[0] = STX_CHAR; - ++encodedIndex; - } - - while (encodedIndex < maxDestLen and sourceIndex < sourceLen) - { - nextByte = sourceStream[sourceIndex]; - // STX, ETX and CR characters in the stream need to be escaped with DLE - if (nextByte == STX_CHAR or nextByte == ETX_CHAR or nextByte == CARRIAGE_RETURN) { - if (encodedIndex + 1 >= maxDestLen) { - return STREAM_TOO_SHORT; - } - else { - destStream[encodedIndex] = DLE_CHAR; - ++encodedIndex; - /* Escaped byte will be actual byte + 0x40. This prevents - * STX, ETX, and carriage return characters from appearing - * in the encoded data stream at all, so when polling an - * encoded stream, the transmission can be stopped at ETX. - * 0x40 was chosen at random with special requirements: - * - Prevent going from one control char to another - * - Prevent overflow for common characters */ - destStream[encodedIndex] = nextByte + 0x40; - } - } - // DLE characters are simply escaped with DLE. - else if (nextByte == DLE_CHAR) { - if (encodedIndex + 1 >= maxDestLen) { - return STREAM_TOO_SHORT; - } - else { - destStream[encodedIndex] = DLE_CHAR; - ++encodedIndex; - destStream[encodedIndex] = DLE_CHAR; - } - } - else { - destStream[encodedIndex] = nextByte; - } - ++encodedIndex; - ++sourceIndex; - } - - if (sourceIndex == sourceLen and encodedIndex < maxDestLen) { - if (addStxEtx) { - destStream[encodedIndex] = ETX_CHAR; - ++encodedIndex; - } - *encodedLen = encodedIndex; - return RETURN_OK; - } - else { - return STREAM_TOO_SHORT; - } -} - -ReturnValue_t DleEncoder::decode(const uint8_t *sourceStream, - size_t sourceStreamLen, size_t *readLen, uint8_t *destStream, - size_t maxDestStreamlen, size_t *decodedLen) { - size_t encodedIndex = 0, decodedIndex = 0; - uint8_t nextByte; - if (*sourceStream != STX_CHAR) { - return DECODING_ERROR; - } - ++encodedIndex; - - while ((encodedIndex < sourceStreamLen) && (decodedIndex < maxDestStreamlen) - && (sourceStream[encodedIndex] != ETX_CHAR) - && (sourceStream[encodedIndex] != STX_CHAR)) { - if (sourceStream[encodedIndex] == DLE_CHAR) { - nextByte = sourceStream[encodedIndex + 1]; - // The next byte is a DLE character that was escaped by another - // DLE character, so we can write it to the destination stream. - if (nextByte == DLE_CHAR) { - destStream[decodedIndex] = nextByte; - } - else { - /* The next byte is a STX, DTX or 0x0D character which - * was escaped by a DLE character. The actual byte was - * also encoded by adding + 0x40 to preven having control chars, - * in the stream at all, so we convert it back. */ - if (nextByte == 0x42 or nextByte == 0x43 or nextByte == 0x4D) { - destStream[decodedIndex] = nextByte - 0x40; - } - else { - return DECODING_ERROR; - } - } - ++encodedIndex; - } - else { - destStream[decodedIndex] = sourceStream[encodedIndex]; - } - - ++encodedIndex; - ++decodedIndex; - } - - if (sourceStream[encodedIndex] != ETX_CHAR) { - *readLen = ++encodedIndex; - return DECODING_ERROR; - } - else { - *readLen = ++encodedIndex; - *decodedLen = decodedIndex; - return RETURN_OK; - } -} - +#include "../globalfunctions/DleEncoder.h" + +DleEncoder::DleEncoder() {} + +DleEncoder::~DleEncoder() {} + +ReturnValue_t DleEncoder::encode(const uint8_t* sourceStream, + size_t sourceLen, uint8_t* destStream, size_t maxDestLen, + size_t* encodedLen, bool addStxEtx) { + if (maxDestLen < 2) { + return STREAM_TOO_SHORT; + } + size_t encodedIndex = 0, sourceIndex = 0; + uint8_t nextByte; + if (addStxEtx) { + destStream[0] = STX_CHAR; + ++encodedIndex; + } + + while (encodedIndex < maxDestLen and sourceIndex < sourceLen) + { + nextByte = sourceStream[sourceIndex]; + // STX, ETX and CR characters in the stream need to be escaped with DLE + if (nextByte == STX_CHAR or nextByte == ETX_CHAR or nextByte == CARRIAGE_RETURN) { + if (encodedIndex + 1 >= maxDestLen) { + return STREAM_TOO_SHORT; + } + else { + destStream[encodedIndex] = DLE_CHAR; + ++encodedIndex; + /* Escaped byte will be actual byte + 0x40. This prevents + * STX, ETX, and carriage return characters from appearing + * in the encoded data stream at all, so when polling an + * encoded stream, the transmission can be stopped at ETX. + * 0x40 was chosen at random with special requirements: + * - Prevent going from one control char to another + * - Prevent overflow for common characters */ + destStream[encodedIndex] = nextByte + 0x40; + } + } + // DLE characters are simply escaped with DLE. + else if (nextByte == DLE_CHAR) { + if (encodedIndex + 1 >= maxDestLen) { + return STREAM_TOO_SHORT; + } + else { + destStream[encodedIndex] = DLE_CHAR; + ++encodedIndex; + destStream[encodedIndex] = DLE_CHAR; + } + } + else { + destStream[encodedIndex] = nextByte; + } + ++encodedIndex; + ++sourceIndex; + } + + if (sourceIndex == sourceLen and encodedIndex < maxDestLen) { + if (addStxEtx) { + destStream[encodedIndex] = ETX_CHAR; + ++encodedIndex; + } + *encodedLen = encodedIndex; + return RETURN_OK; + } + else { + return STREAM_TOO_SHORT; + } +} + +ReturnValue_t DleEncoder::decode(const uint8_t *sourceStream, + size_t sourceStreamLen, size_t *readLen, uint8_t *destStream, + size_t maxDestStreamlen, size_t *decodedLen) { + size_t encodedIndex = 0, decodedIndex = 0; + uint8_t nextByte; + if (*sourceStream != STX_CHAR) { + return DECODING_ERROR; + } + ++encodedIndex; + + while ((encodedIndex < sourceStreamLen) && (decodedIndex < maxDestStreamlen) + && (sourceStream[encodedIndex] != ETX_CHAR) + && (sourceStream[encodedIndex] != STX_CHAR)) { + if (sourceStream[encodedIndex] == DLE_CHAR) { + nextByte = sourceStream[encodedIndex + 1]; + // The next byte is a DLE character that was escaped by another + // DLE character, so we can write it to the destination stream. + if (nextByte == DLE_CHAR) { + destStream[decodedIndex] = nextByte; + } + else { + /* The next byte is a STX, DTX or 0x0D character which + * was escaped by a DLE character. The actual byte was + * also encoded by adding + 0x40 to prevent having control chars, + * in the stream at all, so we convert it back. */ + if (nextByte == 0x42 or nextByte == 0x43 or nextByte == 0x4D) { + destStream[decodedIndex] = nextByte - 0x40; + } + else { + return DECODING_ERROR; + } + } + ++encodedIndex; + } + else { + destStream[decodedIndex] = sourceStream[encodedIndex]; + } + + ++encodedIndex; + ++decodedIndex; + } + + if (sourceStream[encodedIndex] != ETX_CHAR) { + *readLen = ++encodedIndex; + return DECODING_ERROR; + } + else { + *readLen = ++encodedIndex; + *decodedLen = decodedIndex; + return RETURN_OK; + } +} + From 02be87aa03f0919d7f8ba629d6afc7fb9bd02076 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 27 Aug 2020 15:57:47 +0200 Subject: [PATCH 08/20] init after task creation moved to task functionality --- osal/FreeRTOS/PeriodicTask.cpp | 281 +++++++++++++++++---------------- osal/FreeRTOS/PeriodicTask.h | 256 +++++++++++++++--------------- 2 files changed, 271 insertions(+), 266 deletions(-) diff --git a/osal/FreeRTOS/PeriodicTask.cpp b/osal/FreeRTOS/PeriodicTask.cpp index ce981877..f99e8a4e 100644 --- a/osal/FreeRTOS/PeriodicTask.cpp +++ b/osal/FreeRTOS/PeriodicTask.cpp @@ -1,138 +1,143 @@ -#include "PeriodicTask.h" - -#include "../../serviceinterface/ServiceInterfaceStream.h" -#include "../../tasks/ExecutableObjectIF.h" - -PeriodicTask::PeriodicTask(const char *name, TaskPriority setPriority, - TaskStackSize setStack, TaskPeriod setPeriod, - TaskDeadlineMissedFunction deadlineMissedFunc) : - started(false), handle(NULL), period(setPeriod), deadlineMissedFunc( - deadlineMissedFunc) -{ - configSTACK_DEPTH_TYPE stackSize = setStack / sizeof(configSTACK_DEPTH_TYPE); - BaseType_t status = xTaskCreate(taskEntryPoint, name, - stackSize, this, setPriority, &handle); - if(status != pdPASS){ - sif::debug << "PeriodicTask Insufficient heap memory remaining. " - "Status: " << status << std::endl; - } - -} - -PeriodicTask::~PeriodicTask(void) { - //Do not delete objects, we were responsible for ptrs only. -} - -void PeriodicTask::taskEntryPoint(void* argument) { - // The argument is re-interpreted as PeriodicTask. The Task object is - // global, so it is found from any place. - PeriodicTask *originalTask(reinterpret_cast(argument)); - /* Task should not start until explicitly requested, - * but in FreeRTOS, tasks start as soon as they are created if the scheduler - * is running but not if the scheduler is not running. - * To be able to accommodate both cases we check a member which is set in - * #startTask(). If it is not set and we get here, the scheduler was started - * before #startTask() was called and we need to suspend if it is set, - * the scheduler was not running before #startTask() was called and we - * can continue */ - - if (not originalTask->started) { - vTaskSuspend(NULL); - } - - originalTask->taskFunctionality(); - sif::debug << "Polling task " << originalTask->handle - << " returned from taskFunctionality." << std::endl; -} - -ReturnValue_t PeriodicTask::startTask() { - started = true; - - // We must not call resume if scheduler is not started yet - if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) { - vTaskResume(handle); - } - - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t PeriodicTask::sleepFor(uint32_t ms) { - vTaskDelay(pdMS_TO_TICKS(ms)); - return HasReturnvaluesIF::RETURN_OK; -} - -void PeriodicTask::taskFunctionality() { - TickType_t xLastWakeTime; - const TickType_t xPeriod = pdMS_TO_TICKS(this->period * 1000.); - /* The xLastWakeTime variable needs to be initialized with the current tick - count. Note that this is the only time the variable is written to - explicitly. After this assignment, xLastWakeTime is updated automatically - internally within vTaskDelayUntil(). */ - xLastWakeTime = xTaskGetTickCount(); - /* Enter the loop that defines the task behavior. */ - for (;;) { - for (auto const& object: objectList) { - object->performOperation(); - } - - checkMissedDeadline(xLastWakeTime, xPeriod); - - vTaskDelayUntil(&xLastWakeTime, xPeriod); - - } -} - -ReturnValue_t PeriodicTask::addComponent(object_id_t object) { - ExecutableObjectIF* newObject = objectManager->get( - object); - if (newObject == nullptr) { - sif::error << "PeriodicTask::addComponent: Invalid object. Make sure" - "it implement ExecutableObjectIF" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; - } - objectList.push_back(newObject); - newObject->setTaskIF(this); - - return HasReturnvaluesIF::RETURN_OK; -} - -uint32_t PeriodicTask::getPeriodMs() const { - return period * 1000; -} - -void PeriodicTask::checkMissedDeadline(const TickType_t xLastWakeTime, - const TickType_t interval) { - /* Check whether deadline was missed while also taking overflows - * into account. Drawing this on paper with a timeline helps to understand - * it. */ - TickType_t currentTickCount = xTaskGetTickCount(); - TickType_t timeToWake = xLastWakeTime + interval; - // Time to wake has not overflown. - if(timeToWake > xLastWakeTime) { - /* If the current time has overflown exclusively or the current - * tick count is simply larger than the time to wake, a deadline was - * missed */ - if((currentTickCount < xLastWakeTime) or (currentTickCount > timeToWake)) { - handleMissedDeadline(); - } - } - /* Time to wake has overflown. A deadline was missed if the current time - * is larger than the time to wake */ - else if((timeToWake < xLastWakeTime) and (currentTickCount > timeToWake)) { - handleMissedDeadline(); - } -} - -TaskHandle_t PeriodicTask::getTaskHandle() { - return handle; -} - -void PeriodicTask::handleMissedDeadline() { -#ifdef DEBUG - sif::warning << "PeriodicTask: " << pcTaskGetName(NULL) << - " missed deadline!\n" << std::flush; -#endif - if(deadlineMissedFunc != nullptr) { - this->deadlineMissedFunc(); - } -} +#include "PeriodicTask.h" + +#include "../../serviceinterface/ServiceInterfaceStream.h" +#include "../../tasks/ExecutableObjectIF.h" + +PeriodicTask::PeriodicTask(const char *name, TaskPriority setPriority, + TaskStackSize setStack, TaskPeriod setPeriod, + TaskDeadlineMissedFunction deadlineMissedFunc) : + started(false), handle(NULL), period(setPeriod), deadlineMissedFunc( + deadlineMissedFunc) +{ + configSTACK_DEPTH_TYPE stackSize = setStack / sizeof(configSTACK_DEPTH_TYPE); + BaseType_t status = xTaskCreate(taskEntryPoint, name, + stackSize, this, setPriority, &handle); + if(status != pdPASS){ + sif::debug << "PeriodicTask Insufficient heap memory remaining. " + "Status: " << status << std::endl; + } + +} + +PeriodicTask::~PeriodicTask(void) { + //Do not delete objects, we were responsible for ptrs only. +} + +void PeriodicTask::taskEntryPoint(void* argument) { + // The argument is re-interpreted as PeriodicTask. The Task object is + // global, so it is found from any place. + PeriodicTask *originalTask(reinterpret_cast(argument)); + /* Task should not start until explicitly requested, + * but in FreeRTOS, tasks start as soon as they are created if the scheduler + * is running but not if the scheduler is not running. + * To be able to accommodate both cases we check a member which is set in + * #startTask(). If it is not set and we get here, the scheduler was started + * before #startTask() was called and we need to suspend if it is set, + * the scheduler was not running before #startTask() was called and we + * can continue */ + + if (not originalTask->started) { + vTaskSuspend(NULL); + } + + originalTask->taskFunctionality(); + sif::debug << "Polling task " << originalTask->handle + << " returned from taskFunctionality." << std::endl; +} + +ReturnValue_t PeriodicTask::startTask() { + started = true; + + // We must not call resume if scheduler is not started yet + if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) { + vTaskResume(handle); + } + + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t PeriodicTask::sleepFor(uint32_t ms) { + vTaskDelay(pdMS_TO_TICKS(ms)); + return HasReturnvaluesIF::RETURN_OK; +} + +void PeriodicTask::taskFunctionality() { + TickType_t xLastWakeTime; + const TickType_t xPeriod = pdMS_TO_TICKS(this->period * 1000.); + + for (auto const& object: objectList) { + object->initializeAfterTaskCreation(); + } + + /* The xLastWakeTime variable needs to be initialized with the current tick + count. Note that this is the only time the variable is written to + explicitly. After this assignment, xLastWakeTime is updated automatically + internally within vTaskDelayUntil(). */ + xLastWakeTime = xTaskGetTickCount(); + /* Enter the loop that defines the task behavior. */ + for (;;) { + for (auto const& object: objectList) { + object->performOperation(); + } + + checkMissedDeadline(xLastWakeTime, xPeriod); + + vTaskDelayUntil(&xLastWakeTime, xPeriod); + + } +} + +ReturnValue_t PeriodicTask::addComponent(object_id_t object) { + ExecutableObjectIF* newObject = objectManager->get( + object); + if (newObject == nullptr) { + sif::error << "PeriodicTask::addComponent: Invalid object. Make sure" + "it implement ExecutableObjectIF" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + objectList.push_back(newObject); + newObject->setTaskIF(this); + + return HasReturnvaluesIF::RETURN_OK; +} + +uint32_t PeriodicTask::getPeriodMs() const { + return period * 1000; +} + +void PeriodicTask::checkMissedDeadline(const TickType_t xLastWakeTime, + const TickType_t interval) { + /* Check whether deadline was missed while also taking overflows + * into account. Drawing this on paper with a timeline helps to understand + * it. */ + TickType_t currentTickCount = xTaskGetTickCount(); + TickType_t timeToWake = xLastWakeTime + interval; + // Time to wake has not overflown. + if(timeToWake > xLastWakeTime) { + /* If the current time has overflown exclusively or the current + * tick count is simply larger than the time to wake, a deadline was + * missed */ + if((currentTickCount < xLastWakeTime) or (currentTickCount > timeToWake)) { + handleMissedDeadline(); + } + } + /* Time to wake has overflown. A deadline was missed if the current time + * is larger than the time to wake */ + else if((timeToWake < xLastWakeTime) and (currentTickCount > timeToWake)) { + handleMissedDeadline(); + } +} + +TaskHandle_t PeriodicTask::getTaskHandle() { + return handle; +} + +void PeriodicTask::handleMissedDeadline() { +#ifdef DEBUG + sif::warning << "PeriodicTask: " << pcTaskGetName(NULL) << + " missed deadline!\n" << std::flush; +#endif + if(deadlineMissedFunc != nullptr) { + this->deadlineMissedFunc(); + } +} diff --git a/osal/FreeRTOS/PeriodicTask.h b/osal/FreeRTOS/PeriodicTask.h index ea170aba..1cdc3df7 100644 --- a/osal/FreeRTOS/PeriodicTask.h +++ b/osal/FreeRTOS/PeriodicTask.h @@ -1,128 +1,128 @@ -#ifndef FRAMEWORK_OSAL_FREERTOS_PERIODICTASK_H_ -#define FRAMEWORK_OSAL_FREERTOS_PERIODICTASK_H_ - -#include "../../objectmanager/ObjectManagerIF.h" -#include "../../tasks/PeriodicTaskIF.h" -#include "../../tasks/Typedef.h" -#include "FreeRTOSTaskIF.h" - - -#include -#include - -#include - -class ExecutableObjectIF; - -/** - * @brief This class represents a specialized task for - * periodic activities of multiple objects. - * @ingroup task_handling - */ -class PeriodicTask: public PeriodicTaskIF, public FreeRTOSTaskIF { -public: - /** - * Keep in Mind that you need to call before this vTaskStartScheduler()! - * A lot of task parameters are set in "FreeRTOSConfig.h". - * TODO: why does this need to be called before vTaskStartScheduler? - * @details - * The class is initialized without allocated objects. - * These need to be added with #addComponent. - * @param priority - * Sets the priority of a task. Values depend on freeRTOS configuration, - * high number means high priority. - * @param stack_size - * The stack size reserved by the operating system for the task. - * @param setPeriod - * The length of the period with which the task's - * functionality will be executed. It is expressed in clock ticks. - * @param setDeadlineMissedFunc - * The function pointer to the deadline missed function that shall - * be assigned. - */ - PeriodicTask(TaskName name, TaskPriority setPriority, - TaskStackSize setStack, TaskPeriod setPeriod, - TaskDeadlineMissedFunction deadlineMissedFunc); - /** - * @brief Currently, the executed object's lifetime is not coupled with - * the task object's lifetime, so the destructor is empty. - */ - virtual ~PeriodicTask(void); - - /** - * @brief The method to start the task. - * @details The method starts the task with the respective system call. - * Entry point is the taskEntryPoint method described below. - * The address of the task object is passed as an argument - * to the system call. - */ - ReturnValue_t startTask() override; - /** - * Adds an object to the list of objects to be executed. - * The objects are executed in the order added. - * @param object Id of the object to add. - * @return - * -@c RETURN_OK on success - * -@c RETURN_FAILED if the object could not be added. - */ - ReturnValue_t addComponent(object_id_t object) override; - - uint32_t getPeriodMs() const override; - - ReturnValue_t sleepFor(uint32_t ms) override; - - TaskHandle_t getTaskHandle() override; -protected: - bool started; - TaskHandle_t handle; - - //! Typedef for the List of objects. - typedef std::vector ObjectList; - /** - * @brief This attribute holds a list of objects to be executed. - */ - ObjectList objectList; - /** - * @brief The period of the task. - * @details - * The period determines the frequency of the task's execution. - * It is expressed in clock ticks. - */ - TaskPeriod period; - /** - * @brief The pointer to the deadline-missed function. - * @details - * This pointer stores the function that is executed if the task's deadline - * is missed so each may react individually on a timing failure. - * The pointer may be NULL, then nothing happens on missing the deadline. - * The deadline is equal to the next execution of the periodic task. - */ - void (*deadlineMissedFunc)(void); - /** - * @brief This is the function executed in the new task's context. - * @details - * It converts the argument back to the thread object type and copies the - * class instance to the task context. The taskFunctionality method is - * called afterwards. - * @param A pointer to the task object itself is passed as argument. - */ - - static void taskEntryPoint(void* argument); - /** - * @brief The function containing the actual functionality of the task. - * @details - * The method sets and starts the task's period, then enters a loop that is - * repeated as long as the isRunning attribute is true. Within the loop, - * all performOperation methods of the added objects are called. - * Afterwards the checkAndRestartPeriod system call blocks the task until - * the next period. - * On missing the deadline, the deadlineMissedFunction is executed. - */ - void taskFunctionality(void); - - void checkMissedDeadline(const TickType_t xLastWakeTime, - const TickType_t interval); - void handleMissedDeadline(); -}; - -#endif /* PERIODICTASK_H_ */ +#ifndef FRAMEWORK_OSAL_FREERTOS_PERIODICTASK_H_ +#define FRAMEWORK_OSAL_FREERTOS_PERIODICTASK_H_ + +#include "../../osal/FreeRTOS/FreeRTOSTaskIF.h" +#include "../../objectmanager/ObjectManagerIF.h" +#include "../../tasks/PeriodicTaskIF.h" +#include "../../tasks/Typedef.h" + + +#include +#include + +#include + +class ExecutableObjectIF; + +/** + * @brief This class represents a specialized task for + * periodic activities of multiple objects. + * @ingroup task_handling + */ +class PeriodicTask: public PeriodicTaskIF, public FreeRTOSTaskIF { +public: + /** + * Keep in Mind that you need to call before this vTaskStartScheduler()! + * A lot of task parameters are set in "FreeRTOSConfig.h". + * TODO: why does this need to be called before vTaskStartScheduler? + * @details + * The class is initialized without allocated objects. + * These need to be added with #addComponent. + * @param priority + * Sets the priority of a task. Values depend on freeRTOS configuration, + * high number means high priority. + * @param stack_size + * The stack size reserved by the operating system for the task. + * @param setPeriod + * The length of the period with which the task's + * functionality will be executed. It is expressed in clock ticks. + * @param setDeadlineMissedFunc + * The function pointer to the deadline missed function that shall + * be assigned. + */ + PeriodicTask(TaskName name, TaskPriority setPriority, + TaskStackSize setStack, TaskPeriod setPeriod, + TaskDeadlineMissedFunction deadlineMissedFunc); + /** + * @brief Currently, the executed object's lifetime is not coupled with + * the task object's lifetime, so the destructor is empty. + */ + virtual ~PeriodicTask(void); + + /** + * @brief The method to start the task. + * @details The method starts the task with the respective system call. + * Entry point is the taskEntryPoint method described below. + * The address of the task object is passed as an argument + * to the system call. + */ + ReturnValue_t startTask() override; + /** + * Adds an object to the list of objects to be executed. + * The objects are executed in the order added. + * @param object Id of the object to add. + * @return + * -@c RETURN_OK on success + * -@c RETURN_FAILED if the object could not be added. + */ + ReturnValue_t addComponent(object_id_t object) override; + + uint32_t getPeriodMs() const override; + + ReturnValue_t sleepFor(uint32_t ms) override; + + TaskHandle_t getTaskHandle() override; +protected: + bool started; + TaskHandle_t handle; + + //! Typedef for the List of objects. + typedef std::vector ObjectList; + /** + * @brief This attribute holds a list of objects to be executed. + */ + ObjectList objectList; + /** + * @brief The period of the task. + * @details + * The period determines the frequency of the task's execution. + * It is expressed in clock ticks. + */ + TaskPeriod period; + /** + * @brief The pointer to the deadline-missed function. + * @details + * This pointer stores the function that is executed if the task's deadline + * is missed so each may react individually on a timing failure. + * The pointer may be NULL, then nothing happens on missing the deadline. + * The deadline is equal to the next execution of the periodic task. + */ + void (*deadlineMissedFunc)(void); + /** + * @brief This is the function executed in the new task's context. + * @details + * It converts the argument back to the thread object type and copies the + * class instance to the task context. The taskFunctionality method is + * called afterwards. + * @param A pointer to the task object itself is passed as argument. + */ + + static void taskEntryPoint(void* argument); + /** + * @brief The function containing the actual functionality of the task. + * @details + * The method sets and starts the task's period, then enters a loop that is + * repeated as long as the isRunning attribute is true. Within the loop, + * all performOperation methods of the added objects are called. + * Afterwards the checkAndRestartPeriod system call blocks the task until + * the next period. + * On missing the deadline, the deadlineMissedFunction is executed. + */ + void taskFunctionality(void); + + void checkMissedDeadline(const TickType_t xLastWakeTime, + const TickType_t interval); + void handleMissedDeadline(); +}; + +#endif /* PERIODICTASK_H_ */ From 89d3fe509504382980aff1bbeaf9be1ffd508136 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 27 Aug 2020 16:11:40 +0200 Subject: [PATCH 09/20] integrated change manually --- osal/FreeRTOS/PeriodicTask.cpp | 286 ++++++++++++++++----------------- 1 file changed, 143 insertions(+), 143 deletions(-) diff --git a/osal/FreeRTOS/PeriodicTask.cpp b/osal/FreeRTOS/PeriodicTask.cpp index f99e8a4e..990d38d6 100644 --- a/osal/FreeRTOS/PeriodicTask.cpp +++ b/osal/FreeRTOS/PeriodicTask.cpp @@ -1,143 +1,143 @@ -#include "PeriodicTask.h" - -#include "../../serviceinterface/ServiceInterfaceStream.h" -#include "../../tasks/ExecutableObjectIF.h" - -PeriodicTask::PeriodicTask(const char *name, TaskPriority setPriority, - TaskStackSize setStack, TaskPeriod setPeriod, - TaskDeadlineMissedFunction deadlineMissedFunc) : - started(false), handle(NULL), period(setPeriod), deadlineMissedFunc( - deadlineMissedFunc) -{ - configSTACK_DEPTH_TYPE stackSize = setStack / sizeof(configSTACK_DEPTH_TYPE); - BaseType_t status = xTaskCreate(taskEntryPoint, name, - stackSize, this, setPriority, &handle); - if(status != pdPASS){ - sif::debug << "PeriodicTask Insufficient heap memory remaining. " - "Status: " << status << std::endl; - } - -} - -PeriodicTask::~PeriodicTask(void) { - //Do not delete objects, we were responsible for ptrs only. -} - -void PeriodicTask::taskEntryPoint(void* argument) { - // The argument is re-interpreted as PeriodicTask. The Task object is - // global, so it is found from any place. - PeriodicTask *originalTask(reinterpret_cast(argument)); - /* Task should not start until explicitly requested, - * but in FreeRTOS, tasks start as soon as they are created if the scheduler - * is running but not if the scheduler is not running. - * To be able to accommodate both cases we check a member which is set in - * #startTask(). If it is not set and we get here, the scheduler was started - * before #startTask() was called and we need to suspend if it is set, - * the scheduler was not running before #startTask() was called and we - * can continue */ - - if (not originalTask->started) { - vTaskSuspend(NULL); - } - - originalTask->taskFunctionality(); - sif::debug << "Polling task " << originalTask->handle - << " returned from taskFunctionality." << std::endl; -} - -ReturnValue_t PeriodicTask::startTask() { - started = true; - - // We must not call resume if scheduler is not started yet - if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) { - vTaskResume(handle); - } - - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t PeriodicTask::sleepFor(uint32_t ms) { - vTaskDelay(pdMS_TO_TICKS(ms)); - return HasReturnvaluesIF::RETURN_OK; -} - -void PeriodicTask::taskFunctionality() { - TickType_t xLastWakeTime; - const TickType_t xPeriod = pdMS_TO_TICKS(this->period * 1000.); - - for (auto const& object: objectList) { - object->initializeAfterTaskCreation(); - } - - /* The xLastWakeTime variable needs to be initialized with the current tick - count. Note that this is the only time the variable is written to - explicitly. After this assignment, xLastWakeTime is updated automatically - internally within vTaskDelayUntil(). */ - xLastWakeTime = xTaskGetTickCount(); - /* Enter the loop that defines the task behavior. */ - for (;;) { - for (auto const& object: objectList) { - object->performOperation(); - } - - checkMissedDeadline(xLastWakeTime, xPeriod); - - vTaskDelayUntil(&xLastWakeTime, xPeriod); - - } -} - -ReturnValue_t PeriodicTask::addComponent(object_id_t object) { - ExecutableObjectIF* newObject = objectManager->get( - object); - if (newObject == nullptr) { - sif::error << "PeriodicTask::addComponent: Invalid object. Make sure" - "it implement ExecutableObjectIF" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; - } - objectList.push_back(newObject); - newObject->setTaskIF(this); - - return HasReturnvaluesIF::RETURN_OK; -} - -uint32_t PeriodicTask::getPeriodMs() const { - return period * 1000; -} - -void PeriodicTask::checkMissedDeadline(const TickType_t xLastWakeTime, - const TickType_t interval) { - /* Check whether deadline was missed while also taking overflows - * into account. Drawing this on paper with a timeline helps to understand - * it. */ - TickType_t currentTickCount = xTaskGetTickCount(); - TickType_t timeToWake = xLastWakeTime + interval; - // Time to wake has not overflown. - if(timeToWake > xLastWakeTime) { - /* If the current time has overflown exclusively or the current - * tick count is simply larger than the time to wake, a deadline was - * missed */ - if((currentTickCount < xLastWakeTime) or (currentTickCount > timeToWake)) { - handleMissedDeadline(); - } - } - /* Time to wake has overflown. A deadline was missed if the current time - * is larger than the time to wake */ - else if((timeToWake < xLastWakeTime) and (currentTickCount > timeToWake)) { - handleMissedDeadline(); - } -} - -TaskHandle_t PeriodicTask::getTaskHandle() { - return handle; -} - -void PeriodicTask::handleMissedDeadline() { -#ifdef DEBUG - sif::warning << "PeriodicTask: " << pcTaskGetName(NULL) << - " missed deadline!\n" << std::flush; -#endif - if(deadlineMissedFunc != nullptr) { - this->deadlineMissedFunc(); - } -} +#include "PeriodicTask.h" + +#include "../../serviceinterface/ServiceInterfaceStream.h" +#include "../../tasks/ExecutableObjectIF.h" + +PeriodicTask::PeriodicTask(const char *name, TaskPriority setPriority, + TaskStackSize setStack, TaskPeriod setPeriod, + TaskDeadlineMissedFunction deadlineMissedFunc) : + started(false), handle(NULL), period(setPeriod), deadlineMissedFunc( + deadlineMissedFunc) +{ + configSTACK_DEPTH_TYPE stackSize = setStack / sizeof(configSTACK_DEPTH_TYPE); + BaseType_t status = xTaskCreate(taskEntryPoint, name, + stackSize, this, setPriority, &handle); + if(status != pdPASS){ + sif::debug << "PeriodicTask Insufficient heap memory remaining. " + "Status: " << status << std::endl; + } + +} + +PeriodicTask::~PeriodicTask(void) { + //Do not delete objects, we were responsible for ptrs only. +} + +void PeriodicTask::taskEntryPoint(void* argument) { + // The argument is re-interpreted as PeriodicTask. The Task object is + // global, so it is found from any place. + PeriodicTask *originalTask(reinterpret_cast(argument)); + /* Task should not start until explicitly requested, + * but in FreeRTOS, tasks start as soon as they are created if the scheduler + * is running but not if the scheduler is not running. + * To be able to accommodate both cases we check a member which is set in + * #startTask(). If it is not set and we get here, the scheduler was started + * before #startTask() was called and we need to suspend if it is set, + * the scheduler was not running before #startTask() was called and we + * can continue */ + + if (not originalTask->started) { + vTaskSuspend(NULL); + } + + originalTask->taskFunctionality(); + sif::debug << "Polling task " << originalTask->handle + << " returned from taskFunctionality." << std::endl; +} + +ReturnValue_t PeriodicTask::startTask() { + started = true; + + // We must not call resume if scheduler is not started yet + if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) { + vTaskResume(handle); + } + + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t PeriodicTask::sleepFor(uint32_t ms) { + vTaskDelay(pdMS_TO_TICKS(ms)); + return HasReturnvaluesIF::RETURN_OK; +} + +void PeriodicTask::taskFunctionality() { + TickType_t xLastWakeTime; + const TickType_t xPeriod = pdMS_TO_TICKS(this->period * 1000.); + + for (auto const &object: objectList) { + object->initializeAfterTaskCreation(); + } + + /* The xLastWakeTime variable needs to be initialized with the current tick + count. Note that this is the only time the variable is written to + explicitly. After this assignment, xLastWakeTime is updated automatically + internally within vTaskDelayUntil(). */ + xLastWakeTime = xTaskGetTickCount(); + /* Enter the loop that defines the task behavior. */ + for (;;) { + for (auto const& object: objectList) { + object->performOperation(); + } + + checkMissedDeadline(xLastWakeTime, xPeriod); + + vTaskDelayUntil(&xLastWakeTime, xPeriod); + + } +} + +ReturnValue_t PeriodicTask::addComponent(object_id_t object) { + ExecutableObjectIF* newObject = objectManager->get( + object); + if (newObject == nullptr) { + sif::error << "PeriodicTask::addComponent: Invalid object. Make sure" + "it implement ExecutableObjectIF" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + objectList.push_back(newObject); + newObject->setTaskIF(this); + + return HasReturnvaluesIF::RETURN_OK; +} + +uint32_t PeriodicTask::getPeriodMs() const { + return period * 1000; +} + +void PeriodicTask::checkMissedDeadline(const TickType_t xLastWakeTime, + const TickType_t interval) { + /* Check whether deadline was missed while also taking overflows + * into account. Drawing this on paper with a timeline helps to understand + * it. */ + TickType_t currentTickCount = xTaskGetTickCount(); + TickType_t timeToWake = xLastWakeTime + interval; + // Time to wake has not overflown. + if(timeToWake > xLastWakeTime) { + /* If the current time has overflown exclusively or the current + * tick count is simply larger than the time to wake, a deadline was + * missed */ + if((currentTickCount < xLastWakeTime) or (currentTickCount > timeToWake)) { + handleMissedDeadline(); + } + } + /* Time to wake has overflown. A deadline was missed if the current time + * is larger than the time to wake */ + else if((timeToWake < xLastWakeTime) and (currentTickCount > timeToWake)) { + handleMissedDeadline(); + } +} + +TaskHandle_t PeriodicTask::getTaskHandle() { + return handle; +} + +void PeriodicTask::handleMissedDeadline() { +#ifdef DEBUG + sif::warning << "PeriodicTask: " << pcTaskGetName(NULL) << + " missed deadline!\n" << std::flush; +#endif + if(deadlineMissedFunc != nullptr) { + this->deadlineMissedFunc(); + } +} From 4530b1954811727d0f32eea4fd25b8378a153db0 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 27 Aug 2020 16:13:36 +0200 Subject: [PATCH 10/20] header update --- osal/FreeRTOS/PeriodicTask.h | 254 +++++++++++++++++------------------ 1 file changed, 126 insertions(+), 128 deletions(-) diff --git a/osal/FreeRTOS/PeriodicTask.h b/osal/FreeRTOS/PeriodicTask.h index 1cdc3df7..87b8b6d2 100644 --- a/osal/FreeRTOS/PeriodicTask.h +++ b/osal/FreeRTOS/PeriodicTask.h @@ -1,128 +1,126 @@ -#ifndef FRAMEWORK_OSAL_FREERTOS_PERIODICTASK_H_ -#define FRAMEWORK_OSAL_FREERTOS_PERIODICTASK_H_ - -#include "../../osal/FreeRTOS/FreeRTOSTaskIF.h" -#include "../../objectmanager/ObjectManagerIF.h" -#include "../../tasks/PeriodicTaskIF.h" -#include "../../tasks/Typedef.h" - - -#include -#include - -#include - -class ExecutableObjectIF; - -/** - * @brief This class represents a specialized task for - * periodic activities of multiple objects. - * @ingroup task_handling - */ -class PeriodicTask: public PeriodicTaskIF, public FreeRTOSTaskIF { -public: - /** - * Keep in Mind that you need to call before this vTaskStartScheduler()! - * A lot of task parameters are set in "FreeRTOSConfig.h". - * TODO: why does this need to be called before vTaskStartScheduler? - * @details - * The class is initialized without allocated objects. - * These need to be added with #addComponent. - * @param priority - * Sets the priority of a task. Values depend on freeRTOS configuration, - * high number means high priority. - * @param stack_size - * The stack size reserved by the operating system for the task. - * @param setPeriod - * The length of the period with which the task's - * functionality will be executed. It is expressed in clock ticks. - * @param setDeadlineMissedFunc - * The function pointer to the deadline missed function that shall - * be assigned. - */ - PeriodicTask(TaskName name, TaskPriority setPriority, - TaskStackSize setStack, TaskPeriod setPeriod, - TaskDeadlineMissedFunction deadlineMissedFunc); - /** - * @brief Currently, the executed object's lifetime is not coupled with - * the task object's lifetime, so the destructor is empty. - */ - virtual ~PeriodicTask(void); - - /** - * @brief The method to start the task. - * @details The method starts the task with the respective system call. - * Entry point is the taskEntryPoint method described below. - * The address of the task object is passed as an argument - * to the system call. - */ - ReturnValue_t startTask() override; - /** - * Adds an object to the list of objects to be executed. - * The objects are executed in the order added. - * @param object Id of the object to add. - * @return - * -@c RETURN_OK on success - * -@c RETURN_FAILED if the object could not be added. - */ - ReturnValue_t addComponent(object_id_t object) override; - - uint32_t getPeriodMs() const override; - - ReturnValue_t sleepFor(uint32_t ms) override; - - TaskHandle_t getTaskHandle() override; -protected: - bool started; - TaskHandle_t handle; - - //! Typedef for the List of objects. - typedef std::vector ObjectList; - /** - * @brief This attribute holds a list of objects to be executed. - */ - ObjectList objectList; - /** - * @brief The period of the task. - * @details - * The period determines the frequency of the task's execution. - * It is expressed in clock ticks. - */ - TaskPeriod period; - /** - * @brief The pointer to the deadline-missed function. - * @details - * This pointer stores the function that is executed if the task's deadline - * is missed so each may react individually on a timing failure. - * The pointer may be NULL, then nothing happens on missing the deadline. - * The deadline is equal to the next execution of the periodic task. - */ - void (*deadlineMissedFunc)(void); - /** - * @brief This is the function executed in the new task's context. - * @details - * It converts the argument back to the thread object type and copies the - * class instance to the task context. The taskFunctionality method is - * called afterwards. - * @param A pointer to the task object itself is passed as argument. - */ - - static void taskEntryPoint(void* argument); - /** - * @brief The function containing the actual functionality of the task. - * @details - * The method sets and starts the task's period, then enters a loop that is - * repeated as long as the isRunning attribute is true. Within the loop, - * all performOperation methods of the added objects are called. - * Afterwards the checkAndRestartPeriod system call blocks the task until - * the next period. - * On missing the deadline, the deadlineMissedFunction is executed. - */ - void taskFunctionality(void); - - void checkMissedDeadline(const TickType_t xLastWakeTime, - const TickType_t interval); - void handleMissedDeadline(); -}; - -#endif /* PERIODICTASK_H_ */ +#ifndef FSFW_OSAL_FREERTOS_PERIODICTASK_H_ +#define FSFW_OSAL_FREERTOS_PERIODICTASK_H_ + +#include "FreeRTOSTaskIF.h" +#include "../../objectmanager/ObjectManagerIF.h" +#include "../../tasks/PeriodicTaskIF.h" +#include "../../tasks/Typedef.h" + +#include +#include + +#include + +class ExecutableObjectIF; + +/** + * @brief This class represents a specialized task for + * periodic activities of multiple objects. + * @ingroup task_handling + */ +class PeriodicTask: public PeriodicTaskIF, public FreeRTOSTaskIF { +public: + /** + * Keep in Mind that you need to call before this vTaskStartScheduler()! + * A lot of task parameters are set in "FreeRTOSConfig.h". + * @details + * The class is initialized without allocated objects. + * These need to be added with #addComponent. + * @param priority + * Sets the priority of a task. Values depend on freeRTOS configuration, + * high number means high priority. + * @param stack_size + * The stack size reserved by the operating system for the task. + * @param setPeriod + * The length of the period with which the task's + * functionality will be executed. It is expressed in clock ticks. + * @param setDeadlineMissedFunc + * The function pointer to the deadline missed function that shall + * be assigned. + */ + PeriodicTask(TaskName name, TaskPriority setPriority, + TaskStackSize setStack, TaskPeriod setPeriod, + TaskDeadlineMissedFunction deadlineMissedFunc); + /** + * @brief Currently, the executed object's lifetime is not coupled with + * the task object's lifetime, so the destructor is empty. + */ + virtual ~PeriodicTask(void); + + /** + * @brief The method to start the task. + * @details The method starts the task with the respective system call. + * Entry point is the taskEntryPoint method described below. + * The address of the task object is passed as an argument + * to the system call. + */ + ReturnValue_t startTask() override; + /** + * Adds an object to the list of objects to be executed. + * The objects are executed in the order added. + * @param object Id of the object to add. + * @return + * -@c RETURN_OK on success + * -@c RETURN_FAILED if the object could not be added. + */ + ReturnValue_t addComponent(object_id_t object) override; + + uint32_t getPeriodMs() const override; + + ReturnValue_t sleepFor(uint32_t ms) override; + + TaskHandle_t getTaskHandle() override; +protected: + bool started; + TaskHandle_t handle; + + //! Typedef for the List of objects. + typedef std::vector ObjectList; + /** + * @brief This attribute holds a list of objects to be executed. + */ + ObjectList objectList; + /** + * @brief The period of the task. + * @details + * The period determines the frequency of the task's execution. + * It is expressed in clock ticks. + */ + TaskPeriod period; + /** + * @brief The pointer to the deadline-missed function. + * @details + * This pointer stores the function that is executed if the task's deadline + * is missed so each may react individually on a timing failure. + * The pointer may be NULL, then nothing happens on missing the deadline. + * The deadline is equal to the next execution of the periodic task. + */ + void (*deadlineMissedFunc)(void); + /** + * @brief This is the function executed in the new task's context. + * @details + * It converts the argument back to the thread object type and copies the + * class instance to the task context. The taskFunctionality method is + * called afterwards. + * @param A pointer to the task object itself is passed as argument. + */ + + static void taskEntryPoint(void* argument); + /** + * @brief The function containing the actual functionality of the task. + * @details + * The method sets and starts the task's period, then enters a loop that is + * repeated as long as the isRunning attribute is true. Within the loop, + * all performOperation methods of the added objects are called. + * Afterwards the checkAndRestartPeriod system call blocks the task until + * the next period. + * On missing the deadline, the deadlineMissedFunction is executed. + */ + void taskFunctionality(void); + + void checkMissedDeadline(const TickType_t xLastWakeTime, + const TickType_t interval); + void handleMissedDeadline(); +}; + +#endif /* FSFW_OSAL_FREERTOS_PERIODICTASK_H_ */ From 381914886dd445b9af9669af658ac1b00c035e86 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Sep 2020 14:43:53 +0200 Subject: [PATCH 11/20] initAfterTaskCreation added for linux --- osal/linux/PeriodicPosixTask.cpp | 24 +++++++++++++++--------- 1 file changed, 15 insertions(+), 9 deletions(-) diff --git a/osal/linux/PeriodicPosixTask.cpp b/osal/linux/PeriodicPosixTask.cpp index df3694ef..69dbb619 100644 --- a/osal/linux/PeriodicPosixTask.cpp +++ b/osal/linux/PeriodicPosixTask.cpp @@ -46,27 +46,33 @@ ReturnValue_t PeriodicPosixTask::startTask(void){ } void PeriodicPosixTask::taskFunctionality(void){ - if(!started){ + if(not started){ suspend(); } + + for (auto const &object: objectList) { + object->initializeAfterTaskCreation(); + } + uint64_t lastWakeTime = getCurrentMonotonicTimeMs(); //The task's "infinite" inner loop is entered. while (1) { - for (ObjectList::iterator it = objectList.begin(); - it != objectList.end(); ++it) { - (*it)->performOperation(); + for (auto const &object: objectList) { + object->performOperation(); } - if(!PosixThread::delayUntil(&lastWakeTime,periodMs)){ + + if(not PosixThread::delayUntil(&lastWakeTime, periodMs)){ char name[20] = {0}; - int status = pthread_getname_np(pthread_self(),name,sizeof(name)); - if(status==0){ + int status = pthread_getname_np(pthread_self(), name, sizeof(name)); + if(status == 0) { sif::error << "PeriodicPosixTask " << name << ": Deadline " "missed." << std::endl; - }else{ + } + else { sif::error << "PeriodicPosixTask X: Deadline missed. " << status << std::endl; } - if (this->deadlineMissedFunc != NULL) { + if (this->deadlineMissedFunc != nullptr) { this->deadlineMissedFunc(); } } From 4bdac1e0179191c67f559233d8fdebcfedada654 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Sep 2020 14:49:59 +0200 Subject: [PATCH 12/20] changed from mueller/master taken over --- osal/linux/PeriodicPosixTask.cpp | 12 +++++++----- osal/linux/PeriodicPosixTask.h | 2 +- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/osal/linux/PeriodicPosixTask.cpp b/osal/linux/PeriodicPosixTask.cpp index 69dbb619..3c1df6bb 100644 --- a/osal/linux/PeriodicPosixTask.cpp +++ b/osal/linux/PeriodicPosixTask.cpp @@ -5,8 +5,8 @@ PeriodicPosixTask::PeriodicPosixTask(const char* name_, int priority_, size_t stackSize_, uint32_t period_, void(deadlineMissedFunc_)()): - PosixThread(name_,priority_,stackSize_),objectList(),started(false), - periodMs(period_),deadlineMissedFunc(deadlineMissedFunc_) { + PosixThread(name_, priority_, stackSize_), objectList(), started(false), + periodMs(period_), deadlineMissedFunc(deadlineMissedFunc_) { } PeriodicPosixTask::~PeriodicPosixTask() { @@ -25,6 +25,8 @@ ReturnValue_t PeriodicPosixTask::addComponent(object_id_t object) { ExecutableObjectIF* newObject = objectManager->get( object); if (newObject == nullptr) { + sif::error << "PeriodicTask::addComponent: Invalid object. Make sure" + << " it implements ExecutableObjectIF!" << std::endl; return HasReturnvaluesIF::RETURN_FAILED; } objectList.push_back(newObject); @@ -38,15 +40,15 @@ ReturnValue_t PeriodicPosixTask::sleepFor(uint32_t ms) { } -ReturnValue_t PeriodicPosixTask::startTask(void){ +ReturnValue_t PeriodicPosixTask::startTask(void) { started = true; //sif::info << stackSize << std::endl; PosixThread::createTask(&taskEntryPoint,this); return HasReturnvaluesIF::RETURN_OK; } -void PeriodicPosixTask::taskFunctionality(void){ - if(not started){ +void PeriodicPosixTask::taskFunctionality(void) { + if(not started) { suspend(); } diff --git a/osal/linux/PeriodicPosixTask.h b/osal/linux/PeriodicPosixTask.h index f23adf4d..ffee236b 100644 --- a/osal/linux/PeriodicPosixTask.h +++ b/osal/linux/PeriodicPosixTask.h @@ -32,7 +32,7 @@ public: * The address of the task object is passed as an argument * to the system call. */ - ReturnValue_t startTask(void); + ReturnValue_t startTask() override; /** * Adds an object to the list of objects to be executed. * The objects are executed in the order added. From 975fb9832b27372ab2e6c59af2d3642dae5dc7b1 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Sep 2020 14:52:17 +0200 Subject: [PATCH 13/20] added interface ID --- returnvalues/FwClassIds.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/returnvalues/FwClassIds.h b/returnvalues/FwClassIds.h index ddee539e..9ab0bfeb 100644 --- a/returnvalues/FwClassIds.h +++ b/returnvalues/FwClassIds.h @@ -63,7 +63,8 @@ enum { SEMAPHORE_IF, //SPH 57 LOCAL_POOL_OWNER_IF, //LPIF 58 POOL_VARIABLE_IF, //PVA 59 - HOUSEKEEPING_MANAGER, //HKM 60 + DLE_ENCODER, //DLEE 60 + HOUSEKEEPING_MANAGER, //HKM 61 FW_CLASS_ID_COUNT //is actually count + 1 ! }; From 91d5277a94d8bd3e962b23ce22d9ca34d989c811 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Sep 2020 14:54:08 +0200 Subject: [PATCH 14/20] added new returnvalues --- returnvalues/FwClassIds.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/returnvalues/FwClassIds.h b/returnvalues/FwClassIds.h index 9ab0bfeb..781320e3 100644 --- a/returnvalues/FwClassIds.h +++ b/returnvalues/FwClassIds.h @@ -63,8 +63,11 @@ enum { SEMAPHORE_IF, //SPH 57 LOCAL_POOL_OWNER_IF, //LPIF 58 POOL_VARIABLE_IF, //PVA 59 - DLE_ENCODER, //DLEE 60 - HOUSEKEEPING_MANAGER, //HKM 61 + HOUSEKEEPING_MANAGER, //HKM 60 + DLE_ENCODER, //DLEE 61 + PUS_PARSER, //PUSP 62 + SERIAL_ANALYZER, //SERA 63 + PUS_SERVICE_9, // PUS9 64 FW_CLASS_ID_COUNT //is actually count + 1 ! }; From a1155686c5dbb1b844c13ee078ff13579e4de4fb Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sat, 5 Sep 2020 20:18:52 +0200 Subject: [PATCH 15/20] updated host osal --- osal/host/Clock.cpp | 454 +++++++++++++++---------------- osal/host/FixedTimeslotTask.cpp | 382 +++++++++++++------------- osal/host/FixedTimeslotTask.h | 260 +++++++++--------- osal/host/MessageQueue.cpp | 310 ++++++++++----------- osal/host/MessageQueue.h | 460 ++++++++++++++++---------------- osal/host/Mutex.cpp | 80 +++--- osal/host/Mutex.h | 56 ++-- osal/host/MutexFactory.cpp | 56 ++-- osal/host/PeriodicTask.cpp | 352 ++++++++++++------------ osal/host/PeriodicTask.h | 246 ++++++++--------- osal/host/QueueFactory.cpp | 82 +++--- osal/host/QueueMapManager.cpp | 102 +++---- osal/host/QueueMapManager.h | 94 +++---- osal/host/SemaphoreFactory.cpp | 84 +++--- osal/host/TaskFactory.cpp | 110 ++++---- 15 files changed, 1564 insertions(+), 1564 deletions(-) diff --git a/osal/host/Clock.cpp b/osal/host/Clock.cpp index 52d0d5f0..41321eeb 100644 --- a/osal/host/Clock.cpp +++ b/osal/host/Clock.cpp @@ -1,227 +1,227 @@ -#include "../../serviceinterface/ServiceInterfaceStream.h" -#include "../../timemanager/Clock.h" - -#include -#if defined(WIN32) -#include -#elif defined(LINUX) -#include -#endif - -uint16_t Clock::leapSeconds = 0; -MutexIF* Clock::timeMutex = NULL; - -using SystemClock = std::chrono::system_clock; - -uint32_t Clock::getTicksPerSecond(void){ - sif::warning << "Clock::getTicksPerSecond: not implemented yet" << std::endl; - return 0; - //return CLOCKS_PER_SEC; - //uint32_t ticks = sysconf(_SC_CLK_TCK); - //return ticks; -} - -ReturnValue_t Clock::setClock(const TimeOfDay_t* time) { - // do some magic with chrono - sif::warning << "Clock::setClock: not implemented yet" << std::endl; - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t Clock::setClock(const timeval* time) { - // do some magic with chrono -#if defined(WIN32) - return HasReturnvaluesIF::RETURN_OK; -#elif defined(LINUX) - return HasReturnvaluesIF::RETURN_OK; -#else - -#endif - sif::warning << "Clock::getUptime: Not implemented for found OS" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; -} - -ReturnValue_t Clock::getClock_timeval(timeval* time) { -#if defined(WIN32) - auto now = std::chrono::system_clock::now(); - auto secondsChrono = std::chrono::time_point_cast(now); - auto epoch = now.time_since_epoch(); - time->tv_sec = std::chrono::duration_cast(epoch).count(); - auto fraction = now - secondsChrono; - time->tv_usec = std::chrono::duration_cast( - fraction).count(); - return HasReturnvaluesIF::RETURN_OK; -#elif defined(LINUX) - timespec timeUnix; - int status = clock_gettime(CLOCK_REALTIME,&timeUnix); - if(status!=0){ - return HasReturnvaluesIF::RETURN_FAILED; - } - time->tv_sec = timeUnix.tv_sec; - time->tv_usec = timeUnix.tv_nsec / 1000.0; - return HasReturnvaluesIF::RETURN_OK; -#else - sif::warning << "Clock::getUptime: Not implemented for found OS" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; -#endif - -} - -ReturnValue_t Clock::getClock_usecs(uint64_t* time) { - // do some magic with chrono - sif::warning << "Clock::gerClock_usecs: not implemented yet" << std::endl; - return HasReturnvaluesIF::RETURN_OK; -} - -timeval Clock::getUptime() { - timeval timeval; -#if defined(WIN32) - auto uptime = std::chrono::milliseconds(GetTickCount64()); - auto secondsChrono = std::chrono::duration_cast(uptime); - timeval.tv_sec = secondsChrono.count(); - auto fraction = uptime - secondsChrono; - timeval.tv_usec = std::chrono::duration_cast( - fraction).count(); -#elif defined(LINUX) - double uptimeSeconds; - if (std::ifstream("/proc/uptime", std::ios::in) >> uptimeSeconds) - { - // value is rounded down automatically - timeval.tv_sec = uptimeSeconds; - timeval.tv_usec = uptimeSeconds *(double) 1e6 - (timeval.tv_sec *1e6); - } -#else - sif::warning << "Clock::getUptime: Not implemented for found OS" << std::endl; -#endif - return timeval; -} - -ReturnValue_t Clock::getUptime(timeval* uptime) { - *uptime = getUptime(); - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t Clock::getUptime(uint32_t* uptimeMs) { - timeval uptime = getUptime(); - *uptimeMs = uptime.tv_sec * 1000 + uptime.tv_usec / 1000; - return HasReturnvaluesIF::RETURN_OK; -} - - -ReturnValue_t Clock::getDateAndTime(TimeOfDay_t* time) { - // do some magic with chrono (C++20!) - // Right now, the library doesn't have the new features yet. - // so we work around that for now. - auto now = SystemClock::now(); - auto seconds = std::chrono::time_point_cast(now); - auto fraction = now - seconds; - time_t tt = SystemClock::to_time_t(now); - struct tm* timeInfo; - timeInfo = gmtime(&tt); - time->year = timeInfo->tm_year + 1900; - time->month = timeInfo->tm_mon+1; - time->day = timeInfo->tm_mday; - time->hour = timeInfo->tm_hour; - time->minute = timeInfo->tm_min; - time->second = timeInfo->tm_sec; - auto usecond = std::chrono::duration_cast(fraction); - time->usecond = usecond.count(); - - //sif::warning << "Clock::getDateAndTime: not implemented yet" << std::endl; - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t Clock::convertTimeOfDayToTimeval(const TimeOfDay_t* from, - timeval* to) { - struct tm time_tm; - - time_tm.tm_year = from->year - 1900; - time_tm.tm_mon = from->month - 1; - time_tm.tm_mday = from->day; - - time_tm.tm_hour = from->hour; - time_tm.tm_min = from->minute; - time_tm.tm_sec = from->second; - - time_t seconds = mktime(&time_tm); - - to->tv_sec = seconds; - to->tv_usec = from->usecond; - //Fails in 2038.. - return HasReturnvaluesIF::RETURN_OK; - sif::warning << "Clock::convertTimeBla: not implemented yet" << std::endl; - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t Clock::convertTimevalToJD2000(timeval time, double* JD2000) { - *JD2000 = (time.tv_sec - 946728000. + time.tv_usec / 1000000.) / 24. - / 3600.; - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval* tt) { - //SHOULDDO: works not for dates in the past (might have less leap seconds) - if (timeMutex == NULL) { - return HasReturnvaluesIF::RETURN_FAILED; - } - - uint16_t leapSeconds; - ReturnValue_t result = getLeapSeconds(&leapSeconds); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - timeval leapSeconds_timeval = { 0, 0 }; - leapSeconds_timeval.tv_sec = leapSeconds; - - //initial offset between UTC and TAI - timeval UTCtoTAI1972 = { 10, 0 }; - - timeval TAItoTT = { 32, 184000 }; - - *tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT; - - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) { - if(checkOrCreateClockMutex()!=HasReturnvaluesIF::RETURN_OK){ - return HasReturnvaluesIF::RETURN_FAILED; - } - ReturnValue_t result = timeMutex->lockMutex(MutexIF::BLOCKING); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - - leapSeconds = leapSeconds_; - - result = timeMutex->unlockMutex(); - return result; -} - -ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) { - if(timeMutex == nullptr){ - return HasReturnvaluesIF::RETURN_FAILED; - } - ReturnValue_t result = timeMutex->lockMutex(MutexIF::BLOCKING); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - - *leapSeconds_ = leapSeconds; - - result = timeMutex->unlockMutex(); - return result; -} - -ReturnValue_t Clock::checkOrCreateClockMutex(){ - if(timeMutex == nullptr){ - MutexFactory* mutexFactory = MutexFactory::instance(); - if (mutexFactory == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - timeMutex = mutexFactory->createMutex(); - if (timeMutex == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - } - return HasReturnvaluesIF::RETURN_OK; -} +#include "../../serviceinterface/ServiceInterfaceStream.h" +#include "../../timemanager/Clock.h" + +#include +#if defined(WIN32) +#include +#elif defined(LINUX) +#include +#endif + +uint16_t Clock::leapSeconds = 0; +MutexIF* Clock::timeMutex = NULL; + +using SystemClock = std::chrono::system_clock; + +uint32_t Clock::getTicksPerSecond(void){ + sif::warning << "Clock::getTicksPerSecond: not implemented yet" << std::endl; + return 0; + //return CLOCKS_PER_SEC; + //uint32_t ticks = sysconf(_SC_CLK_TCK); + //return ticks; +} + +ReturnValue_t Clock::setClock(const TimeOfDay_t* time) { + // do some magic with chrono + sif::warning << "Clock::setClock: not implemented yet" << std::endl; + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::setClock(const timeval* time) { + // do some magic with chrono +#if defined(WIN32) + return HasReturnvaluesIF::RETURN_OK; +#elif defined(LINUX) + return HasReturnvaluesIF::RETURN_OK; +#else + +#endif + sif::warning << "Clock::getUptime: Not implemented for found OS" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; +} + +ReturnValue_t Clock::getClock_timeval(timeval* time) { +#if defined(WIN32) + auto now = std::chrono::system_clock::now(); + auto secondsChrono = std::chrono::time_point_cast(now); + auto epoch = now.time_since_epoch(); + time->tv_sec = std::chrono::duration_cast(epoch).count(); + auto fraction = now - secondsChrono; + time->tv_usec = std::chrono::duration_cast( + fraction).count(); + return HasReturnvaluesIF::RETURN_OK; +#elif defined(LINUX) + timespec timeUnix; + int status = clock_gettime(CLOCK_REALTIME,&timeUnix); + if(status!=0){ + return HasReturnvaluesIF::RETURN_FAILED; + } + time->tv_sec = timeUnix.tv_sec; + time->tv_usec = timeUnix.tv_nsec / 1000.0; + return HasReturnvaluesIF::RETURN_OK; +#else + sif::warning << "Clock::getUptime: Not implemented for found OS" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; +#endif + +} + +ReturnValue_t Clock::getClock_usecs(uint64_t* time) { + // do some magic with chrono + sif::warning << "Clock::gerClock_usecs: not implemented yet" << std::endl; + return HasReturnvaluesIF::RETURN_OK; +} + +timeval Clock::getUptime() { + timeval timeval; +#if defined(WIN32) + auto uptime = std::chrono::milliseconds(GetTickCount64()); + auto secondsChrono = std::chrono::duration_cast(uptime); + timeval.tv_sec = secondsChrono.count(); + auto fraction = uptime - secondsChrono; + timeval.tv_usec = std::chrono::duration_cast( + fraction).count(); +#elif defined(LINUX) + double uptimeSeconds; + if (std::ifstream("/proc/uptime", std::ios::in) >> uptimeSeconds) + { + // value is rounded down automatically + timeval.tv_sec = uptimeSeconds; + timeval.tv_usec = uptimeSeconds *(double) 1e6 - (timeval.tv_sec *1e6); + } +#else + sif::warning << "Clock::getUptime: Not implemented for found OS" << std::endl; +#endif + return timeval; +} + +ReturnValue_t Clock::getUptime(timeval* uptime) { + *uptime = getUptime(); + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::getUptime(uint32_t* uptimeMs) { + timeval uptime = getUptime(); + *uptimeMs = uptime.tv_sec * 1000 + uptime.tv_usec / 1000; + return HasReturnvaluesIF::RETURN_OK; +} + + +ReturnValue_t Clock::getDateAndTime(TimeOfDay_t* time) { + // do some magic with chrono (C++20!) + // Right now, the library doesn't have the new features yet. + // so we work around that for now. + auto now = SystemClock::now(); + auto seconds = std::chrono::time_point_cast(now); + auto fraction = now - seconds; + time_t tt = SystemClock::to_time_t(now); + struct tm* timeInfo; + timeInfo = gmtime(&tt); + time->year = timeInfo->tm_year + 1900; + time->month = timeInfo->tm_mon+1; + time->day = timeInfo->tm_mday; + time->hour = timeInfo->tm_hour; + time->minute = timeInfo->tm_min; + time->second = timeInfo->tm_sec; + auto usecond = std::chrono::duration_cast(fraction); + time->usecond = usecond.count(); + + //sif::warning << "Clock::getDateAndTime: not implemented yet" << std::endl; + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::convertTimeOfDayToTimeval(const TimeOfDay_t* from, + timeval* to) { + struct tm time_tm; + + time_tm.tm_year = from->year - 1900; + time_tm.tm_mon = from->month - 1; + time_tm.tm_mday = from->day; + + time_tm.tm_hour = from->hour; + time_tm.tm_min = from->minute; + time_tm.tm_sec = from->second; + + time_t seconds = mktime(&time_tm); + + to->tv_sec = seconds; + to->tv_usec = from->usecond; + //Fails in 2038.. + return HasReturnvaluesIF::RETURN_OK; + sif::warning << "Clock::convertTimeBla: not implemented yet" << std::endl; + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::convertTimevalToJD2000(timeval time, double* JD2000) { + *JD2000 = (time.tv_sec - 946728000. + time.tv_usec / 1000000.) / 24. + / 3600.; + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::convertUTCToTT(timeval utc, timeval* tt) { + //SHOULDDO: works not for dates in the past (might have less leap seconds) + if (timeMutex == NULL) { + return HasReturnvaluesIF::RETURN_FAILED; + } + + uint16_t leapSeconds; + ReturnValue_t result = getLeapSeconds(&leapSeconds); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + timeval leapSeconds_timeval = { 0, 0 }; + leapSeconds_timeval.tv_sec = leapSeconds; + + //initial offset between UTC and TAI + timeval UTCtoTAI1972 = { 10, 0 }; + + timeval TAItoTT = { 32, 184000 }; + + *tt = utc + leapSeconds_timeval + UTCtoTAI1972 + TAItoTT; + + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t Clock::setLeapSeconds(const uint16_t leapSeconds_) { + if(checkOrCreateClockMutex()!=HasReturnvaluesIF::RETURN_OK){ + return HasReturnvaluesIF::RETURN_FAILED; + } + ReturnValue_t result = timeMutex->lockMutex(MutexIF::BLOCKING); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + leapSeconds = leapSeconds_; + + result = timeMutex->unlockMutex(); + return result; +} + +ReturnValue_t Clock::getLeapSeconds(uint16_t* leapSeconds_) { + if(timeMutex == nullptr){ + return HasReturnvaluesIF::RETURN_FAILED; + } + ReturnValue_t result = timeMutex->lockMutex(MutexIF::BLOCKING); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + *leapSeconds_ = leapSeconds; + + result = timeMutex->unlockMutex(); + return result; +} + +ReturnValue_t Clock::checkOrCreateClockMutex(){ + if(timeMutex == nullptr){ + MutexFactory* mutexFactory = MutexFactory::instance(); + if (mutexFactory == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + timeMutex = mutexFactory->createMutex(); + if (timeMutex == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + return HasReturnvaluesIF::RETURN_OK; +} diff --git a/osal/host/FixedTimeslotTask.cpp b/osal/host/FixedTimeslotTask.cpp index 1999c9e6..1139badb 100644 --- a/osal/host/FixedTimeslotTask.cpp +++ b/osal/host/FixedTimeslotTask.cpp @@ -1,191 +1,191 @@ -#include "../../osal/host/FixedTimeslotTask.h" - -#include "../../ipc/MutexFactory.h" -#include "../../osal/host/Mutex.h" -#include "../../osal/host/FixedTimeslotTask.h" - -#include "../../serviceinterface/ServiceInterfaceStream.h" -#include "../../tasks/ExecutableObjectIF.h" - -#include -#include - -#if defined(WIN32) -#include -#elif defined(LINUX) -#include -#endif - -FixedTimeslotTask::FixedTimeslotTask(const char *name, TaskPriority setPriority, - TaskStackSize setStack, TaskPeriod setPeriod, - void (*setDeadlineMissedFunc)()) : - started(false), pollingSeqTable(setPeriod*1000), taskName(name), - period(setPeriod), deadlineMissedFunc(setDeadlineMissedFunc) { - // It is propably possible to set task priorities by using the native - // task handles for Windows / Linux - mainThread = std::thread(&FixedTimeslotTask::taskEntryPoint, this, this); -#if defined(WIN32) - /* List of possible priority classes: - * https://docs.microsoft.com/en-us/windows/win32/api/processthreadsapi/ - * nf-processthreadsapi-setpriorityclass - * And respective thread priority numbers: - * https://docs.microsoft.com/en-us/windows/ - * win32/procthread/scheduling-priorities */ - int result = SetPriorityClass( - reinterpret_cast(mainThread.native_handle()), - ABOVE_NORMAL_PRIORITY_CLASS); - if(result != 0) { - sif::error << "FixedTimeslotTask: Windows SetPriorityClass failed with code " - << GetLastError() << std::endl; - } - result = SetThreadPriority( - reinterpret_cast(mainThread.native_handle()), - THREAD_PRIORITY_NORMAL); - if(result != 0) { - sif::error << "FixedTimeslotTask: Windows SetPriorityClass failed with code " - << GetLastError() << std::endl; - } -#elif defined(LINUX) - // we can just copy and paste the code from linux here. -#endif -} - -FixedTimeslotTask::~FixedTimeslotTask(void) { - //Do not delete objects, we were responsible for ptrs only. - terminateThread = true; - if(mainThread.joinable()) { - mainThread.join(); - } - delete this; -} - -void FixedTimeslotTask::taskEntryPoint(void* argument) { - FixedTimeslotTask *originalTask(reinterpret_cast(argument)); - - if (not originalTask->started) { - // we have to suspend/block here until the task is started. - // if semaphores are implemented, use them here. - std::unique_lock lock(initMutex); - initCondition.wait(lock); - } - - this->taskFunctionality(); - sif::debug << "FixedTimeslotTask::taskEntryPoint: " - "Returned from taskFunctionality." << std::endl; -} - -ReturnValue_t FixedTimeslotTask::startTask() { - started = true; - - // Notify task to start. - std::lock_guard lock(initMutex); - initCondition.notify_one(); - - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t FixedTimeslotTask::sleepFor(uint32_t ms) { - std::this_thread::sleep_for(std::chrono::milliseconds(ms)); - return HasReturnvaluesIF::RETURN_OK; -} - -void FixedTimeslotTask::taskFunctionality() { - // A local iterator for the Polling Sequence Table is created to - // find the start time for the first entry. - FixedSlotSequence::SlotListIter slotListIter = pollingSeqTable.current; - // Get start time for first entry. - chron_ms interval(slotListIter->pollingTimeMs); - auto currentStartTime { - std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()) - }; - if(interval.count() > 0) { - delayForInterval(¤tStartTime, interval); - } - - /* Enter the loop that defines the task behavior. */ - for (;;) { - if(terminateThread.load()) { - break; - } - //The component for this slot is executed and the next one is chosen. - this->pollingSeqTable.executeAndAdvance(); - if (not pollingSeqTable.slotFollowsImmediately()) { - // we need to wait before executing the current slot - //this gives us the time to wait: - interval = chron_ms(this->pollingSeqTable.getIntervalToPreviousSlotMs()); - delayForInterval(¤tStartTime, interval); - //TODO deadline missed check - } - } -} - -ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId, - uint32_t slotTimeMs, int8_t executionStep) { - if (objectManager->get(componentId) != nullptr) { - pollingSeqTable.addSlot(componentId, slotTimeMs, executionStep, this); - return HasReturnvaluesIF::RETURN_OK; - } - - sif::error << "Component " << std::hex << componentId << - " not found, not adding it to pst" << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; -} - -ReturnValue_t FixedTimeslotTask::checkSequence() const { - return pollingSeqTable.checkSequence(); -} - -uint32_t FixedTimeslotTask::getPeriodMs() const { - return period * 1000; -} - -bool FixedTimeslotTask::delayForInterval(chron_ms * previousWakeTimeMs, - const chron_ms interval) { - bool shouldDelay = false; - //Get current wakeup time - auto currentStartTime = - std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - /* Generate the tick time at which the task wants to wake. */ - auto nextTimeToWake_ms = (*previousWakeTimeMs) + interval; - - if (currentStartTime < *previousWakeTimeMs) { - /* The tick count has overflowed since this function was - lasted called. In this case the only time we should ever - actually delay is if the wake time has also overflowed, - and the wake time is greater than the tick time. When this - is the case it is as if neither time had overflowed. */ - if ((nextTimeToWake_ms < *previousWakeTimeMs) - && (nextTimeToWake_ms > currentStartTime)) { - shouldDelay = true; - } - } else { - /* The tick time has not overflowed. In this case we will - delay if either the wake time has overflowed, and/or the - tick time is less than the wake time. */ - if ((nextTimeToWake_ms < *previousWakeTimeMs) - || (nextTimeToWake_ms > currentStartTime)) { - shouldDelay = true; - } - } - - /* Update the wake time ready for the next call. */ - - (*previousWakeTimeMs) = nextTimeToWake_ms; - - if (shouldDelay) { - auto sleepTime = std::chrono::duration_cast( - nextTimeToWake_ms - currentStartTime); - std::this_thread::sleep_for(sleepTime); - return true; - } - //We are shifting the time in case the deadline was missed like rtems - (*previousWakeTimeMs) = currentStartTime; - return false; - -} - - - - +#include "../../osal/host/FixedTimeslotTask.h" + +#include "../../ipc/MutexFactory.h" +#include "../../osal/host/Mutex.h" +#include "../../osal/host/FixedTimeslotTask.h" + +#include "../../serviceinterface/ServiceInterfaceStream.h" +#include "../../tasks/ExecutableObjectIF.h" + +#include +#include + +#if defined(WIN32) +#include +#elif defined(LINUX) +#include +#endif + +FixedTimeslotTask::FixedTimeslotTask(const char *name, TaskPriority setPriority, + TaskStackSize setStack, TaskPeriod setPeriod, + void (*setDeadlineMissedFunc)()) : + started(false), pollingSeqTable(setPeriod*1000), taskName(name), + period(setPeriod), deadlineMissedFunc(setDeadlineMissedFunc) { + // It is propably possible to set task priorities by using the native + // task handles for Windows / Linux + mainThread = std::thread(&FixedTimeslotTask::taskEntryPoint, this, this); +#if defined(WIN32) + /* List of possible priority classes: + * https://docs.microsoft.com/en-us/windows/win32/api/processthreadsapi/ + * nf-processthreadsapi-setpriorityclass + * And respective thread priority numbers: + * https://docs.microsoft.com/en-us/windows/ + * win32/procthread/scheduling-priorities */ + int result = SetPriorityClass( + reinterpret_cast(mainThread.native_handle()), + ABOVE_NORMAL_PRIORITY_CLASS); + if(result != 0) { + sif::error << "FixedTimeslotTask: Windows SetPriorityClass failed with code " + << GetLastError() << std::endl; + } + result = SetThreadPriority( + reinterpret_cast(mainThread.native_handle()), + THREAD_PRIORITY_NORMAL); + if(result != 0) { + sif::error << "FixedTimeslotTask: Windows SetPriorityClass failed with code " + << GetLastError() << std::endl; + } +#elif defined(LINUX) + // we can just copy and paste the code from linux here. +#endif +} + +FixedTimeslotTask::~FixedTimeslotTask(void) { + //Do not delete objects, we were responsible for ptrs only. + terminateThread = true; + if(mainThread.joinable()) { + mainThread.join(); + } + delete this; +} + +void FixedTimeslotTask::taskEntryPoint(void* argument) { + FixedTimeslotTask *originalTask(reinterpret_cast(argument)); + + if (not originalTask->started) { + // we have to suspend/block here until the task is started. + // if semaphores are implemented, use them here. + std::unique_lock lock(initMutex); + initCondition.wait(lock); + } + + this->taskFunctionality(); + sif::debug << "FixedTimeslotTask::taskEntryPoint: " + "Returned from taskFunctionality." << std::endl; +} + +ReturnValue_t FixedTimeslotTask::startTask() { + started = true; + + // Notify task to start. + std::lock_guard lock(initMutex); + initCondition.notify_one(); + + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t FixedTimeslotTask::sleepFor(uint32_t ms) { + std::this_thread::sleep_for(std::chrono::milliseconds(ms)); + return HasReturnvaluesIF::RETURN_OK; +} + +void FixedTimeslotTask::taskFunctionality() { + // A local iterator for the Polling Sequence Table is created to + // find the start time for the first entry. + FixedSlotSequence::SlotListIter slotListIter = pollingSeqTable.current; + // Get start time for first entry. + chron_ms interval(slotListIter->pollingTimeMs); + auto currentStartTime { + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + }; + if(interval.count() > 0) { + delayForInterval(¤tStartTime, interval); + } + + /* Enter the loop that defines the task behavior. */ + for (;;) { + if(terminateThread.load()) { + break; + } + //The component for this slot is executed and the next one is chosen. + this->pollingSeqTable.executeAndAdvance(); + if (not pollingSeqTable.slotFollowsImmediately()) { + // we need to wait before executing the current slot + //this gives us the time to wait: + interval = chron_ms(this->pollingSeqTable.getIntervalToPreviousSlotMs()); + delayForInterval(¤tStartTime, interval); + //TODO deadline missed check + } + } +} + +ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId, + uint32_t slotTimeMs, int8_t executionStep) { + if (objectManager->get(componentId) != nullptr) { + pollingSeqTable.addSlot(componentId, slotTimeMs, executionStep, this); + return HasReturnvaluesIF::RETURN_OK; + } + + sif::error << "Component " << std::hex << componentId << + " not found, not adding it to pst" << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; +} + +ReturnValue_t FixedTimeslotTask::checkAndInitializeSequence() const { + return pollingSeqTable.checkSequence(); +} + +uint32_t FixedTimeslotTask::getPeriodMs() const { + return period * 1000; +} + +bool FixedTimeslotTask::delayForInterval(chron_ms * previousWakeTimeMs, + const chron_ms interval) { + bool shouldDelay = false; + //Get current wakeup time + auto currentStartTime = + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()); + /* Generate the tick time at which the task wants to wake. */ + auto nextTimeToWake_ms = (*previousWakeTimeMs) + interval; + + if (currentStartTime < *previousWakeTimeMs) { + /* The tick count has overflowed since this function was + lasted called. In this case the only time we should ever + actually delay is if the wake time has also overflowed, + and the wake time is greater than the tick time. When this + is the case it is as if neither time had overflowed. */ + if ((nextTimeToWake_ms < *previousWakeTimeMs) + && (nextTimeToWake_ms > currentStartTime)) { + shouldDelay = true; + } + } else { + /* The tick time has not overflowed. In this case we will + delay if either the wake time has overflowed, and/or the + tick time is less than the wake time. */ + if ((nextTimeToWake_ms < *previousWakeTimeMs) + || (nextTimeToWake_ms > currentStartTime)) { + shouldDelay = true; + } + } + + /* Update the wake time ready for the next call. */ + + (*previousWakeTimeMs) = nextTimeToWake_ms; + + if (shouldDelay) { + auto sleepTime = std::chrono::duration_cast( + nextTimeToWake_ms - currentStartTime); + std::this_thread::sleep_for(sleepTime); + return true; + } + //We are shifting the time in case the deadline was missed like rtems + (*previousWakeTimeMs) = currentStartTime; + return false; + +} + + + + diff --git a/osal/host/FixedTimeslotTask.h b/osal/host/FixedTimeslotTask.h index bbee94a6..3ea97b97 100644 --- a/osal/host/FixedTimeslotTask.h +++ b/osal/host/FixedTimeslotTask.h @@ -1,130 +1,130 @@ -#ifndef FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ -#define FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ - -#include "../../objectmanager/ObjectManagerIF.h" -#include "../../tasks/FixedSlotSequence.h" -#include "../../tasks/FixedTimeslotTaskIF.h" -#include "../../tasks/Typedef.h" - -#include -#include -#include -#include - -class ExecutableObjectIF; - -/** - * @brief This class represents a task for periodic activities with multiple - * steps and strict timeslot requirements for these steps. - * @details - * @ingroup task_handling - */ -class FixedTimeslotTask: public FixedTimeslotTaskIF { -public: - /** - * @brief Standard constructor of the class. - * @details - * The class is initialized without allocated objects. These need to be - * added with #addComponent. - * @param priority - * @param stack_size - * @param setPeriod - * @param setDeadlineMissedFunc - * The function pointer to the deadline missed function that shall be - * assigned. - */ - FixedTimeslotTask(const char *name, TaskPriority setPriority, - TaskStackSize setStack, TaskPeriod setPeriod, - void (*setDeadlineMissedFunc)()); - /** - * @brief Currently, the executed object's lifetime is not coupled with - * the task object's lifetime, so the destructor is empty. - */ - virtual ~FixedTimeslotTask(void); - - /** - * @brief The method to start the task. - * @details The method starts the task with the respective system call. - * Entry point is the taskEntryPoint method described below. - * The address of the task object is passed as an argument - * to the system call. - */ - ReturnValue_t startTask(void); - - /** - * Add timeslot to the polling sequence table. - * @param componentId - * @param slotTimeMs - * @param executionStep - * @return - */ - ReturnValue_t addSlot(object_id_t componentId, - uint32_t slotTimeMs, int8_t executionStep); - - ReturnValue_t checkSequence() const; - - uint32_t getPeriodMs() const; - - ReturnValue_t sleepFor(uint32_t ms); - -protected: - using chron_ms = std::chrono::milliseconds; - - bool started; - //!< Typedef for the List of objects. - typedef std::vector ObjectList; - std::thread mainThread; - std::atomic terminateThread = false; - - //! Polling sequence table which contains the object to execute - //! and information like the timeslots and the passed execution step. - FixedSlotSequence pollingSeqTable; - - std::condition_variable initCondition; - std::mutex initMutex; - std::string taskName; - /** - * @brief The period of the task. - * @details - * The period determines the frequency of the task's execution. - * It is expressed in clock ticks. - */ - TaskPeriod period; - - /** - * @brief The pointer to the deadline-missed function. - * @details - * This pointer stores the function that is executed if the task's deadline - * is missed. So, each may react individually on a timing failure. - * The pointer may be NULL, then nothing happens on missing the deadline. - * The deadline is equal to the next execution of the periodic task. - */ - void (*deadlineMissedFunc)(void); - /** - * @brief This is the function executed in the new task's context. - * @details - * It converts the argument back to the thread object type and copies the - * class instance to the task context. - * The taskFunctionality method is called afterwards. - * @param A pointer to the task object itself is passed as argument. - */ - - void taskEntryPoint(void* argument); - /** - * @brief The function containing the actual functionality of the task. - * @details - * The method sets and starts the task's period, then enters a loop that is - * repeated as long as the isRunning attribute is true. Within the loop, - * all performOperation methods of the added objects are called. Afterwards - * the checkAndRestartPeriod system call blocks the task until the next - * period. On missing the deadline, the deadlineMissedFunction is executed. - */ - void taskFunctionality(void); - - bool delayForInterval(chron_ms * previousWakeTimeMs, - const chron_ms interval); -}; - - - -#endif /* FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ */ +#ifndef FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ +#define FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ + +#include "../../objectmanager/ObjectManagerIF.h" +#include "../../tasks/FixedSlotSequence.h" +#include "../../tasks/FixedTimeslotTaskIF.h" +#include "../../tasks/Typedef.h" + +#include +#include +#include +#include + +class ExecutableObjectIF; + +/** + * @brief This class represents a task for periodic activities with multiple + * steps and strict timeslot requirements for these steps. + * @details + * @ingroup task_handling + */ +class FixedTimeslotTask: public FixedTimeslotTaskIF { +public: + /** + * @brief Standard constructor of the class. + * @details + * The class is initialized without allocated objects. These need to be + * added with #addComponent. + * @param priority + * @param stack_size + * @param setPeriod + * @param setDeadlineMissedFunc + * The function pointer to the deadline missed function that shall be + * assigned. + */ + FixedTimeslotTask(const char *name, TaskPriority setPriority, + TaskStackSize setStack, TaskPeriod setPeriod, + void (*setDeadlineMissedFunc)()); + /** + * @brief Currently, the executed object's lifetime is not coupled with + * the task object's lifetime, so the destructor is empty. + */ + virtual ~FixedTimeslotTask(void); + + /** + * @brief The method to start the task. + * @details The method starts the task with the respective system call. + * Entry point is the taskEntryPoint method described below. + * The address of the task object is passed as an argument + * to the system call. + */ + ReturnValue_t startTask(void); + + /** + * Add timeslot to the polling sequence table. + * @param componentId + * @param slotTimeMs + * @param executionStep + * @return + */ + ReturnValue_t addSlot(object_id_t componentId, + uint32_t slotTimeMs, int8_t executionStep); + + ReturnValue_t checkAndInitializeSequence() const; + + uint32_t getPeriodMs() const; + + ReturnValue_t sleepFor(uint32_t ms); + +protected: + using chron_ms = std::chrono::milliseconds; + + bool started; + //!< Typedef for the List of objects. + typedef std::vector ObjectList; + std::thread mainThread; + std::atomic terminateThread = false; + + //! Polling sequence table which contains the object to execute + //! and information like the timeslots and the passed execution step. + FixedSlotSequence pollingSeqTable; + + std::condition_variable initCondition; + std::mutex initMutex; + std::string taskName; + /** + * @brief The period of the task. + * @details + * The period determines the frequency of the task's execution. + * It is expressed in clock ticks. + */ + TaskPeriod period; + + /** + * @brief The pointer to the deadline-missed function. + * @details + * This pointer stores the function that is executed if the task's deadline + * is missed. So, each may react individually on a timing failure. + * The pointer may be NULL, then nothing happens on missing the deadline. + * The deadline is equal to the next execution of the periodic task. + */ + void (*deadlineMissedFunc)(void); + /** + * @brief This is the function executed in the new task's context. + * @details + * It converts the argument back to the thread object type and copies the + * class instance to the task context. + * The taskFunctionality method is called afterwards. + * @param A pointer to the task object itself is passed as argument. + */ + + void taskEntryPoint(void* argument); + /** + * @brief The function containing the actual functionality of the task. + * @details + * The method sets and starts the task's period, then enters a loop that is + * repeated as long as the isRunning attribute is true. Within the loop, + * all performOperation methods of the added objects are called. Afterwards + * the checkAndRestartPeriod system call blocks the task until the next + * period. On missing the deadline, the deadlineMissedFunction is executed. + */ + void taskFunctionality(void); + + bool delayForInterval(chron_ms * previousWakeTimeMs, + const chron_ms interval); +}; + + + +#endif /* FRAMEWORK_OSAL_HOST_FIXEDTIMESLOTTASK_H_ */ diff --git a/osal/host/MessageQueue.cpp b/osal/host/MessageQueue.cpp index 6fd42849..8a34282c 100644 --- a/osal/host/MessageQueue.cpp +++ b/osal/host/MessageQueue.cpp @@ -1,155 +1,155 @@ -#include "../../osal/host/MessageQueue.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" -#include "../../osal/host/QueueMapManager.h" -#include "../../ipc/MutexFactory.h" -#include "../../ipc/MutexHelper.h" - -MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize): - messageSize(maxMessageSize), messageDepth(messageDepth) { - queueLock = MutexFactory::instance()->createMutex(); - auto result = QueueMapManager::instance()->addMessageQueue(this, &mqId); - if(result != HasReturnvaluesIF::RETURN_OK) { - sif::error << "MessageQueue: Could not be created" << std::endl; - } -} - -MessageQueue::~MessageQueue() { - MutexFactory::instance()->deleteMutex(queueLock); -} - -ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo, - MessageQueueMessageIF* message, bool ignoreFault) { - return sendMessageFrom(sendTo, message, this->getId(), ignoreFault); -} - -ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) { - return sendToDefaultFrom(message, this->getId()); -} - -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 != 0) { - return sendMessageFrom(this->lastPartner, message, this->getId()); - } else { - return MessageQueueIF::NO_REPLY_PARTNER; - } -} - -ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, - MessageQueueMessageIF* message, MessageQueueId_t sentFrom, - bool ignoreFault) { - return sendMessageFromMessageQueue(sendTo, message, sentFrom, - ignoreFault); -} - -ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message, - MessageQueueId_t* receivedFrom) { - ReturnValue_t status = this->receiveMessage(message); - if(status == HasReturnvaluesIF::RETURN_OK) { - *receivedFrom = this->lastPartner; - } - return status; -} - -ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) { - if(messageQueue.empty()) { - return MessageQueueIF::EMPTY; - } - // not sure this will work.. - //*message = std::move(messageQueue.front()); - MutexHelper mutexLock(queueLock, 20); - MessageQueueMessage* currentMessage = &messageQueue.front(); - std::copy(currentMessage->getBuffer(), - currentMessage->getBuffer() + messageSize, message->getBuffer()); - messageQueue.pop(); - // The last partner is the first uint32_t field in the message - this->lastPartner = message->getSender(); - return HasReturnvaluesIF::RETURN_OK; -} - -MessageQueueId_t MessageQueue::getLastPartner() const { - return lastPartner; -} - -ReturnValue_t MessageQueue::flush(uint32_t* count) { - *count = messageQueue.size(); - // Clears the queue. - messageQueue = std::queue(); - return HasReturnvaluesIF::RETURN_OK; -} - -MessageQueueId_t MessageQueue::getId() const { - return mqId; -} - -void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) { - defaultDestinationSet = true; - this->defaultDestination = defaultDestination; -} - -MessageQueueId_t MessageQueue::getDefaultDestination() const { - return defaultDestination; -} - -bool MessageQueue::isDefaultDestinationSet() const { - return defaultDestinationSet; -} - - -// static core function to send messages. -ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, - MessageQueueMessageIF* message, MessageQueueId_t sentFrom, - bool ignoreFault) { - if(message->getMessageSize() > message->getMaximumMessageSize()) { - // Actually, this should never happen or an error will be emitted - // in MessageQueueMessage. - // But I will still return a failure here. - return HasReturnvaluesIF::RETURN_FAILED; - } - MessageQueue* targetQueue = dynamic_cast( - QueueMapManager::instance()->getMessageQueue(sendTo)); - if(targetQueue == nullptr) { - if(not ignoreFault) { - InternalErrorReporterIF* internalErrorReporter = - objectManager->get( - objects::INTERNAL_ERROR_REPORTER); - if (internalErrorReporter != nullptr) { - internalErrorReporter->queueMessageNotSent(); - } - } - // TODO: Better returnvalue - return HasReturnvaluesIF::RETURN_FAILED; - } - - if(targetQueue->messageQueue.size() < targetQueue->messageDepth) { - MutexHelper mutexLock(targetQueue->queueLock, 20); - // not ideal, works for now though. - MessageQueueMessage* mqmMessage = - dynamic_cast(message); - if(message != nullptr) { - targetQueue->messageQueue.push(*mqmMessage); - } - else { - sif::error << "MessageQueue::sendMessageFromMessageQueue: Message" - "is not MessageQueueMessage!" << std::endl; - } - - } - else { - return MessageQueueIF::FULL; - } - message->setSender(sentFrom); - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t MessageQueue::lockQueue(dur_millis_t lockTimeout) { - return queueLock->lockMutex(lockTimeout); -} - -ReturnValue_t MessageQueue::unlockQueue() { - return queueLock->unlockMutex(); -} +#include "../../osal/host/MessageQueue.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" +#include "../../osal/host/QueueMapManager.h" +#include "../../ipc/MutexFactory.h" +#include "../../ipc/MutexHelper.h" + +MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize): + messageSize(maxMessageSize), messageDepth(messageDepth) { + queueLock = MutexFactory::instance()->createMutex(); + auto result = QueueMapManager::instance()->addMessageQueue(this, &mqId); + if(result != HasReturnvaluesIF::RETURN_OK) { + sif::error << "MessageQueue: Could not be created" << std::endl; + } +} + +MessageQueue::~MessageQueue() { + MutexFactory::instance()->deleteMutex(queueLock); +} + +ReturnValue_t MessageQueue::sendMessage(MessageQueueId_t sendTo, + MessageQueueMessageIF* message, bool ignoreFault) { + return sendMessageFrom(sendTo, message, this->getId(), ignoreFault); +} + +ReturnValue_t MessageQueue::sendToDefault(MessageQueueMessageIF* message) { + return sendToDefaultFrom(message, this->getId()); +} + +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 != 0) { + return sendMessageFrom(this->lastPartner, message, this->getId()); + } else { + return MessageQueueIF::NO_REPLY_PARTNER; + } +} + +ReturnValue_t MessageQueue::sendMessageFrom(MessageQueueId_t sendTo, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom, + bool ignoreFault) { + return sendMessageFromMessageQueue(sendTo, message, sentFrom, + ignoreFault); +} + +ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message, + MessageQueueId_t* receivedFrom) { + ReturnValue_t status = this->receiveMessage(message); + if(status == HasReturnvaluesIF::RETURN_OK) { + *receivedFrom = this->lastPartner; + } + return status; +} + +ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) { + if(messageQueue.empty()) { + return MessageQueueIF::EMPTY; + } + // not sure this will work.. + //*message = std::move(messageQueue.front()); + MutexHelper mutexLock(queueLock, 20); + MessageQueueMessage* currentMessage = &messageQueue.front(); + std::copy(currentMessage->getBuffer(), + currentMessage->getBuffer() + messageSize, message->getBuffer()); + messageQueue.pop(); + // The last partner is the first uint32_t field in the message + this->lastPartner = message->getSender(); + return HasReturnvaluesIF::RETURN_OK; +} + +MessageQueueId_t MessageQueue::getLastPartner() const { + return lastPartner; +} + +ReturnValue_t MessageQueue::flush(uint32_t* count) { + *count = messageQueue.size(); + // Clears the queue. + messageQueue = std::queue(); + return HasReturnvaluesIF::RETURN_OK; +} + +MessageQueueId_t MessageQueue::getId() const { + return mqId; +} + +void MessageQueue::setDefaultDestination(MessageQueueId_t defaultDestination) { + defaultDestinationSet = true; + this->defaultDestination = defaultDestination; +} + +MessageQueueId_t MessageQueue::getDefaultDestination() const { + return defaultDestination; +} + +bool MessageQueue::isDefaultDestinationSet() const { + return defaultDestinationSet; +} + + +// static core function to send messages. +ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom, + bool ignoreFault) { + if(message->getMessageSize() > message->getMaximumMessageSize()) { + // Actually, this should never happen or an error will be emitted + // in MessageQueueMessage. + // But I will still return a failure here. + return HasReturnvaluesIF::RETURN_FAILED; + } + MessageQueue* targetQueue = dynamic_cast( + QueueMapManager::instance()->getMessageQueue(sendTo)); + if(targetQueue == nullptr) { + if(not ignoreFault) { + InternalErrorReporterIF* internalErrorReporter = + objectManager->get( + objects::INTERNAL_ERROR_REPORTER); + if (internalErrorReporter != nullptr) { + internalErrorReporter->queueMessageNotSent(); + } + } + // TODO: Better returnvalue + return HasReturnvaluesIF::RETURN_FAILED; + } + + if(targetQueue->messageQueue.size() < targetQueue->messageDepth) { + MutexHelper mutexLock(targetQueue->queueLock, 20); + // not ideal, works for now though. + MessageQueueMessage* mqmMessage = + dynamic_cast(message); + if(message != nullptr) { + targetQueue->messageQueue.push(*mqmMessage); + } + else { + sif::error << "MessageQueue::sendMessageFromMessageQueue: Message" + "is not MessageQueueMessage!" << std::endl; + } + + } + else { + return MessageQueueIF::FULL; + } + message->setSender(sentFrom); + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t MessageQueue::lockQueue(dur_millis_t lockTimeout) { + return queueLock->lockMutex(lockTimeout); +} + +ReturnValue_t MessageQueue::unlockQueue() { + return queueLock->unlockMutex(); +} diff --git a/osal/host/MessageQueue.h b/osal/host/MessageQueue.h index d2da2402..21a01663 100644 --- a/osal/host/MessageQueue.h +++ b/osal/host/MessageQueue.h @@ -1,230 +1,230 @@ -#ifndef FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ -#define FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ - -#include "../../internalError/InternalErrorReporterIF.h" -#include "../../ipc/MessageQueueIF.h" -#include "../../ipc/MessageQueueMessage.h" -#include "../../ipc/MutexIF.h" -#include "../../timemanager/Clock.h" - -#include -#include - -/** - * @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. - * - * 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 - * 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 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. - */ - 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. - */ - ReturnValue_t sendMessage(MessageQueueId_t sendTo, - MessageQueueMessageIF* message, bool ignoreFault = false) override; - /** - * @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(MessageQueueMessageIF* message) override; - /** - * @brief This operation sends a message to the last communication partner. - * @details This operation simplifies answering an incoming message by using - * the stored lastPartner 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(MessageQueueMessageIF* message) override; - - /** - * @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, - MessageQueueMessageIF* message, MessageQueueId_t sentFrom = NO_QUEUE, - bool ignoreFault = false) override; - - /** - * @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( MessageQueueMessageIF* message, - MessageQueueId_t sentFrom = NO_QUEUE, - 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(MessageQueueMessageIF* message, - MessageQueueId_t *receivedFrom) 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(MessageQueueMessageIF* message) override; - /** - * 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) override; - /** - * @brief This method returns the message queue id of the last - * communication partner. - */ - MessageQueueId_t getLastPartner() const override; - /** - * @brief This method returns the message queue id of this class's - * message queue. - */ - MessageQueueId_t getId() const override; - - /** - * @brief This method is a simple setter for the default destination. - */ - void setDefaultDestination(MessageQueueId_t defaultDestination) override; - /** - * @brief This method is a simple getter for the default destination. - */ - MessageQueueId_t getDefaultDestination() const override; - - bool isDefaultDestinationSet() const override; - - ReturnValue_t lockQueue(dur_millis_t lockTimeout); - ReturnValue_t unlockQueue(); -protected: - /** - * @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, - MessageQueueMessageIF* message, MessageQueueId_t sentFrom = NO_QUEUE, - bool ignoreFault=false); - - //static ReturnValue_t handleSendResult(BaseType_t result, bool ignoreFault); - -private: - std::queue messageQueue; - /** - * @brief The class stores the queue id it got assigned. - * If initialization fails, the queue id is set to zero. - */ - MessageQueueId_t mqId = 0; - size_t messageSize = 0; - size_t messageDepth = 0; - - MutexIF* queueLock; - - bool defaultDestinationSet = false; - MessageQueueId_t defaultDestination = 0; - MessageQueueId_t lastPartner = 0; -}; - -#endif /* FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ */ +#ifndef FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ +#define FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ + +#include "../../internalError/InternalErrorReporterIF.h" +#include "../../ipc/MessageQueueIF.h" +#include "../../ipc/MessageQueueMessage.h" +#include "../../ipc/MutexIF.h" +#include "../../timemanager/Clock.h" + +#include +#include + +/** + * @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. + * + * 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 + * 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 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. + */ + 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. + */ + ReturnValue_t sendMessage(MessageQueueId_t sendTo, + MessageQueueMessageIF* message, bool ignoreFault = false) override; + /** + * @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(MessageQueueMessageIF* message) override; + /** + * @brief This operation sends a message to the last communication partner. + * @details This operation simplifies answering an incoming message by using + * the stored lastPartner 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(MessageQueueMessageIF* message) override; + + /** + * @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, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom = NO_QUEUE, + bool ignoreFault = false) override; + + /** + * @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( MessageQueueMessageIF* message, + MessageQueueId_t sentFrom = NO_QUEUE, + 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(MessageQueueMessageIF* message, + MessageQueueId_t *receivedFrom) 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(MessageQueueMessageIF* message) override; + /** + * 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) override; + /** + * @brief This method returns the message queue id of the last + * communication partner. + */ + MessageQueueId_t getLastPartner() const override; + /** + * @brief This method returns the message queue id of this class's + * message queue. + */ + MessageQueueId_t getId() const override; + + /** + * @brief This method is a simple setter for the default destination. + */ + void setDefaultDestination(MessageQueueId_t defaultDestination) override; + /** + * @brief This method is a simple getter for the default destination. + */ + MessageQueueId_t getDefaultDestination() const override; + + bool isDefaultDestinationSet() const override; + + ReturnValue_t lockQueue(dur_millis_t lockTimeout); + ReturnValue_t unlockQueue(); +protected: + /** + * @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, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom = NO_QUEUE, + bool ignoreFault=false); + + //static ReturnValue_t handleSendResult(BaseType_t result, bool ignoreFault); + +private: + std::queue messageQueue; + /** + * @brief The class stores the queue id it got assigned. + * If initialization fails, the queue id is set to zero. + */ + MessageQueueId_t mqId = 0; + size_t messageSize = 0; + size_t messageDepth = 0; + + MutexIF* queueLock; + + bool defaultDestinationSet = false; + MessageQueueId_t defaultDestination = 0; + MessageQueueId_t lastPartner = 0; +}; + +#endif /* FRAMEWORK_OSAL_HOST_MESSAGEQUEUE_H_ */ diff --git a/osal/host/Mutex.cpp b/osal/host/Mutex.cpp index 28768507..2718f2b9 100644 --- a/osal/host/Mutex.cpp +++ b/osal/host/Mutex.cpp @@ -1,40 +1,40 @@ -#include "../../osal/host/Mutex.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" - -const uint32_t MutexIF::POLLING = 0; -const uint32_t MutexIF::BLOCKING = 0xffffffff; - -ReturnValue_t Mutex::lockMutex(uint32_t timeoutMs) { - if(timeoutMs == MutexIF::BLOCKING) { - mutex.lock(); - locked = true; - return HasReturnvaluesIF::RETURN_OK; - } - else if(timeoutMs == MutexIF::POLLING) { - if(mutex.try_lock()) { - locked = true; - return HasReturnvaluesIF::RETURN_OK; - } - } - else if(timeoutMs > MutexIF::POLLING){ - auto chronoMs = std::chrono::milliseconds(timeoutMs); - if(mutex.try_lock_for(chronoMs)) { - locked = true; - return HasReturnvaluesIF::RETURN_OK; - } - } - return MutexIF::MUTEX_TIMEOUT; -} - -ReturnValue_t Mutex::unlockMutex() { - if(not locked) { - return MutexIF::CURR_THREAD_DOES_NOT_OWN_MUTEX; - } - mutex.unlock(); - locked = false; - return HasReturnvaluesIF::RETURN_OK; -} - -std::timed_mutex* Mutex::getMutexHandle() { - return &mutex; -} +#include "../../osal/host/Mutex.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" + +const uint32_t MutexIF::POLLING = 0; +const uint32_t MutexIF::BLOCKING = 0xffffffff; + +ReturnValue_t Mutex::lockMutex(uint32_t timeoutMs) { + if(timeoutMs == MutexIF::BLOCKING) { + mutex.lock(); + locked = true; + return HasReturnvaluesIF::RETURN_OK; + } + else if(timeoutMs == MutexIF::POLLING) { + if(mutex.try_lock()) { + locked = true; + return HasReturnvaluesIF::RETURN_OK; + } + } + else if(timeoutMs > MutexIF::POLLING){ + auto chronoMs = std::chrono::milliseconds(timeoutMs); + if(mutex.try_lock_for(chronoMs)) { + locked = true; + return HasReturnvaluesIF::RETURN_OK; + } + } + return MutexIF::MUTEX_TIMEOUT; +} + +ReturnValue_t Mutex::unlockMutex() { + if(not locked) { + return MutexIF::CURR_THREAD_DOES_NOT_OWN_MUTEX; + } + mutex.unlock(); + locked = false; + return HasReturnvaluesIF::RETURN_OK; +} + +std::timed_mutex* Mutex::getMutexHandle() { + return &mutex; +} diff --git a/osal/host/Mutex.h b/osal/host/Mutex.h index 4d49bac7..4461e5f2 100644 --- a/osal/host/Mutex.h +++ b/osal/host/Mutex.h @@ -1,28 +1,28 @@ -#ifndef FRAMEWORK_OSAL_FREERTOS_MUTEX_H_ -#define FRAMEWORK_OSAL_FREERTOS_MUTEX_H_ - -#include "../../ipc/MutexIF.h" - -#include - -/** - * @brief OS component to implement MUTual EXclusion - * - * @details - * Mutexes are binary semaphores which include a priority inheritance mechanism. - * Documentation: https://www.freertos.org/Real-time-embedded-RTOS-mutexes.html - * @ingroup osal - */ -class Mutex : public MutexIF { -public: - Mutex() = default; - ReturnValue_t lockMutex(uint32_t timeoutMs = MutexIF::BLOCKING) override; - ReturnValue_t unlockMutex() override; - - std::timed_mutex* getMutexHandle(); -private: - bool locked = false; - std::timed_mutex mutex; -}; - -#endif /* FRAMEWORK_FREERTOS_MUTEX_H_ */ +#ifndef FRAMEWORK_OSAL_FREERTOS_MUTEX_H_ +#define FRAMEWORK_OSAL_FREERTOS_MUTEX_H_ + +#include "../../ipc/MutexIF.h" + +#include + +/** + * @brief OS component to implement MUTual EXclusion + * + * @details + * Mutexes are binary semaphores which include a priority inheritance mechanism. + * Documentation: https://www.freertos.org/Real-time-embedded-RTOS-mutexes.html + * @ingroup osal + */ +class Mutex : public MutexIF { +public: + Mutex() = default; + ReturnValue_t lockMutex(uint32_t timeoutMs = MutexIF::BLOCKING) override; + ReturnValue_t unlockMutex() override; + + std::timed_mutex* getMutexHandle(); +private: + bool locked = false; + std::timed_mutex mutex; +}; + +#endif /* FRAMEWORK_FREERTOS_MUTEX_H_ */ diff --git a/osal/host/MutexFactory.cpp b/osal/host/MutexFactory.cpp index 284ce59f..bf7707d1 100644 --- a/osal/host/MutexFactory.cpp +++ b/osal/host/MutexFactory.cpp @@ -1,28 +1,28 @@ -#include "../../ipc/MutexFactory.h" -#include "../../osal/host/Mutex.h" - -//TODO: Different variant than the lazy loading in QueueFactory. -//What's better and why? -> one is on heap the other on bss/data -//MutexFactory* MutexFactory::factoryInstance = new MutexFactory(); -MutexFactory* MutexFactory::factoryInstance = nullptr; - -MutexFactory::MutexFactory() { -} - -MutexFactory::~MutexFactory() { -} - -MutexFactory* MutexFactory::instance() { - if (factoryInstance == nullptr){ - factoryInstance = new MutexFactory(); - } - return MutexFactory::factoryInstance; -} - -MutexIF* MutexFactory::createMutex() { - return new Mutex(); -} - -void MutexFactory::deleteMutex(MutexIF* mutex) { - delete mutex; -} +#include "../../ipc/MutexFactory.h" +#include "../../osal/host/Mutex.h" + +//TODO: Different variant than the lazy loading in QueueFactory. +//What's better and why? -> one is on heap the other on bss/data +//MutexFactory* MutexFactory::factoryInstance = new MutexFactory(); +MutexFactory* MutexFactory::factoryInstance = nullptr; + +MutexFactory::MutexFactory() { +} + +MutexFactory::~MutexFactory() { +} + +MutexFactory* MutexFactory::instance() { + if (factoryInstance == nullptr){ + factoryInstance = new MutexFactory(); + } + return MutexFactory::factoryInstance; +} + +MutexIF* MutexFactory::createMutex() { + return new Mutex(); +} + +void MutexFactory::deleteMutex(MutexIF* mutex) { + delete mutex; +} diff --git a/osal/host/PeriodicTask.cpp b/osal/host/PeriodicTask.cpp index c4cf9f56..0b5f8bbe 100644 --- a/osal/host/PeriodicTask.cpp +++ b/osal/host/PeriodicTask.cpp @@ -1,176 +1,176 @@ -#include "../../ipc/MutexFactory.h" -#include "../../osal/host/Mutex.h" -#include "../../osal/host/PeriodicTask.h" - -#include "../../serviceinterface/ServiceInterfaceStream.h" -#include "../../tasks/ExecutableObjectIF.h" - -#include -#include - -#if defined(WIN32) -#include -#elif defined(LINUX) -#include -#endif - -PeriodicTask::PeriodicTask(const char *name, TaskPriority setPriority, - TaskStackSize setStack, TaskPeriod setPeriod, - void (*setDeadlineMissedFunc)()) : - started(false), taskName(name), period(setPeriod), - deadlineMissedFunc(setDeadlineMissedFunc) { - // It is propably possible to set task priorities by using the native - // task handles for Windows / Linux - mainThread = std::thread(&PeriodicTask::taskEntryPoint, this, this); -#if defined(WIN32) - /* List of possible priority classes: - * https://docs.microsoft.com/en-us/windows/win32/api/processthreadsapi/ - * nf-processthreadsapi-setpriorityclass - * And respective thread priority numbers: - * https://docs.microsoft.com/en-us/windows/ - * win32/procthread/scheduling-priorities */ - int result = SetPriorityClass( - reinterpret_cast(mainThread.native_handle()), - ABOVE_NORMAL_PRIORITY_CLASS); - if(result != 0) { - sif::error << "PeriodicTask: Windows SetPriorityClass failed with code " - << GetLastError() << std::endl; - } - result = SetThreadPriority( - reinterpret_cast(mainThread.native_handle()), - THREAD_PRIORITY_NORMAL); - if(result != 0) { - sif::error << "PeriodicTask: Windows SetPriorityClass failed with code " - << GetLastError() << std::endl; - } -#elif defined(LINUX) - // we can just copy and paste the code from linux here. -#endif -} - -PeriodicTask::~PeriodicTask(void) { - //Do not delete objects, we were responsible for ptrs only. - terminateThread = true; - if(mainThread.joinable()) { - mainThread.join(); - } - delete this; -} - -void PeriodicTask::taskEntryPoint(void* argument) { - PeriodicTask *originalTask(reinterpret_cast(argument)); - - - if (not originalTask->started) { - // we have to suspend/block here until the task is started. - // if semaphores are implemented, use them here. - std::unique_lock lock(initMutex); - initCondition.wait(lock); - } - - this->taskFunctionality(); - sif::debug << "PeriodicTask::taskEntryPoint: " - "Returned from taskFunctionality." << std::endl; -} - -ReturnValue_t PeriodicTask::startTask() { - started = true; - - // Notify task to start. - std::lock_guard lock(initMutex); - initCondition.notify_one(); - - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t PeriodicTask::sleepFor(uint32_t ms) { - std::this_thread::sleep_for(std::chrono::milliseconds(ms)); - return HasReturnvaluesIF::RETURN_OK; -} - -void PeriodicTask::taskFunctionality() { - std::chrono::milliseconds periodChrono(static_cast(period*1000)); - auto currentStartTime { - std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()) - }; - auto nextStartTime{ currentStartTime }; - - /* Enter the loop that defines the task behavior. */ - for (;;) { - if(terminateThread.load()) { - break; - } - for (ObjectList::iterator it = objectList.begin(); - it != objectList.end(); ++it) { - (*it)->performOperation(); - } - if(not delayForInterval(¤tStartTime, periodChrono)) { - sif::warning << "PeriodicTask: " << taskName << - " missed deadline!\n" << std::flush; - if(deadlineMissedFunc != nullptr) { - this->deadlineMissedFunc(); - } - } - } -} - -ReturnValue_t PeriodicTask::addComponent(object_id_t object) { - ExecutableObjectIF* newObject = objectManager->get( - object); - if (newObject == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - objectList.push_back(newObject); - return HasReturnvaluesIF::RETURN_OK; -} - -uint32_t PeriodicTask::getPeriodMs() const { - return period * 1000; -} - -bool PeriodicTask::delayForInterval(chron_ms* previousWakeTimeMs, - const chron_ms interval) { - bool shouldDelay = false; - //Get current wakeup time - auto currentStartTime = - std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()); - /* Generate the tick time at which the task wants to wake. */ - auto nextTimeToWake_ms = (*previousWakeTimeMs) + interval; - - if (currentStartTime < *previousWakeTimeMs) { - /* The tick count has overflowed since this function was - lasted called. In this case the only time we should ever - actually delay is if the wake time has also overflowed, - and the wake time is greater than the tick time. When this - is the case it is as if neither time had overflowed. */ - if ((nextTimeToWake_ms < *previousWakeTimeMs) - && (nextTimeToWake_ms > currentStartTime)) { - shouldDelay = true; - } - } else { - /* The tick time has not overflowed. In this case we will - delay if either the wake time has overflowed, and/or the - tick time is less than the wake time. */ - if ((nextTimeToWake_ms < *previousWakeTimeMs) - || (nextTimeToWake_ms > currentStartTime)) { - shouldDelay = true; - } - } - - /* Update the wake time ready for the next call. */ - - (*previousWakeTimeMs) = nextTimeToWake_ms; - - if (shouldDelay) { - auto sleepTime = std::chrono::duration_cast( - nextTimeToWake_ms - currentStartTime); - std::this_thread::sleep_for(sleepTime); - return true; - } - //We are shifting the time in case the deadline was missed like rtems - (*previousWakeTimeMs) = currentStartTime; - return false; - -} +#include "../../ipc/MutexFactory.h" +#include "../../osal/host/Mutex.h" +#include "../../osal/host/PeriodicTask.h" + +#include "../../serviceinterface/ServiceInterfaceStream.h" +#include "../../tasks/ExecutableObjectIF.h" + +#include +#include + +#if defined(WIN32) +#include +#elif defined(LINUX) +#include +#endif + +PeriodicTask::PeriodicTask(const char *name, TaskPriority setPriority, + TaskStackSize setStack, TaskPeriod setPeriod, + void (*setDeadlineMissedFunc)()) : + started(false), taskName(name), period(setPeriod), + deadlineMissedFunc(setDeadlineMissedFunc) { + // It is propably possible to set task priorities by using the native + // task handles for Windows / Linux + mainThread = std::thread(&PeriodicTask::taskEntryPoint, this, this); +#if defined(WIN32) + /* List of possible priority classes: + * https://docs.microsoft.com/en-us/windows/win32/api/processthreadsapi/ + * nf-processthreadsapi-setpriorityclass + * And respective thread priority numbers: + * https://docs.microsoft.com/en-us/windows/ + * win32/procthread/scheduling-priorities */ + int result = SetPriorityClass( + reinterpret_cast(mainThread.native_handle()), + ABOVE_NORMAL_PRIORITY_CLASS); + if(result != 0) { + sif::error << "PeriodicTask: Windows SetPriorityClass failed with code " + << GetLastError() << std::endl; + } + result = SetThreadPriority( + reinterpret_cast(mainThread.native_handle()), + THREAD_PRIORITY_NORMAL); + if(result != 0) { + sif::error << "PeriodicTask: Windows SetPriorityClass failed with code " + << GetLastError() << std::endl; + } +#elif defined(LINUX) + // we can just copy and paste the code from linux here. +#endif +} + +PeriodicTask::~PeriodicTask(void) { + //Do not delete objects, we were responsible for ptrs only. + terminateThread = true; + if(mainThread.joinable()) { + mainThread.join(); + } + delete this; +} + +void PeriodicTask::taskEntryPoint(void* argument) { + PeriodicTask *originalTask(reinterpret_cast(argument)); + + + if (not originalTask->started) { + // we have to suspend/block here until the task is started. + // if semaphores are implemented, use them here. + std::unique_lock lock(initMutex); + initCondition.wait(lock); + } + + this->taskFunctionality(); + sif::debug << "PeriodicTask::taskEntryPoint: " + "Returned from taskFunctionality." << std::endl; +} + +ReturnValue_t PeriodicTask::startTask() { + started = true; + + // Notify task to start. + std::lock_guard lock(initMutex); + initCondition.notify_one(); + + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t PeriodicTask::sleepFor(uint32_t ms) { + std::this_thread::sleep_for(std::chrono::milliseconds(ms)); + return HasReturnvaluesIF::RETURN_OK; +} + +void PeriodicTask::taskFunctionality() { + std::chrono::milliseconds periodChrono(static_cast(period*1000)); + auto currentStartTime { + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + }; + auto nextStartTime{ currentStartTime }; + + /* Enter the loop that defines the task behavior. */ + for (;;) { + if(terminateThread.load()) { + break; + } + for (ObjectList::iterator it = objectList.begin(); + it != objectList.end(); ++it) { + (*it)->performOperation(); + } + if(not delayForInterval(¤tStartTime, periodChrono)) { + sif::warning << "PeriodicTask: " << taskName << + " missed deadline!\n" << std::flush; + if(deadlineMissedFunc != nullptr) { + this->deadlineMissedFunc(); + } + } + } +} + +ReturnValue_t PeriodicTask::addComponent(object_id_t object) { + ExecutableObjectIF* newObject = objectManager->get( + object); + if (newObject == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + objectList.push_back(newObject); + return HasReturnvaluesIF::RETURN_OK; +} + +uint32_t PeriodicTask::getPeriodMs() const { + return period * 1000; +} + +bool PeriodicTask::delayForInterval(chron_ms* previousWakeTimeMs, + const chron_ms interval) { + bool shouldDelay = false; + //Get current wakeup time + auto currentStartTime = + std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()); + /* Generate the tick time at which the task wants to wake. */ + auto nextTimeToWake_ms = (*previousWakeTimeMs) + interval; + + if (currentStartTime < *previousWakeTimeMs) { + /* The tick count has overflowed since this function was + lasted called. In this case the only time we should ever + actually delay is if the wake time has also overflowed, + and the wake time is greater than the tick time. When this + is the case it is as if neither time had overflowed. */ + if ((nextTimeToWake_ms < *previousWakeTimeMs) + && (nextTimeToWake_ms > currentStartTime)) { + shouldDelay = true; + } + } else { + /* The tick time has not overflowed. In this case we will + delay if either the wake time has overflowed, and/or the + tick time is less than the wake time. */ + if ((nextTimeToWake_ms < *previousWakeTimeMs) + || (nextTimeToWake_ms > currentStartTime)) { + shouldDelay = true; + } + } + + /* Update the wake time ready for the next call. */ + + (*previousWakeTimeMs) = nextTimeToWake_ms; + + if (shouldDelay) { + auto sleepTime = std::chrono::duration_cast( + nextTimeToWake_ms - currentStartTime); + std::this_thread::sleep_for(sleepTime); + return true; + } + //We are shifting the time in case the deadline was missed like rtems + (*previousWakeTimeMs) = currentStartTime; + return false; + +} diff --git a/osal/host/PeriodicTask.h b/osal/host/PeriodicTask.h index 7eb768cb..7689788a 100644 --- a/osal/host/PeriodicTask.h +++ b/osal/host/PeriodicTask.h @@ -1,123 +1,123 @@ -#ifndef FRAMEWORK_OSAL_HOST_PERIODICTASK_H_ -#define FRAMEWORK_OSAL_HOST_PERIODICTASK_H_ - -#include "../../objectmanager/ObjectManagerIF.h" -#include "../../tasks/PeriodicTaskIF.h" -#include "../../tasks/Typedef.h" - -#include -#include -#include -#include - -class ExecutableObjectIF; - -/** - * @brief This class represents a specialized task for - * periodic activities of multiple objects. - * @details - * - * @ingroup task_handling - */ -class PeriodicTask: public PeriodicTaskIF { -public: - /** - * @brief Standard constructor of the class. - * @details - * The class is initialized without allocated objects. These need to be - * added with #addComponent. - * @param priority - * @param stack_size - * @param setPeriod - * @param setDeadlineMissedFunc - * The function pointer to the deadline missed function that shall be - * assigned. - */ - PeriodicTask(const char *name, TaskPriority setPriority, TaskStackSize setStack, - TaskPeriod setPeriod,void (*setDeadlineMissedFunc)()); - /** - * @brief Currently, the executed object's lifetime is not coupled with - * the task object's lifetime, so the destructor is empty. - */ - virtual ~PeriodicTask(void); - - /** - * @brief The method to start the task. - * @details The method starts the task with the respective system call. - * Entry point is the taskEntryPoint method described below. - * The address of the task object is passed as an argument - * to the system call. - */ - ReturnValue_t startTask(void); - /** - * Adds an object to the list of objects to be executed. - * The objects are executed in the order added. - * @param object Id of the object to add. - * @return - * -@c RETURN_OK on success - * -@c RETURN_FAILED if the object could not be added. - */ - ReturnValue_t addComponent(object_id_t object); - - uint32_t getPeriodMs() const; - - ReturnValue_t sleepFor(uint32_t ms); - -protected: - using chron_ms = std::chrono::milliseconds; - bool started; - //!< Typedef for the List of objects. - typedef std::vector ObjectList; - std::thread mainThread; - std::atomic terminateThread = false; - - /** - * @brief This attribute holds a list of objects to be executed. - */ - ObjectList objectList; - - std::condition_variable initCondition; - std::mutex initMutex; - std::string taskName; - /** - * @brief The period of the task. - * @details - * The period determines the frequency of the task's execution. - * It is expressed in clock ticks. - */ - TaskPeriod period; - /** - * @brief The pointer to the deadline-missed function. - * @details - * This pointer stores the function that is executed if the task's deadline - * is missed. So, each may react individually on a timing failure. - * The pointer may be NULL, then nothing happens on missing the deadline. - * The deadline is equal to the next execution of the periodic task. - */ - void (*deadlineMissedFunc)(void); - /** - * @brief This is the function executed in the new task's context. - * @details - * It converts the argument back to the thread object type and copies the - * class instance to the task context. - * The taskFunctionality method is called afterwards. - * @param A pointer to the task object itself is passed as argument. - */ - - void taskEntryPoint(void* argument); - /** - * @brief The function containing the actual functionality of the task. - * @details - * The method sets and starts the task's period, then enters a loop that is - * repeated as long as the isRunning attribute is true. Within the loop, - * all performOperation methods of the added objects are called. Afterwards - * the checkAndRestartPeriod system call blocks the task until the next - * period. On missing the deadline, the deadlineMissedFunction is executed. - */ - void taskFunctionality(void); - - bool delayForInterval(chron_ms * previousWakeTimeMs, - const chron_ms interval); -}; - -#endif /* PERIODICTASK_H_ */ +#ifndef FRAMEWORK_OSAL_HOST_PERIODICTASK_H_ +#define FRAMEWORK_OSAL_HOST_PERIODICTASK_H_ + +#include "../../objectmanager/ObjectManagerIF.h" +#include "../../tasks/PeriodicTaskIF.h" +#include "../../tasks/Typedef.h" + +#include +#include +#include +#include + +class ExecutableObjectIF; + +/** + * @brief This class represents a specialized task for + * periodic activities of multiple objects. + * @details + * + * @ingroup task_handling + */ +class PeriodicTask: public PeriodicTaskIF { +public: + /** + * @brief Standard constructor of the class. + * @details + * The class is initialized without allocated objects. These need to be + * added with #addComponent. + * @param priority + * @param stack_size + * @param setPeriod + * @param setDeadlineMissedFunc + * The function pointer to the deadline missed function that shall be + * assigned. + */ + PeriodicTask(const char *name, TaskPriority setPriority, TaskStackSize setStack, + TaskPeriod setPeriod,void (*setDeadlineMissedFunc)()); + /** + * @brief Currently, the executed object's lifetime is not coupled with + * the task object's lifetime, so the destructor is empty. + */ + virtual ~PeriodicTask(void); + + /** + * @brief The method to start the task. + * @details The method starts the task with the respective system call. + * Entry point is the taskEntryPoint method described below. + * The address of the task object is passed as an argument + * to the system call. + */ + ReturnValue_t startTask(void); + /** + * Adds an object to the list of objects to be executed. + * The objects are executed in the order added. + * @param object Id of the object to add. + * @return + * -@c RETURN_OK on success + * -@c RETURN_FAILED if the object could not be added. + */ + ReturnValue_t addComponent(object_id_t object); + + uint32_t getPeriodMs() const; + + ReturnValue_t sleepFor(uint32_t ms); + +protected: + using chron_ms = std::chrono::milliseconds; + bool started; + //!< Typedef for the List of objects. + typedef std::vector ObjectList; + std::thread mainThread; + std::atomic terminateThread = false; + + /** + * @brief This attribute holds a list of objects to be executed. + */ + ObjectList objectList; + + std::condition_variable initCondition; + std::mutex initMutex; + std::string taskName; + /** + * @brief The period of the task. + * @details + * The period determines the frequency of the task's execution. + * It is expressed in clock ticks. + */ + TaskPeriod period; + /** + * @brief The pointer to the deadline-missed function. + * @details + * This pointer stores the function that is executed if the task's deadline + * is missed. So, each may react individually on a timing failure. + * The pointer may be NULL, then nothing happens on missing the deadline. + * The deadline is equal to the next execution of the periodic task. + */ + void (*deadlineMissedFunc)(void); + /** + * @brief This is the function executed in the new task's context. + * @details + * It converts the argument back to the thread object type and copies the + * class instance to the task context. + * The taskFunctionality method is called afterwards. + * @param A pointer to the task object itself is passed as argument. + */ + + void taskEntryPoint(void* argument); + /** + * @brief The function containing the actual functionality of the task. + * @details + * The method sets and starts the task's period, then enters a loop that is + * repeated as long as the isRunning attribute is true. Within the loop, + * all performOperation methods of the added objects are called. Afterwards + * the checkAndRestartPeriod system call blocks the task until the next + * period. On missing the deadline, the deadlineMissedFunction is executed. + */ + void taskFunctionality(void); + + bool delayForInterval(chron_ms * previousWakeTimeMs, + const chron_ms interval); +}; + +#endif /* PERIODICTASK_H_ */ diff --git a/osal/host/QueueFactory.cpp b/osal/host/QueueFactory.cpp index 052671ca..da3fea55 100644 --- a/osal/host/QueueFactory.cpp +++ b/osal/host/QueueFactory.cpp @@ -1,41 +1,41 @@ -#include "../../ipc/QueueFactory.h" -#include "../../osal/host/MessageQueue.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" -#include - -QueueFactory* QueueFactory::factoryInstance = nullptr; - - -ReturnValue_t MessageQueueSenderIF::sendMessage(MessageQueueId_t sendTo, - MessageQueueMessageIF* message, MessageQueueId_t sentFrom, - bool ignoreFault) { - return MessageQueue::sendMessageFromMessageQueue(sendTo,message, - sentFrom,ignoreFault); - return HasReturnvaluesIF::RETURN_OK; -} - -QueueFactory* QueueFactory::instance() { - if (factoryInstance == nullptr) { - factoryInstance = new QueueFactory; - } - return factoryInstance; -} - -QueueFactory::QueueFactory() { -} - -QueueFactory::~QueueFactory() { -} - -MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, - size_t maxMessageSize) { - // A thread-safe queue can be implemented by using a combination - // of std::queue and std::mutex. This uses dynamic memory allocation - // which could be alleviated by using a custom allocator, external library - // (etl::queue) or simply using std::queue, we're on a host machine anyway. - return new MessageQueue(messageDepth, maxMessageSize); -} - -void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) { - delete queue; -} +#include "../../ipc/QueueFactory.h" +#include "../../osal/host/MessageQueue.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" +#include + +QueueFactory* QueueFactory::factoryInstance = nullptr; + + +ReturnValue_t MessageQueueSenderIF::sendMessage(MessageQueueId_t sendTo, + MessageQueueMessageIF* message, MessageQueueId_t sentFrom, + bool ignoreFault) { + return MessageQueue::sendMessageFromMessageQueue(sendTo,message, + sentFrom,ignoreFault); + return HasReturnvaluesIF::RETURN_OK; +} + +QueueFactory* QueueFactory::instance() { + if (factoryInstance == nullptr) { + factoryInstance = new QueueFactory; + } + return factoryInstance; +} + +QueueFactory::QueueFactory() { +} + +QueueFactory::~QueueFactory() { +} + +MessageQueueIF* QueueFactory::createMessageQueue(uint32_t messageDepth, + size_t maxMessageSize) { + // A thread-safe queue can be implemented by using a combination + // of std::queue and std::mutex. This uses dynamic memory allocation + // which could be alleviated by using a custom allocator, external library + // (etl::queue) or simply using std::queue, we're on a host machine anyway. + return new MessageQueue(messageDepth, maxMessageSize); +} + +void QueueFactory::deleteMessageQueue(MessageQueueIF* queue) { + delete queue; +} diff --git a/osal/host/QueueMapManager.cpp b/osal/host/QueueMapManager.cpp index 35db204e..47687bc1 100644 --- a/osal/host/QueueMapManager.cpp +++ b/osal/host/QueueMapManager.cpp @@ -1,51 +1,51 @@ -#include "../../ipc/MutexFactory.h" -#include "../../ipc/MutexHelper.h" -#include "../../osal/host/QueueMapManager.h" - -QueueMapManager* QueueMapManager::mqManagerInstance = nullptr; - -QueueMapManager::QueueMapManager() { - mapLock = MutexFactory::instance()->createMutex(); -} - -QueueMapManager* QueueMapManager::instance() { - if (mqManagerInstance == nullptr){ - mqManagerInstance = new QueueMapManager(); - } - return QueueMapManager::mqManagerInstance; -} - -ReturnValue_t QueueMapManager::addMessageQueue( - MessageQueueIF* queueToInsert, MessageQueueId_t* id) { - // Not thread-safe, but it is assumed all message queues are created - // at software initialization now. If this is to be made thread-safe in - // the future, it propably would be sufficient to lock the increment - // operation here - uint32_t currentId = queueCounter++; - auto returnPair = queueMap.emplace(currentId, queueToInsert); - if(not returnPair.second) { - // this should never happen for the atomic variable. - sif::error << "QueueMapManager: This ID is already inside the map!" - << std::endl; - return HasReturnvaluesIF::RETURN_FAILED; - } - if (id != nullptr) { - *id = currentId; - } - return HasReturnvaluesIF::RETURN_OK; -} - -MessageQueueIF* QueueMapManager::getMessageQueue( - MessageQueueId_t messageQueueId) const { - MutexHelper(mapLock, 50); - auto queueIter = queueMap.find(messageQueueId); - if(queueIter != queueMap.end()) { - return queueIter->second; - } - else { - sif::warning << "QueueMapManager::getQueueHandle: The ID" << - messageQueueId << " does not exists in the map" << std::endl; - return nullptr; - } -} - +#include "../../ipc/MutexFactory.h" +#include "../../ipc/MutexHelper.h" +#include "../../osal/host/QueueMapManager.h" + +QueueMapManager* QueueMapManager::mqManagerInstance = nullptr; + +QueueMapManager::QueueMapManager() { + mapLock = MutexFactory::instance()->createMutex(); +} + +QueueMapManager* QueueMapManager::instance() { + if (mqManagerInstance == nullptr){ + mqManagerInstance = new QueueMapManager(); + } + return QueueMapManager::mqManagerInstance; +} + +ReturnValue_t QueueMapManager::addMessageQueue( + MessageQueueIF* queueToInsert, MessageQueueId_t* id) { + // Not thread-safe, but it is assumed all message queues are created + // at software initialization now. If this is to be made thread-safe in + // the future, it propably would be sufficient to lock the increment + // operation here + uint32_t currentId = queueCounter++; + auto returnPair = queueMap.emplace(currentId, queueToInsert); + if(not returnPair.second) { + // this should never happen for the atomic variable. + sif::error << "QueueMapManager: This ID is already inside the map!" + << std::endl; + return HasReturnvaluesIF::RETURN_FAILED; + } + if (id != nullptr) { + *id = currentId; + } + return HasReturnvaluesIF::RETURN_OK; +} + +MessageQueueIF* QueueMapManager::getMessageQueue( + MessageQueueId_t messageQueueId) const { + MutexHelper(mapLock, 50); + auto queueIter = queueMap.find(messageQueueId); + if(queueIter != queueMap.end()) { + return queueIter->second; + } + else { + sif::warning << "QueueMapManager::getQueueHandle: The ID" << + messageQueueId << " does not exists in the map" << std::endl; + return nullptr; + } +} + diff --git a/osal/host/QueueMapManager.h b/osal/host/QueueMapManager.h index acf20389..823fd4c2 100644 --- a/osal/host/QueueMapManager.h +++ b/osal/host/QueueMapManager.h @@ -1,47 +1,47 @@ -#ifndef FRAMEWORK_OSAL_HOST_QUEUEMAP_H_ -#define FRAMEWORK_OSAL_HOST_QUEUEMAP_H_ - -#include "../../ipc/MessageQueueSenderIF.h" -#include "../../osal/host/MessageQueue.h" -#include -#include - -using QueueMap = std::unordered_map; - - -/** - * An internal map to map message queue IDs to message queues. - * This propably should be a singleton.. - */ -class QueueMapManager { -public: - //! Returns the single instance of SemaphoreFactory. - static QueueMapManager* instance(); - - /** - * Insert a message queue into the map and returns a message queue ID - * @param queue The message queue to insert. - * @param id The passed value will be set unless a nullptr is passed - * @return - */ - ReturnValue_t addMessageQueue(MessageQueueIF* queue, MessageQueueId_t* - id = nullptr); - /** - * Get the message queue handle by providing a message queue ID. - * @param messageQueueId - * @return - */ - MessageQueueIF* getMessageQueue(MessageQueueId_t messageQueueId) const; - -private: - //! External instantiation is forbidden. - QueueMapManager(); - uint32_t queueCounter = 1; - MutexIF* mapLock; - QueueMap queueMap; - static QueueMapManager* mqManagerInstance; -}; - - - -#endif /* FRAMEWORK_OSAL_HOST_QUEUEMAP_H_ */ +#ifndef FRAMEWORK_OSAL_HOST_QUEUEMAP_H_ +#define FRAMEWORK_OSAL_HOST_QUEUEMAP_H_ + +#include "../../ipc/MessageQueueSenderIF.h" +#include "../../osal/host/MessageQueue.h" +#include +#include + +using QueueMap = std::unordered_map; + + +/** + * An internal map to map message queue IDs to message queues. + * This propably should be a singleton.. + */ +class QueueMapManager { +public: + //! Returns the single instance of SemaphoreFactory. + static QueueMapManager* instance(); + + /** + * Insert a message queue into the map and returns a message queue ID + * @param queue The message queue to insert. + * @param id The passed value will be set unless a nullptr is passed + * @return + */ + ReturnValue_t addMessageQueue(MessageQueueIF* queue, MessageQueueId_t* + id = nullptr); + /** + * Get the message queue handle by providing a message queue ID. + * @param messageQueueId + * @return + */ + MessageQueueIF* getMessageQueue(MessageQueueId_t messageQueueId) const; + +private: + //! External instantiation is forbidden. + QueueMapManager(); + uint32_t queueCounter = 1; + MutexIF* mapLock; + QueueMap queueMap; + static QueueMapManager* mqManagerInstance; +}; + + + +#endif /* FRAMEWORK_OSAL_HOST_QUEUEMAP_H_ */ diff --git a/osal/host/SemaphoreFactory.cpp b/osal/host/SemaphoreFactory.cpp index 58040428..c354c47c 100644 --- a/osal/host/SemaphoreFactory.cpp +++ b/osal/host/SemaphoreFactory.cpp @@ -1,42 +1,42 @@ -#include "../../tasks/SemaphoreFactory.h" -#include "../../osal/linux/BinarySemaphore.h" -#include "../../osal/linux/CountingSemaphore.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" - -const uint32_t SemaphoreIF::POLLING = 0; -const uint32_t SemaphoreIF::BLOCKING = 0xFFFFFFFF; - -SemaphoreFactory* SemaphoreFactory::factoryInstance = nullptr; - -SemaphoreFactory::SemaphoreFactory() { -} - -SemaphoreFactory::~SemaphoreFactory() { - delete factoryInstance; -} - -SemaphoreFactory* SemaphoreFactory::instance() { - if (factoryInstance == nullptr){ - factoryInstance = new SemaphoreFactory(); - } - return SemaphoreFactory::factoryInstance; -} - -SemaphoreIF* SemaphoreFactory::createBinarySemaphore(uint32_t arguments) { - // Just gonna wait for full C++20 for now. - sif::error << "SemaphoreFactory: Binary Semaphore not implemented yet." - " Returning nullptr!\n" << std::flush; - return nullptr; -} - -SemaphoreIF* SemaphoreFactory::createCountingSemaphore(const uint8_t maxCount, - uint8_t initCount, uint32_t arguments) { - // Just gonna wait for full C++20 for now. - sif::error << "SemaphoreFactory: Counting Semaphore not implemented yet." - " Returning nullptr!\n" << std::flush; - return nullptr; -} - -void SemaphoreFactory::deleteSemaphore(SemaphoreIF* semaphore) { - delete semaphore; -} +#include "../../tasks/SemaphoreFactory.h" +#include "../../osal/linux/BinarySemaphore.h" +#include "../../osal/linux/CountingSemaphore.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" + +const uint32_t SemaphoreIF::POLLING = 0; +const uint32_t SemaphoreIF::BLOCKING = 0xFFFFFFFF; + +SemaphoreFactory* SemaphoreFactory::factoryInstance = nullptr; + +SemaphoreFactory::SemaphoreFactory() { +} + +SemaphoreFactory::~SemaphoreFactory() { + delete factoryInstance; +} + +SemaphoreFactory* SemaphoreFactory::instance() { + if (factoryInstance == nullptr){ + factoryInstance = new SemaphoreFactory(); + } + return SemaphoreFactory::factoryInstance; +} + +SemaphoreIF* SemaphoreFactory::createBinarySemaphore(uint32_t arguments) { + // Just gonna wait for full C++20 for now. + sif::error << "SemaphoreFactory: Binary Semaphore not implemented yet." + " Returning nullptr!\n" << std::flush; + return nullptr; +} + +SemaphoreIF* SemaphoreFactory::createCountingSemaphore(const uint8_t maxCount, + uint8_t initCount, uint32_t arguments) { + // Just gonna wait for full C++20 for now. + sif::error << "SemaphoreFactory: Counting Semaphore not implemented yet." + " Returning nullptr!\n" << std::flush; + return nullptr; +} + +void SemaphoreFactory::deleteSemaphore(SemaphoreIF* semaphore) { + delete semaphore; +} diff --git a/osal/host/TaskFactory.cpp b/osal/host/TaskFactory.cpp index 3f527130..37df981a 100644 --- a/osal/host/TaskFactory.cpp +++ b/osal/host/TaskFactory.cpp @@ -1,55 +1,55 @@ -#include "../../osal/host/FixedTimeslotTask.h" -#include "../../osal/host/PeriodicTask.h" -#include "../../tasks/TaskFactory.h" -#include "../../returnvalues/HasReturnvaluesIF.h" -#include "../../tasks/PeriodicTaskIF.h" - -#include - -TaskFactory* TaskFactory::factoryInstance = new TaskFactory(); - -// Will propably not be used for hosted implementation -const size_t PeriodicTaskIF::MINIMUM_STACK_SIZE = 0; - -TaskFactory::TaskFactory() { -} - -TaskFactory::~TaskFactory() { -} - -TaskFactory* TaskFactory::instance() { - return TaskFactory::factoryInstance; -} - -PeriodicTaskIF* TaskFactory::createPeriodicTask(TaskName name_, - TaskPriority taskPriority_,TaskStackSize stackSize_, - TaskPeriod periodInSeconds_, - TaskDeadlineMissedFunction deadLineMissedFunction_) { - // This is going to be interesting. Time now learn the C++ threading library - // :-) - return new PeriodicTask(name_, taskPriority_, stackSize_, periodInSeconds_, - deadLineMissedFunction_); -} - -FixedTimeslotTaskIF* TaskFactory::createFixedTimeslotTask(TaskName name_, - TaskPriority taskPriority_,TaskStackSize stackSize_, - TaskPeriod periodInSeconds_, - TaskDeadlineMissedFunction deadLineMissedFunction_) { - // This is going to be interesting. Time now learn the C++ threading library - // :-) - return new FixedTimeslotTask(name_, taskPriority_, stackSize_, - periodInSeconds_, deadLineMissedFunction_); -} - -ReturnValue_t TaskFactory::deleteTask(PeriodicTaskIF* task) { - // This might block for some time! - delete task; - return HasReturnvaluesIF::RETURN_FAILED; -} - -ReturnValue_t TaskFactory::delayTask(uint32_t delayMs){ - std::this_thread::sleep_for(std::chrono::milliseconds(delayMs)); - return HasReturnvaluesIF::RETURN_OK; -} - - +#include "../../osal/host/FixedTimeslotTask.h" +#include "../../osal/host/PeriodicTask.h" +#include "../../tasks/TaskFactory.h" +#include "../../returnvalues/HasReturnvaluesIF.h" +#include "../../tasks/PeriodicTaskIF.h" + +#include + +TaskFactory* TaskFactory::factoryInstance = new TaskFactory(); + +// Will propably not be used for hosted implementation +const size_t PeriodicTaskIF::MINIMUM_STACK_SIZE = 0; + +TaskFactory::TaskFactory() { +} + +TaskFactory::~TaskFactory() { +} + +TaskFactory* TaskFactory::instance() { + return TaskFactory::factoryInstance; +} + +PeriodicTaskIF* TaskFactory::createPeriodicTask(TaskName name_, + TaskPriority taskPriority_,TaskStackSize stackSize_, + TaskPeriod periodInSeconds_, + TaskDeadlineMissedFunction deadLineMissedFunction_) { + // This is going to be interesting. Time now learn the C++ threading library + // :-) + return new PeriodicTask(name_, taskPriority_, stackSize_, periodInSeconds_, + deadLineMissedFunction_); +} + +FixedTimeslotTaskIF* TaskFactory::createFixedTimeslotTask(TaskName name_, + TaskPriority taskPriority_,TaskStackSize stackSize_, + TaskPeriod periodInSeconds_, + TaskDeadlineMissedFunction deadLineMissedFunction_) { + // This is going to be interesting. Time now learn the C++ threading library + // :-) + return new FixedTimeslotTask(name_, taskPriority_, stackSize_, + periodInSeconds_, deadLineMissedFunction_); +} + +ReturnValue_t TaskFactory::deleteTask(PeriodicTaskIF* task) { + // This might block for some time! + delete task; + return HasReturnvaluesIF::RETURN_FAILED; +} + +ReturnValue_t TaskFactory::delayTask(uint32_t delayMs){ + std::this_thread::sleep_for(std::chrono::milliseconds(delayMs)); + return HasReturnvaluesIF::RETURN_OK; +} + + From 902cd4d2101368140df6df6424b0eeb50a99faf8 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sat, 5 Sep 2020 21:19:53 +0200 Subject: [PATCH 16/20] host osal update --- osal/host/FixedTimeslotTask.cpp | 14 ++++++++++---- osal/host/FixedTimeslotTask.h | 2 +- osal/host/MessageQueue.cpp | 18 +++++++++++------- osal/host/MessageQueue.h | 3 ++- osal/host/Mutex.cpp | 7 +++---- osal/host/Mutex.h | 11 ++++++----- osal/host/PeriodicTask.cpp | 6 +++--- osal/host/QueueMapManager.cpp | 5 +++-- osal/host/QueueMapManager.h | 6 +++--- osal/host/SemaphoreFactory.cpp | 3 --- osal/host/TaskFactory.cpp | 4 ---- 11 files changed, 42 insertions(+), 37 deletions(-) diff --git a/osal/host/FixedTimeslotTask.cpp b/osal/host/FixedTimeslotTask.cpp index 1139badb..e78c974a 100644 --- a/osal/host/FixedTimeslotTask.cpp +++ b/osal/host/FixedTimeslotTask.cpp @@ -90,9 +90,12 @@ ReturnValue_t FixedTimeslotTask::sleepFor(uint32_t ms) { } void FixedTimeslotTask::taskFunctionality() { + pollingSeqTable.intializeSequenceAfterTaskCreation(); + // A local iterator for the Polling Sequence Table is created to // find the start time for the first entry. - FixedSlotSequence::SlotListIter slotListIter = pollingSeqTable.current; + auto slotListIter = pollingSeqTable.current; + // Get start time for first entry. chron_ms interval(slotListIter->pollingTimeMs); auto currentStartTime { @@ -122,8 +125,11 @@ void FixedTimeslotTask::taskFunctionality() { ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId, uint32_t slotTimeMs, int8_t executionStep) { - if (objectManager->get(componentId) != nullptr) { - pollingSeqTable.addSlot(componentId, slotTimeMs, executionStep, this); + ExecutableObjectIF* executableObject = objectManager-> + get(componentId); + if (executableObject != nullptr) { + pollingSeqTable.addSlot(componentId, slotTimeMs, executionStep, + executableObject, this); return HasReturnvaluesIF::RETURN_OK; } @@ -132,7 +138,7 @@ ReturnValue_t FixedTimeslotTask::addSlot(object_id_t componentId, return HasReturnvaluesIF::RETURN_FAILED; } -ReturnValue_t FixedTimeslotTask::checkAndInitializeSequence() const { +ReturnValue_t FixedTimeslotTask::checkSequence() const { return pollingSeqTable.checkSequence(); } diff --git a/osal/host/FixedTimeslotTask.h b/osal/host/FixedTimeslotTask.h index 3ea97b97..9985e2ee 100644 --- a/osal/host/FixedTimeslotTask.h +++ b/osal/host/FixedTimeslotTask.h @@ -61,7 +61,7 @@ public: ReturnValue_t addSlot(object_id_t componentId, uint32_t slotTimeMs, int8_t executionStep); - ReturnValue_t checkAndInitializeSequence() const; + ReturnValue_t checkSequence() const override; uint32_t getPeriodMs() const; diff --git a/osal/host/MessageQueue.cpp b/osal/host/MessageQueue.cpp index 8a34282c..bced3713 100644 --- a/osal/host/MessageQueue.cpp +++ b/osal/host/MessageQueue.cpp @@ -1,6 +1,7 @@ -#include "../../osal/host/MessageQueue.h" +#include "MessageQueue.h" +#include "QueueMapManager.h" + #include "../../serviceinterface/ServiceInterfaceStream.h" -#include "../../osal/host/QueueMapManager.h" #include "../../ipc/MutexFactory.h" #include "../../ipc/MutexHelper.h" @@ -9,7 +10,8 @@ MessageQueue::MessageQueue(size_t messageDepth, size_t maxMessageSize): queueLock = MutexFactory::instance()->createMutex(); auto result = QueueMapManager::instance()->addMessageQueue(this, &mqId); if(result != HasReturnvaluesIF::RETURN_OK) { - sif::error << "MessageQueue: Could not be created" << std::endl; + sif::error << "MessageQueue::MessageQueue:" + << " Could not be created" << std::endl; } } @@ -61,7 +63,7 @@ ReturnValue_t MessageQueue::receiveMessage(MessageQueueMessageIF* message) { } // not sure this will work.. //*message = std::move(messageQueue.front()); - MutexHelper mutexLock(queueLock, 20); + MutexHelper mutexLock(queueLock, MutexIF::TimeoutType::WAITING, 20); MessageQueueMessage* currentMessage = &messageQueue.front(); std::copy(currentMessage->getBuffer(), currentMessage->getBuffer() + messageSize, message->getBuffer()); @@ -126,7 +128,8 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, } if(targetQueue->messageQueue.size() < targetQueue->messageDepth) { - MutexHelper mutexLock(targetQueue->queueLock, 20); + MutexHelper mutexLock(targetQueue->queueLock, + MutexIF::TimeoutType::WAITING, 20); // not ideal, works for now though. MessageQueueMessage* mqmMessage = dynamic_cast(message); @@ -146,8 +149,9 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, return HasReturnvaluesIF::RETURN_OK; } -ReturnValue_t MessageQueue::lockQueue(dur_millis_t lockTimeout) { - return queueLock->lockMutex(lockTimeout); +ReturnValue_t MessageQueue::lockQueue(MutexIF::TimeoutType timeoutType, + dur_millis_t lockTimeout) { + return queueLock->lockMutex(timeoutType, lockTimeout); } ReturnValue_t MessageQueue::unlockQueue() { diff --git a/osal/host/MessageQueue.h b/osal/host/MessageQueue.h index 21a01663..97a9e491 100644 --- a/osal/host/MessageQueue.h +++ b/osal/host/MessageQueue.h @@ -182,7 +182,8 @@ public: bool isDefaultDestinationSet() const override; - ReturnValue_t lockQueue(dur_millis_t lockTimeout); + ReturnValue_t lockQueue(MutexIF::TimeoutType timeoutType, + dur_millis_t lockTimeout); ReturnValue_t unlockQueue(); protected: /** diff --git a/osal/host/Mutex.cpp b/osal/host/Mutex.cpp index 2718f2b9..8471cab8 100644 --- a/osal/host/Mutex.cpp +++ b/osal/host/Mutex.cpp @@ -1,10 +1,9 @@ -#include "../../osal/host/Mutex.h" +#include "Mutex.h" #include "../../serviceinterface/ServiceInterfaceStream.h" -const uint32_t MutexIF::POLLING = 0; -const uint32_t MutexIF::BLOCKING = 0xffffffff; +Mutex::Mutex() {} -ReturnValue_t Mutex::lockMutex(uint32_t timeoutMs) { +ReturnValue_t Mutex::lockMutex(TimeoutType timeoutType, uint32_t timeoutMs) { if(timeoutMs == MutexIF::BLOCKING) { mutex.lock(); locked = true; diff --git a/osal/host/Mutex.h b/osal/host/Mutex.h index 4461e5f2..24dafbbd 100644 --- a/osal/host/Mutex.h +++ b/osal/host/Mutex.h @@ -1,5 +1,5 @@ -#ifndef FRAMEWORK_OSAL_FREERTOS_MUTEX_H_ -#define FRAMEWORK_OSAL_FREERTOS_MUTEX_H_ +#ifndef FSFW_OSAL_HOSTED_MUTEX_H_ +#define FSFW_OSAL_HOSTED_MUTEX_H_ #include "../../ipc/MutexIF.h" @@ -15,8 +15,9 @@ */ class Mutex : public MutexIF { public: - Mutex() = default; - ReturnValue_t lockMutex(uint32_t timeoutMs = MutexIF::BLOCKING) override; + Mutex(); + ReturnValue_t lockMutex(TimeoutType timeoutType = + TimeoutType::BLOCKING, uint32_t timeoutMs = 0) override; ReturnValue_t unlockMutex() override; std::timed_mutex* getMutexHandle(); @@ -25,4 +26,4 @@ private: std::timed_mutex mutex; }; -#endif /* FRAMEWORK_FREERTOS_MUTEX_H_ */ +#endif /* FSFW_OSAL_HOSTED_MUTEX_H_ */ diff --git a/osal/host/PeriodicTask.cpp b/osal/host/PeriodicTask.cpp index 0b5f8bbe..f4ee079b 100644 --- a/osal/host/PeriodicTask.cpp +++ b/osal/host/PeriodicTask.cpp @@ -1,7 +1,7 @@ -#include "../../ipc/MutexFactory.h" -#include "../../osal/host/Mutex.h" -#include "../../osal/host/PeriodicTask.h" +#include "Mutex.h" +#include "PeriodicTask.h" +#include "../../ipc/MutexFactory.h" #include "../../serviceinterface/ServiceInterfaceStream.h" #include "../../tasks/ExecutableObjectIF.h" diff --git a/osal/host/QueueMapManager.cpp b/osal/host/QueueMapManager.cpp index 47687bc1..1b2094e9 100644 --- a/osal/host/QueueMapManager.cpp +++ b/osal/host/QueueMapManager.cpp @@ -1,6 +1,7 @@ +#include "QueueMapManager.h" + #include "../../ipc/MutexFactory.h" #include "../../ipc/MutexHelper.h" -#include "../../osal/host/QueueMapManager.h" QueueMapManager* QueueMapManager::mqManagerInstance = nullptr; @@ -37,7 +38,7 @@ ReturnValue_t QueueMapManager::addMessageQueue( MessageQueueIF* QueueMapManager::getMessageQueue( MessageQueueId_t messageQueueId) const { - MutexHelper(mapLock, 50); + MutexHelper(mapLock, MutexIF::TimeoutType::WAITING, 50); auto queueIter = queueMap.find(messageQueueId); if(queueIter != queueMap.end()) { return queueIter->second; diff --git a/osal/host/QueueMapManager.h b/osal/host/QueueMapManager.h index 823fd4c2..d7d5c915 100644 --- a/osal/host/QueueMapManager.h +++ b/osal/host/QueueMapManager.h @@ -1,5 +1,5 @@ -#ifndef FRAMEWORK_OSAL_HOST_QUEUEMAP_H_ -#define FRAMEWORK_OSAL_HOST_QUEUEMAP_H_ +#ifndef FSFW_OSAL_HOST_QUEUEMAPMANAGER_H_ +#define FSFW_OSAL_HOST_QUEUEMAPMANAGER_H_ #include "../../ipc/MessageQueueSenderIF.h" #include "../../osal/host/MessageQueue.h" @@ -44,4 +44,4 @@ private: -#endif /* FRAMEWORK_OSAL_HOST_QUEUEMAP_H_ */ +#endif /* FSFW_OSAL_HOST_QUEUEMAPMANAGER_H_ */ diff --git a/osal/host/SemaphoreFactory.cpp b/osal/host/SemaphoreFactory.cpp index c354c47c..b19b2c75 100644 --- a/osal/host/SemaphoreFactory.cpp +++ b/osal/host/SemaphoreFactory.cpp @@ -3,9 +3,6 @@ #include "../../osal/linux/CountingSemaphore.h" #include "../../serviceinterface/ServiceInterfaceStream.h" -const uint32_t SemaphoreIF::POLLING = 0; -const uint32_t SemaphoreIF::BLOCKING = 0xFFFFFFFF; - SemaphoreFactory* SemaphoreFactory::factoryInstance = nullptr; SemaphoreFactory::SemaphoreFactory() { diff --git a/osal/host/TaskFactory.cpp b/osal/host/TaskFactory.cpp index 37df981a..bc65504c 100644 --- a/osal/host/TaskFactory.cpp +++ b/osal/host/TaskFactory.cpp @@ -25,8 +25,6 @@ PeriodicTaskIF* TaskFactory::createPeriodicTask(TaskName name_, TaskPriority taskPriority_,TaskStackSize stackSize_, TaskPeriod periodInSeconds_, TaskDeadlineMissedFunction deadLineMissedFunction_) { - // This is going to be interesting. Time now learn the C++ threading library - // :-) return new PeriodicTask(name_, taskPriority_, stackSize_, periodInSeconds_, deadLineMissedFunction_); } @@ -35,8 +33,6 @@ FixedTimeslotTaskIF* TaskFactory::createFixedTimeslotTask(TaskName name_, TaskPriority taskPriority_,TaskStackSize stackSize_, TaskPeriod periodInSeconds_, TaskDeadlineMissedFunction deadLineMissedFunction_) { - // This is going to be interesting. Time now learn the C++ threading library - // :-) return new FixedTimeslotTask(name_, taskPriority_, stackSize_, periodInSeconds_, deadLineMissedFunction_); } From f7223abaa39a1ac10107f91cd16f18e40932d3fd Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sat, 5 Sep 2020 21:20:10 +0200 Subject: [PATCH 17/20] added define necessary for minGW --- timemanager/CCSDSTime.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/timemanager/CCSDSTime.cpp b/timemanager/CCSDSTime.cpp index a5a38e27..12ecf34a 100644 --- a/timemanager/CCSDSTime.cpp +++ b/timemanager/CCSDSTime.cpp @@ -1,8 +1,12 @@ -#include "CCSDSTime.h" +#include "../timemanager/CCSDSTime.h" #include #include #include +#ifndef SCNu8 + #define SCNu8 "hhu" +#endif + CCSDSTime::CCSDSTime() { } From d83573cefc14ea6ff6f45e694a3f29d533117797 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 10 Sep 2020 16:47:01 +0200 Subject: [PATCH 18/20] additional IDs removed --- returnvalues/FwClassIds.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/returnvalues/FwClassIds.h b/returnvalues/FwClassIds.h index 781320e3..80df5741 100644 --- a/returnvalues/FwClassIds.h +++ b/returnvalues/FwClassIds.h @@ -65,9 +65,6 @@ enum { POOL_VARIABLE_IF, //PVA 59 HOUSEKEEPING_MANAGER, //HKM 60 DLE_ENCODER, //DLEE 61 - PUS_PARSER, //PUSP 62 - SERIAL_ANALYZER, //SERA 63 - PUS_SERVICE_9, // PUS9 64 FW_CLASS_ID_COUNT //is actually count + 1 ! }; From 4f48ed9756a520520277d2efe2ebc46ed881eefd Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 10 Sep 2020 19:53:52 +0200 Subject: [PATCH 19/20] added new periodic op divider --- globalfunctions/PeriodicOperationDivider.cpp | 34 ++++++++++++ globalfunctions/PeriodicOperationDivider.h | 55 ++++++++++++++++++++ 2 files changed, 89 insertions(+) create mode 100644 globalfunctions/PeriodicOperationDivider.cpp create mode 100644 globalfunctions/PeriodicOperationDivider.h diff --git a/globalfunctions/PeriodicOperationDivider.cpp b/globalfunctions/PeriodicOperationDivider.cpp new file mode 100644 index 00000000..ad3b8bbd --- /dev/null +++ b/globalfunctions/PeriodicOperationDivider.cpp @@ -0,0 +1,34 @@ +#include "PeriodicOperationDivider.h" + + +PeriodicOperationDivider::PeriodicOperationDivider(uint32_t divider, + bool resetAutomatically): resetAutomatically(resetAutomatically), + counter(divider), divider(divider) { +} + +bool PeriodicOperationDivider::checkAndIncrement() { + if(counter >= divider) { + if(resetAutomatically) { + counter = 0; + } + return true; + } + counter ++; + return false; +} + +void PeriodicOperationDivider::resetCounter() { + counter = 0; +} + +void PeriodicOperationDivider::setDivider(uint32_t newDivider) { + divider = newDivider; +} + +uint32_t PeriodicOperationDivider::getCounter() const { + return counter; +} + +uint32_t PeriodicOperationDivider::getDivider() const { + return divider; +} diff --git a/globalfunctions/PeriodicOperationDivider.h b/globalfunctions/PeriodicOperationDivider.h new file mode 100644 index 00000000..dd970fb8 --- /dev/null +++ b/globalfunctions/PeriodicOperationDivider.h @@ -0,0 +1,55 @@ +#ifndef FSFW_GLOBALFUNCTIONS_PERIODICOPERATIONDIVIDER_H_ +#define FSFW_GLOBALFUNCTIONS_PERIODICOPERATIONDIVIDER_H_ + +#include + +/** + * @brief Lightweight helper class to facilitate periodic operation with + * decreased frequencies. + * @details + * This class is useful to perform operations which have to be performed + * with a reduced frequency, like debugging printouts in high periodic tasks + * or low priority operations. + */ +class PeriodicOperationDivider { +public: + /** + * Initialize with the desired divider and specify whether the internal + * counter will be reset automatically. + * @param divider + * @param resetAutomatically + */ + PeriodicOperationDivider(uint32_t divider, bool resetAutomatically = true); + + /** + * Check whether operation is necessary. + * If an operation is necessary and the class has been + * configured to be reset automatically, the counter will be reset. + * If not, the counter will be incremented. + * @return + * -@c true if the counter is larger or equal to the divider + * -@c false otherwise + */ + bool checkAndIncrement(); + + /** + * Can be used to reset the counter to 0 manually. + */ + void resetCounter(); + uint32_t getCounter() const; + + /** + * Can be used to set a new divider value. + * @param newDivider + */ + void setDivider(uint32_t newDivider); + uint32_t getDivider() const; +private: + bool resetAutomatically = true; + uint32_t counter = 0; + uint32_t divider = 0; +}; + + + +#endif /* FSFW_GLOBALFUNCTIONS_PERIODICOPERATIONDIVIDER_H_ */ From b4ca42f1fb080951b0df65669759e985328c2284 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Tue, 15 Sep 2020 15:28:05 +0200 Subject: [PATCH 20/20] ccsds time update --- timemanager/CCSDSTime.cpp | 9 +++------ timemanager/CCSDSTime.h | 2 +- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/timemanager/CCSDSTime.cpp b/timemanager/CCSDSTime.cpp index 12ecf34a..f99f8fbb 100644 --- a/timemanager/CCSDSTime.cpp +++ b/timemanager/CCSDSTime.cpp @@ -1,11 +1,8 @@ #include "../timemanager/CCSDSTime.h" -#include -#include -#include +#include +#include +#include -#ifndef SCNu8 - #define SCNu8 "hhu" -#endif CCSDSTime::CCSDSTime() { } diff --git a/timemanager/CCSDSTime.h b/timemanager/CCSDSTime.h index 8563cf0f..89fcff92 100644 --- a/timemanager/CCSDSTime.h +++ b/timemanager/CCSDSTime.h @@ -5,7 +5,7 @@ #include "Clock.h" #include "../returnvalues/HasReturnvaluesIF.h" -#include +#include bool operator<(const timeval& lhs, const timeval& rhs); bool operator<=(const timeval& lhs, const timeval& rhs);