From 111f9dce7dc4babf632ce8353def90729555b504 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sun, 16 Aug 2020 23:06:32 +0200 Subject: [PATCH 01/53] 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 549ad97bd3b2ee6b05af2383e7a30ef1ac846f5c Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Tue, 18 Aug 2020 11:25:57 +0200 Subject: [PATCH 02/53] shared ring buffer pull request --- container/SharedRingBuffer.cpp | 30 +++++++++++++++ container/SharedRingBuffer.h | 68 ++++++++++++++++++++++++++++++++++ 2 files changed, 98 insertions(+) create mode 100644 container/SharedRingBuffer.cpp create mode 100644 container/SharedRingBuffer.h diff --git a/container/SharedRingBuffer.cpp b/container/SharedRingBuffer.cpp new file mode 100644 index 00000000..001a21ed --- /dev/null +++ b/container/SharedRingBuffer.cpp @@ -0,0 +1,30 @@ +#include +#include +#include + +SharedRingBuffer::SharedRingBuffer(object_id_t objectId, const size_t size, + bool overwriteOld, size_t maxExcessBytes): + SystemObject(objectId), SimpleRingBuffer(size, overwriteOld, + maxExcessBytes) { + mutex = MutexFactory::instance()->createMutex(); +} + +SharedRingBuffer::SharedRingBuffer(object_id_t objectId, uint8_t *buffer, + const size_t size, bool overwriteOld, size_t maxExcessBytes): + SystemObject(objectId), SimpleRingBuffer(buffer, size, overwriteOld, + maxExcessBytes) { + mutex = MutexFactory::instance()->createMutex(); +} + +ReturnValue_t SharedRingBuffer::lockRingBufferMutex( + MutexIF::TimeoutType timeoutType, dur_millis_t timeout) { + return mutex->lockMutex(timeoutType, timeout); +} + +ReturnValue_t SharedRingBuffer::unlockRingBufferMutex() { + return mutex->unlockMutex(); +} + +MutexIF* SharedRingBuffer::getMutexHandle() const { + return mutex; +} diff --git a/container/SharedRingBuffer.h b/container/SharedRingBuffer.h new file mode 100644 index 00000000..0b5be052 --- /dev/null +++ b/container/SharedRingBuffer.h @@ -0,0 +1,68 @@ +#ifndef FRAMEWORK_CONTAINER_SHAREDRINGBUFFER_H_ +#define FRAMEWORK_CONTAINER_SHAREDRINGBUFFER_H_ + +#include +#include +#include +#include + +/** + * @brief Ring buffer which can be shared among multiple objects + * @details + * This class offers a mutex to perform thread-safe operation on the ring + * buffer. It is still up to the developer to actually perform the lock + * and unlock operations. + */ +class SharedRingBuffer: public SystemObject, + public SimpleRingBuffer { +public: + /** + * This constructor allocates a new internal buffer with the supplied size. + * @param size + * @param overwriteOld + * If the ring buffer is overflowing at a write operartion, the oldest data + * will be overwritten. + */ + SharedRingBuffer(object_id_t objectId, const size_t size, + bool overwriteOld, size_t maxExcessBytes); + + /** + * This constructor takes an external buffer with the specified size. + * @param buffer + * @param size + * @param overwriteOld + * If the ring buffer is overflowing at a write operartion, the oldest data + * will be overwritten. + */ + SharedRingBuffer(object_id_t objectId, uint8_t* buffer, const size_t size, + bool overwriteOld, size_t maxExcessBytes); + + /** + * Unless a read-only constant value is read, all operations on the + * shared ring buffer should be protected by calling this function. + * @param timeoutType + * @param timeout + * @return + */ + virtual ReturnValue_t lockRingBufferMutex(MutexIF::TimeoutType timeoutType, + dur_millis_t timeout); + /** + * Any locked mutex also has to be unlocked, otherwise, access to the + * shared ring buffer will be blocked. + * @return + */ + virtual ReturnValue_t unlockRingBufferMutex(); + + /** + * The mutex handle can be accessed directly, for example to perform + * the lock with the #MutexHelper for a RAII compliant lock operation. + * @return + */ + MutexIF* getMutexHandle() const; +private: + MutexIF* mutex = nullptr; +}; + + + +#endif /* FRAMEWORK_CONTAINER_SHAREDRINGBUFFER_H_ */ From b7612bee37fdcfefa879ae80b71fdd8ebeaf198e Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Mon, 24 Aug 2020 14:55:32 +0200 Subject: [PATCH 03/53] getFreeELement function added --- container/SimpleRingBuffer.cpp | 203 ++++++++++++++++++++++----------- container/SimpleRingBuffer.h | 149 ++++++++++++++++++++---- 2 files changed, 263 insertions(+), 89 deletions(-) diff --git a/container/SimpleRingBuffer.cpp b/container/SimpleRingBuffer.cpp index ee087643..7ed019b2 100644 --- a/container/SimpleRingBuffer.cpp +++ b/container/SimpleRingBuffer.cpp @@ -1,68 +1,135 @@ -#include "SimpleRingBuffer.h" -#include - -SimpleRingBuffer::SimpleRingBuffer(uint32_t size, bool overwriteOld) : - RingBufferBase<>(0, size, overwriteOld), buffer(NULL) { - buffer = new uint8_t[size]; -} - -SimpleRingBuffer::~SimpleRingBuffer() { - delete[] buffer; -} - -ReturnValue_t SimpleRingBuffer::writeData(const uint8_t* data, - uint32_t amount) { - if (availableWriteSpace() >= amount || overwriteOld) { - uint32_t amountTillWrap = writeTillWrap(); - if (amountTillWrap >= amount) { - memcpy(&buffer[write], data, amount); - } else { - memcpy(&buffer[write], data, amountTillWrap); - memcpy(buffer, data + amountTillWrap, amount - amountTillWrap); - } - incrementWrite(amount); - return HasReturnvaluesIF::RETURN_OK; - } else { - return HasReturnvaluesIF::RETURN_FAILED; - } -} - -ReturnValue_t SimpleRingBuffer::readData(uint8_t* data, uint32_t amount, - bool readRemaining, uint32_t* trueAmount) { - uint32_t availableData = availableReadData(READ_PTR); - uint32_t amountTillWrap = readTillWrap(READ_PTR); - if (availableData < amount) { - if (readRemaining) { - amount = availableData; - } else { - return HasReturnvaluesIF::RETURN_FAILED; - } - } - if (trueAmount != NULL) { - *trueAmount = amount; - } - if (amountTillWrap >= amount) { - memcpy(data, &buffer[read[READ_PTR]], amount); - } else { - memcpy(data, &buffer[read[READ_PTR]], amountTillWrap); - memcpy(data + amountTillWrap, buffer, amount - amountTillWrap); - } - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t SimpleRingBuffer::deleteData(uint32_t amount, - bool deleteRemaining, uint32_t* trueAmount) { - uint32_t availableData = availableReadData(READ_PTR); - if (availableData < amount) { - if (deleteRemaining) { - amount = availableData; - } else { - return HasReturnvaluesIF::RETURN_FAILED; - } - } - if (trueAmount != NULL) { - *trueAmount = amount; - } - incrementRead(amount, READ_PTR); - return HasReturnvaluesIF::RETURN_OK; -} +#include "../container/SimpleRingBuffer.h" +#include + +SimpleRingBuffer::SimpleRingBuffer(const size_t size, bool overwriteOld, + size_t maxExcessBytes) : + RingBufferBase<>(0, size, overwriteOld), + maxExcessBytes(maxExcessBytes) { + if(maxExcessBytes > size) { + this->maxExcessBytes = size; + } + else { + this->maxExcessBytes = maxExcessBytes; + } + buffer = new uint8_t[size + maxExcessBytes]; +} + +SimpleRingBuffer::SimpleRingBuffer(uint8_t *buffer, const size_t size, + bool overwriteOld, size_t maxExcessBytes): + RingBufferBase<>(0, size, overwriteOld), buffer(buffer) { + if(maxExcessBytes > size) { + this->maxExcessBytes = size; + } + else { + this->maxExcessBytes = maxExcessBytes; + } +} + + +SimpleRingBuffer::~SimpleRingBuffer() { + delete[] buffer; +} + + +ReturnValue_t SimpleRingBuffer::getFreeElement(uint8_t **writePointer, + size_t amount) { + if (availableWriteSpace() >= amount or overwriteOld) { + size_t amountTillWrap = writeTillWrap(); + if (amountTillWrap < amount) { + if((amount - amountTillWrap + excessBytes) > maxExcessBytes) { + return HasReturnvaluesIF::RETURN_FAILED; + } + excessBytes = amount - amountTillWrap; + } + *writePointer = &buffer[write]; + return HasReturnvaluesIF::RETURN_OK; + } + else { + return HasReturnvaluesIF::RETURN_FAILED; + } +} + +void SimpleRingBuffer::confirmBytesWritten(size_t amount) { + if(getExcessBytes() > 0) { + moveExcessBytesToStart(); + } + incrementWrite(amount); + +} + +ReturnValue_t SimpleRingBuffer::writeData(const uint8_t* data, + size_t amount) { + if (availableWriteSpace() >= amount or overwriteOld) { + size_t amountTillWrap = writeTillWrap(); + if (amountTillWrap >= amount) { + // remaining size in buffer is sufficient to fit full amount. + memcpy(&buffer[write], data, amount); + } + else { + memcpy(&buffer[write], data, amountTillWrap); + memcpy(buffer, data + amountTillWrap, amount - amountTillWrap); + } + incrementWrite(amount); + return HasReturnvaluesIF::RETURN_OK; + } else { + return HasReturnvaluesIF::RETURN_FAILED; + } +} + +ReturnValue_t SimpleRingBuffer::readData(uint8_t* data, size_t amount, + bool incrementReadPtr, bool readRemaining, size_t* trueAmount) { + size_t availableData = getAvailableReadData(READ_PTR); + size_t amountTillWrap = readTillWrap(READ_PTR); + if (availableData < amount) { + if (readRemaining) { + // more data available than amount specified. + amount = availableData; + } else { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + if (trueAmount != nullptr) { + *trueAmount = amount; + } + if (amountTillWrap >= amount) { + memcpy(data, &buffer[read[READ_PTR]], amount); + } else { + memcpy(data, &buffer[read[READ_PTR]], amountTillWrap); + memcpy(data + amountTillWrap, buffer, amount - amountTillWrap); + } + + if(incrementReadPtr) { + deleteData(amount, readRemaining); + } + return HasReturnvaluesIF::RETURN_OK; +} + +size_t SimpleRingBuffer::getExcessBytes() const { + return excessBytes; +} + +void SimpleRingBuffer::moveExcessBytesToStart() { + if(excessBytes > 0) { + std::memcpy(buffer, &buffer[size], excessBytes); + excessBytes = 0; + } +} + +ReturnValue_t SimpleRingBuffer::deleteData(size_t amount, + bool deleteRemaining, size_t* trueAmount) { + size_t availableData = getAvailableReadData(READ_PTR); + if (availableData < amount) { + if (deleteRemaining) { + amount = availableData; + } else { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + if (trueAmount != nullptr) { + *trueAmount = amount; + } + incrementRead(amount, READ_PTR); + return HasReturnvaluesIF::RETURN_OK; +} + + diff --git a/container/SimpleRingBuffer.h b/container/SimpleRingBuffer.h index 5d4fecab..b13e9f2a 100644 --- a/container/SimpleRingBuffer.h +++ b/container/SimpleRingBuffer.h @@ -1,21 +1,128 @@ -#ifndef FRAMEWORK_CONTAINER_SIMPLERINGBUFFER_H_ -#define FRAMEWORK_CONTAINER_SIMPLERINGBUFFER_H_ - -#include "RingBufferBase.h" -#include - -class SimpleRingBuffer: public RingBufferBase<> { -public: - SimpleRingBuffer(uint32_t size, bool overwriteOld); - virtual ~SimpleRingBuffer(); - ReturnValue_t writeData(const uint8_t* data, uint32_t amount); - ReturnValue_t readData(uint8_t* data, uint32_t amount, bool readRemaining = false, uint32_t* trueAmount = NULL); - ReturnValue_t deleteData(uint32_t amount, bool deleteRemaining = false, uint32_t* trueAmount = NULL); -private: -// static const uint8_t TEMP_READ_PTR = 1; - static const uint8_t READ_PTR = 0; - uint8_t* buffer; -}; - -#endif /* FRAMEWORK_CONTAINER_SIMPLERINGBUFFER_H_ */ - +#ifndef FRAMEWORK_CONTAINER_SIMPLERINGBUFFER_H_ +#define FRAMEWORK_CONTAINER_SIMPLERINGBUFFER_H_ + +#include "../container/RingBufferBase.h" +#include + +/** + * @brief Circular buffer implementation, useful for buffering + * into data streams. + * @details + * Note that the deleteData() has to be called to increment the read pointer. + * This class allocated dynamically, so + * @ingroup containers + */ +class SimpleRingBuffer: public RingBufferBase<> { +public: + /** + * This constructor allocates a new internal buffer with the supplied size. + * + * @param size + * @param overwriteOld If the ring buffer is overflowing at a write + * operation, the oldest data will be overwritten. + * @param maxExcessBytes These additional bytes will be allocated in addtion + * to the specified size to accomodate contiguous write operations + * with getFreeElement. + * + */ + SimpleRingBuffer(const size_t size, bool overwriteOld, + size_t maxExcessBytes = 0); + /** + * This constructor takes an external buffer with the specified size. + * @param buffer + * @param size + * @param overwriteOld + * If the ring buffer is overflowing at a write operartion, the oldest data + * will be overwritten. + * @param maxExcessBytes + * If the buffer can accomodate additional bytes for contigous write + * operations with getFreeElement, this is the maximum allowed additional + * size + */ + SimpleRingBuffer(uint8_t* buffer, const size_t size, bool overwriteOld, + size_t maxExcessBytes = 0); + + virtual ~SimpleRingBuffer(); + + /** + * Write to circular buffer and increment write pointer by amount. + * @param data + * @param amount + * @return -@c RETURN_OK if write operation was successfull + * -@c RETURN_FAILED if + */ + ReturnValue_t writeData(const uint8_t* data, size_t amount); + + /** + * Returns a pointer to a free element. If the remaining buffer is + * not large enough, the data will be written past the actual size + * and the amount of excess bytes will be cached. This function + * does not increment the write pointer! + * @param writePointer Pointer to a pointer which can be used to write + * contiguous blocks into the ring buffer + * @param amount + * @return + */ + ReturnValue_t getFreeElement(uint8_t** writePointer, size_t amount); + + /** + * This increments the write pointer and also copies the excess bytes + * to the beginning. It should be called if the write operation + * conducted after calling getFreeElement() was performed. + * @return + */ + void confirmBytesWritten(size_t amount); + + virtual size_t getExcessBytes() const; + /** + * Helper functions which moves any excess bytes to the start + * of the ring buffer. + * @return + */ + virtual void moveExcessBytesToStart(); + + /** + * Read from circular buffer at read pointer. + * @param data + * @param amount + * @param incrementReadPtr + * If this is set to true, the read pointer will be incremented. + * If readRemaining is set to true, the read pointer will be incremented + * accordingly. + * @param readRemaining + * If this is set to true, the data will be read even if the amount + * specified exceeds the read data available. + * @param trueAmount [out] + * If readRemaining was set to true, the true amount read will be assigned + * to the passed value. + * @return + * - @c RETURN_OK if data was read successfully + * - @c RETURN_FAILED if not enough data was available and readRemaining + * was set to false. + */ + ReturnValue_t readData(uint8_t* data, size_t amount, + bool incrementReadPtr = false, bool readRemaining = false, + size_t* trueAmount = nullptr); + + /** + * Delete data by incrementing read pointer. + * @param amount + * @param deleteRemaining + * If the amount specified is larger than the remaing size to read and this + * is set to true, the remaining amount will be deleted as well + * @param trueAmount [out] + * If deleteRemaining was set to true, the amount deleted will be assigned + * to the passed value. + * @return + */ + ReturnValue_t deleteData(size_t amount, bool deleteRemaining = false, + size_t* trueAmount = nullptr); +private: + static const uint8_t READ_PTR = 0; + uint8_t* buffer = nullptr; + size_t maxExcessBytes; + size_t excessBytes = 0; +}; + +#endif /* FRAMEWORK_CONTAINER_SIMPLERINGBUFFER_H_ */ + From 84d99a6f04b5ccd6d544c6e633247e0d10bb1fd7 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Mon, 24 Aug 2020 14:56:50 +0200 Subject: [PATCH 04/53] added brackets for boolean expressions --- container/SimpleRingBuffer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/container/SimpleRingBuffer.cpp b/container/SimpleRingBuffer.cpp index 7ed019b2..2a19d619 100644 --- a/container/SimpleRingBuffer.cpp +++ b/container/SimpleRingBuffer.cpp @@ -33,7 +33,7 @@ SimpleRingBuffer::~SimpleRingBuffer() { ReturnValue_t SimpleRingBuffer::getFreeElement(uint8_t **writePointer, size_t amount) { - if (availableWriteSpace() >= amount or overwriteOld) { + if ((availableWriteSpace() >= amount) or overwriteOld) { size_t amountTillWrap = writeTillWrap(); if (amountTillWrap < amount) { if((amount - amountTillWrap + excessBytes) > maxExcessBytes) { @@ -59,7 +59,7 @@ void SimpleRingBuffer::confirmBytesWritten(size_t amount) { ReturnValue_t SimpleRingBuffer::writeData(const uint8_t* data, size_t amount) { - if (availableWriteSpace() >= amount or overwriteOld) { + if ((availableWriteSpace() >= amount) or overwriteOld) { size_t amountTillWrap = writeTillWrap(); if (amountTillWrap >= amount) { // remaining size in buffer is sufficient to fit full amount. From f13e7b4255005a1857abe9734cd8e1df325a4b0a Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Tue, 25 Aug 2020 18:30:11 +0200 Subject: [PATCH 05/53] 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 06/53] 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 07/53] 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 08/53] 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 09/53] 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 10/53] 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 11/53] 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 12/53] 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 13/53] 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 14/53] 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 15/53] 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 16/53] 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 17/53] 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 d885dddee8c8f053207c5c652f5acc81d66efff8 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Sep 2020 15:04:53 +0200 Subject: [PATCH 18/53] renormalize files --- container/SimpleRingBuffer.cpp | 270 ++++++++++++++++----------------- container/SimpleRingBuffer.h | 256 +++++++++++++++---------------- 2 files changed, 263 insertions(+), 263 deletions(-) diff --git a/container/SimpleRingBuffer.cpp b/container/SimpleRingBuffer.cpp index 2a19d619..d6b6f5af 100644 --- a/container/SimpleRingBuffer.cpp +++ b/container/SimpleRingBuffer.cpp @@ -1,135 +1,135 @@ -#include "../container/SimpleRingBuffer.h" -#include - -SimpleRingBuffer::SimpleRingBuffer(const size_t size, bool overwriteOld, - size_t maxExcessBytes) : - RingBufferBase<>(0, size, overwriteOld), - maxExcessBytes(maxExcessBytes) { - if(maxExcessBytes > size) { - this->maxExcessBytes = size; - } - else { - this->maxExcessBytes = maxExcessBytes; - } - buffer = new uint8_t[size + maxExcessBytes]; -} - -SimpleRingBuffer::SimpleRingBuffer(uint8_t *buffer, const size_t size, - bool overwriteOld, size_t maxExcessBytes): - RingBufferBase<>(0, size, overwriteOld), buffer(buffer) { - if(maxExcessBytes > size) { - this->maxExcessBytes = size; - } - else { - this->maxExcessBytes = maxExcessBytes; - } -} - - -SimpleRingBuffer::~SimpleRingBuffer() { - delete[] buffer; -} - - -ReturnValue_t SimpleRingBuffer::getFreeElement(uint8_t **writePointer, - size_t amount) { - if ((availableWriteSpace() >= amount) or overwriteOld) { - size_t amountTillWrap = writeTillWrap(); - if (amountTillWrap < amount) { - if((amount - amountTillWrap + excessBytes) > maxExcessBytes) { - return HasReturnvaluesIF::RETURN_FAILED; - } - excessBytes = amount - amountTillWrap; - } - *writePointer = &buffer[write]; - return HasReturnvaluesIF::RETURN_OK; - } - else { - return HasReturnvaluesIF::RETURN_FAILED; - } -} - -void SimpleRingBuffer::confirmBytesWritten(size_t amount) { - if(getExcessBytes() > 0) { - moveExcessBytesToStart(); - } - incrementWrite(amount); - -} - -ReturnValue_t SimpleRingBuffer::writeData(const uint8_t* data, - size_t amount) { - if ((availableWriteSpace() >= amount) or overwriteOld) { - size_t amountTillWrap = writeTillWrap(); - if (amountTillWrap >= amount) { - // remaining size in buffer is sufficient to fit full amount. - memcpy(&buffer[write], data, amount); - } - else { - memcpy(&buffer[write], data, amountTillWrap); - memcpy(buffer, data + amountTillWrap, amount - amountTillWrap); - } - incrementWrite(amount); - return HasReturnvaluesIF::RETURN_OK; - } else { - return HasReturnvaluesIF::RETURN_FAILED; - } -} - -ReturnValue_t SimpleRingBuffer::readData(uint8_t* data, size_t amount, - bool incrementReadPtr, bool readRemaining, size_t* trueAmount) { - size_t availableData = getAvailableReadData(READ_PTR); - size_t amountTillWrap = readTillWrap(READ_PTR); - if (availableData < amount) { - if (readRemaining) { - // more data available than amount specified. - amount = availableData; - } else { - return HasReturnvaluesIF::RETURN_FAILED; - } - } - if (trueAmount != nullptr) { - *trueAmount = amount; - } - if (amountTillWrap >= amount) { - memcpy(data, &buffer[read[READ_PTR]], amount); - } else { - memcpy(data, &buffer[read[READ_PTR]], amountTillWrap); - memcpy(data + amountTillWrap, buffer, amount - amountTillWrap); - } - - if(incrementReadPtr) { - deleteData(amount, readRemaining); - } - return HasReturnvaluesIF::RETURN_OK; -} - -size_t SimpleRingBuffer::getExcessBytes() const { - return excessBytes; -} - -void SimpleRingBuffer::moveExcessBytesToStart() { - if(excessBytes > 0) { - std::memcpy(buffer, &buffer[size], excessBytes); - excessBytes = 0; - } -} - -ReturnValue_t SimpleRingBuffer::deleteData(size_t amount, - bool deleteRemaining, size_t* trueAmount) { - size_t availableData = getAvailableReadData(READ_PTR); - if (availableData < amount) { - if (deleteRemaining) { - amount = availableData; - } else { - return HasReturnvaluesIF::RETURN_FAILED; - } - } - if (trueAmount != nullptr) { - *trueAmount = amount; - } - incrementRead(amount, READ_PTR); - return HasReturnvaluesIF::RETURN_OK; -} - - +#include "../container/SimpleRingBuffer.h" +#include + +SimpleRingBuffer::SimpleRingBuffer(const size_t size, bool overwriteOld, + size_t maxExcessBytes) : + RingBufferBase<>(0, size, overwriteOld), + maxExcessBytes(maxExcessBytes) { + if(maxExcessBytes > size) { + this->maxExcessBytes = size; + } + else { + this->maxExcessBytes = maxExcessBytes; + } + buffer = new uint8_t[size + maxExcessBytes]; +} + +SimpleRingBuffer::SimpleRingBuffer(uint8_t *buffer, const size_t size, + bool overwriteOld, size_t maxExcessBytes): + RingBufferBase<>(0, size, overwriteOld), buffer(buffer) { + if(maxExcessBytes > size) { + this->maxExcessBytes = size; + } + else { + this->maxExcessBytes = maxExcessBytes; + } +} + + +SimpleRingBuffer::~SimpleRingBuffer() { + delete[] buffer; +} + + +ReturnValue_t SimpleRingBuffer::getFreeElement(uint8_t **writePointer, + size_t amount) { + if (availableWriteSpace() >= amount or overwriteOld) { + size_t amountTillWrap = writeTillWrap(); + if (amountTillWrap < amount) { + if((amount - amountTillWrap + excessBytes) > maxExcessBytes) { + return HasReturnvaluesIF::RETURN_FAILED; + } + excessBytes = amount - amountTillWrap; + } + *writePointer = &buffer[write]; + return HasReturnvaluesIF::RETURN_OK; + } + else { + return HasReturnvaluesIF::RETURN_FAILED; + } +} + +void SimpleRingBuffer::confirmBytesWritten(size_t amount) { + if(getExcessBytes() > 0) { + moveExcessBytesToStart(); + } + incrementWrite(amount); + +} + +ReturnValue_t SimpleRingBuffer::writeData(const uint8_t* data, + size_t amount) { + if (availableWriteSpace() >= amount or overwriteOld) { + size_t amountTillWrap = writeTillWrap(); + if (amountTillWrap >= amount) { + // remaining size in buffer is sufficient to fit full amount. + memcpy(&buffer[write], data, amount); + } + else { + memcpy(&buffer[write], data, amountTillWrap); + memcpy(buffer, data + amountTillWrap, amount - amountTillWrap); + } + incrementWrite(amount); + return HasReturnvaluesIF::RETURN_OK; + } else { + return HasReturnvaluesIF::RETURN_FAILED; + } +} + +ReturnValue_t SimpleRingBuffer::readData(uint8_t* data, size_t amount, + bool incrementReadPtr, bool readRemaining, size_t* trueAmount) { + size_t availableData = getAvailableReadData(READ_PTR); + size_t amountTillWrap = readTillWrap(READ_PTR); + if (availableData < amount) { + if (readRemaining) { + // more data available than amount specified. + amount = availableData; + } else { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + if (trueAmount != nullptr) { + *trueAmount = amount; + } + if (amountTillWrap >= amount) { + memcpy(data, &buffer[read[READ_PTR]], amount); + } else { + memcpy(data, &buffer[read[READ_PTR]], amountTillWrap); + memcpy(data + amountTillWrap, buffer, amount - amountTillWrap); + } + + if(incrementReadPtr) { + deleteData(amount, readRemaining); + } + return HasReturnvaluesIF::RETURN_OK; +} + +size_t SimpleRingBuffer::getExcessBytes() const { + return excessBytes; +} + +void SimpleRingBuffer::moveExcessBytesToStart() { + if(excessBytes > 0) { + std::memcpy(buffer, &buffer[size], excessBytes); + excessBytes = 0; + } +} + +ReturnValue_t SimpleRingBuffer::deleteData(size_t amount, + bool deleteRemaining, size_t* trueAmount) { + size_t availableData = getAvailableReadData(READ_PTR); + if (availableData < amount) { + if (deleteRemaining) { + amount = availableData; + } else { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + if (trueAmount != nullptr) { + *trueAmount = amount; + } + incrementRead(amount, READ_PTR); + return HasReturnvaluesIF::RETURN_OK; +} + + diff --git a/container/SimpleRingBuffer.h b/container/SimpleRingBuffer.h index b13e9f2a..dcf2684b 100644 --- a/container/SimpleRingBuffer.h +++ b/container/SimpleRingBuffer.h @@ -1,128 +1,128 @@ -#ifndef FRAMEWORK_CONTAINER_SIMPLERINGBUFFER_H_ -#define FRAMEWORK_CONTAINER_SIMPLERINGBUFFER_H_ - -#include "../container/RingBufferBase.h" -#include - -/** - * @brief Circular buffer implementation, useful for buffering - * into data streams. - * @details - * Note that the deleteData() has to be called to increment the read pointer. - * This class allocated dynamically, so - * @ingroup containers - */ -class SimpleRingBuffer: public RingBufferBase<> { -public: - /** - * This constructor allocates a new internal buffer with the supplied size. - * - * @param size - * @param overwriteOld If the ring buffer is overflowing at a write - * operation, the oldest data will be overwritten. - * @param maxExcessBytes These additional bytes will be allocated in addtion - * to the specified size to accomodate contiguous write operations - * with getFreeElement. - * - */ - SimpleRingBuffer(const size_t size, bool overwriteOld, - size_t maxExcessBytes = 0); - /** - * This constructor takes an external buffer with the specified size. - * @param buffer - * @param size - * @param overwriteOld - * If the ring buffer is overflowing at a write operartion, the oldest data - * will be overwritten. - * @param maxExcessBytes - * If the buffer can accomodate additional bytes for contigous write - * operations with getFreeElement, this is the maximum allowed additional - * size - */ - SimpleRingBuffer(uint8_t* buffer, const size_t size, bool overwriteOld, - size_t maxExcessBytes = 0); - - virtual ~SimpleRingBuffer(); - - /** - * Write to circular buffer and increment write pointer by amount. - * @param data - * @param amount - * @return -@c RETURN_OK if write operation was successfull - * -@c RETURN_FAILED if - */ - ReturnValue_t writeData(const uint8_t* data, size_t amount); - - /** - * Returns a pointer to a free element. If the remaining buffer is - * not large enough, the data will be written past the actual size - * and the amount of excess bytes will be cached. This function - * does not increment the write pointer! - * @param writePointer Pointer to a pointer which can be used to write - * contiguous blocks into the ring buffer - * @param amount - * @return - */ - ReturnValue_t getFreeElement(uint8_t** writePointer, size_t amount); - - /** - * This increments the write pointer and also copies the excess bytes - * to the beginning. It should be called if the write operation - * conducted after calling getFreeElement() was performed. - * @return - */ - void confirmBytesWritten(size_t amount); - - virtual size_t getExcessBytes() const; - /** - * Helper functions which moves any excess bytes to the start - * of the ring buffer. - * @return - */ - virtual void moveExcessBytesToStart(); - - /** - * Read from circular buffer at read pointer. - * @param data - * @param amount - * @param incrementReadPtr - * If this is set to true, the read pointer will be incremented. - * If readRemaining is set to true, the read pointer will be incremented - * accordingly. - * @param readRemaining - * If this is set to true, the data will be read even if the amount - * specified exceeds the read data available. - * @param trueAmount [out] - * If readRemaining was set to true, the true amount read will be assigned - * to the passed value. - * @return - * - @c RETURN_OK if data was read successfully - * - @c RETURN_FAILED if not enough data was available and readRemaining - * was set to false. - */ - ReturnValue_t readData(uint8_t* data, size_t amount, - bool incrementReadPtr = false, bool readRemaining = false, - size_t* trueAmount = nullptr); - - /** - * Delete data by incrementing read pointer. - * @param amount - * @param deleteRemaining - * If the amount specified is larger than the remaing size to read and this - * is set to true, the remaining amount will be deleted as well - * @param trueAmount [out] - * If deleteRemaining was set to true, the amount deleted will be assigned - * to the passed value. - * @return - */ - ReturnValue_t deleteData(size_t amount, bool deleteRemaining = false, - size_t* trueAmount = nullptr); -private: - static const uint8_t READ_PTR = 0; - uint8_t* buffer = nullptr; - size_t maxExcessBytes; - size_t excessBytes = 0; -}; - -#endif /* FRAMEWORK_CONTAINER_SIMPLERINGBUFFER_H_ */ - +#ifndef FRAMEWORK_CONTAINER_SIMPLERINGBUFFER_H_ +#define FRAMEWORK_CONTAINER_SIMPLERINGBUFFER_H_ + +#include "../container/RingBufferBase.h" +#include + +/** + * @brief Circular buffer implementation, useful for buffering + * into data streams. + * @details + * Note that the deleteData() has to be called to increment the read pointer. + * This class allocated dynamically, so + * @ingroup containers + */ +class SimpleRingBuffer: public RingBufferBase<> { +public: + /** + * This constructor allocates a new internal buffer with the supplied size. + * + * @param size + * @param overwriteOld If the ring buffer is overflowing at a write + * operation, the oldest data will be overwritten. + * @param maxExcessBytes These additional bytes will be allocated in addtion + * to the specified size to accomodate contiguous write operations + * with getFreeElement. + * + */ + SimpleRingBuffer(const size_t size, bool overwriteOld, + size_t maxExcessBytes = 0); + /** + * This constructor takes an external buffer with the specified size. + * @param buffer + * @param size + * @param overwriteOld + * If the ring buffer is overflowing at a write operartion, the oldest data + * will be overwritten. + * @param maxExcessBytes + * If the buffer can accomodate additional bytes for contigous write + * operations with getFreeElement, this is the maximum allowed additional + * size + */ + SimpleRingBuffer(uint8_t* buffer, const size_t size, bool overwriteOld, + size_t maxExcessBytes = 0); + + virtual ~SimpleRingBuffer(); + + /** + * Write to circular buffer and increment write pointer by amount. + * @param data + * @param amount + * @return -@c RETURN_OK if write operation was successfull + * -@c RETURN_FAILED if + */ + ReturnValue_t writeData(const uint8_t* data, size_t amount); + + /** + * Returns a pointer to a free element. If the remaining buffer is + * not large enough, the data will be written past the actual size + * and the amount of excess bytes will be cached. This function + * does not increment the write pointer! + * @param writePointer Pointer to a pointer which can be used to write + * contiguous blocks into the ring buffer + * @param amount + * @return + */ + ReturnValue_t getFreeElement(uint8_t** writePointer, size_t amount); + + /** + * This increments the write pointer and also copies the excess bytes + * to the beginning. It should be called if the write operation + * conducted after calling getFreeElement() was performed. + * @return + */ + void confirmBytesWritten(size_t amount); + + virtual size_t getExcessBytes() const; + /** + * Helper functions which moves any excess bytes to the start + * of the ring buffer. + * @return + */ + virtual void moveExcessBytesToStart(); + + /** + * Read from circular buffer at read pointer. + * @param data + * @param amount + * @param incrementReadPtr + * If this is set to true, the read pointer will be incremented. + * If readRemaining is set to true, the read pointer will be incremented + * accordingly. + * @param readRemaining + * If this is set to true, the data will be read even if the amount + * specified exceeds the read data available. + * @param trueAmount [out] + * If readRemaining was set to true, the true amount read will be assigned + * to the passed value. + * @return + * - @c RETURN_OK if data was read successfully + * - @c RETURN_FAILED if not enough data was available and readRemaining + * was set to false. + */ + ReturnValue_t readData(uint8_t* data, size_t amount, + bool incrementReadPtr = false, bool readRemaining = false, + size_t* trueAmount = nullptr); + + /** + * Delete data by incrementing read pointer. + * @param amount + * @param deleteRemaining + * If the amount specified is larger than the remaing size to read and this + * is set to true, the remaining amount will be deleted as well + * @param trueAmount [out] + * If deleteRemaining was set to true, the amount deleted will be assigned + * to the passed value. + * @return + */ + ReturnValue_t deleteData(size_t amount, bool deleteRemaining = false, + size_t* trueAmount = nullptr); +private: + static const uint8_t READ_PTR = 0; + uint8_t* buffer = nullptr; + size_t maxExcessBytes; + size_t excessBytes = 0; +}; + +#endif /* FRAMEWORK_CONTAINER_SIMPLERINGBUFFER_H_ */ + From e105be229a075bb83e867cf00baa9f392d9d5468 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Sep 2020 15:06:48 +0200 Subject: [PATCH 19/53] include guards fand include improvements --- container/SimpleRingBuffer.cpp | 2 +- container/SimpleRingBuffer.h | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/container/SimpleRingBuffer.cpp b/container/SimpleRingBuffer.cpp index d6b6f5af..01c22463 100644 --- a/container/SimpleRingBuffer.cpp +++ b/container/SimpleRingBuffer.cpp @@ -1,4 +1,4 @@ -#include "../container/SimpleRingBuffer.h" +#include "SimpleRingBuffer.h" #include SimpleRingBuffer::SimpleRingBuffer(const size_t size, bool overwriteOld, diff --git a/container/SimpleRingBuffer.h b/container/SimpleRingBuffer.h index dcf2684b..a790712f 100644 --- a/container/SimpleRingBuffer.h +++ b/container/SimpleRingBuffer.h @@ -1,7 +1,7 @@ -#ifndef FRAMEWORK_CONTAINER_SIMPLERINGBUFFER_H_ -#define FRAMEWORK_CONTAINER_SIMPLERINGBUFFER_H_ +#ifndef FSFW_CONTAINER_SIMPLERINGBUFFER_H_ +#define FSFW_CONTAINER_SIMPLERINGBUFFER_H_ -#include "../container/RingBufferBase.h" +#include "RingBufferBase.h" #include /** @@ -124,5 +124,5 @@ private: size_t excessBytes = 0; }; -#endif /* FRAMEWORK_CONTAINER_SIMPLERINGBUFFER_H_ */ +#endif /* FSFW_CONTAINER_SIMPLERINGBUFFER_H_ */ From 3e67701933e6e64ac998691cd4e2e043e7786427 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Sep 2020 15:09:24 +0200 Subject: [PATCH 20/53] taken over ring buffer base changes --- container/RingBufferBase.h | 125 +++++++++++++++++++++---------------- 1 file changed, 71 insertions(+), 54 deletions(-) diff --git a/container/RingBufferBase.h b/container/RingBufferBase.h index e3a87d9d..886b9fab 100644 --- a/container/RingBufferBase.h +++ b/container/RingBufferBase.h @@ -1,96 +1,113 @@ -#ifndef FRAMEWORK_CONTAINER_RINGBUFFERBASE_H_ -#define FRAMEWORK_CONTAINER_RINGBUFFERBASE_H_ +#ifndef FSFW_CONTAINER_RINGBUFFERBASE_H_ +#define FSFW_CONTAINER_RINGBUFFERBASE_H_ #include "../returnvalues/HasReturnvaluesIF.h" +#include template class RingBufferBase { public: - RingBufferBase(uint32_t startAddress, uint32_t size, bool overwriteOld) : - start(startAddress), write(startAddress), size(size), overwriteOld(overwriteOld) { + RingBufferBase(size_t startAddress, const size_t size, bool overwriteOld) : + start(startAddress), write(startAddress), size(size), + overwriteOld(overwriteOld) { for (uint8_t count = 0; count < N_READ_PTRS; count++) { read[count] = startAddress; } } - ReturnValue_t readData(uint32_t amount, uint8_t n = 0) { - if (availableReadData(n) >= amount) { - incrementRead(amount, n); - return HasReturnvaluesIF::RETURN_OK; - } else { - return HasReturnvaluesIF::RETURN_FAILED; - } - } - ReturnValue_t writeData(uint32_t amount) { - if (availableWriteSpace() >= amount || overwriteOld) { - incrementWrite(amount); - return HasReturnvaluesIF::RETURN_OK; - } else { - return HasReturnvaluesIF::RETURN_FAILED; - } - } - uint32_t availableReadData(uint8_t n = 0) const { - return ((write + size) - read[n]) % size; - } - uint32_t availableWriteSpace(uint8_t n = 0) const { - //One less to avoid ambiguous full/empty problem. - return (((read[n] + size) - write - 1) % size); - } + + virtual ~RingBufferBase() {} + bool isFull(uint8_t n = 0) { return (availableWriteSpace(n) == 0); } bool isEmpty(uint8_t n = 0) { - return (availableReadData(n) == 0); + return (getAvailableReadData(n) == 0); } - virtual ~RingBufferBase() { + size_t getAvailableReadData(uint8_t n = 0) const { + return ((write + size) - read[n]) % size; } - uint32_t getRead(uint8_t n = 0) const { - return read[n]; + size_t availableWriteSpace(uint8_t n = 0) const { + //One less to avoid ambiguous full/empty problem. + return (((read[n] + size) - write - 1) % size); } - void setRead(uint32_t read, uint8_t n = 0) { - if (read >= start && read < (start+size)) { - this->read[n] = read; - } + + bool overwritesOld() const { + return overwriteOld; } - uint32_t getWrite() const { - return write; - } - void setWrite(uint32_t write) { - this->write = write; + + size_t getMaxSize() const { + return size - 1; } + void clear() { write = start; for (uint8_t count = 0; count < N_READ_PTRS; count++) { read[count] = start; } } - uint32_t writeTillWrap() { + + size_t writeTillWrap() { return (start + size) - write; } - uint32_t readTillWrap(uint8_t n = 0) { + + size_t readTillWrap(uint8_t n = 0) { return (start + size) - read[n]; } - uint32_t getStart() const { + + size_t getStart() const { return start; } - bool overwritesOld() const { - return overwriteOld; - } - uint32_t maxSize() const { - return size - 1; - } + protected: - const uint32_t start; - uint32_t write; - uint32_t read[N_READ_PTRS]; - const uint32_t size; + const size_t start; + size_t write; + size_t read[N_READ_PTRS]; + const size_t size; const bool overwriteOld; + void incrementWrite(uint32_t amount) { write = ((write + amount - start) % size) + start; } void incrementRead(uint32_t amount, uint8_t n = 0) { read[n] = ((read[n] + amount - start) % size) + start; } + + ReturnValue_t readData(uint32_t amount, uint8_t n = 0) { + if (getAvailableReadData(n) >= amount) { + incrementRead(amount, n); + return HasReturnvaluesIF::RETURN_OK; + } else { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + + ReturnValue_t writeData(uint32_t amount) { + if (availableWriteSpace() >= amount or overwriteOld) { + incrementWrite(amount); + return HasReturnvaluesIF::RETURN_OK; + } else { + return HasReturnvaluesIF::RETURN_FAILED; + } + } + + size_t getRead(uint8_t n = 0) const { + return read[n]; + } + + void setRead(uint32_t read, uint8_t n = 0) { + if (read >= start && read < (start+size)) { + this->read[n] = read; + } + } + + uint32_t getWrite() const { + return write; + } + + void setWrite(uint32_t write) { + this->write = write; + } }; -#endif /* FRAMEWORK_CONTAINER_RINGBUFFERBASE_H_ */ +#endif /* FSFW_CONTAINER_RINGBUFFERBASE_H_ */ From 06400fa7bbd89792bc1e508aa6ecf424981ab453 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Sep 2020 15:10:44 +0200 Subject: [PATCH 21/53] share dring buffer update --- container/SharedRingBuffer.cpp | 6 +++--- container/SharedRingBuffer.h | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/container/SharedRingBuffer.cpp b/container/SharedRingBuffer.cpp index 001a21ed..490988e5 100644 --- a/container/SharedRingBuffer.cpp +++ b/container/SharedRingBuffer.cpp @@ -1,6 +1,6 @@ -#include -#include -#include +#include "../container/SharedRingBuffer.h" +#include "../ipc/MutexFactory.h" +#include "../ipc/MutexHelper.h" SharedRingBuffer::SharedRingBuffer(object_id_t objectId, const size_t size, bool overwriteOld, size_t maxExcessBytes): diff --git a/container/SharedRingBuffer.h b/container/SharedRingBuffer.h index 0b5be052..394dcaaa 100644 --- a/container/SharedRingBuffer.h +++ b/container/SharedRingBuffer.h @@ -1,10 +1,10 @@ #ifndef FRAMEWORK_CONTAINER_SHAREDRINGBUFFER_H_ #define FRAMEWORK_CONTAINER_SHAREDRINGBUFFER_H_ -#include -#include -#include -#include +#include "../container/SimpleRingBuffer.h" +#include "../ipc/MutexIF.h" +#include "../objectmanager/SystemObject.h" +#include "../timemanager/Clock.h" /** * @brief Ring buffer which can be shared among multiple objects From dbac6e139b6e7991d5f0997d7b2a9f7daebbbe44 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Sep 2020 15:11:53 +0200 Subject: [PATCH 22/53] shared ring buffer update --- container/SharedRingBuffer.cpp | 2 +- container/SharedRingBuffer.h | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/container/SharedRingBuffer.cpp b/container/SharedRingBuffer.cpp index 490988e5..800e75d3 100644 --- a/container/SharedRingBuffer.cpp +++ b/container/SharedRingBuffer.cpp @@ -1,4 +1,4 @@ -#include "../container/SharedRingBuffer.h" +#include "SharedRingBuffer.h" #include "../ipc/MutexFactory.h" #include "../ipc/MutexHelper.h" diff --git a/container/SharedRingBuffer.h b/container/SharedRingBuffer.h index 394dcaaa..80c068b3 100644 --- a/container/SharedRingBuffer.h +++ b/container/SharedRingBuffer.h @@ -1,7 +1,7 @@ -#ifndef FRAMEWORK_CONTAINER_SHAREDRINGBUFFER_H_ -#define FRAMEWORK_CONTAINER_SHAREDRINGBUFFER_H_ +#ifndef FSFW_CONTAINER_SHAREDRINGBUFFER_H_ +#define FSFW_CONTAINER_SHAREDRINGBUFFER_H_ -#include "../container/SimpleRingBuffer.h" +#include "SimpleRingBuffer.h" #include "../ipc/MutexIF.h" #include "../objectmanager/SystemObject.h" #include "../timemanager/Clock.h" @@ -65,4 +65,4 @@ private: -#endif /* FRAMEWORK_CONTAINER_SHAREDRINGBUFFER_H_ */ +#endif /* FSFW_CONTAINER_SHAREDRINGBUFFER_H_ */ From 62eb327df6f86c8e5232cad04b3193bf22759a4b Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Sep 2020 15:19:33 +0200 Subject: [PATCH 23/53] health helper update --- health/HealthHelper.cpp | 47 ++++++++++++++++++++++++----------------- health/HealthHelper.h | 20 ++++++++++-------- 2 files changed, 39 insertions(+), 28 deletions(-) diff --git a/health/HealthHelper.cpp b/health/HealthHelper.cpp index 1cc7d858..cced11d9 100644 --- a/health/HealthHelper.cpp +++ b/health/HealthHelper.cpp @@ -1,9 +1,8 @@ #include "HealthHelper.h" -#include "../ipc/MessageQueueSenderIF.h" #include "../serviceinterface/ServiceInterfaceStream.h" + HealthHelper::HealthHelper(HasHealthIF* owner, object_id_t objectId) : - healthTable(NULL), eventSender(NULL), objectId(objectId), parentQueue( - 0), owner(owner) { + objectId(objectId), owner(owner) { } HealthHelper::~HealthHelper() { @@ -40,9 +39,19 @@ void HealthHelper::setParentQueue(MessageQueueId_t parentQueue) { ReturnValue_t HealthHelper::initialize() { healthTable = objectManager->get(objects::HEALTH_TABLE); eventSender = objectManager->get(objectId); - if ((healthTable == NULL) || eventSender == NULL) { - return HasReturnvaluesIF::RETURN_FAILED; + + if (healthTable == nullptr) { + sif::error << "HealthHelper::initialize: Health table object needs" + "to be created in factory." << std::endl; + return ObjectManagerIF::CHILD_INIT_FAILED; } + + if(eventSender == nullptr) { + sif::error << "HealthHelper::initialize: Owner has to implement " + "ReportingProxyIF." << std::endl; + return ObjectManagerIF::CHILD_INIT_FAILED; + } + ReturnValue_t result = healthTable->registerObject(objectId, HasHealthIF::HEALTHY); if (result != HasReturnvaluesIF::RETURN_OK) { @@ -62,22 +71,22 @@ void HealthHelper::setHealth(HasHealthIF::HealthState health) { void HealthHelper::informParent(HasHealthIF::HealthState health, HasHealthIF::HealthState oldHealth) { - if (parentQueue == 0) { + if (parentQueue == MessageQueueMessageIF::NO_QUEUE) { return; } - CommandMessage message; - HealthMessage::setHealthMessage(&message, HealthMessage::HEALTH_INFO, + CommandMessage information; + HealthMessage::setHealthMessage(&information, HealthMessage::HEALTH_INFO, health, oldHealth); - if (MessageQueueSenderIF::sendMessage(parentQueue, &message, - owner->getCommandQueue()) != HasReturnvaluesIF::RETURN_OK) { + if (MessageQueueSenderIF::sendMessage(parentQueue, &information, + owner->getCommandQueue()) != HasReturnvaluesIF::RETURN_OK) { sif::debug << "HealthHelper::informParent: sending health reply failed." << std::endl; } } -void HealthHelper::handleSetHealthCommand(CommandMessage* message) { - ReturnValue_t result = owner->setHealth(HealthMessage::getHealth(message)); - if (message->getSender() == 0) { +void HealthHelper::handleSetHealthCommand(CommandMessage* command) { + ReturnValue_t result = owner->setHealth(HealthMessage::getHealth(command)); + if (command->getSender() == MessageQueueMessageIF::NO_QUEUE) { return; } CommandMessage reply; @@ -85,12 +94,12 @@ void HealthHelper::handleSetHealthCommand(CommandMessage* message) { HealthMessage::setHealthMessage(&reply, HealthMessage::REPLY_HEALTH_SET); } else { - reply.setReplyRejected(result, message->getCommand()); + reply.setReplyRejected(result, command->getCommand()); } - if (MessageQueueSenderIF::sendMessage(message->getSender(), &reply, - owner->getCommandQueue()) != HasReturnvaluesIF::RETURN_OK) { - sif::debug - << "HealthHelper::handleHealthCommand: sending health reply failed." - << std::endl; + if (MessageQueueSenderIF::sendMessage(command->getSender(), &reply, + owner->getCommandQueue()) != HasReturnvaluesIF::RETURN_OK) { + sif::debug << "HealthHelper::handleHealthCommand: sending health " + "reply failed." << std::endl; + } } diff --git a/health/HealthHelper.h b/health/HealthHelper.h index 90155ad4..d1f1945c 100644 --- a/health/HealthHelper.h +++ b/health/HealthHelper.h @@ -1,11 +1,13 @@ -#ifndef HEALTHHELPER_H_ -#define HEALTHHELPER_H_ +#ifndef FSFW_HEALTH_HEALTHHELPER_H_ +#define FSFW_HEALTH_HEALTHHELPER_H_ -#include "../events/EventManagerIF.h" -#include "../events/EventReportingProxyIF.h" #include "HasHealthIF.h" #include "HealthMessage.h" #include "HealthTableIF.h" + +#include "../events/EventManagerIF.h" +#include "../events/EventReportingProxyIF.h" +#include "../ipc/MessageQueueIF.h" #include "../objectmanager/ObjectManagerIF.h" #include "../returnvalues/HasReturnvaluesIF.h" @@ -27,8 +29,8 @@ public: /** * ctor * + * @param owner * @param objectId the object Id to use when communication with the HealthTable - * @param useAsFrom id to use as from id when sending replies, can be set to 0 */ HealthHelper(HasHealthIF* owner, object_id_t objectId); @@ -39,12 +41,12 @@ public: * * only valid after initialize() has been called */ - HealthTableIF *healthTable; + HealthTableIF *healthTable = nullptr; /** * Proxy to forward events. */ - EventReportingProxyIF* eventSender; + EventReportingProxyIF* eventSender = nullptr; /** * Try to handle the message. @@ -100,7 +102,7 @@ private: /** * The Queue of the parent */ - MessageQueueId_t parentQueue; + MessageQueueId_t parentQueue = MessageQueueIF::NO_QUEUE; /** * The one using the healthHelper. @@ -117,4 +119,4 @@ private: void handleSetHealthCommand(CommandMessage *message); }; -#endif /* HEALTHHELPER_H_ */ +#endif /* FSFW_HEALTH_HEALTHHELPER_H_ */ From e83de8248129398c1c0ed6ed1e7a500ef41ac4cc Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Sep 2020 15:40:42 +0200 Subject: [PATCH 24/53] gathering remaining differences.. --- serialize/EndianConverter.h | 10 ++++------ serialize/SerializeIF.h | 7 +++++-- 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/serialize/EndianConverter.h b/serialize/EndianConverter.h index 7461576d..c888a905 100644 --- a/serialize/EndianConverter.h +++ b/serialize/EndianConverter.h @@ -1,5 +1,5 @@ -#ifndef ENDIANSWAPPER_H_ -#define ENDIANSWAPPER_H_ +#ifndef FSFW_SERIALIZE_ENDIANCONVERTER_H_ +#define FSFW_SERIALIZE_ENDIANCONVERTER_H_ #include "../osal/Endiness.h" #include @@ -35,9 +35,7 @@ */ class EndianConverter { private: - EndianConverter() { - } - ; + EndianConverter() {}; public: /** * Convert a typed variable between big endian and machine endian. @@ -123,4 +121,4 @@ public: } }; -#endif /* ENDIANSWAPPER_H_ */ +#endif /* FSFW_SERIALIZE_ENDIANCONVERTER_H_ */ diff --git a/serialize/SerializeIF.h b/serialize/SerializeIF.h index 7f9ea9df..3c0540f9 100644 --- a/serialize/SerializeIF.h +++ b/serialize/SerializeIF.h @@ -2,7 +2,7 @@ #define FSFW_SERIALIZE_SERIALIZEIF_H_ #include "../returnvalues/HasReturnvaluesIF.h" -#include +#include /** * @defgroup serialize Serialization @@ -10,7 +10,10 @@ */ /** - * Translation of objects into data streams and from data streams. + * @brief Translation of objects into data streams and from data streams. + * @details + * Also provides options to convert from/to data with different endianness. + * variables. * @ingroup serialize */ class SerializeIF { From d4d96a128e07f9158a202f2667bb265b25886cd6 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Sep 2020 15:47:08 +0200 Subject: [PATCH 25/53] adapted doc --- serialize/SerialFixedArrayListAdapter.h | 70 +++++++++++++++++-------- 1 file changed, 48 insertions(+), 22 deletions(-) diff --git a/serialize/SerialFixedArrayListAdapter.h b/serialize/SerialFixedArrayListAdapter.h index 38e0d438..70ca987a 100644 --- a/serialize/SerialFixedArrayListAdapter.h +++ b/serialize/SerialFixedArrayListAdapter.h @@ -1,31 +1,57 @@ -#ifndef SERIALFIXEDARRAYLISTADAPTER_H_ -#define SERIALFIXEDARRAYLISTADAPTER_H_ +#ifndef FSFW_SERIALIZE_SERIALFIXEDARRAYLISTADAPTER_H_ +#define FSFW_SERIALIZE_SERIALFIXEDARRAYLISTADAPTER_H_ -#include "../container/FixedArrayList.h" #include "SerialArrayListAdapter.h" +#include "../container/FixedArrayList.h" /** - * \ingroup serialize + * @brief This adapter provides an interface for SerializeIF to serialize and + * deserialize buffers with a header containing the buffer length. + * @details + * Can be used by SerialLinkedListAdapter by declaring + * as a linked element with SerializeElement>. + * The sequence of objects is defined in the constructor by + * using the setStart and setNext functions. + * + * @tparam BUFFER_TYPE: Specifies the data type of the buffer + * @tparam MAX_SIZE: Specifies the maximum allowed number of elements + * (not bytes!) + * @tparam count_t: specifies the type/size of the length field which defaults + * to one byte. + * @ingroup serialize */ -template -class SerialFixedArrayListAdapter : public FixedArrayList, public SerializeIF { +template +class SerialFixedArrayListAdapter : + public FixedArrayList, + public SerializeIF { public: + /** + * Constructor arguments are forwarded to FixedArrayList constructor. + * Refer to the fixed array list constructors for different options. + * @param args + */ template - SerialFixedArrayListAdapter(Args... args) : FixedArrayList(std::forward(args)...) { - } - ReturnValue_t serialize(uint8_t** buffer, size_t* size, - size_t maxSize, Endianness streamEndianness) const { - return SerialArrayListAdapter::serialize(this, buffer, size, maxSize, streamEndianness); - } - size_t getSerializedSize() const { - return SerialArrayListAdapter::getSerializedSize(this); - } - ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size, - Endianness streamEndianness) { - return SerialArrayListAdapter::deSerialize(this, buffer, size, streamEndianness); - } + SerialFixedArrayListAdapter(Args... args) : + FixedArrayList( + std::forward(args)...){} + + ReturnValue_t serialize(uint8_t** buffer, size_t* size, + size_t maxSize, Endianness streamEndianness) const { + return SerialArrayListAdapter::serialize(this, + buffer, size, maxSize, streamEndianness); + } + + size_t getSerializedSize() const { + return SerialArrayListAdapter:: + getSerializedSize(this); + } + + ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size, + Endianness streamEndianness) { + return SerialArrayListAdapter::deSerialize(this, + buffer, size, streamEndianness); + } + }; - - -#endif /* SERIALFIXEDARRAYLISTADAPTER_H_ */ +#endif /* FSFW_SERIALIZE_SERIALFIXEDARRAYLISTADAPTER_H_ */ From 84308c74d91b2082e458c4d39aa054060e6a2b06 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Sep 2020 15:56:08 +0200 Subject: [PATCH 26/53] taken over some changes --- serialize/SerializeAdapter.h | 35 ++++++++++++++++++++++------------- 1 file changed, 22 insertions(+), 13 deletions(-) diff --git a/serialize/SerializeAdapter.h b/serialize/SerializeAdapter.h index af5ec116..24f6111e 100644 --- a/serialize/SerializeAdapter.h +++ b/serialize/SerializeAdapter.h @@ -1,16 +1,24 @@ -#ifndef SERIALIZEADAPTER_H_ -#define SERIALIZEADAPTER_H_ +#ifndef FSFW_SERIALIZE_SERIALIZEADAPTER_H_ +#define FSFW_SERIALIZE_SERIALIZEADAPTER_H_ + +#include "EndianConverter.h" +#include "SerializeIF.h" #include "../container/IsDerivedFrom.h" #include "../returnvalues/HasReturnvaluesIF.h" -#include "EndianConverter.h" -#include "SerializeIF.h" -#include +#include -/** - * \ingroup serialize + /** + * @brief These adapters provides an interface to use the SerializeIF functions + * with arbitrary template objects to facilitate and simplify the + * serialization of classes with different multiple different data types + * into buffers and vice-versa. + * @details + * The correct serialization or deserialization function is chosen at + * compile time with template type deduction. + * + * @ingroup serialize */ - class SerializeAdapter { public: template @@ -36,9 +44,10 @@ private: class InternalSerializeAdapter { public: static ReturnValue_t serialize(const T *object, uint8_t **buffer, - size_t *size, size_t max_size, SerializeIF::Endianness streamEndianness) { + size_t *size, size_t max_size, + SerializeIF::Endianness streamEndianness) { size_t ignoredSize = 0; - if (size == NULL) { + if (size == nullptr) { size = &ignoredSize; } //TODO check integer overflow of *size @@ -56,7 +65,7 @@ private: tmp = *object; break; } - memcpy(*buffer, &tmp, sizeof(T)); + std::memcpy(*buffer, &tmp, sizeof(T)); *size += sizeof(T); (*buffer) += sizeof(T); return HasReturnvaluesIF::RETURN_OK; @@ -70,7 +79,7 @@ private: T tmp; if (*size >= sizeof(T)) { *size -= sizeof(T); - memcpy(&tmp, *buffer, sizeof(T)); + std::memcpy(&tmp, *buffer, sizeof(T)); switch (streamEndianness) { case SerializeIF::Endianness::BIG: *object = EndianConverter::convertBigEndian(tmp); @@ -104,7 +113,7 @@ private: size_t *size, size_t max_size, SerializeIF::Endianness streamEndianness) const { size_t ignoredSize = 0; - if (size == NULL) { + if (size == nullptr) { size = &ignoredSize; } return object->serialize(buffer, size, max_size, streamEndianness); From ed21ec6c78bc722538ae16f5b3fb8a078c97300d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Sep 2020 16:00:45 +0200 Subject: [PATCH 27/53] changes taken over for serial linked list adapter --- serialize/SerialLinkedListAdapter.h | 67 +++++++++++++++++++++-------- 1 file changed, 50 insertions(+), 17 deletions(-) diff --git a/serialize/SerialLinkedListAdapter.h b/serialize/SerialLinkedListAdapter.h index 5de8c242..430a21ac 100644 --- a/serialize/SerialLinkedListAdapter.h +++ b/serialize/SerialLinkedListAdapter.h @@ -1,32 +1,52 @@ -/** - * @file SerialLinkedListAdapter.h - * @brief This file defines the SerialLinkedListAdapter class. - * @date 22.07.2014 - * @author baetz - */ -#ifndef SERIALLINKEDLISTADAPTER_H_ -#define SERIALLINKEDLISTADAPTER_H_ +#ifndef FSFW_SERIALIZE_SERIALLINKEDLISTADAPTER_H_ +#define FSFW_SERIALIZE_SERIALLINKEDLISTADAPTER_H_ #include "../container/SinglyLinkedList.h" #include "SerializeAdapter.h" #include "SerializeElement.h" #include "SerializeIF.h" -//This is where we need the SerializeAdapter! -/** - * \ingroup serialize + /** + * @brief Implement the conversion of object data to data streams + * or vice-versa, using linked lists. + * @details + * An alternative to the AutoSerializeAdapter functions + * - All object members with a datatype are declared as + * SerializeElement members inside the class + * implementing this adapter. + * - The element type can also be a SerialBufferAdapter to + * de-/serialize buffers. + * - The element type can also be a SerialFixedArrayListAdapter to + * de-/serialize buffers with a size header, which is scanned automatically. + * + * The sequence of objects is defined in the constructor by using + * the setStart and setNext functions. + * + * 1. The serialization process is done by instantiating the class and + * calling serialize after all SerializeElement entries have been set by + * using the constructor or setter functions. An additional size variable + * can be supplied which is calculated/incremented automatically. + * 2. The deserialization process is done by instantiating the class and + * supplying a buffer with the data which is converted into an object. + * The size of data to serialize can be supplied and is + * decremented in the function. Range checking is done internally. + * @author baetz + * @ingroup serialize */ template class SerialLinkedListAdapter: public SinglyLinkedList, public SerializeIF { public: + SerialLinkedListAdapter(typename LinkedElement::Iterator start, bool printCount = false) : SinglyLinkedList(start), printCount(printCount) { } + SerialLinkedListAdapter(LinkedElement* first, bool printCount = false) : SinglyLinkedList(first), printCount(printCount) { } + SerialLinkedListAdapter(bool printCount = false) : SinglyLinkedList(), printCount(printCount) { } @@ -49,13 +69,14 @@ public: uint8_t** buffer, size_t* size, size_t maxSize, Endianness streamEndianness) { ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; - while ((result == HasReturnvaluesIF::RETURN_OK) && (element != NULL)) { + while ((result == HasReturnvaluesIF::RETURN_OK) and (element != nullptr)) { result = element->value->serialize(buffer, size, maxSize, streamEndianness); element = element->getNext(); } return result; } + virtual size_t getSerializedSize() const override { if (printCount) { return SerialLinkedListAdapter::getSerializedSize() @@ -64,32 +85,44 @@ public: return getSerializedSize(SinglyLinkedList::start); } } + static size_t getSerializedSize(const LinkedElement *element) { size_t size = 0; - while (element != NULL) { + while (element != nullptr) { size += element->value->getSerializedSize(); element = element->getNext(); } return size; } + virtual ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size, Endianness streamEndianness) override { - return deSerialize(SinglyLinkedList::start, buffer, size, streamEndianness); + return deSerialize(SinglyLinkedList::start, buffer, size, + streamEndianness); } static ReturnValue_t deSerialize(LinkedElement* element, const uint8_t** buffer, size_t* size, Endianness streamEndianness) { ReturnValue_t result = HasReturnvaluesIF::RETURN_OK; - while ((result == HasReturnvaluesIF::RETURN_OK) && (element != NULL)) { + while ((result == HasReturnvaluesIF::RETURN_OK) and (element != nullptr)) { result = element->value->deSerialize(buffer, size, streamEndianness); element = element->getNext(); } return result; } - bool printCount; + /** + * Copying is forbidden by deleting the copy constructor and the copy + * assignment operator because of the pointers to the linked list members. + * Unless the child class implements an own copy constructor or + * copy assignment operator, these operation will throw a compiler error. + * @param + */ + SerialLinkedListAdapter(const SerialLinkedListAdapter &) = delete; + SerialLinkedListAdapter& operator=(const SerialLinkedListAdapter&) = delete; + bool printCount; }; -#endif /* SERIALLINKEDLISTADAPTER_H_ */ +#endif /* FSFW_SERIALIZE_SERIALLINKEDLISTADAPTER_H_ */ From 328c1b71957eedb0277a3815da8880c3a60391e3 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Sep 2020 16:03:36 +0200 Subject: [PATCH 28/53] takne over serialize element changes --- serialize/SerializeElement.h | 24 ++++++++++++++++++------ 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/serialize/SerializeElement.h b/serialize/SerializeElement.h index ffedeff9..47080292 100644 --- a/serialize/SerializeElement.h +++ b/serialize/SerializeElement.h @@ -1,12 +1,20 @@ -#ifndef SERIALIZEELEMENT_H_ -#define SERIALIZEELEMENT_H_ +#ifndef FSFW_SERIALIZE_SERIALIZEELEMENT_H_ +#define FSFW_SERIALIZE_SERIALIZEELEMENT_H_ -#include "../container/SinglyLinkedList.h" #include "SerializeAdapter.h" +#include "../container/SinglyLinkedList.h" #include /** - * \ingroup serialize + * @brief This class is used to mark datatypes for serialization with the + * SerialLinkedListAdapter + * @details + * Used by declaring any arbitrary datatype with SerializeElement myVariable, + * inside a SerialLinkedListAdapter implementation and setting the sequence + * of objects with setNext() and setStart(). + * Serialization and Deserialization is then performed automatically in + * specified sequence order. + * @ingroup serialize */ template class SerializeElement: public SerializeIF, public LinkedElement { @@ -19,7 +27,7 @@ public: SerializeElement() : LinkedElement(this) { } - T entry; + ReturnValue_t serialize(uint8_t **buffer, size_t *size, size_t maxSize, Endianness streamEndianness) const override { return SerializeAdapter::serialize(&entry, buffer, size, maxSize, @@ -35,6 +43,7 @@ public: return SerializeAdapter::deSerialize(&entry, buffer, size, streamEndianness); } + operator T() { return entry; } @@ -43,9 +52,12 @@ public: entry = newValue; return *this; } + T* operator->() { return &entry; } + + T entry; }; -#endif /* SERIALIZEELEMENT_H_ */ +#endif /* FSFW_SERIALIZE_SERIALIZEELEMENT_H_ */ From 1a62158f330d0d5dc21df9a14c6c3a791a40cd7a Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Fri, 4 Sep 2020 16:07:11 +0200 Subject: [PATCH 29/53] took over serial array list adapter changes --- serialize/SerialArrayListAdapter.h | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/serialize/SerialArrayListAdapter.h b/serialize/SerialArrayListAdapter.h index 7576505a..daa3fe7f 100644 --- a/serialize/SerialArrayListAdapter.h +++ b/serialize/SerialArrayListAdapter.h @@ -1,13 +1,14 @@ -#ifndef FRAMEWORK_SERIALIZE_SERIALARRAYLISTADAPTER_H_ -#define FRAMEWORK_SERIALIZE_SERIALARRAYLISTADAPTER_H_ +#ifndef FSFW_SERIALIZE_SERIALARRAYLISTADAPTER_H_ +#define FSFW_SERIALIZE_SERIALARRAYLISTADAPTER_H_ -#include "../container/ArrayList.h" #include "SerializeIF.h" +#include "../container/ArrayList.h" #include /** + * Also serializes length field ! + * @author baetz * @ingroup serialize - * @author baetz */ template class SerialArrayListAdapter : public SerializeIF { @@ -21,14 +22,14 @@ public: } static ReturnValue_t serialize(const ArrayList* list, - uint8_t** buffer, size_t* size, size_t maxSize, - Endianness streamEndianness) { + uint8_t** buffer, size_t* size, size_t maxSize, + Endianness streamEndianness) { ReturnValue_t result = SerializeAdapter::serialize(&list->size, buffer, size, maxSize, streamEndianness); count_t i = 0; while ((result == HasReturnvaluesIF::RETURN_OK) && (i < list->size)) { - result = SerializeAdapter::serialize(&list->entries[i], buffer, - size, maxSize, streamEndianness); + result = SerializeAdapter::serialize(&list->entries[i], buffer, size, + maxSize, streamEndianness); ++i; } return result; @@ -55,17 +56,18 @@ public: } static ReturnValue_t deSerialize(ArrayList* list, - const uint8_t** buffer, size_t* size, + const uint8_t** buffer, size_t* size, Endianness streamEndianness) { count_t tempSize = 0; ReturnValue_t result = SerializeAdapter::deSerialize(&tempSize, buffer, size, streamEndianness); if(result != HasReturnvaluesIF::RETURN_OK) { - return result; + return result; } if (tempSize > list->maxSize()) { return SerializeIF::TOO_MANY_ELEMENTS; } + list->size = tempSize; count_t i = 0; while ((result == HasReturnvaluesIF::RETURN_OK) && (i < list->size)) { @@ -76,10 +78,9 @@ public: } return result; } + private: ArrayList *adaptee; }; - - -#endif /* FRAMEWORK_SERIALIZE_SERIALARRAYLISTADAPTER_H_ */ +#endif /* FSFW_SERIALIZE_SERIALARRAYLISTADAPTER_H_ */ From a1155686c5dbb1b844c13ee078ff13579e4de4fb Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sat, 5 Sep 2020 20:18:52 +0200 Subject: [PATCH 30/53] 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 31/53] 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 32/53] 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 d807ea3afe15a69efbc87a8da59d37052e277238 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 10 Sep 2020 16:42:11 +0200 Subject: [PATCH 33/53] using MessageQueueIF::NO_QUEUE now --- health/HealthHelper.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/health/HealthHelper.cpp b/health/HealthHelper.cpp index cced11d9..d574634d 100644 --- a/health/HealthHelper.cpp +++ b/health/HealthHelper.cpp @@ -71,7 +71,7 @@ void HealthHelper::setHealth(HasHealthIF::HealthState health) { void HealthHelper::informParent(HasHealthIF::HealthState health, HasHealthIF::HealthState oldHealth) { - if (parentQueue == MessageQueueMessageIF::NO_QUEUE) { + if (parentQueue == MessageQueueIF::NO_QUEUE) { return; } CommandMessage information; @@ -86,7 +86,7 @@ void HealthHelper::informParent(HasHealthIF::HealthState health, void HealthHelper::handleSetHealthCommand(CommandMessage* command) { ReturnValue_t result = owner->setHealth(HealthMessage::getHealth(command)); - if (command->getSender() == MessageQueueMessageIF::NO_QUEUE) { + if (command->getSender() == MessageQueueIF::NO_QUEUE) { return; } CommandMessage reply; From d83573cefc14ea6ff6f45e694a3f29d533117797 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 10 Sep 2020 16:47:01 +0200 Subject: [PATCH 34/53] 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 35/53] 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 36/53] 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); From a5c6be9dd94cc213c752936634a991545a5ffa7e Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Wed, 16 Sep 2020 19:05:25 +0200 Subject: [PATCH 37/53] added srv17 --- events/fwSubsystemIdRanges.h | 7 +++--- pus/Service17Test.cpp | 41 +++++++++++++++++++++++++++++++++ pus/Service17Test.h | 44 ++++++++++++++++++++++++++++++++++++ 3 files changed, 89 insertions(+), 3 deletions(-) create mode 100644 pus/Service17Test.cpp create mode 100644 pus/Service17Test.h diff --git a/events/fwSubsystemIdRanges.h b/events/fwSubsystemIdRanges.h index a05652c2..e21d16b8 100644 --- a/events/fwSubsystemIdRanges.h +++ b/events/fwSubsystemIdRanges.h @@ -1,5 +1,5 @@ -#ifndef FRAMEWORK_EVENTS_FWSUBSYSTEMIDRANGES_H_ -#define FRAMEWORK_EVENTS_FWSUBSYSTEMIDRANGES_H_ +#ifndef FSFW_EVENTS_FWSUBSYSTEMIDRANGES_H_ +#define FSFW_EVENTS_FWSUBSYSTEMIDRANGES_H_ namespace SUBSYSTEM_ID { enum { @@ -19,10 +19,11 @@ enum { SYSTEM_MANAGER_1 = 75, SYSTEM_1 = 79, PUS_SERVICE_1 = 80, + PUS_SERVICE_17 = 97, FW_SUBSYSTEM_ID_RANGE }; } -#endif /* FRAMEWORK_EVENTS_FWSUBSYSTEMIDRANGES_H_ */ +#endif /* FSFW_EVENTS_FWSUBSYSTEMIDRANGES_H_ */ diff --git a/pus/Service17Test.cpp b/pus/Service17Test.cpp new file mode 100644 index 00000000..2ef44490 --- /dev/null +++ b/pus/Service17Test.cpp @@ -0,0 +1,41 @@ +#include "Service17Test.h" + +#include "../serviceinterface/ServiceInterfaceStream.h" +#include "../objectmanager/SystemObject.h" +#include "../tmtcpacket/pus/TmPacketStored.h" + + +Service17Test::Service17Test(object_id_t objectId, + uint16_t apid, uint8_t serviceId): + PusServiceBase(objectId, apid, serviceId), + packetSubCounter(0) { +} + +Service17Test::~Service17Test() { +} + +ReturnValue_t Service17Test::handleRequest(uint8_t subservice) { + switch(subservice){ + case Subservice::CONNECTION_TEST: { + TmPacketStored connectionPacket(apid, serviceId, + Subservice::CONNECTION_TEST_REPORT, packetSubCounter++); + connectionPacket.sendPacket(requestQueue->getDefaultDestination(), + requestQueue->getId()); + return HasReturnvaluesIF::RETURN_OK; + } + case Subservice::EVENT_TRIGGER_TEST: { + TmPacketStored connectionPacket(apid, serviceId, + Subservice::CONNECTION_TEST_REPORT, packetSubCounter++); + connectionPacket.sendPacket(requestQueue->getDefaultDestination(), + requestQueue->getId()); + triggerEvent(TEST, 1234, 5678); + return RETURN_OK; + } + default: + return AcceptsTelecommandsIF::INVALID_SUBSERVICE; + } +} + +ReturnValue_t Service17Test::performService() { + return HasReturnvaluesIF::RETURN_OK; +} diff --git a/pus/Service17Test.h b/pus/Service17Test.h new file mode 100644 index 00000000..e2681865 --- /dev/null +++ b/pus/Service17Test.h @@ -0,0 +1,44 @@ +#ifndef FSFW_PUS_SERVICE17TEST_H_ +#define FSFW_PUS_SERVICE17TEST_H_ + +#include "../tmtcservices/PusServiceBase.h" +#include "../objectmanager/SystemObject.h" + +/** + * @brief Test Service + * Full Documentation: ECSS-E70-41A p.167 + * + * The test service provides the capability to activate test functions + * implemented on-board and to report the results of such tests. + * Service capability: + * - TC[17,1]: Perform connection test + * - TM[17,2]: Send Connection Test Report + * - TC[17,128]: Perform connection test and trigger event + * + * @ingroup pus_services + */ +class Service17Test: public PusServiceBase { +public: + // Custom events which can be triggered + static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_17; + static constexpr Event TEST = MAKE_EVENT(0, SEVERITY::INFO); + + enum Subservice: uint8_t { + //! [EXPORT] : [COMMAND] Perform connection test + CONNECTION_TEST = 1, + //! [EXPORT] : [REPLY] Connection test reply + CONNECTION_TEST_REPORT = 2, + //! [EXPORT] : [COMMAND] Trigger test reply and test event + EVENT_TRIGGER_TEST = 128, + }; + + Service17Test(object_id_t objectId, uint16_t apid, uint8_t serviceId); + virtual ~Service17Test(); + virtual ReturnValue_t handleRequest(uint8_t subservice) override; + virtual ReturnValue_t performService() override; + +protected: + uint16_t packetSubCounter = 0; +}; + +#endif /* FSFW_PUS_SERVICE17TEST_H_ */ From ae426c50baf54d7c8f6b022da434dbc510fcef0c Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Wed, 16 Sep 2020 19:09:51 +0200 Subject: [PATCH 38/53] space replaced by tab --- events/fwSubsystemIdRanges.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/events/fwSubsystemIdRanges.h b/events/fwSubsystemIdRanges.h index e21d16b8..5e086f0e 100644 --- a/events/fwSubsystemIdRanges.h +++ b/events/fwSubsystemIdRanges.h @@ -19,7 +19,7 @@ enum { SYSTEM_MANAGER_1 = 75, SYSTEM_1 = 79, PUS_SERVICE_1 = 80, - PUS_SERVICE_17 = 97, + PUS_SERVICE_17 = 97, FW_SUBSYSTEM_ID_RANGE }; } From b31bee4fdac61bc103b62fb881b617b808840129 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Wed, 16 Sep 2020 19:11:50 +0200 Subject: [PATCH 39/53] added srv9 --- pus/Service9TimeManagement.cpp | 58 ++++++++++++++++++++++++++++ pus/Service9TimeManagement.h | 42 ++++++++++++++++++++ pus/servicepackets/Service9Packets.h | 32 +++++++++++++++ 3 files changed, 132 insertions(+) create mode 100644 pus/Service9TimeManagement.cpp create mode 100644 pus/Service9TimeManagement.h create mode 100644 pus/servicepackets/Service9Packets.h diff --git a/pus/Service9TimeManagement.cpp b/pus/Service9TimeManagement.cpp new file mode 100644 index 00000000..5625bdd8 --- /dev/null +++ b/pus/Service9TimeManagement.cpp @@ -0,0 +1,58 @@ +#include "Service9TimeManagement.h" +#include "servicepackets/Service9Packets.h" + +#include "../timemanager/CCSDSTime.h" +#include "../events/EventManagerIF.h" +#include "../serviceinterface/ServiceInterfaceStream.h" + + +Service9TimeManagement::Service9TimeManagement(object_id_t objectId, + uint16_t apid, uint8_t serviceId) : + PusServiceBase(objectId, apid , serviceId) { +} + +Service9TimeManagement::~Service9TimeManagement() {} + +ReturnValue_t Service9TimeManagement::performService() { + return RETURN_OK; +} + +ReturnValue_t Service9TimeManagement::handleRequest(uint8_t subservice) { + switch(subservice){ + case SUBSERVICE::SET_TIME:{ + return setTime(); + } + default: + return AcceptsTelecommandsIF::INVALID_SUBSERVICE; + } +} + +ReturnValue_t Service9TimeManagement::setTime() { + Clock::TimeOfDay_t timeToSet; + TimePacket timePacket(currentPacket.getApplicationData(), + currentPacket.getApplicationDataSize()); + ReturnValue_t result = CCSDSTime::convertFromCcsds(&timeToSet, + timePacket.getTime(), timePacket.getTimeSize()); + if(result != RETURN_OK) { + triggerEvent(CLOCK_SET_FAILURE, result, 0); + return result; + } + + uint32_t formerUptime; + Clock::getUptime(&formerUptime); + result = Clock::setClock(&timeToSet); + + if(result == RETURN_OK) { + uint32_t newUptime; + Clock::getUptime(&newUptime); + triggerEvent(CLOCK_SET,newUptime,formerUptime); + return RETURN_OK; + } + else { + triggerEvent(CLOCK_SET_FAILURE, result, 0); + return RETURN_FAILED; + } +} + + + diff --git a/pus/Service9TimeManagement.h b/pus/Service9TimeManagement.h new file mode 100644 index 00000000..4ad6be6e --- /dev/null +++ b/pus/Service9TimeManagement.h @@ -0,0 +1,42 @@ +#ifndef FSFW_PUS_SERVICE9TIMEMANAGEMENT_H_ +#define FSFW_PUS_SERVICE9TIMEMANAGEMENT_H_ + +#include + +class Service9TimeManagement: public PusServiceBase { +public: + + static constexpr uint8_t SUBSYSTEM_ID = SUBSYSTEM_ID::PUS_SERVICE_9; + static constexpr Event CLOCK_SET = MAKE_EVENT(0, SEVERITY::INFO); //!< Clock has been set. P1: New Uptime. P2: Old Uptime + static constexpr Event CLOCK_SET_FAILURE = MAKE_EVENT(1, SEVERITY::LOW); //!< Clock could not be set. P1: Returncode. + + static constexpr uint8_t CLASS_ID = CLASS_ID::PUS_SERVICE_9; + + /** + * @brief This service provides the capability to set the on-board time. + */ + Service9TimeManagement(object_id_t objectId, uint16_t apid, + uint8_t serviceId); + + virtual ~Service9TimeManagement(); + + virtual ReturnValue_t performService() override; + + /** + * @brief Sets the onboard-time by retrieving the time to set from TC[9,128]. + */ + virtual ReturnValue_t handleRequest(uint8_t subservice) override; + + virtual ReturnValue_t setTime(); +private: + + enum SUBSERVICE { + SET_TIME = 128 //!< [EXPORT] : [COMMAND] Time command in ASCII, CUC or CDS format + }; + + void setIsisClock(Clock::TimeOfDay_t& timeOfDay); +}; + + + +#endif /* FSFW_PUS_SERVICE9TIMEMANAGEMENT_H_ */ diff --git a/pus/servicepackets/Service9Packets.h b/pus/servicepackets/Service9Packets.h new file mode 100644 index 00000000..11bd2600 --- /dev/null +++ b/pus/servicepackets/Service9Packets.h @@ -0,0 +1,32 @@ +#ifndef FSFW_PUS_SERVICEPACKETS_SERVICE9PACKETS_H_ +#define FSFW_PUS_SERVICEPACKETS_SERVICE9PACKETS_H_ + +#include "../../serialize/SerialLinkedListAdapter.h" + +/** + * @brief Subservice 128 + * @details + * It only contains the time encoded as ASCII, CRC, CUC or CDS + * @ingroup spacepackets + */ +class TimePacket : SerialLinkedListAdapter { //!< [EXPORT] : [SUBSERVICE] 128 +public: + TimePacket(const uint8_t * timeBuffer_, uint32_t timeSize_) { + timeBuffer = timeBuffer_; + timeSize = timeSize_; + } + const uint8_t* getTime() { + return timeBuffer; + } + + uint32_t getTimeSize() const { + return timeSize; + } + +private: + TimePacket(const TimePacket &command); + const uint8_t * timeBuffer; + uint32_t timeSize; //!< [EXPORT] : [IGNORE] +}; + +#endif /* FSFW_PUS_SERVICEPACKETS_SERVICE9PACKETS_H_ */ From 85d6e81881f22037176c2f3e457bbd35733d9035 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Wed, 16 Sep 2020 19:17:00 +0200 Subject: [PATCH 40/53] added srv9 to class IDs --- returnvalues/FwClassIds.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/returnvalues/FwClassIds.h b/returnvalues/FwClassIds.h index 80df5741..e547025b 100644 --- a/returnvalues/FwClassIds.h +++ b/returnvalues/FwClassIds.h @@ -1,5 +1,5 @@ -#ifndef FRAMEWORK_RETURNVALUES_FWCLASSIDS_H_ -#define FRAMEWORK_RETURNVALUES_FWCLASSIDS_H_ +#ifndef FSFW_RETURNVALUES_FWCLASSIDS_H_ +#define FSFW_RETURNVALUES_FWCLASSIDS_H_ namespace CLASS_ID { enum { @@ -65,9 +65,10 @@ enum { POOL_VARIABLE_IF, //PVA 59 HOUSEKEEPING_MANAGER, //HKM 60 DLE_ENCODER, //DLEE 61 + PUS_SERVICE_9, //PUS9 62 FW_CLASS_ID_COUNT //is actually count + 1 ! }; } -#endif /* FRAMEWORK_RETURNVALUES_FWCLASSIDS_H_ */ +#endif /* FSFW_RETURNVALUES_FWCLASSIDS_H_ */ From 963015513f8e8825deaa95a7cabafd878d4811f4 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Wed, 16 Sep 2020 19:36:15 +0200 Subject: [PATCH 41/53] added time stamper to framework --- timemanager/TimeStamper.cpp | 23 +++++++++++++++++++++++ timemanager/TimeStamper.h | 36 ++++++++++++++++++++++++++++++++++++ 2 files changed, 59 insertions(+) create mode 100644 timemanager/TimeStamper.cpp create mode 100644 timemanager/TimeStamper.h diff --git a/timemanager/TimeStamper.cpp b/timemanager/TimeStamper.cpp new file mode 100644 index 00000000..07c3530c --- /dev/null +++ b/timemanager/TimeStamper.cpp @@ -0,0 +1,23 @@ +#include +#include +#include + +TimeStamper::TimeStamper(object_id_t objectId): SystemObject(objectId) {} + + +ReturnValue_t TimeStamper::addTimeStamp(uint8_t* buffer, + const uint8_t maxSize) { + if(maxSize < TimeStamperIF::MISSION_TIMESTAMP_SIZE){ + return HasReturnvaluesIF::RETURN_FAILED; + } + + timeval now; + Clock::getClock_timeval(&now); + CCSDSTime::CDS_short cds; + ReturnValue_t result = CCSDSTime::convertToCcsds(&cds,&now); + if(result != HasReturnvaluesIF::RETURN_OK){ + return result; + } + memcpy(buffer,&cds,sizeof(cds)); + return result; +} diff --git a/timemanager/TimeStamper.h b/timemanager/TimeStamper.h new file mode 100644 index 00000000..80139c50 --- /dev/null +++ b/timemanager/TimeStamper.h @@ -0,0 +1,36 @@ +#ifndef FSFW_TIMEMANAGER_TIMESTAMPER_H_ +#define FSFW_TIMEMANAGER_TIMESTAMPER_H_ + +#include +#include +#include + +/** + * @brief Time stamper which can be used to add any timestamp to a + * given buffer. + * @details + * This time stamper uses the CCSDS CDC short timestamp as a fault timestamp. + * This timestamp has a size of 8 bytes. A custom timestamp can be used by + * overriding the #addTimeStamp function. + * @ingroup utility + */ +class TimeStamper: public TimeStamperIF, public SystemObject { +public: + /** + * @brief Default constructor which also registers the time stamper as a + * system object so it can be found with the #objectManager. + * @param objectId + */ + TimeStamper(object_id_t objectId); + + /** + * Adds a CCSDS CDC short 8 byte timestamp to the given buffer. + * This function can be overriden to use a custom timestamp. + * @param buffer + * @param maxSize + * @return + */ + virtual ReturnValue_t addTimeStamp(uint8_t* buffer, const uint8_t maxSize); +}; + +#endif /* FSFW_TIMEMANAGER_TIMESTAMPER_H_ */ From b0a816490e89c639e34849587eb4bc11d619b99f Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Wed, 16 Sep 2020 19:37:17 +0200 Subject: [PATCH 42/53] fixed includes --- timemanager/TimeStamper.cpp | 6 +++--- timemanager/TimeStamper.h | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/timemanager/TimeStamper.cpp b/timemanager/TimeStamper.cpp index 07c3530c..d9f0f2f3 100644 --- a/timemanager/TimeStamper.cpp +++ b/timemanager/TimeStamper.cpp @@ -1,5 +1,5 @@ -#include -#include +#include "TimeStamper.h" +#include "Clock.h" #include TimeStamper::TimeStamper(object_id_t objectId): SystemObject(objectId) {} @@ -18,6 +18,6 @@ ReturnValue_t TimeStamper::addTimeStamp(uint8_t* buffer, if(result != HasReturnvaluesIF::RETURN_OK){ return result; } - memcpy(buffer,&cds,sizeof(cds)); + std::memcpy(buffer,&cds,sizeof(cds)); return result; } diff --git a/timemanager/TimeStamper.h b/timemanager/TimeStamper.h index 80139c50..6895c14b 100644 --- a/timemanager/TimeStamper.h +++ b/timemanager/TimeStamper.h @@ -1,9 +1,9 @@ #ifndef FSFW_TIMEMANAGER_TIMESTAMPER_H_ #define FSFW_TIMEMANAGER_TIMESTAMPER_H_ -#include -#include -#include +#include "TimeStamperIF.h" +#include "CCSDSTime.h" +#include "../objectmanager/SystemObject.h" /** * @brief Time stamper which can be used to add any timestamp to a From 53723b0795f703cc1c2e004df37882d2a6da5424 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Wed, 16 Sep 2020 19:38:43 +0200 Subject: [PATCH 43/53] include fix --- pus/Service9TimeManagement.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pus/Service9TimeManagement.h b/pus/Service9TimeManagement.h index 4ad6be6e..9eed12ca 100644 --- a/pus/Service9TimeManagement.h +++ b/pus/Service9TimeManagement.h @@ -1,7 +1,7 @@ #ifndef FSFW_PUS_SERVICE9TIMEMANAGEMENT_H_ #define FSFW_PUS_SERVICE9TIMEMANAGEMENT_H_ -#include +#include "../tmtcservices/PusServiceBase.h" class Service9TimeManagement: public PusServiceBase { public: From 0f0ddfc37553fa2b922696ba373c2450d7cbb45d Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Fri, 18 Sep 2020 12:27:40 +0200 Subject: [PATCH 44/53] stopwatch update --- osal/linux/Clock.cpp | 27 +++++++++------------------ timemanager/Clock.h | 3 +-- timemanager/Stopwatch.cpp | 15 +++++++++------ timemanager/Stopwatch.h | 13 ++++++------- 4 files changed, 25 insertions(+), 33 deletions(-) diff --git a/osal/linux/Clock.cpp b/osal/linux/Clock.cpp index 5f764a6f..b3b09923 100644 --- a/osal/linux/Clock.cpp +++ b/osal/linux/Clock.cpp @@ -75,24 +75,15 @@ timeval Clock::getUptime() { } ReturnValue_t Clock::getUptime(timeval* uptime) { - //TODO This is not posix compatible and delivers only seconds precision - struct sysinfo sysInfo; - int result = sysinfo(&sysInfo); - if(result != 0){ - return HasReturnvaluesIF::RETURN_FAILED; - } - uptime->tv_sec = sysInfo.uptime; - uptime->tv_usec = 0; - - - //Linux specific file read but more precise -// double uptimeSeconds; -// if(std::ifstream("/proc/uptime",std::ios::in) >> uptimeSeconds){ -// uptime->tv_sec = uptimeSeconds; -// uptime->tv_usec = uptimeSeconds *(double) 1e6 - (uptime->tv_sec *1e6); -// } - - return HasReturnvaluesIF::RETURN_OK; + //TODO This is not posix compatible and delivers only seconds precision + // is the OS not called Linux? + //Linux specific file read but more precise + double uptimeSeconds; + if(std::ifstream("/proc/uptime",std::ios::in) >> uptimeSeconds){ + uptime->tv_sec = uptimeSeconds; + uptime->tv_usec = uptimeSeconds *(double) 1e6 - (uptime->tv_sec *1e6); + } + return HasReturnvaluesIF::RETURN_OK; } ReturnValue_t Clock::getUptime(uint32_t* uptimeMs) { diff --git a/timemanager/Clock.h b/timemanager/Clock.h index 6f6a97da..acb68e2e 100644 --- a/timemanager/Clock.h +++ b/timemanager/Clock.h @@ -9,8 +9,7 @@ #include //! Don't use these for time points, type is not large enough for UNIX epoch. -typedef uint32_t dur_millis_t; -typedef double dur_seconds_t; +using dur_millis_t = uint32_t; class Clock { public: diff --git a/timemanager/Stopwatch.cpp b/timemanager/Stopwatch.cpp index 00373bd7..2cd31969 100644 --- a/timemanager/Stopwatch.cpp +++ b/timemanager/Stopwatch.cpp @@ -1,4 +1,4 @@ -#include "Stopwatch.h" +#include "../timemanager/Stopwatch.h" #include "../serviceinterface/ServiceInterfaceStream.h" #include @@ -6,19 +6,22 @@ Stopwatch::Stopwatch(bool displayOnDestruction, StopwatchDisplayMode displayMode): displayOnDestruction( displayOnDestruction), displayMode(displayMode) { // Measures start time on initialization. - Clock::getClock_timeval(&startTime); + Clock::getUptime(&startTime); } void Stopwatch::start() { - Clock::getClock_timeval(&startTime); + Clock::getUptime(&startTime); } -dur_millis_t Stopwatch::stop() { +dur_millis_t Stopwatch::stop(bool display) { stopInternal(); + if(display) { + this->display(); + } return elapsedTime.tv_sec * 1000 + elapsedTime.tv_usec / 1000; } -dur_seconds_t Stopwatch::stopSeconds() { +double Stopwatch::stopSeconds() { stopInternal(); return timevalOperations::toDouble(elapsedTime); } @@ -52,6 +55,6 @@ StopwatchDisplayMode Stopwatch::getDisplayMode() const { void Stopwatch::stopInternal() { timeval endTime; - Clock::getClock_timeval(&endTime); + Clock::getUptime(&endTime); elapsedTime = endTime - startTime; } diff --git a/timemanager/Stopwatch.h b/timemanager/Stopwatch.h index f216b7e1..ea72c66e 100644 --- a/timemanager/Stopwatch.h +++ b/timemanager/Stopwatch.h @@ -1,5 +1,6 @@ -#ifndef FRAMEWORK_TIMEMANAGER_STOPWATCH_H_ -#define FRAMEWORK_TIMEMANAGER_STOPWATCH_H_ +#ifndef FSFW_TIMEMANAGER_STOPWATCH_H_ +#define FSFW_TIMEMANAGER_STOPWATCH_H_ + #include "Clock.h" enum class StopwatchDisplayMode { @@ -40,12 +41,12 @@ public: * Calculates the elapsed time since start and returns it * @return elapsed time in milliseconds (rounded) */ - dur_millis_t stop(); + dur_millis_t stop(bool display = false); /** * Calculates the elapsed time since start and returns it * @return elapsed time in seconds (double precision) */ - dur_seconds_t stopSeconds(); + double stopSeconds(); /** * Displays the elapsed times on the osstream, depending on internal display @@ -66,6 +67,4 @@ private: }; - - -#endif /* FRAMEWORK_TIMEMANAGER_STOPWATCH_H_ */ +#endif /* FSFW_TIMEMANAGER_STOPWATCH_H_ */ From ad8c6f3528a27dcc1153264e9e1fb7fd8d687c04 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Fri, 18 Sep 2020 12:58:38 +0200 Subject: [PATCH 45/53] commented out code added back --- osal/linux/Clock.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/osal/linux/Clock.cpp b/osal/linux/Clock.cpp index b3b09923..b14f2a97 100644 --- a/osal/linux/Clock.cpp +++ b/osal/linux/Clock.cpp @@ -83,6 +83,16 @@ ReturnValue_t Clock::getUptime(timeval* uptime) { uptime->tv_sec = uptimeSeconds; uptime->tv_usec = uptimeSeconds *(double) 1e6 - (uptime->tv_sec *1e6); } + + //TODO This is not posix compatible and delivers only seconds precision + // I suggest this is moved into another clock function which will + // deliver second precision later. +// struct sysinfo sysInfo; +// int result = sysinfo(&sysInfo); +// if(result != 0){ +// return HasReturnvaluesIF::RETURN_FAILED; +// } +// return sysInfo.uptime; return HasReturnvaluesIF::RETURN_OK; } From 56ff2aef26209d7c5db76ab2794df4517b0ca35d Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Fri, 18 Sep 2020 13:03:48 +0200 Subject: [PATCH 46/53] include replacement --- timemanager/Stopwatch.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/timemanager/Stopwatch.cpp b/timemanager/Stopwatch.cpp index 2cd31969..302e2ac0 100644 --- a/timemanager/Stopwatch.cpp +++ b/timemanager/Stopwatch.cpp @@ -1,4 +1,4 @@ -#include "../timemanager/Stopwatch.h" +#include "Stopwatch.h" #include "../serviceinterface/ServiceInterfaceStream.h" #include From 386f3475741bd07af46a313b78dd77882a697f76 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Fri, 18 Sep 2020 13:15:14 +0200 Subject: [PATCH 47/53] renormalized files --- contrib/sgp4/sgp4unit.cpp | 4180 ++++++++++----------- contrib/sgp4/sgp4unit.h | 234 +- globalfunctions/timevalOperations.cpp | 198 +- osal/FreeRTOS/BinSemaphUsingTask.cpp | 190 +- osal/FreeRTOS/BinSemaphUsingTask.h | 152 +- osal/FreeRTOS/BinarySemaphore.cpp | 216 +- osal/FreeRTOS/BinarySemaphore.h | 214 +- osal/FreeRTOS/CountingSemaphUsingTask.cpp | 228 +- osal/FreeRTOS/CountingSemaphUsingTask.h | 204 +- osal/FreeRTOS/CountingSemaphore.cpp | 86 +- osal/FreeRTOS/CountingSemaphore.h | 68 +- osal/FreeRTOS/SemaphoreFactory.cpp | 118 +- osal/FreeRTOS/TaskManagement.cpp | 48 +- osal/FreeRTOS/TaskManagement.h | 128 +- osal/linux/BinarySemaphore.cpp | 298 +- osal/linux/BinarySemaphore.h | 162 +- osal/linux/CountingSemaphore.cpp | 108 +- osal/linux/CountingSemaphore.h | 74 +- osal/linux/SemaphoreFactory.cpp | 66 +- serialize/SerialBufferAdapter.cpp | 258 +- serialize/SerialBufferAdapter.h | 156 +- tasks/SemaphoreFactory.h | 100 +- tasks/SemaphoreIF.h | 136 +- 23 files changed, 3811 insertions(+), 3811 deletions(-) diff --git a/contrib/sgp4/sgp4unit.cpp b/contrib/sgp4/sgp4unit.cpp index 24d63aa9..b707a7a2 100644 --- a/contrib/sgp4/sgp4unit.cpp +++ b/contrib/sgp4/sgp4unit.cpp @@ -1,2090 +1,2090 @@ -/* ---------------------------------------------------------------- -* -* sgp4unit.cpp -* -* this file contains the sgp4 procedures for analytical propagation -* of a satellite. the code was originally released in the 1980 and 1986 -* spacetrack papers. a detailed discussion of the theory and history -* may be found in the 2006 aiaa paper by vallado, crawford, hujsak, -* and kelso. -* -* companion code for -* fundamentals of astrodynamics and applications -* 2007 -* by david vallado -* -* (w) 719-573-2600, email dvallado@agi.com -* -* current : -* 16 nov 07 david vallado -* misc fixes for better compliance -* changes : -* 20 apr 07 david vallado -* misc fixes for constants -* 11 aug 06 david vallado -* chg lyddane choice back to strn3, constants, misc doc -* 15 dec 05 david vallado -* misc fixes -* 26 jul 05 david vallado -* fixes for paper -* note that each fix is preceded by a -* comment with "sgp4fix" and an explanation of -* what was changed -* 10 aug 04 david vallado -* 2nd printing baseline working -* 14 may 01 david vallado -* 2nd edition baseline -* 80 norad -* original baseline -* ---------------------------------------------------------------- */ - -#include "sgp4unit.h" - -const char help = 'n'; -FILE *dbgfile; - -#define pi 3.14159265358979323846 - - -/* ----------- local functions - only ever used internally by sgp4 ---------- */ -static void dpper - ( - double e3, double ee2, double peo, double pgho, double pho, - double pinco, double plo, double se2, double se3, double sgh2, - double sgh3, double sgh4, double sh2, double sh3, double si2, - double si3, double sl2, double sl3, double sl4, double t, - double xgh2, double xgh3, double xgh4, double xh2, double xh3, - double xi2, double xi3, double xl2, double xl3, double xl4, - double zmol, double zmos, double inclo, - char init, - double& ep, double& inclp, double& nodep, double& argpp, double& mp - ); - -static void dscom - ( - double epoch, double ep, double argpp, double tc, double inclp, - double nodep, double np, - double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, - double& cosomm,double& day, double& e3, double& ee2, double& em, - double& emsq, double& gam, double& peo, double& pgho, double& pho, - double& pinco, double& plo, double& rtemsq, double& se2, double& se3, - double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, - double& si2, double& si3, double& sl2, double& sl3, double& sl4, - double& s1, double& s2, double& s3, double& s4, double& s5, - double& s6, double& s7, double& ss1, double& ss2, double& ss3, - double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, - double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, - double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, - double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, - double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, - double& xl4, double& nm, double& z1, double& z2, double& z3, - double& z11, double& z12, double& z13, double& z21, double& z22, - double& z23, double& z31, double& z32, double& z33, double& zmol, - double& zmos - ); - -static void dsinit - ( - gravconsttype whichconst, - double cosim, double emsq, double argpo, double s1, double s2, - double s3, double s4, double s5, double sinim, double ss1, - double ss2, double ss3, double ss4, double ss5, double sz1, - double sz3, double sz11, double sz13, double sz21, double sz23, - double sz31, double sz33, double t, double tc, double gsto, - double mo, double mdot, double no, double nodeo, double nodedot, - double xpidot, double z1, double z3, double z11, double z13, - double z21, double z23, double z31, double z33, double ecco, - double eccsq, double& em, double& argpm, double& inclm, double& mm, - double& nm, double& nodem, - int& irez, - double& atime, double& d2201, double& d2211, double& d3210, double& d3222, - double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, - double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, - double& dnodt, double& domdt, double& del1, double& del2, double& del3, - double& xfact, double& xlamo, double& xli, double& xni - ); - -static void dspace - ( - int irez, - double d2201, double d2211, double d3210, double d3222, double d4410, - double d4422, double d5220, double d5232, double d5421, double d5433, - double dedt, double del1, double del2, double del3, double didt, - double dmdt, double dnodt, double domdt, double argpo, double argpdot, - double t, double tc, double gsto, double xfact, double xlamo, - double no, - double& atime, double& em, double& argpm, double& inclm, double& xli, - double& mm, double& xni, double& nodem, double& dndt, double& nm - ); - -static void initl - ( - int satn, gravconsttype whichconst, - double ecco, double epoch, double inclo, double& no, - char& method, - double& ainv, double& ao, double& con41, double& con42, double& cosio, - double& cosio2,double& eccsq, double& omeosq, double& posq, - double& rp, double& rteosq,double& sinio , double& gsto - ); - -/* ----------------------------------------------------------------------------- -* -* procedure dpper -* -* this procedure provides deep space long period periodic contributions -* to the mean elements. by design, these periodics are zero at epoch. -* this used to be dscom which included initialization, but it's really a -* recurring function. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* e3 - -* ee2 - -* peo - -* pgho - -* pho - -* pinco - -* plo - -* se2 , se3 , sgh2, sgh3, sgh4, sh2, sh3, si2, si3, sl2, sl3, sl4 - -* t - -* xh2, xh3, xi2, xi3, xl2, xl3, xl4 - -* zmol - -* zmos - -* ep - eccentricity 0.0 - 1.0 -* inclo - inclination - needed for lyddane modification -* nodep - right ascension of ascending node -* argpp - argument of perigee -* mp - mean anomaly -* -* outputs : -* ep - eccentricity 0.0 - 1.0 -* inclp - inclination -* nodep - right ascension of ascending node -* argpp - argument of perigee -* mp - mean anomaly -* -* locals : -* alfdp - -* betdp - -* cosip , sinip , cosop , sinop , -* dalf - -* dbet - -* dls - -* f2, f3 - -* pe - -* pgh - -* ph - -* pinc - -* pl - -* sel , ses , sghl , sghs , shl , shs , sil , sinzf , sis , -* sll , sls -* xls - -* xnoh - -* zf - -* zm - -* -* coupling : -* none. -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -static void dpper - ( - double e3, double ee2, double peo, double pgho, double pho, - double pinco, double plo, double se2, double se3, double sgh2, - double sgh3, double sgh4, double sh2, double sh3, double si2, - double si3, double sl2, double sl3, double sl4, double t, - double xgh2, double xgh3, double xgh4, double xh2, double xh3, - double xi2, double xi3, double xl2, double xl3, double xl4, - double zmol, double zmos, double inclo, - char init, - double& ep, double& inclp, double& nodep, double& argpp, double& mp - ) -{ - /* --------------------- local variables ------------------------ */ - const double twopi = 2.0 * pi; - double alfdp, betdp, cosip, cosop, dalf, dbet, dls, - f2, f3, pe, pgh, ph, pinc, pl , - sel, ses, sghl, sghs, shll, shs, sil, - sinip, sinop, sinzf, sis, sll, sls, xls, - xnoh, zf, zm, zel, zes, znl, zns; - - /* ---------------------- constants ----------------------------- */ - zns = 1.19459e-5; - zes = 0.01675; - znl = 1.5835218e-4; - zel = 0.05490; - - /* --------------- calculate time varying periodics ----------- */ - zm = zmos + zns * t; - // be sure that the initial call has time set to zero - if (init == 'y') - zm = zmos; - zf = zm + 2.0 * zes * sin(zm); - sinzf = sin(zf); - f2 = 0.5 * sinzf * sinzf - 0.25; - f3 = -0.5 * sinzf * cos(zf); - ses = se2* f2 + se3 * f3; - sis = si2 * f2 + si3 * f3; - sls = sl2 * f2 + sl3 * f3 + sl4 * sinzf; - sghs = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf; - shs = sh2 * f2 + sh3 * f3; - zm = zmol + znl * t; - if (init == 'y') - zm = zmol; - zf = zm + 2.0 * zel * sin(zm); - sinzf = sin(zf); - f2 = 0.5 * sinzf * sinzf - 0.25; - f3 = -0.5 * sinzf * cos(zf); - sel = ee2 * f2 + e3 * f3; - sil = xi2 * f2 + xi3 * f3; - sll = xl2 * f2 + xl3 * f3 + xl4 * sinzf; - sghl = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf; - shll = xh2 * f2 + xh3 * f3; - pe = ses + sel; - pinc = sis + sil; - pl = sls + sll; - pgh = sghs + sghl; - ph = shs + shll; - - if (init == 'n') - { - pe = pe - peo; - pinc = pinc - pinco; - pl = pl - plo; - pgh = pgh - pgho; - ph = ph - pho; - inclp = inclp + pinc; - ep = ep + pe; - sinip = sin(inclp); - cosip = cos(inclp); - - /* ----------------- apply periodics directly ------------ */ - // sgp4fix for lyddane choice - // strn3 used original inclination - this is technically feasible - // gsfc used perturbed inclination - also technically feasible - // probably best to readjust the 0.2 limit value and limit discontinuity - // 0.2 rad = 11.45916 deg - // use next line for original strn3 approach and original inclination - // if (inclo >= 0.2) - // use next line for gsfc version and perturbed inclination - if (inclp >= 0.2) - { - ph = ph / sinip; - pgh = pgh - cosip * ph; - argpp = argpp + pgh; - nodep = nodep + ph; - mp = mp + pl; - } - else - { - /* ---- apply periodics with lyddane modification ---- */ - sinop = sin(nodep); - cosop = cos(nodep); - alfdp = sinip * sinop; - betdp = sinip * cosop; - dalf = ph * cosop + pinc * cosip * sinop; - dbet = -ph * sinop + pinc * cosip * cosop; - alfdp = alfdp + dalf; - betdp = betdp + dbet; - nodep = fmod(nodep, twopi); - // sgp4fix for afspc written intrinsic functions - // nodep used without a trigonometric function ahead - if (nodep < 0.0) - nodep = nodep + twopi; - xls = mp + argpp + cosip * nodep; - dls = pl + pgh - pinc * nodep * sinip; - xls = xls + dls; - xnoh = nodep; - nodep = atan2(alfdp, betdp); - // sgp4fix for afspc written intrinsic functions - // nodep used without a trigonometric function ahead - if (nodep < 0.0) - nodep = nodep + twopi; - if (fabs(xnoh - nodep) > pi){ - if (nodep < xnoh) - nodep = nodep + twopi; - else - nodep = nodep - twopi; - } - mp = mp + pl; - argpp = xls - mp - cosip * nodep; - } - } // if init == 'n' - -//#include "debug1.cpp" -} // end dpper - -/*----------------------------------------------------------------------------- -* -* procedure dscom -* -* this procedure provides deep space common items used by both the secular -* and periodics subroutines. input is provided as shown. this routine -* used to be called dpper, but the functions inside weren't well organized. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* epoch - -* ep - eccentricity -* argpp - argument of perigee -* tc - -* inclp - inclination -* nodep - right ascension of ascending node -* np - mean motion -* -* outputs : -* sinim , cosim , sinomm , cosomm , snodm , cnodm -* day - -* e3 - -* ee2 - -* em - eccentricity -* emsq - eccentricity squared -* gam - -* peo - -* pgho - -* pho - -* pinco - -* plo - -* rtemsq - -* se2, se3 - -* sgh2, sgh3, sgh4 - -* sh2, sh3, si2, si3, sl2, sl3, sl4 - -* s1, s2, s3, s4, s5, s6, s7 - -* ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3 - -* sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - -* xgh2, xgh3, xgh4, xh2, xh3, xi2, xi3, xl2, xl3, xl4 - -* nm - mean motion -* z1, z2, z3, z11, z12, z13, z21, z22, z23, z31, z32, z33 - -* zmol - -* zmos - -* -* locals : -* a1, a2, a3, a4, a5, a6, a7, a8, a9, a10 - -* betasq - -* cc - -* ctem, stem - -* x1, x2, x3, x4, x5, x6, x7, x8 - -* xnodce - -* xnoi - -* zcosg , zsing , zcosgl , zsingl , zcosh , zsinh , zcoshl , zsinhl , -* zcosi , zsini , zcosil , zsinil , -* zx - -* zy - -* -* coupling : -* none. -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -static void dscom - ( - double epoch, double ep, double argpp, double tc, double inclp, - double nodep, double np, - double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, - double& cosomm,double& day, double& e3, double& ee2, double& em, - double& emsq, double& gam, double& peo, double& pgho, double& pho, - double& pinco, double& plo, double& rtemsq, double& se2, double& se3, - double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, - double& si2, double& si3, double& sl2, double& sl3, double& sl4, - double& s1, double& s2, double& s3, double& s4, double& s5, - double& s6, double& s7, double& ss1, double& ss2, double& ss3, - double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, - double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, - double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, - double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, - double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, - double& xl4, double& nm, double& z1, double& z2, double& z3, - double& z11, double& z12, double& z13, double& z21, double& z22, - double& z23, double& z31, double& z32, double& z33, double& zmol, - double& zmos - ) -{ - /* -------------------------- constants ------------------------- */ - const double zes = 0.01675; - const double zel = 0.05490; - const double c1ss = 2.9864797e-6; - const double c1l = 4.7968065e-7; - const double zsinis = 0.39785416; - const double zcosis = 0.91744867; - const double zcosgs = 0.1945905; - const double zsings = -0.98088458; - const double twopi = 2.0 * pi; - - /* --------------------- local variables ------------------------ */ - int lsflg; - double a1 , a2 , a3 , a4 , a5 , a6 , a7 , - a8 , a9 , a10 , betasq, cc , ctem , stem , - x1 , x2 , x3 , x4 , x5 , x6 , x7 , - x8 , xnodce, xnoi , zcosg , zcosgl, zcosh , zcoshl, - zcosi , zcosil, zsing , zsingl, zsinh , zsinhl, zsini , - zsinil, zx , zy; - - nm = np; - em = ep; - snodm = sin(nodep); - cnodm = cos(nodep); - sinomm = sin(argpp); - cosomm = cos(argpp); - sinim = sin(inclp); - cosim = cos(inclp); - emsq = em * em; - betasq = 1.0 - emsq; - rtemsq = sqrt(betasq); - - /* ----------------- initialize lunar solar terms --------------- */ - peo = 0.0; - pinco = 0.0; - plo = 0.0; - pgho = 0.0; - pho = 0.0; - day = epoch + 18261.5 + tc / 1440.0; - xnodce = fmod(4.5236020 - 9.2422029e-4 * day, twopi); - stem = sin(xnodce); - ctem = cos(xnodce); - zcosil = 0.91375164 - 0.03568096 * ctem; - zsinil = sqrt(1.0 - zcosil * zcosil); - zsinhl = 0.089683511 * stem / zsinil; - zcoshl = sqrt(1.0 - zsinhl * zsinhl); - gam = 5.8351514 + 0.0019443680 * day; - zx = 0.39785416 * stem / zsinil; - zy = zcoshl * ctem + 0.91744867 * zsinhl * stem; - zx = atan2(zx, zy); - zx = gam + zx - xnodce; - zcosgl = cos(zx); - zsingl = sin(zx); - - /* ------------------------- do solar terms --------------------- */ - zcosg = zcosgs; - zsing = zsings; - zcosi = zcosis; - zsini = zsinis; - zcosh = cnodm; - zsinh = snodm; - cc = c1ss; - xnoi = 1.0 / nm; - - for (lsflg = 1; lsflg <= 2; lsflg++) - { - a1 = zcosg * zcosh + zsing * zcosi * zsinh; - a3 = -zsing * zcosh + zcosg * zcosi * zsinh; - a7 = -zcosg * zsinh + zsing * zcosi * zcosh; - a8 = zsing * zsini; - a9 = zsing * zsinh + zcosg * zcosi * zcosh; - a10 = zcosg * zsini; - a2 = cosim * a7 + sinim * a8; - a4 = cosim * a9 + sinim * a10; - a5 = -sinim * a7 + cosim * a8; - a6 = -sinim * a9 + cosim * a10; - - x1 = a1 * cosomm + a2 * sinomm; - x2 = a3 * cosomm + a4 * sinomm; - x3 = -a1 * sinomm + a2 * cosomm; - x4 = -a3 * sinomm + a4 * cosomm; - x5 = a5 * sinomm; - x6 = a6 * sinomm; - x7 = a5 * cosomm; - x8 = a6 * cosomm; - - z31 = 12.0 * x1 * x1 - 3.0 * x3 * x3; - z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4; - z33 = 12.0 * x2 * x2 - 3.0 * x4 * x4; - z1 = 3.0 * (a1 * a1 + a2 * a2) + z31 * emsq; - z2 = 6.0 * (a1 * a3 + a2 * a4) + z32 * emsq; - z3 = 3.0 * (a3 * a3 + a4 * a4) + z33 * emsq; - z11 = -6.0 * a1 * a5 + emsq * (-24.0 * x1 * x7-6.0 * x3 * x5); - z12 = -6.0 * (a1 * a6 + a3 * a5) + emsq * - (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5)); - z13 = -6.0 * a3 * a6 + emsq * (-24.0 * x2 * x8 - 6.0 * x4 * x6); - z21 = 6.0 * a2 * a5 + emsq * (24.0 * x1 * x5 - 6.0 * x3 * x7); - z22 = 6.0 * (a4 * a5 + a2 * a6) + emsq * - (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8)); - z23 = 6.0 * a4 * a6 + emsq * (24.0 * x2 * x6 - 6.0 * x4 * x8); - z1 = z1 + z1 + betasq * z31; - z2 = z2 + z2 + betasq * z32; - z3 = z3 + z3 + betasq * z33; - s3 = cc * xnoi; - s2 = -0.5 * s3 / rtemsq; - s4 = s3 * rtemsq; - s1 = -15.0 * em * s4; - s5 = x1 * x3 + x2 * x4; - s6 = x2 * x3 + x1 * x4; - s7 = x2 * x4 - x1 * x3; - - /* ----------------------- do lunar terms ------------------- */ - if (lsflg == 1) - { - ss1 = s1; - ss2 = s2; - ss3 = s3; - ss4 = s4; - ss5 = s5; - ss6 = s6; - ss7 = s7; - sz1 = z1; - sz2 = z2; - sz3 = z3; - sz11 = z11; - sz12 = z12; - sz13 = z13; - sz21 = z21; - sz22 = z22; - sz23 = z23; - sz31 = z31; - sz32 = z32; - sz33 = z33; - zcosg = zcosgl; - zsing = zsingl; - zcosi = zcosil; - zsini = zsinil; - zcosh = zcoshl * cnodm + zsinhl * snodm; - zsinh = snodm * zcoshl - cnodm * zsinhl; - cc = c1l; - } - } - - zmol = fmod(4.7199672 + 0.22997150 * day - gam, twopi); - zmos = fmod(6.2565837 + 0.017201977 * day, twopi); - - /* ------------------------ do solar terms ---------------------- */ - se2 = 2.0 * ss1 * ss6; - se3 = 2.0 * ss1 * ss7; - si2 = 2.0 * ss2 * sz12; - si3 = 2.0 * ss2 * (sz13 - sz11); - sl2 = -2.0 * ss3 * sz2; - sl3 = -2.0 * ss3 * (sz3 - sz1); - sl4 = -2.0 * ss3 * (-21.0 - 9.0 * emsq) * zes; - sgh2 = 2.0 * ss4 * sz32; - sgh3 = 2.0 * ss4 * (sz33 - sz31); - sgh4 = -18.0 * ss4 * zes; - sh2 = -2.0 * ss2 * sz22; - sh3 = -2.0 * ss2 * (sz23 - sz21); - - /* ------------------------ do lunar terms ---------------------- */ - ee2 = 2.0 * s1 * s6; - e3 = 2.0 * s1 * s7; - xi2 = 2.0 * s2 * z12; - xi3 = 2.0 * s2 * (z13 - z11); - xl2 = -2.0 * s3 * z2; - xl3 = -2.0 * s3 * (z3 - z1); - xl4 = -2.0 * s3 * (-21.0 - 9.0 * emsq) * zel; - xgh2 = 2.0 * s4 * z32; - xgh3 = 2.0 * s4 * (z33 - z31); - xgh4 = -18.0 * s4 * zel; - xh2 = -2.0 * s2 * z22; - xh3 = -2.0 * s2 * (z23 - z21); - -//#include "debug2.cpp" -} // end dscom - -/*----------------------------------------------------------------------------- -* -* procedure dsinit -* -* this procedure provides deep space contributions to mean motion dot due -* to geopotential resonance with half day and one day orbits. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* cosim, sinim- -* emsq - eccentricity squared -* argpo - argument of perigee -* s1, s2, s3, s4, s5 - -* ss1, ss2, ss3, ss4, ss5 - -* sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33 - -* t - time -* tc - -* gsto - greenwich sidereal time rad -* mo - mean anomaly -* mdot - mean anomaly dot (rate) -* no - mean motion -* nodeo - right ascension of ascending node -* nodedot - right ascension of ascending node dot (rate) -* xpidot - -* z1, z3, z11, z13, z21, z23, z31, z33 - -* eccm - eccentricity -* argpm - argument of perigee -* inclm - inclination -* mm - mean anomaly -* xn - mean motion -* nodem - right ascension of ascending node -* -* outputs : -* em - eccentricity -* argpm - argument of perigee -* inclm - inclination -* mm - mean anomaly -* nm - mean motion -* nodem - right ascension of ascending node -* irez - flag for resonance 0-none, 1-one day, 2-half day -* atime - -* d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - -* dedt - -* didt - -* dmdt - -* dndt - -* dnodt - -* domdt - -* del1, del2, del3 - -* ses , sghl , sghs , sgs , shl , shs , sis , sls -* theta - -* xfact - -* xlamo - -* xli - -* xni -* -* locals : -* ainv2 - -* aonv - -* cosisq - -* eoc - -* f220, f221, f311, f321, f322, f330, f441, f442, f522, f523, f542, f543 - -* g200, g201, g211, g300, g310, g322, g410, g422, g520, g521, g532, g533 - -* sini2 - -* temp - -* temp1 - -* theta - -* xno2 - -* -* coupling : -* getgravconst -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -static void dsinit - ( - gravconsttype whichconst, - double cosim, double emsq, double argpo, double s1, double s2, - double s3, double s4, double s5, double sinim, double ss1, - double ss2, double ss3, double ss4, double ss5, double sz1, - double sz3, double sz11, double sz13, double sz21, double sz23, - double sz31, double sz33, double t, double tc, double gsto, - double mo, double mdot, double no, double nodeo, double nodedot, - double xpidot, double z1, double z3, double z11, double z13, - double z21, double z23, double z31, double z33, double ecco, - double eccsq, double& em, double& argpm, double& inclm, double& mm, - double& nm, double& nodem, - int& irez, - double& atime, double& d2201, double& d2211, double& d3210, double& d3222, - double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, - double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, - double& dnodt, double& domdt, double& del1, double& del2, double& del3, - double& xfact, double& xlamo, double& xli, double& xni - ) -{ - /* --------------------- local variables ------------------------ */ - const double twopi = 2.0 * pi; - - double ainv2 , aonv=0.0, cosisq, eoc, f220 , f221 , f311 , - f321 , f322 , f330 , f441 , f442 , f522 , f523 , - f542 , f543 , g200 , g201 , g211 , g300 , g310 , - g322 , g410 , g422 , g520 , g521 , g532 , g533 , - ses , sgs , sghl , sghs , shs , shll , sis , - sini2 , sls , temp , temp1 , theta , xno2 , q22 , - q31 , q33 , root22, root44, root54, rptim , root32, - root52, x2o3 , xke , znl , emo , zns , emsqo, - tumin, mu, radiusearthkm, j2, j3, j4, j3oj2; - - q22 = 1.7891679e-6; - q31 = 2.1460748e-6; - q33 = 2.2123015e-7; - root22 = 1.7891679e-6; - root44 = 7.3636953e-9; - root54 = 2.1765803e-9; - rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec - root32 = 3.7393792e-7; - root52 = 1.1428639e-7; - x2o3 = 2.0 / 3.0; - znl = 1.5835218e-4; - zns = 1.19459e-5; - - // sgp4fix identify constants and allow alternate values - getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); - - /* -------------------- deep space initialization ------------ */ - irez = 0; - if ((nm < 0.0052359877) && (nm > 0.0034906585)) - irez = 1; - if ((nm >= 8.26e-3) && (nm <= 9.24e-3) && (em >= 0.5)) - irez = 2; - - /* ------------------------ do solar terms ------------------- */ - ses = ss1 * zns * ss5; - sis = ss2 * zns * (sz11 + sz13); - sls = -zns * ss3 * (sz1 + sz3 - 14.0 - 6.0 * emsq); - sghs = ss4 * zns * (sz31 + sz33 - 6.0); - shs = -zns * ss2 * (sz21 + sz23); - // sgp4fix for 180 deg incl - if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2)) - shs = 0.0; - if (sinim != 0.0) - shs = shs / sinim; - sgs = sghs - cosim * shs; - - /* ------------------------- do lunar terms ------------------ */ - dedt = ses + s1 * znl * s5; - didt = sis + s2 * znl * (z11 + z13); - dmdt = sls - znl * s3 * (z1 + z3 - 14.0 - 6.0 * emsq); - sghl = s4 * znl * (z31 + z33 - 6.0); - shll = -znl * s2 * (z21 + z23); - // sgp4fix for 180 deg incl - if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2)) - shll = 0.0; - domdt = sgs + sghl; - dnodt = shs; - if (sinim != 0.0) - { - domdt = domdt - cosim / sinim * shll; - dnodt = dnodt + shll / sinim; - } - - /* ----------- calculate deep space resonance effects -------- */ - dndt = 0.0; - theta = fmod(gsto + tc * rptim, twopi); - em = em + dedt * t; - inclm = inclm + didt * t; - argpm = argpm + domdt * t; - nodem = nodem + dnodt * t; - mm = mm + dmdt * t; - // sgp4fix for negative inclinations - // the following if statement should be commented out - //if (inclm < 0.0) - // { - // inclm = -inclm; - // argpm = argpm - pi; - // nodem = nodem + pi; - // } - - /* -------------- initialize the resonance terms ------------- */ - if (irez != 0) - { - aonv = pow(nm / xke, x2o3); - - /* ---------- geopotential resonance for 12 hour orbits ------ */ - if (irez == 2) - { - cosisq = cosim * cosim; - emo = em; - em = ecco; - emsqo = emsq; - emsq = eccsq; - eoc = em * emsq; - g201 = -0.306 - (em - 0.64) * 0.440; - - if (em <= 0.65) - { - g211 = 3.616 - 13.2470 * em + 16.2900 * emsq; - g310 = -19.302 + 117.3900 * em - 228.4190 * emsq + 156.5910 * eoc; - g322 = -18.9068 + 109.7927 * em - 214.6334 * emsq + 146.5816 * eoc; - g410 = -41.122 + 242.6940 * em - 471.0940 * emsq + 313.9530 * eoc; - g422 = -146.407 + 841.8800 * em - 1629.014 * emsq + 1083.4350 * eoc; - g520 = -532.114 + 3017.977 * em - 5740.032 * emsq + 3708.2760 * eoc; - } - else - { - g211 = -72.099 + 331.819 * em - 508.738 * emsq + 266.724 * eoc; - g310 = -346.844 + 1582.851 * em - 2415.925 * emsq + 1246.113 * eoc; - g322 = -342.585 + 1554.908 * em - 2366.899 * emsq + 1215.972 * eoc; - g410 = -1052.797 + 4758.686 * em - 7193.992 * emsq + 3651.957 * eoc; - g422 = -3581.690 + 16178.110 * em - 24462.770 * emsq + 12422.520 * eoc; - if (em > 0.715) - g520 =-5149.66 + 29936.92 * em - 54087.36 * emsq + 31324.56 * eoc; - else - g520 = 1464.74 - 4664.75 * em + 3763.64 * emsq; - } - if (em < 0.7) - { - g533 = -919.22770 + 4988.6100 * em - 9064.7700 * emsq + 5542.21 * eoc; - g521 = -822.71072 + 4568.6173 * em - 8491.4146 * emsq + 5337.524 * eoc; - g532 = -853.66600 + 4690.2500 * em - 8624.7700 * emsq + 5341.4 * eoc; - } - else - { - g533 =-37995.780 + 161616.52 * em - 229838.20 * emsq + 109377.94 * eoc; - g521 =-51752.104 + 218913.95 * em - 309468.16 * emsq + 146349.42 * eoc; - g532 =-40023.880 + 170470.89 * em - 242699.48 * emsq + 115605.82 * eoc; - } - - sini2= sinim * sinim; - f220 = 0.75 * (1.0 + 2.0 * cosim+cosisq); - f221 = 1.5 * sini2; - f321 = 1.875 * sinim * (1.0 - 2.0 * cosim - 3.0 * cosisq); - f322 = -1.875 * sinim * (1.0 + 2.0 * cosim - 3.0 * cosisq); - f441 = 35.0 * sini2 * f220; - f442 = 39.3750 * sini2 * sini2; - f522 = 9.84375 * sinim * (sini2 * (1.0 - 2.0 * cosim- 5.0 * cosisq) + - 0.33333333 * (-2.0 + 4.0 * cosim + 6.0 * cosisq) ); - f523 = sinim * (4.92187512 * sini2 * (-2.0 - 4.0 * cosim + - 10.0 * cosisq) + 6.56250012 * (1.0+2.0 * cosim - 3.0 * cosisq)); - f542 = 29.53125 * sinim * (2.0 - 8.0 * cosim+cosisq * - (-12.0 + 8.0 * cosim + 10.0 * cosisq)); - f543 = 29.53125 * sinim * (-2.0 - 8.0 * cosim+cosisq * - (12.0 + 8.0 * cosim - 10.0 * cosisq)); - xno2 = nm * nm; - ainv2 = aonv * aonv; - temp1 = 3.0 * xno2 * ainv2; - temp = temp1 * root22; - d2201 = temp * f220 * g201; - d2211 = temp * f221 * g211; - temp1 = temp1 * aonv; - temp = temp1 * root32; - d3210 = temp * f321 * g310; - d3222 = temp * f322 * g322; - temp1 = temp1 * aonv; - temp = 2.0 * temp1 * root44; - d4410 = temp * f441 * g410; - d4422 = temp * f442 * g422; - temp1 = temp1 * aonv; - temp = temp1 * root52; - d5220 = temp * f522 * g520; - d5232 = temp * f523 * g532; - temp = 2.0 * temp1 * root54; - d5421 = temp * f542 * g521; - d5433 = temp * f543 * g533; - xlamo = fmod(mo + nodeo + nodeo-theta - theta, twopi); - xfact = mdot + dmdt + 2.0 * (nodedot + dnodt - rptim) - no; - em = emo; - emsq = emsqo; - } - - /* ---------------- synchronous resonance terms -------------- */ - if (irez == 1) - { - g200 = 1.0 + emsq * (-2.5 + 0.8125 * emsq); - g310 = 1.0 + 2.0 * emsq; - g300 = 1.0 + emsq * (-6.0 + 6.60937 * emsq); - f220 = 0.75 * (1.0 + cosim) * (1.0 + cosim); - f311 = 0.9375 * sinim * sinim * (1.0 + 3.0 * cosim) - 0.75 * (1.0 + cosim); - f330 = 1.0 + cosim; - f330 = 1.875 * f330 * f330 * f330; - del1 = 3.0 * nm * nm * aonv * aonv; - del2 = 2.0 * del1 * f220 * g200 * q22; - del3 = 3.0 * del1 * f330 * g300 * q33 * aonv; - del1 = del1 * f311 * g310 * q31 * aonv; - xlamo = fmod(mo + nodeo + argpo - theta, twopi); - xfact = mdot + xpidot - rptim + dmdt + domdt + dnodt - no; - } - - /* ------------ for sgp4, initialize the integrator ---------- */ - xli = xlamo; - xni = no; - atime = 0.0; - nm = no + dndt; - } - -//#include "debug3.cpp" -} // end dsinit - -/*----------------------------------------------------------------------------- -* -* procedure dspace -* -* this procedure provides deep space contributions to mean elements for -* perturbing third body. these effects have been averaged over one -* revolution of the sun and moon. for earth resonance effects, the -* effects have been averaged over no revolutions of the satellite. -* (mean motion) -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - -* dedt - -* del1, del2, del3 - -* didt - -* dmdt - -* dnodt - -* domdt - -* irez - flag for resonance 0-none, 1-one day, 2-half day -* argpo - argument of perigee -* argpdot - argument of perigee dot (rate) -* t - time -* tc - -* gsto - gst -* xfact - -* xlamo - -* no - mean motion -* atime - -* em - eccentricity -* ft - -* argpm - argument of perigee -* inclm - inclination -* xli - -* mm - mean anomaly -* xni - mean motion -* nodem - right ascension of ascending node -* -* outputs : -* atime - -* em - eccentricity -* argpm - argument of perigee -* inclm - inclination -* xli - -* mm - mean anomaly -* xni - -* nodem - right ascension of ascending node -* dndt - -* nm - mean motion -* -* locals : -* delt - -* ft - -* theta - -* x2li - -* x2omi - -* xl - -* xldot - -* xnddt - -* xndt - -* xomi - -* -* coupling : -* none - -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -static void dspace - ( - int irez, - double d2201, double d2211, double d3210, double d3222, double d4410, - double d4422, double d5220, double d5232, double d5421, double d5433, - double dedt, double del1, double del2, double del3, double didt, - double dmdt, double dnodt, double domdt, double argpo, double argpdot, - double t, double tc, double gsto, double xfact, double xlamo, - double no, - double& atime, double& em, double& argpm, double& inclm, double& xli, - double& mm, double& xni, double& nodem, double& dndt, double& nm - ) -{ - const double twopi = 2.0 * pi; - int iretn , iret; - double delt, ft, theta, x2li, x2omi, xl, xldot , xnddt, xndt, xomi, g22, g32, - g44, g52, g54, fasx2, fasx4, fasx6, rptim , step2, stepn , stepp; - - ft = 0.0; - fasx2 = 0.13130908; - fasx4 = 2.8843198; - fasx6 = 0.37448087; - g22 = 5.7686396; - g32 = 0.95240898; - g44 = 1.8014998; - g52 = 1.0508330; - g54 = 4.4108898; - rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec - stepp = 720.0; - stepn = -720.0; - step2 = 259200.0; - - /* ----------- calculate deep space resonance effects ----------- */ - dndt = 0.0; - theta = fmod(gsto + tc * rptim, twopi); - em = em + dedt * t; - - inclm = inclm + didt * t; - argpm = argpm + domdt * t; - nodem = nodem + dnodt * t; - mm = mm + dmdt * t; - - // sgp4fix for negative inclinations - // the following if statement should be commented out - // if (inclm < 0.0) - // { - // inclm = -inclm; - // argpm = argpm - pi; - // nodem = nodem + pi; - // } - - /* - update resonances : numerical (euler-maclaurin) integration - */ - /* ------------------------- epoch restart ---------------------- */ - // sgp4fix for propagator problems - // the following integration works for negative time steps and periods - // the specific changes are unknown because the original code was so convoluted - - ft = 0.0; - atime = 0.0; - if (irez != 0) - { - if ((atime == 0.0) || ((t >= 0.0) && (atime < 0.0)) || - ((t < 0.0) && (atime >= 0.0))) - { - if (t >= 0.0) - delt = stepp; - else - delt = stepn; - atime = 0.0; - xni = no; - xli = xlamo; - } - iretn = 381; // added for do loop - iret = 0; // added for loop - while (iretn == 381) - { - if ((fabs(t) < fabs(atime)) || (iret == 351)) - { - if (t >= 0.0) - delt = stepn; - else - delt = stepp; - iret = 351; - iretn = 381; - } - else - { - if (t > 0.0) // error if prev if has atime:=0.0 and t:=0.0 (ge) - delt = stepp; - else - delt = stepn; - if (fabs(t - atime) >= stepp) - { - iret = 0; - iretn = 381; - } - else - { - ft = t - atime; - iretn = 0; - } - } - - /* ------------------- dot terms calculated ------------- */ - /* ----------- near - synchronous resonance terms ------- */ - if (irez != 2) - { - xndt = del1 * sin(xli - fasx2) + del2 * sin(2.0 * (xli - fasx4)) + - del3 * sin(3.0 * (xli - fasx6)); - xldot = xni + xfact; - xnddt = del1 * cos(xli - fasx2) + - 2.0 * del2 * cos(2.0 * (xli - fasx4)) + - 3.0 * del3 * cos(3.0 * (xli - fasx6)); - xnddt = xnddt * xldot; - } - else - { - /* --------- near - half-day resonance terms -------- */ - xomi = argpo + argpdot * atime; - x2omi = xomi + xomi; - x2li = xli + xli; - xndt = d2201 * sin(x2omi + xli - g22) + d2211 * sin(xli - g22) + - d3210 * sin(xomi + xli - g32) + d3222 * sin(-xomi + xli - g32)+ - d4410 * sin(x2omi + x2li - g44)+ d4422 * sin(x2li - g44) + - d5220 * sin(xomi + xli - g52) + d5232 * sin(-xomi + xli - g52)+ - d5421 * sin(xomi + x2li - g54) + d5433 * sin(-xomi + x2li - g54); - xldot = xni + xfact; - xnddt = d2201 * cos(x2omi + xli - g22) + d2211 * cos(xli - g22) + - d3210 * cos(xomi + xli - g32) + d3222 * cos(-xomi + xli - g32) + - d5220 * cos(xomi + xli - g52) + d5232 * cos(-xomi + xli - g52) + - 2.0 * (d4410 * cos(x2omi + x2li - g44) + - d4422 * cos(x2li - g44) + d5421 * cos(xomi + x2li - g54) + - d5433 * cos(-xomi + x2li - g54)); - xnddt = xnddt * xldot; - } - - /* ----------------------- integrator ------------------- */ - if (iretn == 381) - { - xli = xli + xldot * delt + xndt * step2; - xni = xni + xndt * delt + xnddt * step2; - atime = atime + delt; - } - } // while iretn = 381 - - nm = xni + xndt * ft + xnddt * ft * ft * 0.5; - xl = xli + xldot * ft + xndt * ft * ft * 0.5; - if (irez != 1) - { - mm = xl - 2.0 * nodem + 2.0 * theta; - dndt = nm - no; - } - else - { - mm = xl - nodem - argpm + theta; - dndt = nm - no; - } - nm = no + dndt; - } - -//#include "debug4.cpp" -} // end dsspace - -/*----------------------------------------------------------------------------- -* -* procedure initl -* -* this procedure initializes the spg4 propagator. all the initialization is -* consolidated here instead of having multiple loops inside other routines. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* ecco - eccentricity 0.0 - 1.0 -* epoch - epoch time in days from jan 0, 1950. 0 hr -* inclo - inclination of satellite -* no - mean motion of satellite -* satn - satellite number -* -* outputs : -* ainv - 1.0 / a -* ao - semi major axis -* con41 - -* con42 - 1.0 - 5.0 cos(i) -* cosio - cosine of inclination -* cosio2 - cosio squared -* eccsq - eccentricity squared -* method - flag for deep space 'd', 'n' -* omeosq - 1.0 - ecco * ecco -* posq - semi-parameter squared -* rp - radius of perigee -* rteosq - square root of (1.0 - ecco*ecco) -* sinio - sine of inclination -* gsto - gst at time of observation rad -* no - mean motion of satellite -* -* locals : -* ak - -* d1 - -* del - -* adel - -* po - -* -* coupling : -* getgravconst -* gstime - find greenwich sidereal time from the julian date -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -static void initl - ( - int satn, gravconsttype whichconst, - double ecco, double epoch, double inclo, double& no, - char& method, - double& ainv, double& ao, double& con41, double& con42, double& cosio, - double& cosio2,double& eccsq, double& omeosq, double& posq, - double& rp, double& rteosq,double& sinio , double& gsto - ) -{ - /* --------------------- local variables ------------------------ */ - double ak, d1, del, adel, po, x2o3, j2, xke, - tumin, mu, radiusearthkm, j3, j4, j3oj2; - - // sgp4fix use old way of finding gst - int ids70; - double ts70, ds70, tfrac, c1, thgr70, fk5r, c1p2p; - const double twopi = 2.0 * pi; - - /* ----------------------- earth constants ---------------------- */ - // sgp4fix identify constants and allow alternate values - getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); - x2o3 = 2.0 / 3.0; - - /* ------------- calculate auxillary epoch quantities ---------- */ - eccsq = ecco * ecco; - omeosq = 1.0 - eccsq; - rteosq = sqrt(omeosq); - cosio = cos(inclo); - cosio2 = cosio * cosio; - - /* ------------------ un-kozai the mean motion ----------------- */ - ak = pow(xke / no, x2o3); - d1 = 0.75 * j2 * (3.0 * cosio2 - 1.0) / (rteosq * omeosq); - del = d1 / (ak * ak); - adel = ak * (1.0 - del * del - del * - (1.0 / 3.0 + 134.0 * del * del / 81.0)); - del = d1/(adel * adel); - no = no / (1.0 + del); - - ao = pow(xke / no, x2o3); - sinio = sin(inclo); - po = ao * omeosq; - con42 = 1.0 - 5.0 * cosio2; - con41 = -con42-cosio2-cosio2; - ainv = 1.0 / ao; - posq = po * po; - rp = ao * (1.0 - ecco); - method = 'n'; - - // sgp4fix modern approach to finding sidereal timew - // gsto = gstime(epoch + 2433281.5); - - // sgp4fix use old way of finding gst - // count integer number of days from 0 jan 1970 - ts70 = epoch - 7305.0; - ids70 = floor(ts70 + 1.0e-8); - ds70 = ids70; - tfrac = ts70 - ds70; - // find greenwich location at epoch - c1 = 1.72027916940703639e-2; - thgr70= 1.7321343856509374; - fk5r = 5.07551419432269442e-15; - c1p2p = c1 + twopi; - gsto = fmod( thgr70 + c1*ds70 + c1p2p*tfrac + ts70*ts70*fk5r, twopi); - if ( gsto < 0.0 ) - gsto = gsto + twopi; - -//#include "debug5.cpp" -} // end initl - -/*----------------------------------------------------------------------------- -* -* procedure sgp4init -* -* this procedure initializes variables for sgp4. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* satn - satellite number -* bstar - sgp4 type drag coefficient kg/m2er -* ecco - eccentricity -* epoch - epoch time in days from jan 0, 1950. 0 hr -* argpo - argument of perigee (output if ds) -* inclo - inclination -* mo - mean anomaly (output if ds) -* no - mean motion -* nodeo - right ascension of ascending node -* -* outputs : -* satrec - common values for subsequent calls -* return code - non-zero on error. -* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er -* 2 - mean motion less than 0.0 -* 3 - pert elements, ecc < 0.0 or ecc > 1.0 -* 4 - semi-latus rectum < 0.0 -* 5 - epoch elements are sub-orbital -* 6 - satellite has decayed -* -* locals : -* cnodm , snodm , cosim , sinim , cosomm , sinomm -* cc1sq , cc2 , cc3 -* coef , coef1 -* cosio4 - -* day - -* dndt - -* em - eccentricity -* emsq - eccentricity squared -* eeta - -* etasq - -* gam - -* argpm - argument of perigee -* nodem - -* inclm - inclination -* mm - mean anomaly -* nm - mean motion -* perige - perigee -* pinvsq - -* psisq - -* qzms24 - -* rtemsq - -* s1, s2, s3, s4, s5, s6, s7 - -* sfour - -* ss1, ss2, ss3, ss4, ss5, ss6, ss7 - -* sz1, sz2, sz3 -* sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - -* tc - -* temp - -* temp1, temp2, temp3 - -* tsi - -* xpidot - -* xhdot1 - -* z1, z2, z3 - -* z11, z12, z13, z21, z22, z23, z31, z32, z33 - -* -* coupling : -* getgravconst- -* initl - -* dscom - -* dpper - -* dsinit - -* sgp4 - -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -int sgp4init - ( - gravconsttype whichconst, const int satn, const double epoch, - const double xbstar, const double xecco, const double xargpo, - const double xinclo, const double xmo, const double xno, - const double xnodeo, elsetrec& satrec - ) -{ - /* --------------------- local variables ------------------------ */ - double ao, ainv, con42, cosio, sinio, cosio2, eccsq, - omeosq, posq, rp, rteosq, - cnodm , snodm , cosim , sinim , cosomm, sinomm, cc1sq , - cc2 , cc3 , coef , coef1 , cosio4, day , dndt , - em , emsq , eeta , etasq , gam , argpm , nodem , - inclm , mm , nm , perige, pinvsq, psisq , qzms24, - rtemsq, s1 , s2 , s3 , s4 , s5 , s6 , - s7 , sfour , ss1 = 0 , ss2 = 0 , ss3 = 0 , ss4 = 0 , ss5 = 0 , - ss6 = 0 , ss7 = 0 , sz1 = 0 , sz2 = 0 , sz3 = 0 , sz11 = 0 , sz12 = 0 , - sz13 = 0 , sz21 = 0 , sz22 = 0 , sz23 = 0 , sz31 = 0 , sz32 = 0 , sz33 = 0 , - tc , temp , temp1 , temp2 , temp3 , tsi , xpidot, - xhdot1, z1 , z2 , z3 , z11 , z12 , z13 , - z21 , z22 , z23 , z31 , z32 , z33, - qzms2t, ss, j2, j3oj2, j4, x2o3, r[3], v[3], - tumin, mu, radiusearthkm, xke, j3; - - /* ------------------------ initialization --------------------- */ - // sgp4fix divisor for divide by zero check on inclination - const double temp4 = 1.0 + cos(pi-1.0e-9); - - /* ----------- set all near earth variables to zero ------------ */ - satrec.isimp = 0; satrec.method = 'n'; satrec.aycof = 0.0; - satrec.con41 = 0.0; satrec.cc1 = 0.0; satrec.cc4 = 0.0; - satrec.cc5 = 0.0; satrec.d2 = 0.0; satrec.d3 = 0.0; - satrec.d4 = 0.0; satrec.delmo = 0.0; satrec.eta = 0.0; - satrec.argpdot = 0.0; satrec.omgcof = 0.0; satrec.sinmao = 0.0; - satrec.t = 0.0; satrec.t2cof = 0.0; satrec.t3cof = 0.0; - satrec.t4cof = 0.0; satrec.t5cof = 0.0; satrec.x1mth2 = 0.0; - satrec.x7thm1 = 0.0; satrec.mdot = 0.0; satrec.nodedot = 0.0; - satrec.xlcof = 0.0; satrec.xmcof = 0.0; satrec.nodecf = 0.0; - - /* ----------- set all deep space variables to zero ------------ */ - satrec.irez = 0; satrec.d2201 = 0.0; satrec.d2211 = 0.0; - satrec.d3210 = 0.0; satrec.d3222 = 0.0; satrec.d4410 = 0.0; - satrec.d4422 = 0.0; satrec.d5220 = 0.0; satrec.d5232 = 0.0; - satrec.d5421 = 0.0; satrec.d5433 = 0.0; satrec.dedt = 0.0; - satrec.del1 = 0.0; satrec.del2 = 0.0; satrec.del3 = 0.0; - satrec.didt = 0.0; satrec.dmdt = 0.0; satrec.dnodt = 0.0; - satrec.domdt = 0.0; satrec.e3 = 0.0; satrec.ee2 = 0.0; - satrec.peo = 0.0; satrec.pgho = 0.0; satrec.pho = 0.0; - satrec.pinco = 0.0; satrec.plo = 0.0; satrec.se2 = 0.0; - satrec.se3 = 0.0; satrec.sgh2 = 0.0; satrec.sgh3 = 0.0; - satrec.sgh4 = 0.0; satrec.sh2 = 0.0; satrec.sh3 = 0.0; - satrec.si2 = 0.0; satrec.si3 = 0.0; satrec.sl2 = 0.0; - satrec.sl3 = 0.0; satrec.sl4 = 0.0; satrec.gsto = 0.0; - satrec.xfact = 0.0; satrec.xgh2 = 0.0; satrec.xgh3 = 0.0; - satrec.xgh4 = 0.0; satrec.xh2 = 0.0; satrec.xh3 = 0.0; - satrec.xi2 = 0.0; satrec.xi3 = 0.0; satrec.xl2 = 0.0; - satrec.xl3 = 0.0; satrec.xl4 = 0.0; satrec.xlamo = 0.0; - satrec.zmol = 0.0; satrec.zmos = 0.0; satrec.atime = 0.0; - satrec.xli = 0.0; satrec.xni = 0.0; - - // sgp4fix - note the following variables are also passed directly via satrec. - // it is possible to streamline the sgp4init call by deleting the "x" - // variables, but the user would need to set the satrec.* values first. we - // include the additional assignments in case twoline2rv is not used. - satrec.bstar = xbstar; - satrec.ecco = xecco; - satrec.argpo = xargpo; - satrec.inclo = xinclo; - satrec.mo = xmo; - satrec.no = xno; - satrec.nodeo = xnodeo; - - /* ------------------------ earth constants ----------------------- */ - // sgp4fix identify constants and allow alternate values - getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); - ss = 78.0 / radiusearthkm + 1.0; - qzms2t = pow(((120.0 - 78.0) / radiusearthkm), 4); - x2o3 = 2.0 / 3.0; - - satrec.init = 'y'; - satrec.t = 0.0; - - initl - ( - satn, whichconst, satrec.ecco, epoch, satrec.inclo, satrec.no, satrec.method, - ainv, ao, satrec.con41, con42, cosio, cosio2, eccsq, omeosq, - posq, rp, rteosq, sinio, satrec.gsto - ); - satrec.error = 0; - - if (rp < 1.0) - { -// printf("# *** satn%d epoch elts sub-orbital ***\n", satn); - satrec.error = 5; - } - - if ((omeosq >= 0.0 ) || ( satrec.no >= 0.0)) - { - satrec.isimp = 0; - if (rp < (220.0 / radiusearthkm + 1.0)) - satrec.isimp = 1; - sfour = ss; - qzms24 = qzms2t; - perige = (rp - 1.0) * radiusearthkm; - - /* - for perigees below 156 km, s and qoms2t are altered - */ - if (perige < 156.0) - { - sfour = perige - 78.0; - if (perige < 98.0) - sfour = 20.0; - qzms24 = pow(((120.0 - sfour) / radiusearthkm), 4.0); - sfour = sfour / radiusearthkm + 1.0; - } - pinvsq = 1.0 / posq; - - tsi = 1.0 / (ao - sfour); - satrec.eta = ao * satrec.ecco * tsi; - etasq = satrec.eta * satrec.eta; - eeta = satrec.ecco * satrec.eta; - psisq = fabs(1.0 - etasq); - coef = qzms24 * pow(tsi, 4.0); - coef1 = coef / pow(psisq, 3.5); - cc2 = coef1 * satrec.no * (ao * (1.0 + 1.5 * etasq + eeta * - (4.0 + etasq)) + 0.375 * j2 * tsi / psisq * satrec.con41 * - (8.0 + 3.0 * etasq * (8.0 + etasq))); - satrec.cc1 = satrec.bstar * cc2; - cc3 = 0.0; - if (satrec.ecco > 1.0e-4) - cc3 = -2.0 * coef * tsi * j3oj2 * satrec.no * sinio / satrec.ecco; - satrec.x1mth2 = 1.0 - cosio2; - satrec.cc4 = 2.0* satrec.no * coef1 * ao * omeosq * - (satrec.eta * (2.0 + 0.5 * etasq) + satrec.ecco * - (0.5 + 2.0 * etasq) - j2 * tsi / (ao * psisq) * - (-3.0 * satrec.con41 * (1.0 - 2.0 * eeta + etasq * - (1.5 - 0.5 * eeta)) + 0.75 * satrec.x1mth2 * - (2.0 * etasq - eeta * (1.0 + etasq)) * cos(2.0 * satrec.argpo))); - satrec.cc5 = 2.0 * coef1 * ao * omeosq * (1.0 + 2.75 * - (etasq + eeta) + eeta * etasq); - cosio4 = cosio2 * cosio2; - temp1 = 1.5 * j2 * pinvsq * satrec.no; - temp2 = 0.5 * temp1 * j2 * pinvsq; - temp3 = -0.46875 * j4 * pinvsq * pinvsq * satrec.no; - satrec.mdot = satrec.no + 0.5 * temp1 * rteosq * satrec.con41 + 0.0625 * - temp2 * rteosq * (13.0 - 78.0 * cosio2 + 137.0 * cosio4); - satrec.argpdot = -0.5 * temp1 * con42 + 0.0625 * temp2 * - (7.0 - 114.0 * cosio2 + 395.0 * cosio4) + - temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio4); - xhdot1 = -temp1 * cosio; - satrec.nodedot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * cosio2) + - 2.0 * temp3 * (3.0 - 7.0 * cosio2)) * cosio; - xpidot = satrec.argpdot+ satrec.nodedot; - satrec.omgcof = satrec.bstar * cc3 * cos(satrec.argpo); - satrec.xmcof = 0.0; - if (satrec.ecco > 1.0e-4) - satrec.xmcof = -x2o3 * coef * satrec.bstar / eeta; - satrec.nodecf = 3.5 * omeosq * xhdot1 * satrec.cc1; - satrec.t2cof = 1.5 * satrec.cc1; - // sgp4fix for divide by zero with xinco = 180 deg - if (fabs(cosio+1.0) > 1.5e-12) - satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio); - else - satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / temp4; - satrec.aycof = -0.5 * j3oj2 * sinio; - satrec.delmo = pow((1.0 + satrec.eta * cos(satrec.mo)), 3); - satrec.sinmao = sin(satrec.mo); - satrec.x7thm1 = 7.0 * cosio2 - 1.0; - - /* --------------- deep space initialization ------------- */ - if ((2*pi / satrec.no) >= 225.0) - { - satrec.method = 'd'; - satrec.isimp = 1; - tc = 0.0; - inclm = satrec.inclo; - - dscom - ( - epoch, satrec.ecco, satrec.argpo, tc, satrec.inclo, satrec.nodeo, - satrec.no, snodm, cnodm, sinim, cosim,sinomm, cosomm, - day, satrec.e3, satrec.ee2, em, emsq, gam, - satrec.peo, satrec.pgho, satrec.pho, satrec.pinco, - satrec.plo, rtemsq, satrec.se2, satrec.se3, - satrec.sgh2, satrec.sgh3, satrec.sgh4, - satrec.sh2, satrec.sh3, satrec.si2, satrec.si3, - satrec.sl2, satrec.sl3, satrec.sl4, s1, s2, s3, s4, s5, - s6, s7, ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3, - sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33, - satrec.xgh2, satrec.xgh3, satrec.xgh4, satrec.xh2, - satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2, - satrec.xl3, satrec.xl4, nm, z1, z2, z3, z11, - z12, z13, z21, z22, z23, z31, z32, z33, - satrec.zmol, satrec.zmos - ); - dpper - ( - satrec.e3, satrec.ee2, satrec.peo, satrec.pgho, - satrec.pho, satrec.pinco, satrec.plo, satrec.se2, - satrec.se3, satrec.sgh2, satrec.sgh3, satrec.sgh4, - satrec.sh2, satrec.sh3, satrec.si2, satrec.si3, - satrec.sl2, satrec.sl3, satrec.sl4, satrec.t, - satrec.xgh2,satrec.xgh3,satrec.xgh4, satrec.xh2, - satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2, - satrec.xl3, satrec.xl4, satrec.zmol, satrec.zmos, inclm, satrec.init, - satrec.ecco, satrec.inclo, satrec.nodeo, satrec.argpo, satrec.mo - ); - - argpm = 0.0; - nodem = 0.0; - mm = 0.0; - - dsinit - ( - whichconst, - cosim, emsq, satrec.argpo, s1, s2, s3, s4, s5, sinim, ss1, ss2, ss3, ss4, - ss5, sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33, satrec.t, tc, - satrec.gsto, satrec.mo, satrec.mdot, satrec.no, satrec.nodeo, - satrec.nodedot, xpidot, z1, z3, z11, z13, z21, z23, z31, z33, - satrec.ecco, eccsq, em, argpm, inclm, mm, nm, nodem, - satrec.irez, satrec.atime, - satrec.d2201, satrec.d2211, satrec.d3210, satrec.d3222 , - satrec.d4410, satrec.d4422, satrec.d5220, satrec.d5232, - satrec.d5421, satrec.d5433, satrec.dedt, satrec.didt, - satrec.dmdt, dndt, satrec.dnodt, satrec.domdt , - satrec.del1, satrec.del2, satrec.del3, satrec.xfact, - satrec.xlamo, satrec.xli, satrec.xni - ); - } - - /* ----------- set variables if not deep space ----------- */ - if (satrec.isimp != 1) - { - cc1sq = satrec.cc1 * satrec.cc1; - satrec.d2 = 4.0 * ao * tsi * cc1sq; - temp = satrec.d2 * tsi * satrec.cc1 / 3.0; - satrec.d3 = (17.0 * ao + sfour) * temp; - satrec.d4 = 0.5 * temp * ao * tsi * (221.0 * ao + 31.0 * sfour) * - satrec.cc1; - satrec.t3cof = satrec.d2 + 2.0 * cc1sq; - satrec.t4cof = 0.25 * (3.0 * satrec.d3 + satrec.cc1 * - (12.0 * satrec.d2 + 10.0 * cc1sq)); - satrec.t5cof = 0.2 * (3.0 * satrec.d4 + - 12.0 * satrec.cc1 * satrec.d3 + - 6.0 * satrec.d2 * satrec.d2 + - 15.0 * cc1sq * (2.0 * satrec.d2 + cc1sq)); - } - } // if omeosq = 0 ... - - /* finally propogate to zero epoch to initialise all others. */ - if(satrec.error == 0) - sgp4(whichconst, satrec, 0.0, r, v); - - satrec.init = 'n'; - -//#include "debug6.cpp" - return satrec.error; -} // end sgp4init - -/*----------------------------------------------------------------------------- -* -* procedure sgp4 -* -* this procedure is the sgp4 prediction model from space command. this is an -* updated and combined version of sgp4 and sdp4, which were originally -* published separately in spacetrack report #3. this version follows the -* methodology from the aiaa paper (2006) describing the history and -* development of the code. -* -* author : david vallado 719-573-2600 28 jun 2005 -* -* inputs : -* satrec - initialised structure from sgp4init() call. -* tsince - time eince epoch (minutes) -* -* outputs : -* r - position vector km -* v - velocity km/sec -* return code - non-zero on error. -* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er -* 2 - mean motion less than 0.0 -* 3 - pert elements, ecc < 0.0 or ecc > 1.0 -* 4 - semi-latus rectum < 0.0 -* 5 - epoch elements are sub-orbital -* 6 - satellite has decayed -* -* locals : -* am - -* axnl, aynl - -* betal - -* cosim , sinim , cosomm , sinomm , cnod , snod , cos2u , -* sin2u , coseo1 , sineo1 , cosi , sini , cosip , sinip , -* cosisq , cossu , sinsu , cosu , sinu -* delm - -* delomg - -* dndt - -* eccm - -* emsq - -* ecose - -* el2 - -* eo1 - -* eccp - -* esine - -* argpm - -* argpp - -* omgadf - -* pl - -* r - -* rtemsq - -* rdotl - -* rl - -* rvdot - -* rvdotl - -* su - -* t2 , t3 , t4 , tc -* tem5, temp , temp1 , temp2 , tempa , tempe , templ -* u , ux , uy , uz , vx , vy , vz -* inclm - inclination -* mm - mean anomaly -* nm - mean motion -* nodem - right asc of ascending node -* xinc - -* xincp - -* xl - -* xlm - -* mp - -* xmdf - -* xmx - -* xmy - -* nodedf - -* xnode - -* nodep - -* np - -* -* coupling : -* getgravconst- -* dpper -* dpspace -* -* references : -* hoots, roehrich, norad spacetrack report #3 1980 -* hoots, norad spacetrack report #6 1986 -* hoots, schumacher and glover 2004 -* vallado, crawford, hujsak, kelso 2006 - ----------------------------------------------------------------------------*/ - -int sgp4 - ( - gravconsttype whichconst, elsetrec& satrec, double tsince, - double r[3], double v[3] - ) -{ - double am , axnl , aynl , betal , cosim , cnod , - cos2u, coseo1, cosi , cosip , cosisq, cossu , cosu, - delm , delomg, em , emsq , ecose , el2 , eo1 , - ep , esine , argpm, argpp , argpdf, pl, mrt = 0.0, - mvt , rdotl , rl , rvdot , rvdotl, sinim , - sin2u, sineo1, sini , sinip , sinsu , sinu , - snod , su , t2 , t3 , t4 , tem5 , temp, - temp1, temp2 , tempa, tempe , templ , u , ux , - uy , uz , vx , vy , vz , inclm , mm , - nm , nodem, xinc , xincp , xl , xlm , mp , - xmdf , xmx , xmy , nodedf, xnode , nodep, tc , dndt, - twopi, x2o3 , j2 , j3 , tumin, j4 , xke , j3oj2, radiusearthkm, - mu, vkmpersec; - int ktr; - - /* ------------------ set mathematical constants --------------- */ - // sgp4fix divisor for divide by zero check on inclination - const double temp4 = 1.0 + cos(pi-1.0e-9); - twopi = 2.0 * pi; - x2o3 = 2.0 / 3.0; - // sgp4fix identify constants and allow alternate values - getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); - vkmpersec = radiusearthkm * xke/60.0; - - /* --------------------- clear sgp4 error flag ----------------- */ - satrec.t = tsince; - satrec.error = 0; - - /* ------- update for secular gravity and atmospheric drag ----- */ - xmdf = satrec.mo + satrec.mdot * satrec.t; - argpdf = satrec.argpo + satrec.argpdot * satrec.t; - nodedf = satrec.nodeo + satrec.nodedot * satrec.t; - argpm = argpdf; - mm = xmdf; - t2 = satrec.t * satrec.t; - nodem = nodedf + satrec.nodecf * t2; - tempa = 1.0 - satrec.cc1 * satrec.t; - tempe = satrec.bstar * satrec.cc4 * satrec.t; - templ = satrec.t2cof * t2; - - if (satrec.isimp != 1) - { - delomg = satrec.omgcof * satrec.t; - delm = satrec.xmcof * - (pow((1.0 + satrec.eta * cos(xmdf)), 3) - - satrec.delmo); - temp = delomg + delm; - mm = xmdf + temp; - argpm = argpdf - temp; - t3 = t2 * satrec.t; - t4 = t3 * satrec.t; - tempa = tempa - satrec.d2 * t2 - satrec.d3 * t3 - - satrec.d4 * t4; - tempe = tempe + satrec.bstar * satrec.cc5 * (sin(mm) - - satrec.sinmao); - templ = templ + satrec.t3cof * t3 + t4 * (satrec.t4cof + - satrec.t * satrec.t5cof); - } - - nm = satrec.no; - em = satrec.ecco; - inclm = satrec.inclo; - if (satrec.method == 'd') - { - tc = satrec.t; - dspace - ( - satrec.irez, - satrec.d2201, satrec.d2211, satrec.d3210, - satrec.d3222, satrec.d4410, satrec.d4422, - satrec.d5220, satrec.d5232, satrec.d5421, - satrec.d5433, satrec.dedt, satrec.del1, - satrec.del2, satrec.del3, satrec.didt, - satrec.dmdt, satrec.dnodt, satrec.domdt, - satrec.argpo, satrec.argpdot, satrec.t, tc, - satrec.gsto, satrec.xfact, satrec.xlamo, - satrec.no, satrec.atime, - em, argpm, inclm, satrec.xli, mm, satrec.xni, - nodem, dndt, nm - ); - } // if method = d - - if (nm <= 0.0) - { -// printf("# error nm %f\n", nm); - satrec.error = 2; - } - am = pow((xke / nm),x2o3) * tempa * tempa; - nm = xke / pow(am, 1.5); - em = em - tempe; - - // fix tolerance for error recognition - if ((em >= 1.0) || (em < -0.001) || (am < 0.95)) - { -// printf("# error em %f\n", em); - satrec.error = 1; - } - if (em < 0.0) - em = 1.0e-6; - mm = mm + satrec.no * templ; - xlm = mm + argpm + nodem; - emsq = em * em; - temp = 1.0 - emsq; - - nodem = fmod(nodem, twopi); - argpm = fmod(argpm, twopi); - xlm = fmod(xlm, twopi); - mm = fmod(xlm - argpm - nodem, twopi); - - /* ----------------- compute extra mean quantities ------------- */ - sinim = sin(inclm); - cosim = cos(inclm); - - /* -------------------- add lunar-solar periodics -------------- */ - ep = em; - xincp = inclm; - argpp = argpm; - nodep = nodem; - mp = mm; - sinip = sinim; - cosip = cosim; - if (satrec.method == 'd') - { - dpper - ( - satrec.e3, satrec.ee2, satrec.peo, - satrec.pgho, satrec.pho, satrec.pinco, - satrec.plo, satrec.se2, satrec.se3, - satrec.sgh2, satrec.sgh3, satrec.sgh4, - satrec.sh2, satrec.sh3, satrec.si2, - satrec.si3, satrec.sl2, satrec.sl3, - satrec.sl4, satrec.t, satrec.xgh2, - satrec.xgh3, satrec.xgh4, satrec.xh2, - satrec.xh3, satrec.xi2, satrec.xi3, - satrec.xl2, satrec.xl3, satrec.xl4, - satrec.zmol, satrec.zmos, satrec.inclo, - 'n', ep, xincp, nodep, argpp, mp - ); - if (xincp < 0.0) - { - xincp = -xincp; - nodep = nodep + pi; - argpp = argpp - pi; - } - if ((ep < 0.0 ) || ( ep > 1.0)) - { - // printf("# error ep %f\n", ep); - satrec.error = 3; - } - } // if method = d - - /* -------------------- long period periodics ------------------ */ - if (satrec.method == 'd') - { - sinip = sin(xincp); - cosip = cos(xincp); - satrec.aycof = -0.5*j3oj2*sinip; - // sgp4fix for divide by zero for xincp = 180 deg - if (fabs(cosip+1.0) > 1.5e-12) - satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / (1.0 + cosip); - else - satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / temp4; - } - axnl = ep * cos(argpp); - temp = 1.0 / (am * (1.0 - ep * ep)); - aynl = ep* sin(argpp) + temp * satrec.aycof; - xl = mp + argpp + nodep + temp * satrec.xlcof * axnl; - - /* --------------------- solve kepler's equation --------------- */ - u = fmod(xl - nodep, twopi); - eo1 = u; - tem5 = 9999.9; - ktr = 1; - // sgp4fix for kepler iteration - // the following iteration needs better limits on corrections - while (( fabs(tem5) >= 1.0e-12) && (ktr <= 10) ) - { - sineo1 = sin(eo1); - coseo1 = cos(eo1); - tem5 = 1.0 - coseo1 * axnl - sineo1 * aynl; - tem5 = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5; - if(fabs(tem5) >= 0.95) - tem5 = tem5 > 0.0 ? 0.95 : -0.95; - eo1 = eo1 + tem5; - ktr = ktr + 1; - } - - /* ------------- short period preliminary quantities ----------- */ - ecose = axnl*coseo1 + aynl*sineo1; - esine = axnl*sineo1 - aynl*coseo1; - el2 = axnl*axnl + aynl*aynl; - pl = am*(1.0-el2); - if (pl < 0.0) - { -// printf("# error pl %f\n", pl); - satrec.error = 4; - } - else - { - rl = am * (1.0 - ecose); - rdotl = sqrt(am) * esine/rl; - rvdotl = sqrt(pl) / rl; - betal = sqrt(1.0 - el2); - temp = esine / (1.0 + betal); - sinu = am / rl * (sineo1 - aynl - axnl * temp); - cosu = am / rl * (coseo1 - axnl + aynl * temp); - su = atan2(sinu, cosu); - sin2u = (cosu + cosu) * sinu; - cos2u = 1.0 - 2.0 * sinu * sinu; - temp = 1.0 / pl; - temp1 = 0.5 * j2 * temp; - temp2 = temp1 * temp; - - /* -------------- update for short period periodics ------------ */ - if (satrec.method == 'd') - { - cosisq = cosip * cosip; - satrec.con41 = 3.0*cosisq - 1.0; - satrec.x1mth2 = 1.0 - cosisq; - satrec.x7thm1 = 7.0*cosisq - 1.0; - } - mrt = rl * (1.0 - 1.5 * temp2 * betal * satrec.con41) + - 0.5 * temp1 * satrec.x1mth2 * cos2u; - su = su - 0.25 * temp2 * satrec.x7thm1 * sin2u; - xnode = nodep + 1.5 * temp2 * cosip * sin2u; - xinc = xincp + 1.5 * temp2 * cosip * sinip * cos2u; - mvt = rdotl - nm * temp1 * satrec.x1mth2 * sin2u / xke; - rvdot = rvdotl + nm * temp1 * (satrec.x1mth2 * cos2u + - 1.5 * satrec.con41) / xke; - - /* --------------------- orientation vectors ------------------- */ - sinsu = sin(su); - cossu = cos(su); - snod = sin(xnode); - cnod = cos(xnode); - sini = sin(xinc); - cosi = cos(xinc); - xmx = -snod * cosi; - xmy = cnod * cosi; - ux = xmx * sinsu + cnod * cossu; - uy = xmy * sinsu + snod * cossu; - uz = sini * sinsu; - vx = xmx * cossu - cnod * sinsu; - vy = xmy * cossu - snod * sinsu; - vz = sini * cossu; - - /* --------- position and velocity (in km and km/sec) ---------- */ - r[0] = (mrt * ux)* radiusearthkm; - r[1] = (mrt * uy)* radiusearthkm; - r[2] = (mrt * uz)* radiusearthkm; - v[0] = (mvt * ux + rvdot * vx) * vkmpersec; - v[1] = (mvt * uy + rvdot * vy) * vkmpersec; - v[2] = (mvt * uz + rvdot * vz) * vkmpersec; - } // if pl > 0 - - // sgp4fix for decaying satellites - if (mrt < 1.0) - { -// printf("# decay condition %11.6f \n",mrt); - satrec.error = 6; - } - - -//#include "debug7.cpp" - return satrec.error; -} // end sgp4 - - -/* ----------------------------------------------------------------------------- -* -* function gstime -* -* this function finds the greenwich sidereal time. -* -* author : david vallado 719-573-2600 1 mar 2001 -* -* inputs description range / units -* jdut1 - julian date in ut1 days from 4713 bc -* -* outputs : -* gstime - greenwich sidereal time 0 to 2pi rad -* -* locals : -* temp - temporary variable for doubles rad -* tut1 - julian centuries from the -* jan 1, 2000 12 h epoch (ut1) -* -* coupling : -* none -* -* references : -* vallado 2004, 191, eq 3-45 -* --------------------------------------------------------------------------- */ - -double gstime - ( - double jdut1 - ) - { - const double twopi = 2.0 * pi; - const double deg2rad = pi / 180.0; - double temp, tut1; - - tut1 = (jdut1 - 2451545.0) / 36525.0; - temp = -6.2e-6* tut1 * tut1 * tut1 + 0.093104 * tut1 * tut1 + - (876600.0*3600 + 8640184.812866) * tut1 + 67310.54841; // sec - temp = fmod(temp * deg2rad / 240.0, twopi); //360/86400 = 1/240, to deg, to rad - - // ------------------------ check quadrants --------------------- - if (temp < 0.0) - temp += twopi; - - return temp; - } // end gstime - -/* ----------------------------------------------------------------------------- -* -* function getgravconst -* -* this function gets constants for the propagator. note that mu is identified to -* facilitiate comparisons with newer models. the common useage is wgs72. -* -* author : david vallado 719-573-2600 21 jul 2006 -* -* inputs : -* whichconst - which set of constants to use wgs72old, wgs72, wgs84 -* -* outputs : -* tumin - minutes in one time unit -* mu - earth gravitational parameter -* radiusearthkm - radius of the earth in km -* xke - reciprocal of tumin -* j2, j3, j4 - un-normalized zonal harmonic values -* j3oj2 - j3 divided by j2 -* -* locals : -* -* coupling : -* none -* -* references : -* norad spacetrack report #3 -* vallado, crawford, hujsak, kelso 2006 - --------------------------------------------------------------------------- */ - -void getgravconst - ( - gravconsttype whichconst, - double& tumin, - double& mu, - double& radiusearthkm, - double& xke, - double& j2, - double& j3, - double& j4, - double& j3oj2 - ) - { - - switch (whichconst) - { - // -- wgs-72 low precision str#3 constants -- - case wgs72old: - mu = 398600.79964; // in km3 / s2 - radiusearthkm = 6378.135; // km - xke = 0.0743669161; - tumin = 1.0 / xke; - j2 = 0.001082616; - j3 = -0.00000253881; - j4 = -0.00000165597; - j3oj2 = j3 / j2; - break; - // ------------ wgs-72 constants ------------ - case wgs72: - mu = 398600.8; // in km3 / s2 - radiusearthkm = 6378.135; // km - xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu); - tumin = 1.0 / xke; - j2 = 0.001082616; - j3 = -0.00000253881; - j4 = -0.00000165597; - j3oj2 = j3 / j2; - break; - case wgs84: - // ------------ wgs-84 constants ------------ - mu = 398600.5; // in km3 / s2 - radiusearthkm = 6378.137; // km - xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu); - tumin = 1.0 / xke; - j2 = 0.00108262998905; - j3 = -0.00000253215306; - j4 = -0.00000161098761; - j3oj2 = j3 / j2; - break; - default: - fprintf(stderr,"unknown gravity option (%d)\n",whichconst); - break; - } - - } // end getgravconst - - - - - +/* ---------------------------------------------------------------- +* +* sgp4unit.cpp +* +* this file contains the sgp4 procedures for analytical propagation +* of a satellite. the code was originally released in the 1980 and 1986 +* spacetrack papers. a detailed discussion of the theory and history +* may be found in the 2006 aiaa paper by vallado, crawford, hujsak, +* and kelso. +* +* companion code for +* fundamentals of astrodynamics and applications +* 2007 +* by david vallado +* +* (w) 719-573-2600, email dvallado@agi.com +* +* current : +* 16 nov 07 david vallado +* misc fixes for better compliance +* changes : +* 20 apr 07 david vallado +* misc fixes for constants +* 11 aug 06 david vallado +* chg lyddane choice back to strn3, constants, misc doc +* 15 dec 05 david vallado +* misc fixes +* 26 jul 05 david vallado +* fixes for paper +* note that each fix is preceded by a +* comment with "sgp4fix" and an explanation of +* what was changed +* 10 aug 04 david vallado +* 2nd printing baseline working +* 14 may 01 david vallado +* 2nd edition baseline +* 80 norad +* original baseline +* ---------------------------------------------------------------- */ + +#include "sgp4unit.h" + +const char help = 'n'; +FILE *dbgfile; + +#define pi 3.14159265358979323846 + + +/* ----------- local functions - only ever used internally by sgp4 ---------- */ +static void dpper + ( + double e3, double ee2, double peo, double pgho, double pho, + double pinco, double plo, double se2, double se3, double sgh2, + double sgh3, double sgh4, double sh2, double sh3, double si2, + double si3, double sl2, double sl3, double sl4, double t, + double xgh2, double xgh3, double xgh4, double xh2, double xh3, + double xi2, double xi3, double xl2, double xl3, double xl4, + double zmol, double zmos, double inclo, + char init, + double& ep, double& inclp, double& nodep, double& argpp, double& mp + ); + +static void dscom + ( + double epoch, double ep, double argpp, double tc, double inclp, + double nodep, double np, + double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, + double& cosomm,double& day, double& e3, double& ee2, double& em, + double& emsq, double& gam, double& peo, double& pgho, double& pho, + double& pinco, double& plo, double& rtemsq, double& se2, double& se3, + double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, + double& si2, double& si3, double& sl2, double& sl3, double& sl4, + double& s1, double& s2, double& s3, double& s4, double& s5, + double& s6, double& s7, double& ss1, double& ss2, double& ss3, + double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, + double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, + double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, + double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, + double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, + double& xl4, double& nm, double& z1, double& z2, double& z3, + double& z11, double& z12, double& z13, double& z21, double& z22, + double& z23, double& z31, double& z32, double& z33, double& zmol, + double& zmos + ); + +static void dsinit + ( + gravconsttype whichconst, + double cosim, double emsq, double argpo, double s1, double s2, + double s3, double s4, double s5, double sinim, double ss1, + double ss2, double ss3, double ss4, double ss5, double sz1, + double sz3, double sz11, double sz13, double sz21, double sz23, + double sz31, double sz33, double t, double tc, double gsto, + double mo, double mdot, double no, double nodeo, double nodedot, + double xpidot, double z1, double z3, double z11, double z13, + double z21, double z23, double z31, double z33, double ecco, + double eccsq, double& em, double& argpm, double& inclm, double& mm, + double& nm, double& nodem, + int& irez, + double& atime, double& d2201, double& d2211, double& d3210, double& d3222, + double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, + double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, + double& dnodt, double& domdt, double& del1, double& del2, double& del3, + double& xfact, double& xlamo, double& xli, double& xni + ); + +static void dspace + ( + int irez, + double d2201, double d2211, double d3210, double d3222, double d4410, + double d4422, double d5220, double d5232, double d5421, double d5433, + double dedt, double del1, double del2, double del3, double didt, + double dmdt, double dnodt, double domdt, double argpo, double argpdot, + double t, double tc, double gsto, double xfact, double xlamo, + double no, + double& atime, double& em, double& argpm, double& inclm, double& xli, + double& mm, double& xni, double& nodem, double& dndt, double& nm + ); + +static void initl + ( + int satn, gravconsttype whichconst, + double ecco, double epoch, double inclo, double& no, + char& method, + double& ainv, double& ao, double& con41, double& con42, double& cosio, + double& cosio2,double& eccsq, double& omeosq, double& posq, + double& rp, double& rteosq,double& sinio , double& gsto + ); + +/* ----------------------------------------------------------------------------- +* +* procedure dpper +* +* this procedure provides deep space long period periodic contributions +* to the mean elements. by design, these periodics are zero at epoch. +* this used to be dscom which included initialization, but it's really a +* recurring function. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* e3 - +* ee2 - +* peo - +* pgho - +* pho - +* pinco - +* plo - +* se2 , se3 , sgh2, sgh3, sgh4, sh2, sh3, si2, si3, sl2, sl3, sl4 - +* t - +* xh2, xh3, xi2, xi3, xl2, xl3, xl4 - +* zmol - +* zmos - +* ep - eccentricity 0.0 - 1.0 +* inclo - inclination - needed for lyddane modification +* nodep - right ascension of ascending node +* argpp - argument of perigee +* mp - mean anomaly +* +* outputs : +* ep - eccentricity 0.0 - 1.0 +* inclp - inclination +* nodep - right ascension of ascending node +* argpp - argument of perigee +* mp - mean anomaly +* +* locals : +* alfdp - +* betdp - +* cosip , sinip , cosop , sinop , +* dalf - +* dbet - +* dls - +* f2, f3 - +* pe - +* pgh - +* ph - +* pinc - +* pl - +* sel , ses , sghl , sghs , shl , shs , sil , sinzf , sis , +* sll , sls +* xls - +* xnoh - +* zf - +* zm - +* +* coupling : +* none. +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +static void dpper + ( + double e3, double ee2, double peo, double pgho, double pho, + double pinco, double plo, double se2, double se3, double sgh2, + double sgh3, double sgh4, double sh2, double sh3, double si2, + double si3, double sl2, double sl3, double sl4, double t, + double xgh2, double xgh3, double xgh4, double xh2, double xh3, + double xi2, double xi3, double xl2, double xl3, double xl4, + double zmol, double zmos, double inclo, + char init, + double& ep, double& inclp, double& nodep, double& argpp, double& mp + ) +{ + /* --------------------- local variables ------------------------ */ + const double twopi = 2.0 * pi; + double alfdp, betdp, cosip, cosop, dalf, dbet, dls, + f2, f3, pe, pgh, ph, pinc, pl , + sel, ses, sghl, sghs, shll, shs, sil, + sinip, sinop, sinzf, sis, sll, sls, xls, + xnoh, zf, zm, zel, zes, znl, zns; + + /* ---------------------- constants ----------------------------- */ + zns = 1.19459e-5; + zes = 0.01675; + znl = 1.5835218e-4; + zel = 0.05490; + + /* --------------- calculate time varying periodics ----------- */ + zm = zmos + zns * t; + // be sure that the initial call has time set to zero + if (init == 'y') + zm = zmos; + zf = zm + 2.0 * zes * sin(zm); + sinzf = sin(zf); + f2 = 0.5 * sinzf * sinzf - 0.25; + f3 = -0.5 * sinzf * cos(zf); + ses = se2* f2 + se3 * f3; + sis = si2 * f2 + si3 * f3; + sls = sl2 * f2 + sl3 * f3 + sl4 * sinzf; + sghs = sgh2 * f2 + sgh3 * f3 + sgh4 * sinzf; + shs = sh2 * f2 + sh3 * f3; + zm = zmol + znl * t; + if (init == 'y') + zm = zmol; + zf = zm + 2.0 * zel * sin(zm); + sinzf = sin(zf); + f2 = 0.5 * sinzf * sinzf - 0.25; + f3 = -0.5 * sinzf * cos(zf); + sel = ee2 * f2 + e3 * f3; + sil = xi2 * f2 + xi3 * f3; + sll = xl2 * f2 + xl3 * f3 + xl4 * sinzf; + sghl = xgh2 * f2 + xgh3 * f3 + xgh4 * sinzf; + shll = xh2 * f2 + xh3 * f3; + pe = ses + sel; + pinc = sis + sil; + pl = sls + sll; + pgh = sghs + sghl; + ph = shs + shll; + + if (init == 'n') + { + pe = pe - peo; + pinc = pinc - pinco; + pl = pl - plo; + pgh = pgh - pgho; + ph = ph - pho; + inclp = inclp + pinc; + ep = ep + pe; + sinip = sin(inclp); + cosip = cos(inclp); + + /* ----------------- apply periodics directly ------------ */ + // sgp4fix for lyddane choice + // strn3 used original inclination - this is technically feasible + // gsfc used perturbed inclination - also technically feasible + // probably best to readjust the 0.2 limit value and limit discontinuity + // 0.2 rad = 11.45916 deg + // use next line for original strn3 approach and original inclination + // if (inclo >= 0.2) + // use next line for gsfc version and perturbed inclination + if (inclp >= 0.2) + { + ph = ph / sinip; + pgh = pgh - cosip * ph; + argpp = argpp + pgh; + nodep = nodep + ph; + mp = mp + pl; + } + else + { + /* ---- apply periodics with lyddane modification ---- */ + sinop = sin(nodep); + cosop = cos(nodep); + alfdp = sinip * sinop; + betdp = sinip * cosop; + dalf = ph * cosop + pinc * cosip * sinop; + dbet = -ph * sinop + pinc * cosip * cosop; + alfdp = alfdp + dalf; + betdp = betdp + dbet; + nodep = fmod(nodep, twopi); + // sgp4fix for afspc written intrinsic functions + // nodep used without a trigonometric function ahead + if (nodep < 0.0) + nodep = nodep + twopi; + xls = mp + argpp + cosip * nodep; + dls = pl + pgh - pinc * nodep * sinip; + xls = xls + dls; + xnoh = nodep; + nodep = atan2(alfdp, betdp); + // sgp4fix for afspc written intrinsic functions + // nodep used without a trigonometric function ahead + if (nodep < 0.0) + nodep = nodep + twopi; + if (fabs(xnoh - nodep) > pi){ + if (nodep < xnoh) + nodep = nodep + twopi; + else + nodep = nodep - twopi; + } + mp = mp + pl; + argpp = xls - mp - cosip * nodep; + } + } // if init == 'n' + +//#include "debug1.cpp" +} // end dpper + +/*----------------------------------------------------------------------------- +* +* procedure dscom +* +* this procedure provides deep space common items used by both the secular +* and periodics subroutines. input is provided as shown. this routine +* used to be called dpper, but the functions inside weren't well organized. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* epoch - +* ep - eccentricity +* argpp - argument of perigee +* tc - +* inclp - inclination +* nodep - right ascension of ascending node +* np - mean motion +* +* outputs : +* sinim , cosim , sinomm , cosomm , snodm , cnodm +* day - +* e3 - +* ee2 - +* em - eccentricity +* emsq - eccentricity squared +* gam - +* peo - +* pgho - +* pho - +* pinco - +* plo - +* rtemsq - +* se2, se3 - +* sgh2, sgh3, sgh4 - +* sh2, sh3, si2, si3, sl2, sl3, sl4 - +* s1, s2, s3, s4, s5, s6, s7 - +* ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3 - +* sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - +* xgh2, xgh3, xgh4, xh2, xh3, xi2, xi3, xl2, xl3, xl4 - +* nm - mean motion +* z1, z2, z3, z11, z12, z13, z21, z22, z23, z31, z32, z33 - +* zmol - +* zmos - +* +* locals : +* a1, a2, a3, a4, a5, a6, a7, a8, a9, a10 - +* betasq - +* cc - +* ctem, stem - +* x1, x2, x3, x4, x5, x6, x7, x8 - +* xnodce - +* xnoi - +* zcosg , zsing , zcosgl , zsingl , zcosh , zsinh , zcoshl , zsinhl , +* zcosi , zsini , zcosil , zsinil , +* zx - +* zy - +* +* coupling : +* none. +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +static void dscom + ( + double epoch, double ep, double argpp, double tc, double inclp, + double nodep, double np, + double& snodm, double& cnodm, double& sinim, double& cosim, double& sinomm, + double& cosomm,double& day, double& e3, double& ee2, double& em, + double& emsq, double& gam, double& peo, double& pgho, double& pho, + double& pinco, double& plo, double& rtemsq, double& se2, double& se3, + double& sgh2, double& sgh3, double& sgh4, double& sh2, double& sh3, + double& si2, double& si3, double& sl2, double& sl3, double& sl4, + double& s1, double& s2, double& s3, double& s4, double& s5, + double& s6, double& s7, double& ss1, double& ss2, double& ss3, + double& ss4, double& ss5, double& ss6, double& ss7, double& sz1, + double& sz2, double& sz3, double& sz11, double& sz12, double& sz13, + double& sz21, double& sz22, double& sz23, double& sz31, double& sz32, + double& sz33, double& xgh2, double& xgh3, double& xgh4, double& xh2, + double& xh3, double& xi2, double& xi3, double& xl2, double& xl3, + double& xl4, double& nm, double& z1, double& z2, double& z3, + double& z11, double& z12, double& z13, double& z21, double& z22, + double& z23, double& z31, double& z32, double& z33, double& zmol, + double& zmos + ) +{ + /* -------------------------- constants ------------------------- */ + const double zes = 0.01675; + const double zel = 0.05490; + const double c1ss = 2.9864797e-6; + const double c1l = 4.7968065e-7; + const double zsinis = 0.39785416; + const double zcosis = 0.91744867; + const double zcosgs = 0.1945905; + const double zsings = -0.98088458; + const double twopi = 2.0 * pi; + + /* --------------------- local variables ------------------------ */ + int lsflg; + double a1 , a2 , a3 , a4 , a5 , a6 , a7 , + a8 , a9 , a10 , betasq, cc , ctem , stem , + x1 , x2 , x3 , x4 , x5 , x6 , x7 , + x8 , xnodce, xnoi , zcosg , zcosgl, zcosh , zcoshl, + zcosi , zcosil, zsing , zsingl, zsinh , zsinhl, zsini , + zsinil, zx , zy; + + nm = np; + em = ep; + snodm = sin(nodep); + cnodm = cos(nodep); + sinomm = sin(argpp); + cosomm = cos(argpp); + sinim = sin(inclp); + cosim = cos(inclp); + emsq = em * em; + betasq = 1.0 - emsq; + rtemsq = sqrt(betasq); + + /* ----------------- initialize lunar solar terms --------------- */ + peo = 0.0; + pinco = 0.0; + plo = 0.0; + pgho = 0.0; + pho = 0.0; + day = epoch + 18261.5 + tc / 1440.0; + xnodce = fmod(4.5236020 - 9.2422029e-4 * day, twopi); + stem = sin(xnodce); + ctem = cos(xnodce); + zcosil = 0.91375164 - 0.03568096 * ctem; + zsinil = sqrt(1.0 - zcosil * zcosil); + zsinhl = 0.089683511 * stem / zsinil; + zcoshl = sqrt(1.0 - zsinhl * zsinhl); + gam = 5.8351514 + 0.0019443680 * day; + zx = 0.39785416 * stem / zsinil; + zy = zcoshl * ctem + 0.91744867 * zsinhl * stem; + zx = atan2(zx, zy); + zx = gam + zx - xnodce; + zcosgl = cos(zx); + zsingl = sin(zx); + + /* ------------------------- do solar terms --------------------- */ + zcosg = zcosgs; + zsing = zsings; + zcosi = zcosis; + zsini = zsinis; + zcosh = cnodm; + zsinh = snodm; + cc = c1ss; + xnoi = 1.0 / nm; + + for (lsflg = 1; lsflg <= 2; lsflg++) + { + a1 = zcosg * zcosh + zsing * zcosi * zsinh; + a3 = -zsing * zcosh + zcosg * zcosi * zsinh; + a7 = -zcosg * zsinh + zsing * zcosi * zcosh; + a8 = zsing * zsini; + a9 = zsing * zsinh + zcosg * zcosi * zcosh; + a10 = zcosg * zsini; + a2 = cosim * a7 + sinim * a8; + a4 = cosim * a9 + sinim * a10; + a5 = -sinim * a7 + cosim * a8; + a6 = -sinim * a9 + cosim * a10; + + x1 = a1 * cosomm + a2 * sinomm; + x2 = a3 * cosomm + a4 * sinomm; + x3 = -a1 * sinomm + a2 * cosomm; + x4 = -a3 * sinomm + a4 * cosomm; + x5 = a5 * sinomm; + x6 = a6 * sinomm; + x7 = a5 * cosomm; + x8 = a6 * cosomm; + + z31 = 12.0 * x1 * x1 - 3.0 * x3 * x3; + z32 = 24.0 * x1 * x2 - 6.0 * x3 * x4; + z33 = 12.0 * x2 * x2 - 3.0 * x4 * x4; + z1 = 3.0 * (a1 * a1 + a2 * a2) + z31 * emsq; + z2 = 6.0 * (a1 * a3 + a2 * a4) + z32 * emsq; + z3 = 3.0 * (a3 * a3 + a4 * a4) + z33 * emsq; + z11 = -6.0 * a1 * a5 + emsq * (-24.0 * x1 * x7-6.0 * x3 * x5); + z12 = -6.0 * (a1 * a6 + a3 * a5) + emsq * + (-24.0 * (x2 * x7 + x1 * x8) - 6.0 * (x3 * x6 + x4 * x5)); + z13 = -6.0 * a3 * a6 + emsq * (-24.0 * x2 * x8 - 6.0 * x4 * x6); + z21 = 6.0 * a2 * a5 + emsq * (24.0 * x1 * x5 - 6.0 * x3 * x7); + z22 = 6.0 * (a4 * a5 + a2 * a6) + emsq * + (24.0 * (x2 * x5 + x1 * x6) - 6.0 * (x4 * x7 + x3 * x8)); + z23 = 6.0 * a4 * a6 + emsq * (24.0 * x2 * x6 - 6.0 * x4 * x8); + z1 = z1 + z1 + betasq * z31; + z2 = z2 + z2 + betasq * z32; + z3 = z3 + z3 + betasq * z33; + s3 = cc * xnoi; + s2 = -0.5 * s3 / rtemsq; + s4 = s3 * rtemsq; + s1 = -15.0 * em * s4; + s5 = x1 * x3 + x2 * x4; + s6 = x2 * x3 + x1 * x4; + s7 = x2 * x4 - x1 * x3; + + /* ----------------------- do lunar terms ------------------- */ + if (lsflg == 1) + { + ss1 = s1; + ss2 = s2; + ss3 = s3; + ss4 = s4; + ss5 = s5; + ss6 = s6; + ss7 = s7; + sz1 = z1; + sz2 = z2; + sz3 = z3; + sz11 = z11; + sz12 = z12; + sz13 = z13; + sz21 = z21; + sz22 = z22; + sz23 = z23; + sz31 = z31; + sz32 = z32; + sz33 = z33; + zcosg = zcosgl; + zsing = zsingl; + zcosi = zcosil; + zsini = zsinil; + zcosh = zcoshl * cnodm + zsinhl * snodm; + zsinh = snodm * zcoshl - cnodm * zsinhl; + cc = c1l; + } + } + + zmol = fmod(4.7199672 + 0.22997150 * day - gam, twopi); + zmos = fmod(6.2565837 + 0.017201977 * day, twopi); + + /* ------------------------ do solar terms ---------------------- */ + se2 = 2.0 * ss1 * ss6; + se3 = 2.0 * ss1 * ss7; + si2 = 2.0 * ss2 * sz12; + si3 = 2.0 * ss2 * (sz13 - sz11); + sl2 = -2.0 * ss3 * sz2; + sl3 = -2.0 * ss3 * (sz3 - sz1); + sl4 = -2.0 * ss3 * (-21.0 - 9.0 * emsq) * zes; + sgh2 = 2.0 * ss4 * sz32; + sgh3 = 2.0 * ss4 * (sz33 - sz31); + sgh4 = -18.0 * ss4 * zes; + sh2 = -2.0 * ss2 * sz22; + sh3 = -2.0 * ss2 * (sz23 - sz21); + + /* ------------------------ do lunar terms ---------------------- */ + ee2 = 2.0 * s1 * s6; + e3 = 2.0 * s1 * s7; + xi2 = 2.0 * s2 * z12; + xi3 = 2.0 * s2 * (z13 - z11); + xl2 = -2.0 * s3 * z2; + xl3 = -2.0 * s3 * (z3 - z1); + xl4 = -2.0 * s3 * (-21.0 - 9.0 * emsq) * zel; + xgh2 = 2.0 * s4 * z32; + xgh3 = 2.0 * s4 * (z33 - z31); + xgh4 = -18.0 * s4 * zel; + xh2 = -2.0 * s2 * z22; + xh3 = -2.0 * s2 * (z23 - z21); + +//#include "debug2.cpp" +} // end dscom + +/*----------------------------------------------------------------------------- +* +* procedure dsinit +* +* this procedure provides deep space contributions to mean motion dot due +* to geopotential resonance with half day and one day orbits. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* cosim, sinim- +* emsq - eccentricity squared +* argpo - argument of perigee +* s1, s2, s3, s4, s5 - +* ss1, ss2, ss3, ss4, ss5 - +* sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33 - +* t - time +* tc - +* gsto - greenwich sidereal time rad +* mo - mean anomaly +* mdot - mean anomaly dot (rate) +* no - mean motion +* nodeo - right ascension of ascending node +* nodedot - right ascension of ascending node dot (rate) +* xpidot - +* z1, z3, z11, z13, z21, z23, z31, z33 - +* eccm - eccentricity +* argpm - argument of perigee +* inclm - inclination +* mm - mean anomaly +* xn - mean motion +* nodem - right ascension of ascending node +* +* outputs : +* em - eccentricity +* argpm - argument of perigee +* inclm - inclination +* mm - mean anomaly +* nm - mean motion +* nodem - right ascension of ascending node +* irez - flag for resonance 0-none, 1-one day, 2-half day +* atime - +* d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - +* dedt - +* didt - +* dmdt - +* dndt - +* dnodt - +* domdt - +* del1, del2, del3 - +* ses , sghl , sghs , sgs , shl , shs , sis , sls +* theta - +* xfact - +* xlamo - +* xli - +* xni +* +* locals : +* ainv2 - +* aonv - +* cosisq - +* eoc - +* f220, f221, f311, f321, f322, f330, f441, f442, f522, f523, f542, f543 - +* g200, g201, g211, g300, g310, g322, g410, g422, g520, g521, g532, g533 - +* sini2 - +* temp - +* temp1 - +* theta - +* xno2 - +* +* coupling : +* getgravconst +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +static void dsinit + ( + gravconsttype whichconst, + double cosim, double emsq, double argpo, double s1, double s2, + double s3, double s4, double s5, double sinim, double ss1, + double ss2, double ss3, double ss4, double ss5, double sz1, + double sz3, double sz11, double sz13, double sz21, double sz23, + double sz31, double sz33, double t, double tc, double gsto, + double mo, double mdot, double no, double nodeo, double nodedot, + double xpidot, double z1, double z3, double z11, double z13, + double z21, double z23, double z31, double z33, double ecco, + double eccsq, double& em, double& argpm, double& inclm, double& mm, + double& nm, double& nodem, + int& irez, + double& atime, double& d2201, double& d2211, double& d3210, double& d3222, + double& d4410, double& d4422, double& d5220, double& d5232, double& d5421, + double& d5433, double& dedt, double& didt, double& dmdt, double& dndt, + double& dnodt, double& domdt, double& del1, double& del2, double& del3, + double& xfact, double& xlamo, double& xli, double& xni + ) +{ + /* --------------------- local variables ------------------------ */ + const double twopi = 2.0 * pi; + + double ainv2 , aonv=0.0, cosisq, eoc, f220 , f221 , f311 , + f321 , f322 , f330 , f441 , f442 , f522 , f523 , + f542 , f543 , g200 , g201 , g211 , g300 , g310 , + g322 , g410 , g422 , g520 , g521 , g532 , g533 , + ses , sgs , sghl , sghs , shs , shll , sis , + sini2 , sls , temp , temp1 , theta , xno2 , q22 , + q31 , q33 , root22, root44, root54, rptim , root32, + root52, x2o3 , xke , znl , emo , zns , emsqo, + tumin, mu, radiusearthkm, j2, j3, j4, j3oj2; + + q22 = 1.7891679e-6; + q31 = 2.1460748e-6; + q33 = 2.2123015e-7; + root22 = 1.7891679e-6; + root44 = 7.3636953e-9; + root54 = 2.1765803e-9; + rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec + root32 = 3.7393792e-7; + root52 = 1.1428639e-7; + x2o3 = 2.0 / 3.0; + znl = 1.5835218e-4; + zns = 1.19459e-5; + + // sgp4fix identify constants and allow alternate values + getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); + + /* -------------------- deep space initialization ------------ */ + irez = 0; + if ((nm < 0.0052359877) && (nm > 0.0034906585)) + irez = 1; + if ((nm >= 8.26e-3) && (nm <= 9.24e-3) && (em >= 0.5)) + irez = 2; + + /* ------------------------ do solar terms ------------------- */ + ses = ss1 * zns * ss5; + sis = ss2 * zns * (sz11 + sz13); + sls = -zns * ss3 * (sz1 + sz3 - 14.0 - 6.0 * emsq); + sghs = ss4 * zns * (sz31 + sz33 - 6.0); + shs = -zns * ss2 * (sz21 + sz23); + // sgp4fix for 180 deg incl + if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2)) + shs = 0.0; + if (sinim != 0.0) + shs = shs / sinim; + sgs = sghs - cosim * shs; + + /* ------------------------- do lunar terms ------------------ */ + dedt = ses + s1 * znl * s5; + didt = sis + s2 * znl * (z11 + z13); + dmdt = sls - znl * s3 * (z1 + z3 - 14.0 - 6.0 * emsq); + sghl = s4 * znl * (z31 + z33 - 6.0); + shll = -znl * s2 * (z21 + z23); + // sgp4fix for 180 deg incl + if ((inclm < 5.2359877e-2) || (inclm > pi - 5.2359877e-2)) + shll = 0.0; + domdt = sgs + sghl; + dnodt = shs; + if (sinim != 0.0) + { + domdt = domdt - cosim / sinim * shll; + dnodt = dnodt + shll / sinim; + } + + /* ----------- calculate deep space resonance effects -------- */ + dndt = 0.0; + theta = fmod(gsto + tc * rptim, twopi); + em = em + dedt * t; + inclm = inclm + didt * t; + argpm = argpm + domdt * t; + nodem = nodem + dnodt * t; + mm = mm + dmdt * t; + // sgp4fix for negative inclinations + // the following if statement should be commented out + //if (inclm < 0.0) + // { + // inclm = -inclm; + // argpm = argpm - pi; + // nodem = nodem + pi; + // } + + /* -------------- initialize the resonance terms ------------- */ + if (irez != 0) + { + aonv = pow(nm / xke, x2o3); + + /* ---------- geopotential resonance for 12 hour orbits ------ */ + if (irez == 2) + { + cosisq = cosim * cosim; + emo = em; + em = ecco; + emsqo = emsq; + emsq = eccsq; + eoc = em * emsq; + g201 = -0.306 - (em - 0.64) * 0.440; + + if (em <= 0.65) + { + g211 = 3.616 - 13.2470 * em + 16.2900 * emsq; + g310 = -19.302 + 117.3900 * em - 228.4190 * emsq + 156.5910 * eoc; + g322 = -18.9068 + 109.7927 * em - 214.6334 * emsq + 146.5816 * eoc; + g410 = -41.122 + 242.6940 * em - 471.0940 * emsq + 313.9530 * eoc; + g422 = -146.407 + 841.8800 * em - 1629.014 * emsq + 1083.4350 * eoc; + g520 = -532.114 + 3017.977 * em - 5740.032 * emsq + 3708.2760 * eoc; + } + else + { + g211 = -72.099 + 331.819 * em - 508.738 * emsq + 266.724 * eoc; + g310 = -346.844 + 1582.851 * em - 2415.925 * emsq + 1246.113 * eoc; + g322 = -342.585 + 1554.908 * em - 2366.899 * emsq + 1215.972 * eoc; + g410 = -1052.797 + 4758.686 * em - 7193.992 * emsq + 3651.957 * eoc; + g422 = -3581.690 + 16178.110 * em - 24462.770 * emsq + 12422.520 * eoc; + if (em > 0.715) + g520 =-5149.66 + 29936.92 * em - 54087.36 * emsq + 31324.56 * eoc; + else + g520 = 1464.74 - 4664.75 * em + 3763.64 * emsq; + } + if (em < 0.7) + { + g533 = -919.22770 + 4988.6100 * em - 9064.7700 * emsq + 5542.21 * eoc; + g521 = -822.71072 + 4568.6173 * em - 8491.4146 * emsq + 5337.524 * eoc; + g532 = -853.66600 + 4690.2500 * em - 8624.7700 * emsq + 5341.4 * eoc; + } + else + { + g533 =-37995.780 + 161616.52 * em - 229838.20 * emsq + 109377.94 * eoc; + g521 =-51752.104 + 218913.95 * em - 309468.16 * emsq + 146349.42 * eoc; + g532 =-40023.880 + 170470.89 * em - 242699.48 * emsq + 115605.82 * eoc; + } + + sini2= sinim * sinim; + f220 = 0.75 * (1.0 + 2.0 * cosim+cosisq); + f221 = 1.5 * sini2; + f321 = 1.875 * sinim * (1.0 - 2.0 * cosim - 3.0 * cosisq); + f322 = -1.875 * sinim * (1.0 + 2.0 * cosim - 3.0 * cosisq); + f441 = 35.0 * sini2 * f220; + f442 = 39.3750 * sini2 * sini2; + f522 = 9.84375 * sinim * (sini2 * (1.0 - 2.0 * cosim- 5.0 * cosisq) + + 0.33333333 * (-2.0 + 4.0 * cosim + 6.0 * cosisq) ); + f523 = sinim * (4.92187512 * sini2 * (-2.0 - 4.0 * cosim + + 10.0 * cosisq) + 6.56250012 * (1.0+2.0 * cosim - 3.0 * cosisq)); + f542 = 29.53125 * sinim * (2.0 - 8.0 * cosim+cosisq * + (-12.0 + 8.0 * cosim + 10.0 * cosisq)); + f543 = 29.53125 * sinim * (-2.0 - 8.0 * cosim+cosisq * + (12.0 + 8.0 * cosim - 10.0 * cosisq)); + xno2 = nm * nm; + ainv2 = aonv * aonv; + temp1 = 3.0 * xno2 * ainv2; + temp = temp1 * root22; + d2201 = temp * f220 * g201; + d2211 = temp * f221 * g211; + temp1 = temp1 * aonv; + temp = temp1 * root32; + d3210 = temp * f321 * g310; + d3222 = temp * f322 * g322; + temp1 = temp1 * aonv; + temp = 2.0 * temp1 * root44; + d4410 = temp * f441 * g410; + d4422 = temp * f442 * g422; + temp1 = temp1 * aonv; + temp = temp1 * root52; + d5220 = temp * f522 * g520; + d5232 = temp * f523 * g532; + temp = 2.0 * temp1 * root54; + d5421 = temp * f542 * g521; + d5433 = temp * f543 * g533; + xlamo = fmod(mo + nodeo + nodeo-theta - theta, twopi); + xfact = mdot + dmdt + 2.0 * (nodedot + dnodt - rptim) - no; + em = emo; + emsq = emsqo; + } + + /* ---------------- synchronous resonance terms -------------- */ + if (irez == 1) + { + g200 = 1.0 + emsq * (-2.5 + 0.8125 * emsq); + g310 = 1.0 + 2.0 * emsq; + g300 = 1.0 + emsq * (-6.0 + 6.60937 * emsq); + f220 = 0.75 * (1.0 + cosim) * (1.0 + cosim); + f311 = 0.9375 * sinim * sinim * (1.0 + 3.0 * cosim) - 0.75 * (1.0 + cosim); + f330 = 1.0 + cosim; + f330 = 1.875 * f330 * f330 * f330; + del1 = 3.0 * nm * nm * aonv * aonv; + del2 = 2.0 * del1 * f220 * g200 * q22; + del3 = 3.0 * del1 * f330 * g300 * q33 * aonv; + del1 = del1 * f311 * g310 * q31 * aonv; + xlamo = fmod(mo + nodeo + argpo - theta, twopi); + xfact = mdot + xpidot - rptim + dmdt + domdt + dnodt - no; + } + + /* ------------ for sgp4, initialize the integrator ---------- */ + xli = xlamo; + xni = no; + atime = 0.0; + nm = no + dndt; + } + +//#include "debug3.cpp" +} // end dsinit + +/*----------------------------------------------------------------------------- +* +* procedure dspace +* +* this procedure provides deep space contributions to mean elements for +* perturbing third body. these effects have been averaged over one +* revolution of the sun and moon. for earth resonance effects, the +* effects have been averaged over no revolutions of the satellite. +* (mean motion) +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* d2201, d2211, d3210, d3222, d4410, d4422, d5220, d5232, d5421, d5433 - +* dedt - +* del1, del2, del3 - +* didt - +* dmdt - +* dnodt - +* domdt - +* irez - flag for resonance 0-none, 1-one day, 2-half day +* argpo - argument of perigee +* argpdot - argument of perigee dot (rate) +* t - time +* tc - +* gsto - gst +* xfact - +* xlamo - +* no - mean motion +* atime - +* em - eccentricity +* ft - +* argpm - argument of perigee +* inclm - inclination +* xli - +* mm - mean anomaly +* xni - mean motion +* nodem - right ascension of ascending node +* +* outputs : +* atime - +* em - eccentricity +* argpm - argument of perigee +* inclm - inclination +* xli - +* mm - mean anomaly +* xni - +* nodem - right ascension of ascending node +* dndt - +* nm - mean motion +* +* locals : +* delt - +* ft - +* theta - +* x2li - +* x2omi - +* xl - +* xldot - +* xnddt - +* xndt - +* xomi - +* +* coupling : +* none - +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +static void dspace + ( + int irez, + double d2201, double d2211, double d3210, double d3222, double d4410, + double d4422, double d5220, double d5232, double d5421, double d5433, + double dedt, double del1, double del2, double del3, double didt, + double dmdt, double dnodt, double domdt, double argpo, double argpdot, + double t, double tc, double gsto, double xfact, double xlamo, + double no, + double& atime, double& em, double& argpm, double& inclm, double& xli, + double& mm, double& xni, double& nodem, double& dndt, double& nm + ) +{ + const double twopi = 2.0 * pi; + int iretn , iret; + double delt, ft, theta, x2li, x2omi, xl, xldot , xnddt, xndt, xomi, g22, g32, + g44, g52, g54, fasx2, fasx4, fasx6, rptim , step2, stepn , stepp; + + ft = 0.0; + fasx2 = 0.13130908; + fasx4 = 2.8843198; + fasx6 = 0.37448087; + g22 = 5.7686396; + g32 = 0.95240898; + g44 = 1.8014998; + g52 = 1.0508330; + g54 = 4.4108898; + rptim = 4.37526908801129966e-3; // this equates to 7.29211514668855e-5 rad/sec + stepp = 720.0; + stepn = -720.0; + step2 = 259200.0; + + /* ----------- calculate deep space resonance effects ----------- */ + dndt = 0.0; + theta = fmod(gsto + tc * rptim, twopi); + em = em + dedt * t; + + inclm = inclm + didt * t; + argpm = argpm + domdt * t; + nodem = nodem + dnodt * t; + mm = mm + dmdt * t; + + // sgp4fix for negative inclinations + // the following if statement should be commented out + // if (inclm < 0.0) + // { + // inclm = -inclm; + // argpm = argpm - pi; + // nodem = nodem + pi; + // } + + /* - update resonances : numerical (euler-maclaurin) integration - */ + /* ------------------------- epoch restart ---------------------- */ + // sgp4fix for propagator problems + // the following integration works for negative time steps and periods + // the specific changes are unknown because the original code was so convoluted + + ft = 0.0; + atime = 0.0; + if (irez != 0) + { + if ((atime == 0.0) || ((t >= 0.0) && (atime < 0.0)) || + ((t < 0.0) && (atime >= 0.0))) + { + if (t >= 0.0) + delt = stepp; + else + delt = stepn; + atime = 0.0; + xni = no; + xli = xlamo; + } + iretn = 381; // added for do loop + iret = 0; // added for loop + while (iretn == 381) + { + if ((fabs(t) < fabs(atime)) || (iret == 351)) + { + if (t >= 0.0) + delt = stepn; + else + delt = stepp; + iret = 351; + iretn = 381; + } + else + { + if (t > 0.0) // error if prev if has atime:=0.0 and t:=0.0 (ge) + delt = stepp; + else + delt = stepn; + if (fabs(t - atime) >= stepp) + { + iret = 0; + iretn = 381; + } + else + { + ft = t - atime; + iretn = 0; + } + } + + /* ------------------- dot terms calculated ------------- */ + /* ----------- near - synchronous resonance terms ------- */ + if (irez != 2) + { + xndt = del1 * sin(xli - fasx2) + del2 * sin(2.0 * (xli - fasx4)) + + del3 * sin(3.0 * (xli - fasx6)); + xldot = xni + xfact; + xnddt = del1 * cos(xli - fasx2) + + 2.0 * del2 * cos(2.0 * (xli - fasx4)) + + 3.0 * del3 * cos(3.0 * (xli - fasx6)); + xnddt = xnddt * xldot; + } + else + { + /* --------- near - half-day resonance terms -------- */ + xomi = argpo + argpdot * atime; + x2omi = xomi + xomi; + x2li = xli + xli; + xndt = d2201 * sin(x2omi + xli - g22) + d2211 * sin(xli - g22) + + d3210 * sin(xomi + xli - g32) + d3222 * sin(-xomi + xli - g32)+ + d4410 * sin(x2omi + x2li - g44)+ d4422 * sin(x2li - g44) + + d5220 * sin(xomi + xli - g52) + d5232 * sin(-xomi + xli - g52)+ + d5421 * sin(xomi + x2li - g54) + d5433 * sin(-xomi + x2li - g54); + xldot = xni + xfact; + xnddt = d2201 * cos(x2omi + xli - g22) + d2211 * cos(xli - g22) + + d3210 * cos(xomi + xli - g32) + d3222 * cos(-xomi + xli - g32) + + d5220 * cos(xomi + xli - g52) + d5232 * cos(-xomi + xli - g52) + + 2.0 * (d4410 * cos(x2omi + x2li - g44) + + d4422 * cos(x2li - g44) + d5421 * cos(xomi + x2li - g54) + + d5433 * cos(-xomi + x2li - g54)); + xnddt = xnddt * xldot; + } + + /* ----------------------- integrator ------------------- */ + if (iretn == 381) + { + xli = xli + xldot * delt + xndt * step2; + xni = xni + xndt * delt + xnddt * step2; + atime = atime + delt; + } + } // while iretn = 381 + + nm = xni + xndt * ft + xnddt * ft * ft * 0.5; + xl = xli + xldot * ft + xndt * ft * ft * 0.5; + if (irez != 1) + { + mm = xl - 2.0 * nodem + 2.0 * theta; + dndt = nm - no; + } + else + { + mm = xl - nodem - argpm + theta; + dndt = nm - no; + } + nm = no + dndt; + } + +//#include "debug4.cpp" +} // end dsspace + +/*----------------------------------------------------------------------------- +* +* procedure initl +* +* this procedure initializes the spg4 propagator. all the initialization is +* consolidated here instead of having multiple loops inside other routines. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* ecco - eccentricity 0.0 - 1.0 +* epoch - epoch time in days from jan 0, 1950. 0 hr +* inclo - inclination of satellite +* no - mean motion of satellite +* satn - satellite number +* +* outputs : +* ainv - 1.0 / a +* ao - semi major axis +* con41 - +* con42 - 1.0 - 5.0 cos(i) +* cosio - cosine of inclination +* cosio2 - cosio squared +* eccsq - eccentricity squared +* method - flag for deep space 'd', 'n' +* omeosq - 1.0 - ecco * ecco +* posq - semi-parameter squared +* rp - radius of perigee +* rteosq - square root of (1.0 - ecco*ecco) +* sinio - sine of inclination +* gsto - gst at time of observation rad +* no - mean motion of satellite +* +* locals : +* ak - +* d1 - +* del - +* adel - +* po - +* +* coupling : +* getgravconst +* gstime - find greenwich sidereal time from the julian date +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +static void initl + ( + int satn, gravconsttype whichconst, + double ecco, double epoch, double inclo, double& no, + char& method, + double& ainv, double& ao, double& con41, double& con42, double& cosio, + double& cosio2,double& eccsq, double& omeosq, double& posq, + double& rp, double& rteosq,double& sinio , double& gsto + ) +{ + /* --------------------- local variables ------------------------ */ + double ak, d1, del, adel, po, x2o3, j2, xke, + tumin, mu, radiusearthkm, j3, j4, j3oj2; + + // sgp4fix use old way of finding gst + int ids70; + double ts70, ds70, tfrac, c1, thgr70, fk5r, c1p2p; + const double twopi = 2.0 * pi; + + /* ----------------------- earth constants ---------------------- */ + // sgp4fix identify constants and allow alternate values + getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); + x2o3 = 2.0 / 3.0; + + /* ------------- calculate auxillary epoch quantities ---------- */ + eccsq = ecco * ecco; + omeosq = 1.0 - eccsq; + rteosq = sqrt(omeosq); + cosio = cos(inclo); + cosio2 = cosio * cosio; + + /* ------------------ un-kozai the mean motion ----------------- */ + ak = pow(xke / no, x2o3); + d1 = 0.75 * j2 * (3.0 * cosio2 - 1.0) / (rteosq * omeosq); + del = d1 / (ak * ak); + adel = ak * (1.0 - del * del - del * + (1.0 / 3.0 + 134.0 * del * del / 81.0)); + del = d1/(adel * adel); + no = no / (1.0 + del); + + ao = pow(xke / no, x2o3); + sinio = sin(inclo); + po = ao * omeosq; + con42 = 1.0 - 5.0 * cosio2; + con41 = -con42-cosio2-cosio2; + ainv = 1.0 / ao; + posq = po * po; + rp = ao * (1.0 - ecco); + method = 'n'; + + // sgp4fix modern approach to finding sidereal timew + // gsto = gstime(epoch + 2433281.5); + + // sgp4fix use old way of finding gst + // count integer number of days from 0 jan 1970 + ts70 = epoch - 7305.0; + ids70 = floor(ts70 + 1.0e-8); + ds70 = ids70; + tfrac = ts70 - ds70; + // find greenwich location at epoch + c1 = 1.72027916940703639e-2; + thgr70= 1.7321343856509374; + fk5r = 5.07551419432269442e-15; + c1p2p = c1 + twopi; + gsto = fmod( thgr70 + c1*ds70 + c1p2p*tfrac + ts70*ts70*fk5r, twopi); + if ( gsto < 0.0 ) + gsto = gsto + twopi; + +//#include "debug5.cpp" +} // end initl + +/*----------------------------------------------------------------------------- +* +* procedure sgp4init +* +* this procedure initializes variables for sgp4. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* satn - satellite number +* bstar - sgp4 type drag coefficient kg/m2er +* ecco - eccentricity +* epoch - epoch time in days from jan 0, 1950. 0 hr +* argpo - argument of perigee (output if ds) +* inclo - inclination +* mo - mean anomaly (output if ds) +* no - mean motion +* nodeo - right ascension of ascending node +* +* outputs : +* satrec - common values for subsequent calls +* return code - non-zero on error. +* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er +* 2 - mean motion less than 0.0 +* 3 - pert elements, ecc < 0.0 or ecc > 1.0 +* 4 - semi-latus rectum < 0.0 +* 5 - epoch elements are sub-orbital +* 6 - satellite has decayed +* +* locals : +* cnodm , snodm , cosim , sinim , cosomm , sinomm +* cc1sq , cc2 , cc3 +* coef , coef1 +* cosio4 - +* day - +* dndt - +* em - eccentricity +* emsq - eccentricity squared +* eeta - +* etasq - +* gam - +* argpm - argument of perigee +* nodem - +* inclm - inclination +* mm - mean anomaly +* nm - mean motion +* perige - perigee +* pinvsq - +* psisq - +* qzms24 - +* rtemsq - +* s1, s2, s3, s4, s5, s6, s7 - +* sfour - +* ss1, ss2, ss3, ss4, ss5, ss6, ss7 - +* sz1, sz2, sz3 +* sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33 - +* tc - +* temp - +* temp1, temp2, temp3 - +* tsi - +* xpidot - +* xhdot1 - +* z1, z2, z3 - +* z11, z12, z13, z21, z22, z23, z31, z32, z33 - +* +* coupling : +* getgravconst- +* initl - +* dscom - +* dpper - +* dsinit - +* sgp4 - +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +int sgp4init + ( + gravconsttype whichconst, const int satn, const double epoch, + const double xbstar, const double xecco, const double xargpo, + const double xinclo, const double xmo, const double xno, + const double xnodeo, elsetrec& satrec + ) +{ + /* --------------------- local variables ------------------------ */ + double ao, ainv, con42, cosio, sinio, cosio2, eccsq, + omeosq, posq, rp, rteosq, + cnodm , snodm , cosim , sinim , cosomm, sinomm, cc1sq , + cc2 , cc3 , coef , coef1 , cosio4, day , dndt , + em , emsq , eeta , etasq , gam , argpm , nodem , + inclm , mm , nm , perige, pinvsq, psisq , qzms24, + rtemsq, s1 , s2 , s3 , s4 , s5 , s6 , + s7 , sfour , ss1 = 0 , ss2 = 0 , ss3 = 0 , ss4 = 0 , ss5 = 0 , + ss6 = 0 , ss7 = 0 , sz1 = 0 , sz2 = 0 , sz3 = 0 , sz11 = 0 , sz12 = 0 , + sz13 = 0 , sz21 = 0 , sz22 = 0 , sz23 = 0 , sz31 = 0 , sz32 = 0 , sz33 = 0 , + tc , temp , temp1 , temp2 , temp3 , tsi , xpidot, + xhdot1, z1 , z2 , z3 , z11 , z12 , z13 , + z21 , z22 , z23 , z31 , z32 , z33, + qzms2t, ss, j2, j3oj2, j4, x2o3, r[3], v[3], + tumin, mu, radiusearthkm, xke, j3; + + /* ------------------------ initialization --------------------- */ + // sgp4fix divisor for divide by zero check on inclination + const double temp4 = 1.0 + cos(pi-1.0e-9); + + /* ----------- set all near earth variables to zero ------------ */ + satrec.isimp = 0; satrec.method = 'n'; satrec.aycof = 0.0; + satrec.con41 = 0.0; satrec.cc1 = 0.0; satrec.cc4 = 0.0; + satrec.cc5 = 0.0; satrec.d2 = 0.0; satrec.d3 = 0.0; + satrec.d4 = 0.0; satrec.delmo = 0.0; satrec.eta = 0.0; + satrec.argpdot = 0.0; satrec.omgcof = 0.0; satrec.sinmao = 0.0; + satrec.t = 0.0; satrec.t2cof = 0.0; satrec.t3cof = 0.0; + satrec.t4cof = 0.0; satrec.t5cof = 0.0; satrec.x1mth2 = 0.0; + satrec.x7thm1 = 0.0; satrec.mdot = 0.0; satrec.nodedot = 0.0; + satrec.xlcof = 0.0; satrec.xmcof = 0.0; satrec.nodecf = 0.0; + + /* ----------- set all deep space variables to zero ------------ */ + satrec.irez = 0; satrec.d2201 = 0.0; satrec.d2211 = 0.0; + satrec.d3210 = 0.0; satrec.d3222 = 0.0; satrec.d4410 = 0.0; + satrec.d4422 = 0.0; satrec.d5220 = 0.0; satrec.d5232 = 0.0; + satrec.d5421 = 0.0; satrec.d5433 = 0.0; satrec.dedt = 0.0; + satrec.del1 = 0.0; satrec.del2 = 0.0; satrec.del3 = 0.0; + satrec.didt = 0.0; satrec.dmdt = 0.0; satrec.dnodt = 0.0; + satrec.domdt = 0.0; satrec.e3 = 0.0; satrec.ee2 = 0.0; + satrec.peo = 0.0; satrec.pgho = 0.0; satrec.pho = 0.0; + satrec.pinco = 0.0; satrec.plo = 0.0; satrec.se2 = 0.0; + satrec.se3 = 0.0; satrec.sgh2 = 0.0; satrec.sgh3 = 0.0; + satrec.sgh4 = 0.0; satrec.sh2 = 0.0; satrec.sh3 = 0.0; + satrec.si2 = 0.0; satrec.si3 = 0.0; satrec.sl2 = 0.0; + satrec.sl3 = 0.0; satrec.sl4 = 0.0; satrec.gsto = 0.0; + satrec.xfact = 0.0; satrec.xgh2 = 0.0; satrec.xgh3 = 0.0; + satrec.xgh4 = 0.0; satrec.xh2 = 0.0; satrec.xh3 = 0.0; + satrec.xi2 = 0.0; satrec.xi3 = 0.0; satrec.xl2 = 0.0; + satrec.xl3 = 0.0; satrec.xl4 = 0.0; satrec.xlamo = 0.0; + satrec.zmol = 0.0; satrec.zmos = 0.0; satrec.atime = 0.0; + satrec.xli = 0.0; satrec.xni = 0.0; + + // sgp4fix - note the following variables are also passed directly via satrec. + // it is possible to streamline the sgp4init call by deleting the "x" + // variables, but the user would need to set the satrec.* values first. we + // include the additional assignments in case twoline2rv is not used. + satrec.bstar = xbstar; + satrec.ecco = xecco; + satrec.argpo = xargpo; + satrec.inclo = xinclo; + satrec.mo = xmo; + satrec.no = xno; + satrec.nodeo = xnodeo; + + /* ------------------------ earth constants ----------------------- */ + // sgp4fix identify constants and allow alternate values + getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); + ss = 78.0 / radiusearthkm + 1.0; + qzms2t = pow(((120.0 - 78.0) / radiusearthkm), 4); + x2o3 = 2.0 / 3.0; + + satrec.init = 'y'; + satrec.t = 0.0; + + initl + ( + satn, whichconst, satrec.ecco, epoch, satrec.inclo, satrec.no, satrec.method, + ainv, ao, satrec.con41, con42, cosio, cosio2, eccsq, omeosq, + posq, rp, rteosq, sinio, satrec.gsto + ); + satrec.error = 0; + + if (rp < 1.0) + { +// printf("# *** satn%d epoch elts sub-orbital ***\n", satn); + satrec.error = 5; + } + + if ((omeosq >= 0.0 ) || ( satrec.no >= 0.0)) + { + satrec.isimp = 0; + if (rp < (220.0 / radiusearthkm + 1.0)) + satrec.isimp = 1; + sfour = ss; + qzms24 = qzms2t; + perige = (rp - 1.0) * radiusearthkm; + + /* - for perigees below 156 km, s and qoms2t are altered - */ + if (perige < 156.0) + { + sfour = perige - 78.0; + if (perige < 98.0) + sfour = 20.0; + qzms24 = pow(((120.0 - sfour) / radiusearthkm), 4.0); + sfour = sfour / radiusearthkm + 1.0; + } + pinvsq = 1.0 / posq; + + tsi = 1.0 / (ao - sfour); + satrec.eta = ao * satrec.ecco * tsi; + etasq = satrec.eta * satrec.eta; + eeta = satrec.ecco * satrec.eta; + psisq = fabs(1.0 - etasq); + coef = qzms24 * pow(tsi, 4.0); + coef1 = coef / pow(psisq, 3.5); + cc2 = coef1 * satrec.no * (ao * (1.0 + 1.5 * etasq + eeta * + (4.0 + etasq)) + 0.375 * j2 * tsi / psisq * satrec.con41 * + (8.0 + 3.0 * etasq * (8.0 + etasq))); + satrec.cc1 = satrec.bstar * cc2; + cc3 = 0.0; + if (satrec.ecco > 1.0e-4) + cc3 = -2.0 * coef * tsi * j3oj2 * satrec.no * sinio / satrec.ecco; + satrec.x1mth2 = 1.0 - cosio2; + satrec.cc4 = 2.0* satrec.no * coef1 * ao * omeosq * + (satrec.eta * (2.0 + 0.5 * etasq) + satrec.ecco * + (0.5 + 2.0 * etasq) - j2 * tsi / (ao * psisq) * + (-3.0 * satrec.con41 * (1.0 - 2.0 * eeta + etasq * + (1.5 - 0.5 * eeta)) + 0.75 * satrec.x1mth2 * + (2.0 * etasq - eeta * (1.0 + etasq)) * cos(2.0 * satrec.argpo))); + satrec.cc5 = 2.0 * coef1 * ao * omeosq * (1.0 + 2.75 * + (etasq + eeta) + eeta * etasq); + cosio4 = cosio2 * cosio2; + temp1 = 1.5 * j2 * pinvsq * satrec.no; + temp2 = 0.5 * temp1 * j2 * pinvsq; + temp3 = -0.46875 * j4 * pinvsq * pinvsq * satrec.no; + satrec.mdot = satrec.no + 0.5 * temp1 * rteosq * satrec.con41 + 0.0625 * + temp2 * rteosq * (13.0 - 78.0 * cosio2 + 137.0 * cosio4); + satrec.argpdot = -0.5 * temp1 * con42 + 0.0625 * temp2 * + (7.0 - 114.0 * cosio2 + 395.0 * cosio4) + + temp3 * (3.0 - 36.0 * cosio2 + 49.0 * cosio4); + xhdot1 = -temp1 * cosio; + satrec.nodedot = xhdot1 + (0.5 * temp2 * (4.0 - 19.0 * cosio2) + + 2.0 * temp3 * (3.0 - 7.0 * cosio2)) * cosio; + xpidot = satrec.argpdot+ satrec.nodedot; + satrec.omgcof = satrec.bstar * cc3 * cos(satrec.argpo); + satrec.xmcof = 0.0; + if (satrec.ecco > 1.0e-4) + satrec.xmcof = -x2o3 * coef * satrec.bstar / eeta; + satrec.nodecf = 3.5 * omeosq * xhdot1 * satrec.cc1; + satrec.t2cof = 1.5 * satrec.cc1; + // sgp4fix for divide by zero with xinco = 180 deg + if (fabs(cosio+1.0) > 1.5e-12) + satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / (1.0 + cosio); + else + satrec.xlcof = -0.25 * j3oj2 * sinio * (3.0 + 5.0 * cosio) / temp4; + satrec.aycof = -0.5 * j3oj2 * sinio; + satrec.delmo = pow((1.0 + satrec.eta * cos(satrec.mo)), 3); + satrec.sinmao = sin(satrec.mo); + satrec.x7thm1 = 7.0 * cosio2 - 1.0; + + /* --------------- deep space initialization ------------- */ + if ((2*pi / satrec.no) >= 225.0) + { + satrec.method = 'd'; + satrec.isimp = 1; + tc = 0.0; + inclm = satrec.inclo; + + dscom + ( + epoch, satrec.ecco, satrec.argpo, tc, satrec.inclo, satrec.nodeo, + satrec.no, snodm, cnodm, sinim, cosim,sinomm, cosomm, + day, satrec.e3, satrec.ee2, em, emsq, gam, + satrec.peo, satrec.pgho, satrec.pho, satrec.pinco, + satrec.plo, rtemsq, satrec.se2, satrec.se3, + satrec.sgh2, satrec.sgh3, satrec.sgh4, + satrec.sh2, satrec.sh3, satrec.si2, satrec.si3, + satrec.sl2, satrec.sl3, satrec.sl4, s1, s2, s3, s4, s5, + s6, s7, ss1, ss2, ss3, ss4, ss5, ss6, ss7, sz1, sz2, sz3, + sz11, sz12, sz13, sz21, sz22, sz23, sz31, sz32, sz33, + satrec.xgh2, satrec.xgh3, satrec.xgh4, satrec.xh2, + satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2, + satrec.xl3, satrec.xl4, nm, z1, z2, z3, z11, + z12, z13, z21, z22, z23, z31, z32, z33, + satrec.zmol, satrec.zmos + ); + dpper + ( + satrec.e3, satrec.ee2, satrec.peo, satrec.pgho, + satrec.pho, satrec.pinco, satrec.plo, satrec.se2, + satrec.se3, satrec.sgh2, satrec.sgh3, satrec.sgh4, + satrec.sh2, satrec.sh3, satrec.si2, satrec.si3, + satrec.sl2, satrec.sl3, satrec.sl4, satrec.t, + satrec.xgh2,satrec.xgh3,satrec.xgh4, satrec.xh2, + satrec.xh3, satrec.xi2, satrec.xi3, satrec.xl2, + satrec.xl3, satrec.xl4, satrec.zmol, satrec.zmos, inclm, satrec.init, + satrec.ecco, satrec.inclo, satrec.nodeo, satrec.argpo, satrec.mo + ); + + argpm = 0.0; + nodem = 0.0; + mm = 0.0; + + dsinit + ( + whichconst, + cosim, emsq, satrec.argpo, s1, s2, s3, s4, s5, sinim, ss1, ss2, ss3, ss4, + ss5, sz1, sz3, sz11, sz13, sz21, sz23, sz31, sz33, satrec.t, tc, + satrec.gsto, satrec.mo, satrec.mdot, satrec.no, satrec.nodeo, + satrec.nodedot, xpidot, z1, z3, z11, z13, z21, z23, z31, z33, + satrec.ecco, eccsq, em, argpm, inclm, mm, nm, nodem, + satrec.irez, satrec.atime, + satrec.d2201, satrec.d2211, satrec.d3210, satrec.d3222 , + satrec.d4410, satrec.d4422, satrec.d5220, satrec.d5232, + satrec.d5421, satrec.d5433, satrec.dedt, satrec.didt, + satrec.dmdt, dndt, satrec.dnodt, satrec.domdt , + satrec.del1, satrec.del2, satrec.del3, satrec.xfact, + satrec.xlamo, satrec.xli, satrec.xni + ); + } + + /* ----------- set variables if not deep space ----------- */ + if (satrec.isimp != 1) + { + cc1sq = satrec.cc1 * satrec.cc1; + satrec.d2 = 4.0 * ao * tsi * cc1sq; + temp = satrec.d2 * tsi * satrec.cc1 / 3.0; + satrec.d3 = (17.0 * ao + sfour) * temp; + satrec.d4 = 0.5 * temp * ao * tsi * (221.0 * ao + 31.0 * sfour) * + satrec.cc1; + satrec.t3cof = satrec.d2 + 2.0 * cc1sq; + satrec.t4cof = 0.25 * (3.0 * satrec.d3 + satrec.cc1 * + (12.0 * satrec.d2 + 10.0 * cc1sq)); + satrec.t5cof = 0.2 * (3.0 * satrec.d4 + + 12.0 * satrec.cc1 * satrec.d3 + + 6.0 * satrec.d2 * satrec.d2 + + 15.0 * cc1sq * (2.0 * satrec.d2 + cc1sq)); + } + } // if omeosq = 0 ... + + /* finally propogate to zero epoch to initialise all others. */ + if(satrec.error == 0) + sgp4(whichconst, satrec, 0.0, r, v); + + satrec.init = 'n'; + +//#include "debug6.cpp" + return satrec.error; +} // end sgp4init + +/*----------------------------------------------------------------------------- +* +* procedure sgp4 +* +* this procedure is the sgp4 prediction model from space command. this is an +* updated and combined version of sgp4 and sdp4, which were originally +* published separately in spacetrack report #3. this version follows the +* methodology from the aiaa paper (2006) describing the history and +* development of the code. +* +* author : david vallado 719-573-2600 28 jun 2005 +* +* inputs : +* satrec - initialised structure from sgp4init() call. +* tsince - time eince epoch (minutes) +* +* outputs : +* r - position vector km +* v - velocity km/sec +* return code - non-zero on error. +* 1 - mean elements, ecc >= 1.0 or ecc < -0.001 or a < 0.95 er +* 2 - mean motion less than 0.0 +* 3 - pert elements, ecc < 0.0 or ecc > 1.0 +* 4 - semi-latus rectum < 0.0 +* 5 - epoch elements are sub-orbital +* 6 - satellite has decayed +* +* locals : +* am - +* axnl, aynl - +* betal - +* cosim , sinim , cosomm , sinomm , cnod , snod , cos2u , +* sin2u , coseo1 , sineo1 , cosi , sini , cosip , sinip , +* cosisq , cossu , sinsu , cosu , sinu +* delm - +* delomg - +* dndt - +* eccm - +* emsq - +* ecose - +* el2 - +* eo1 - +* eccp - +* esine - +* argpm - +* argpp - +* omgadf - +* pl - +* r - +* rtemsq - +* rdotl - +* rl - +* rvdot - +* rvdotl - +* su - +* t2 , t3 , t4 , tc +* tem5, temp , temp1 , temp2 , tempa , tempe , templ +* u , ux , uy , uz , vx , vy , vz +* inclm - inclination +* mm - mean anomaly +* nm - mean motion +* nodem - right asc of ascending node +* xinc - +* xincp - +* xl - +* xlm - +* mp - +* xmdf - +* xmx - +* xmy - +* nodedf - +* xnode - +* nodep - +* np - +* +* coupling : +* getgravconst- +* dpper +* dpspace +* +* references : +* hoots, roehrich, norad spacetrack report #3 1980 +* hoots, norad spacetrack report #6 1986 +* hoots, schumacher and glover 2004 +* vallado, crawford, hujsak, kelso 2006 + ----------------------------------------------------------------------------*/ + +int sgp4 + ( + gravconsttype whichconst, elsetrec& satrec, double tsince, + double r[3], double v[3] + ) +{ + double am , axnl , aynl , betal , cosim , cnod , + cos2u, coseo1, cosi , cosip , cosisq, cossu , cosu, + delm , delomg, em , emsq , ecose , el2 , eo1 , + ep , esine , argpm, argpp , argpdf, pl, mrt = 0.0, + mvt , rdotl , rl , rvdot , rvdotl, sinim , + sin2u, sineo1, sini , sinip , sinsu , sinu , + snod , su , t2 , t3 , t4 , tem5 , temp, + temp1, temp2 , tempa, tempe , templ , u , ux , + uy , uz , vx , vy , vz , inclm , mm , + nm , nodem, xinc , xincp , xl , xlm , mp , + xmdf , xmx , xmy , nodedf, xnode , nodep, tc , dndt, + twopi, x2o3 , j2 , j3 , tumin, j4 , xke , j3oj2, radiusearthkm, + mu, vkmpersec; + int ktr; + + /* ------------------ set mathematical constants --------------- */ + // sgp4fix divisor for divide by zero check on inclination + const double temp4 = 1.0 + cos(pi-1.0e-9); + twopi = 2.0 * pi; + x2o3 = 2.0 / 3.0; + // sgp4fix identify constants and allow alternate values + getgravconst( whichconst, tumin, mu, radiusearthkm, xke, j2, j3, j4, j3oj2 ); + vkmpersec = radiusearthkm * xke/60.0; + + /* --------------------- clear sgp4 error flag ----------------- */ + satrec.t = tsince; + satrec.error = 0; + + /* ------- update for secular gravity and atmospheric drag ----- */ + xmdf = satrec.mo + satrec.mdot * satrec.t; + argpdf = satrec.argpo + satrec.argpdot * satrec.t; + nodedf = satrec.nodeo + satrec.nodedot * satrec.t; + argpm = argpdf; + mm = xmdf; + t2 = satrec.t * satrec.t; + nodem = nodedf + satrec.nodecf * t2; + tempa = 1.0 - satrec.cc1 * satrec.t; + tempe = satrec.bstar * satrec.cc4 * satrec.t; + templ = satrec.t2cof * t2; + + if (satrec.isimp != 1) + { + delomg = satrec.omgcof * satrec.t; + delm = satrec.xmcof * + (pow((1.0 + satrec.eta * cos(xmdf)), 3) - + satrec.delmo); + temp = delomg + delm; + mm = xmdf + temp; + argpm = argpdf - temp; + t3 = t2 * satrec.t; + t4 = t3 * satrec.t; + tempa = tempa - satrec.d2 * t2 - satrec.d3 * t3 - + satrec.d4 * t4; + tempe = tempe + satrec.bstar * satrec.cc5 * (sin(mm) - + satrec.sinmao); + templ = templ + satrec.t3cof * t3 + t4 * (satrec.t4cof + + satrec.t * satrec.t5cof); + } + + nm = satrec.no; + em = satrec.ecco; + inclm = satrec.inclo; + if (satrec.method == 'd') + { + tc = satrec.t; + dspace + ( + satrec.irez, + satrec.d2201, satrec.d2211, satrec.d3210, + satrec.d3222, satrec.d4410, satrec.d4422, + satrec.d5220, satrec.d5232, satrec.d5421, + satrec.d5433, satrec.dedt, satrec.del1, + satrec.del2, satrec.del3, satrec.didt, + satrec.dmdt, satrec.dnodt, satrec.domdt, + satrec.argpo, satrec.argpdot, satrec.t, tc, + satrec.gsto, satrec.xfact, satrec.xlamo, + satrec.no, satrec.atime, + em, argpm, inclm, satrec.xli, mm, satrec.xni, + nodem, dndt, nm + ); + } // if method = d + + if (nm <= 0.0) + { +// printf("# error nm %f\n", nm); + satrec.error = 2; + } + am = pow((xke / nm),x2o3) * tempa * tempa; + nm = xke / pow(am, 1.5); + em = em - tempe; + + // fix tolerance for error recognition + if ((em >= 1.0) || (em < -0.001) || (am < 0.95)) + { +// printf("# error em %f\n", em); + satrec.error = 1; + } + if (em < 0.0) + em = 1.0e-6; + mm = mm + satrec.no * templ; + xlm = mm + argpm + nodem; + emsq = em * em; + temp = 1.0 - emsq; + + nodem = fmod(nodem, twopi); + argpm = fmod(argpm, twopi); + xlm = fmod(xlm, twopi); + mm = fmod(xlm - argpm - nodem, twopi); + + /* ----------------- compute extra mean quantities ------------- */ + sinim = sin(inclm); + cosim = cos(inclm); + + /* -------------------- add lunar-solar periodics -------------- */ + ep = em; + xincp = inclm; + argpp = argpm; + nodep = nodem; + mp = mm; + sinip = sinim; + cosip = cosim; + if (satrec.method == 'd') + { + dpper + ( + satrec.e3, satrec.ee2, satrec.peo, + satrec.pgho, satrec.pho, satrec.pinco, + satrec.plo, satrec.se2, satrec.se3, + satrec.sgh2, satrec.sgh3, satrec.sgh4, + satrec.sh2, satrec.sh3, satrec.si2, + satrec.si3, satrec.sl2, satrec.sl3, + satrec.sl4, satrec.t, satrec.xgh2, + satrec.xgh3, satrec.xgh4, satrec.xh2, + satrec.xh3, satrec.xi2, satrec.xi3, + satrec.xl2, satrec.xl3, satrec.xl4, + satrec.zmol, satrec.zmos, satrec.inclo, + 'n', ep, xincp, nodep, argpp, mp + ); + if (xincp < 0.0) + { + xincp = -xincp; + nodep = nodep + pi; + argpp = argpp - pi; + } + if ((ep < 0.0 ) || ( ep > 1.0)) + { + // printf("# error ep %f\n", ep); + satrec.error = 3; + } + } // if method = d + + /* -------------------- long period periodics ------------------ */ + if (satrec.method == 'd') + { + sinip = sin(xincp); + cosip = cos(xincp); + satrec.aycof = -0.5*j3oj2*sinip; + // sgp4fix for divide by zero for xincp = 180 deg + if (fabs(cosip+1.0) > 1.5e-12) + satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / (1.0 + cosip); + else + satrec.xlcof = -0.25 * j3oj2 * sinip * (3.0 + 5.0 * cosip) / temp4; + } + axnl = ep * cos(argpp); + temp = 1.0 / (am * (1.0 - ep * ep)); + aynl = ep* sin(argpp) + temp * satrec.aycof; + xl = mp + argpp + nodep + temp * satrec.xlcof * axnl; + + /* --------------------- solve kepler's equation --------------- */ + u = fmod(xl - nodep, twopi); + eo1 = u; + tem5 = 9999.9; + ktr = 1; + // sgp4fix for kepler iteration + // the following iteration needs better limits on corrections + while (( fabs(tem5) >= 1.0e-12) && (ktr <= 10) ) + { + sineo1 = sin(eo1); + coseo1 = cos(eo1); + tem5 = 1.0 - coseo1 * axnl - sineo1 * aynl; + tem5 = (u - aynl * coseo1 + axnl * sineo1 - eo1) / tem5; + if(fabs(tem5) >= 0.95) + tem5 = tem5 > 0.0 ? 0.95 : -0.95; + eo1 = eo1 + tem5; + ktr = ktr + 1; + } + + /* ------------- short period preliminary quantities ----------- */ + ecose = axnl*coseo1 + aynl*sineo1; + esine = axnl*sineo1 - aynl*coseo1; + el2 = axnl*axnl + aynl*aynl; + pl = am*(1.0-el2); + if (pl < 0.0) + { +// printf("# error pl %f\n", pl); + satrec.error = 4; + } + else + { + rl = am * (1.0 - ecose); + rdotl = sqrt(am) * esine/rl; + rvdotl = sqrt(pl) / rl; + betal = sqrt(1.0 - el2); + temp = esine / (1.0 + betal); + sinu = am / rl * (sineo1 - aynl - axnl * temp); + cosu = am / rl * (coseo1 - axnl + aynl * temp); + su = atan2(sinu, cosu); + sin2u = (cosu + cosu) * sinu; + cos2u = 1.0 - 2.0 * sinu * sinu; + temp = 1.0 / pl; + temp1 = 0.5 * j2 * temp; + temp2 = temp1 * temp; + + /* -------------- update for short period periodics ------------ */ + if (satrec.method == 'd') + { + cosisq = cosip * cosip; + satrec.con41 = 3.0*cosisq - 1.0; + satrec.x1mth2 = 1.0 - cosisq; + satrec.x7thm1 = 7.0*cosisq - 1.0; + } + mrt = rl * (1.0 - 1.5 * temp2 * betal * satrec.con41) + + 0.5 * temp1 * satrec.x1mth2 * cos2u; + su = su - 0.25 * temp2 * satrec.x7thm1 * sin2u; + xnode = nodep + 1.5 * temp2 * cosip * sin2u; + xinc = xincp + 1.5 * temp2 * cosip * sinip * cos2u; + mvt = rdotl - nm * temp1 * satrec.x1mth2 * sin2u / xke; + rvdot = rvdotl + nm * temp1 * (satrec.x1mth2 * cos2u + + 1.5 * satrec.con41) / xke; + + /* --------------------- orientation vectors ------------------- */ + sinsu = sin(su); + cossu = cos(su); + snod = sin(xnode); + cnod = cos(xnode); + sini = sin(xinc); + cosi = cos(xinc); + xmx = -snod * cosi; + xmy = cnod * cosi; + ux = xmx * sinsu + cnod * cossu; + uy = xmy * sinsu + snod * cossu; + uz = sini * sinsu; + vx = xmx * cossu - cnod * sinsu; + vy = xmy * cossu - snod * sinsu; + vz = sini * cossu; + + /* --------- position and velocity (in km and km/sec) ---------- */ + r[0] = (mrt * ux)* radiusearthkm; + r[1] = (mrt * uy)* radiusearthkm; + r[2] = (mrt * uz)* radiusearthkm; + v[0] = (mvt * ux + rvdot * vx) * vkmpersec; + v[1] = (mvt * uy + rvdot * vy) * vkmpersec; + v[2] = (mvt * uz + rvdot * vz) * vkmpersec; + } // if pl > 0 + + // sgp4fix for decaying satellites + if (mrt < 1.0) + { +// printf("# decay condition %11.6f \n",mrt); + satrec.error = 6; + } + + +//#include "debug7.cpp" + return satrec.error; +} // end sgp4 + + +/* ----------------------------------------------------------------------------- +* +* function gstime +* +* this function finds the greenwich sidereal time. +* +* author : david vallado 719-573-2600 1 mar 2001 +* +* inputs description range / units +* jdut1 - julian date in ut1 days from 4713 bc +* +* outputs : +* gstime - greenwich sidereal time 0 to 2pi rad +* +* locals : +* temp - temporary variable for doubles rad +* tut1 - julian centuries from the +* jan 1, 2000 12 h epoch (ut1) +* +* coupling : +* none +* +* references : +* vallado 2004, 191, eq 3-45 +* --------------------------------------------------------------------------- */ + +double gstime + ( + double jdut1 + ) + { + const double twopi = 2.0 * pi; + const double deg2rad = pi / 180.0; + double temp, tut1; + + tut1 = (jdut1 - 2451545.0) / 36525.0; + temp = -6.2e-6* tut1 * tut1 * tut1 + 0.093104 * tut1 * tut1 + + (876600.0*3600 + 8640184.812866) * tut1 + 67310.54841; // sec + temp = fmod(temp * deg2rad / 240.0, twopi); //360/86400 = 1/240, to deg, to rad + + // ------------------------ check quadrants --------------------- + if (temp < 0.0) + temp += twopi; + + return temp; + } // end gstime + +/* ----------------------------------------------------------------------------- +* +* function getgravconst +* +* this function gets constants for the propagator. note that mu is identified to +* facilitiate comparisons with newer models. the common useage is wgs72. +* +* author : david vallado 719-573-2600 21 jul 2006 +* +* inputs : +* whichconst - which set of constants to use wgs72old, wgs72, wgs84 +* +* outputs : +* tumin - minutes in one time unit +* mu - earth gravitational parameter +* radiusearthkm - radius of the earth in km +* xke - reciprocal of tumin +* j2, j3, j4 - un-normalized zonal harmonic values +* j3oj2 - j3 divided by j2 +* +* locals : +* +* coupling : +* none +* +* references : +* norad spacetrack report #3 +* vallado, crawford, hujsak, kelso 2006 + --------------------------------------------------------------------------- */ + +void getgravconst + ( + gravconsttype whichconst, + double& tumin, + double& mu, + double& radiusearthkm, + double& xke, + double& j2, + double& j3, + double& j4, + double& j3oj2 + ) + { + + switch (whichconst) + { + // -- wgs-72 low precision str#3 constants -- + case wgs72old: + mu = 398600.79964; // in km3 / s2 + radiusearthkm = 6378.135; // km + xke = 0.0743669161; + tumin = 1.0 / xke; + j2 = 0.001082616; + j3 = -0.00000253881; + j4 = -0.00000165597; + j3oj2 = j3 / j2; + break; + // ------------ wgs-72 constants ------------ + case wgs72: + mu = 398600.8; // in km3 / s2 + radiusearthkm = 6378.135; // km + xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu); + tumin = 1.0 / xke; + j2 = 0.001082616; + j3 = -0.00000253881; + j4 = -0.00000165597; + j3oj2 = j3 / j2; + break; + case wgs84: + // ------------ wgs-84 constants ------------ + mu = 398600.5; // in km3 / s2 + radiusearthkm = 6378.137; // km + xke = 60.0 / sqrt(radiusearthkm*radiusearthkm*radiusearthkm/mu); + tumin = 1.0 / xke; + j2 = 0.00108262998905; + j3 = -0.00000253215306; + j4 = -0.00000161098761; + j3oj2 = j3 / j2; + break; + default: + fprintf(stderr,"unknown gravity option (%d)\n",whichconst); + break; + } + + } // end getgravconst + + + + + diff --git a/contrib/sgp4/sgp4unit.h b/contrib/sgp4/sgp4unit.h index 48c113d7..f16acbd9 100644 --- a/contrib/sgp4/sgp4unit.h +++ b/contrib/sgp4/sgp4unit.h @@ -1,117 +1,117 @@ -#ifndef _sgp4unit_ -#define _sgp4unit_ -/* ---------------------------------------------------------------- -* -* sgp4unit.h -* -* this file contains the sgp4 procedures for analytical propagation -* of a satellite. the code was originally released in the 1980 and 1986 -* spacetrack papers. a detailed discussion of the theory and history -* may be found in the 2006 aiaa paper by vallado, crawford, hujsak, -* and kelso. -* -* companion code for -* fundamentals of astrodynamics and applications -* 2007 -* by david vallado -* -* (w) 719-573-2600, email dvallado@agi.com -* -* current : -* 20 apr 07 david vallado -* misc fixes for constants -* changes : -* 11 aug 06 david vallado -* chg lyddane choice back to strn3, constants, misc doc -* 15 dec 05 david vallado -* misc fixes -* 26 jul 05 david vallado -* fixes for paper -* note that each fix is preceded by a -* comment with "sgp4fix" and an explanation of -* what was changed -* 10 aug 04 david vallado -* 2nd printing baseline working -* 14 may 01 david vallado -* 2nd edition baseline -* 80 norad -* original baseline -* ---------------------------------------------------------------- */ - -#include -#include - -// -------------------------- structure declarations ---------------------------- -typedef enum -{ - wgs72old, - wgs72, - wgs84 -} gravconsttype; - -typedef struct elsetrec -{ - long int satnum; - int epochyr, epochtynumrev; - int error; - char init, method; - - /* Near Earth */ - int isimp; - double aycof , con41 , cc1 , cc4 , cc5 , d2 , d3 , d4 , - delmo , eta , argpdot, omgcof , sinmao , t , t2cof, t3cof , - t4cof , t5cof , x1mth2 , x7thm1 , mdot , nodedot, xlcof , xmcof , - nodecf; - - /* Deep Space */ - int irez; - double d2201 , d2211 , d3210 , d3222 , d4410 , d4422 , d5220 , d5232 , - d5421 , d5433 , dedt , del1 , del2 , del3 , didt , dmdt , - dnodt , domdt , e3 , ee2 , peo , pgho , pho , pinco , - plo , se2 , se3 , sgh2 , sgh3 , sgh4 , sh2 , sh3 , - si2 , si3 , sl2 , sl3 , sl4 , gsto , xfact , xgh2 , - xgh3 , xgh4 , xh2 , xh3 , xi2 , xi3 , xl2 , xl3 , - xl4 , xlamo , zmol , zmos , atime , xli , xni; - - double a , altp , alta , epochdays, jdsatepoch , nddot , ndot , - bstar , rcse , inclo , nodeo , ecco , argpo , mo , - no; -} elsetrec; - -// --------------------------- function declarations ---------------------------- -int sgp4init - ( - gravconsttype whichconst, const int satn, const double epoch, - const double xbstar, const double xecco, const double xargpo, - const double xinclo, const double xmo, const double xno, - const double xnodeo, - elsetrec& satrec - ); - -int sgp4 - ( - gravconsttype whichconst, - elsetrec& satrec, double tsince, - double r[], double v[] - ); - -double gstime - ( - double - ); - -void getgravconst - ( - gravconsttype, - double&, - double&, - double&, - double&, - double&, - double&, - double&, - double& - ); - -#endif - +#ifndef _sgp4unit_ +#define _sgp4unit_ +/* ---------------------------------------------------------------- +* +* sgp4unit.h +* +* this file contains the sgp4 procedures for analytical propagation +* of a satellite. the code was originally released in the 1980 and 1986 +* spacetrack papers. a detailed discussion of the theory and history +* may be found in the 2006 aiaa paper by vallado, crawford, hujsak, +* and kelso. +* +* companion code for +* fundamentals of astrodynamics and applications +* 2007 +* by david vallado +* +* (w) 719-573-2600, email dvallado@agi.com +* +* current : +* 20 apr 07 david vallado +* misc fixes for constants +* changes : +* 11 aug 06 david vallado +* chg lyddane choice back to strn3, constants, misc doc +* 15 dec 05 david vallado +* misc fixes +* 26 jul 05 david vallado +* fixes for paper +* note that each fix is preceded by a +* comment with "sgp4fix" and an explanation of +* what was changed +* 10 aug 04 david vallado +* 2nd printing baseline working +* 14 may 01 david vallado +* 2nd edition baseline +* 80 norad +* original baseline +* ---------------------------------------------------------------- */ + +#include +#include + +// -------------------------- structure declarations ---------------------------- +typedef enum +{ + wgs72old, + wgs72, + wgs84 +} gravconsttype; + +typedef struct elsetrec +{ + long int satnum; + int epochyr, epochtynumrev; + int error; + char init, method; + + /* Near Earth */ + int isimp; + double aycof , con41 , cc1 , cc4 , cc5 , d2 , d3 , d4 , + delmo , eta , argpdot, omgcof , sinmao , t , t2cof, t3cof , + t4cof , t5cof , x1mth2 , x7thm1 , mdot , nodedot, xlcof , xmcof , + nodecf; + + /* Deep Space */ + int irez; + double d2201 , d2211 , d3210 , d3222 , d4410 , d4422 , d5220 , d5232 , + d5421 , d5433 , dedt , del1 , del2 , del3 , didt , dmdt , + dnodt , domdt , e3 , ee2 , peo , pgho , pho , pinco , + plo , se2 , se3 , sgh2 , sgh3 , sgh4 , sh2 , sh3 , + si2 , si3 , sl2 , sl3 , sl4 , gsto , xfact , xgh2 , + xgh3 , xgh4 , xh2 , xh3 , xi2 , xi3 , xl2 , xl3 , + xl4 , xlamo , zmol , zmos , atime , xli , xni; + + double a , altp , alta , epochdays, jdsatepoch , nddot , ndot , + bstar , rcse , inclo , nodeo , ecco , argpo , mo , + no; +} elsetrec; + +// --------------------------- function declarations ---------------------------- +int sgp4init + ( + gravconsttype whichconst, const int satn, const double epoch, + const double xbstar, const double xecco, const double xargpo, + const double xinclo, const double xmo, const double xno, + const double xnodeo, + elsetrec& satrec + ); + +int sgp4 + ( + gravconsttype whichconst, + elsetrec& satrec, double tsince, + double r[], double v[] + ); + +double gstime + ( + double + ); + +void getgravconst + ( + gravconsttype, + double&, + double&, + double&, + double&, + double&, + double&, + double&, + double& + ); + +#endif + diff --git a/globalfunctions/timevalOperations.cpp b/globalfunctions/timevalOperations.cpp index ae49ef21..1228da04 100644 --- a/globalfunctions/timevalOperations.cpp +++ b/globalfunctions/timevalOperations.cpp @@ -1,99 +1,99 @@ -#include "timevalOperations.h" - -timeval& operator+=(timeval& lhs, const timeval& rhs) { - int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec; - sum += rhs.tv_sec * 1000000. + rhs.tv_usec; - lhs.tv_sec = sum / 1000000; - lhs.tv_usec = sum - lhs.tv_sec * 1000000; - return lhs; -} - -timeval operator+(timeval lhs, const timeval& rhs) { - lhs += rhs; - return lhs; -} - -timeval& operator-=(timeval& lhs, const timeval& rhs) { - int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec; - sum -= rhs.tv_sec * 1000000. + rhs.tv_usec; - lhs.tv_sec = sum / 1000000; - lhs.tv_usec = sum - lhs.tv_sec * 1000000; - return lhs; -} - -timeval operator-(timeval lhs, const timeval& rhs) { - lhs -= rhs; - return lhs; -} - -double operator/(const timeval& lhs, const timeval& rhs) { - double lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec; - double rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec; - return lhs64 / rhs64; -} - -timeval& operator/=(timeval& lhs, double scalar) { - int64_t product = lhs.tv_sec * 1000000. + lhs.tv_usec; - product /= scalar; - lhs.tv_sec = product / 1000000; - lhs.tv_usec = product - lhs.tv_sec * 1000000; - return lhs; -} - -timeval operator/(timeval lhs, double scalar) { - lhs /= scalar; - return lhs; -} - -timeval& operator*=(timeval& lhs, double scalar) { - int64_t product = lhs.tv_sec * 1000000. + lhs.tv_usec; - product *= scalar; - lhs.tv_sec = product / 1000000; - lhs.tv_usec = product - lhs.tv_sec * 1000000; - return lhs; -} - -timeval operator*(timeval lhs, double scalar) { - lhs *= scalar; - return lhs; -} - -timeval operator*(double scalar, timeval rhs) { - rhs *= scalar; - return rhs; -} - -bool operator==(const timeval& lhs, const timeval& rhs) { - int64_t lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec; - int64_t rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec; - return lhs64 == rhs64; -} -bool operator!=(const timeval& lhs, const timeval& rhs) { - return !operator==(lhs, rhs); -} -bool operator<(const timeval& lhs, const timeval& rhs) { - int64_t lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec; - int64_t rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec; - return lhs64 < rhs64; -} -bool operator>(const timeval& lhs, const timeval& rhs) { - return operator<(rhs, lhs); -} -bool operator<=(const timeval& lhs, const timeval& rhs) { - return !operator>(lhs, rhs); -} -bool operator>=(const timeval& lhs, const timeval& rhs) { - return !operator<(lhs, rhs); -} - -double timevalOperations::toDouble(const timeval timeval) { - double result = timeval.tv_sec * 1000000. + timeval.tv_usec; - return result / 1000000.; -} - -timeval timevalOperations::toTimeval(const double seconds) { - timeval tval; - tval.tv_sec = seconds; - tval.tv_usec = seconds *(double) 1e6 - (tval.tv_sec *1e6); - return tval; -} +#include "timevalOperations.h" + +timeval& operator+=(timeval& lhs, const timeval& rhs) { + int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec; + sum += rhs.tv_sec * 1000000. + rhs.tv_usec; + lhs.tv_sec = sum / 1000000; + lhs.tv_usec = sum - lhs.tv_sec * 1000000; + return lhs; +} + +timeval operator+(timeval lhs, const timeval& rhs) { + lhs += rhs; + return lhs; +} + +timeval& operator-=(timeval& lhs, const timeval& rhs) { + int64_t sum = lhs.tv_sec * 1000000. + lhs.tv_usec; + sum -= rhs.tv_sec * 1000000. + rhs.tv_usec; + lhs.tv_sec = sum / 1000000; + lhs.tv_usec = sum - lhs.tv_sec * 1000000; + return lhs; +} + +timeval operator-(timeval lhs, const timeval& rhs) { + lhs -= rhs; + return lhs; +} + +double operator/(const timeval& lhs, const timeval& rhs) { + double lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec; + double rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec; + return lhs64 / rhs64; +} + +timeval& operator/=(timeval& lhs, double scalar) { + int64_t product = lhs.tv_sec * 1000000. + lhs.tv_usec; + product /= scalar; + lhs.tv_sec = product / 1000000; + lhs.tv_usec = product - lhs.tv_sec * 1000000; + return lhs; +} + +timeval operator/(timeval lhs, double scalar) { + lhs /= scalar; + return lhs; +} + +timeval& operator*=(timeval& lhs, double scalar) { + int64_t product = lhs.tv_sec * 1000000. + lhs.tv_usec; + product *= scalar; + lhs.tv_sec = product / 1000000; + lhs.tv_usec = product - lhs.tv_sec * 1000000; + return lhs; +} + +timeval operator*(timeval lhs, double scalar) { + lhs *= scalar; + return lhs; +} + +timeval operator*(double scalar, timeval rhs) { + rhs *= scalar; + return rhs; +} + +bool operator==(const timeval& lhs, const timeval& rhs) { + int64_t lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec; + int64_t rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec; + return lhs64 == rhs64; +} +bool operator!=(const timeval& lhs, const timeval& rhs) { + return !operator==(lhs, rhs); +} +bool operator<(const timeval& lhs, const timeval& rhs) { + int64_t lhs64 = lhs.tv_sec * 1000000. + lhs.tv_usec; + int64_t rhs64 = rhs.tv_sec * 1000000. + rhs.tv_usec; + return lhs64 < rhs64; +} +bool operator>(const timeval& lhs, const timeval& rhs) { + return operator<(rhs, lhs); +} +bool operator<=(const timeval& lhs, const timeval& rhs) { + return !operator>(lhs, rhs); +} +bool operator>=(const timeval& lhs, const timeval& rhs) { + return !operator<(lhs, rhs); +} + +double timevalOperations::toDouble(const timeval timeval) { + double result = timeval.tv_sec * 1000000. + timeval.tv_usec; + return result / 1000000.; +} + +timeval timevalOperations::toTimeval(const double seconds) { + timeval tval; + tval.tv_sec = seconds; + tval.tv_usec = seconds *(double) 1e6 - (tval.tv_sec *1e6); + return tval; +} diff --git a/osal/FreeRTOS/BinSemaphUsingTask.cpp b/osal/FreeRTOS/BinSemaphUsingTask.cpp index 8c831bbe..dd1e48ca 100644 --- a/osal/FreeRTOS/BinSemaphUsingTask.cpp +++ b/osal/FreeRTOS/BinSemaphUsingTask.cpp @@ -1,95 +1,95 @@ -#include "../../osal/FreeRTOS/BinSemaphUsingTask.h" -#include "../../osal/FreeRTOS/TaskManagement.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" - -BinarySemaphoreUsingTask::BinarySemaphoreUsingTask() { - handle = TaskManagement::getCurrentTaskHandle(); - if(handle == nullptr) { - sif::error << "Could not retrieve task handle. Please ensure the" - "constructor was called inside a task." << std::endl; - } - xTaskNotifyGive(handle); -} - -BinarySemaphoreUsingTask::~BinarySemaphoreUsingTask() { - // Clear notification value on destruction. - xTaskNotifyAndQuery(handle, 0, eSetValueWithOverwrite, nullptr); -} - -ReturnValue_t BinarySemaphoreUsingTask::acquire(TimeoutType timeoutType, - uint32_t timeoutMs) { - TickType_t timeout = 0; - if(timeoutType == TimeoutType::POLLING) { - timeout = 0; - } - else if(timeoutType == TimeoutType::WAITING){ - timeout = pdMS_TO_TICKS(timeoutMs); - } - else { - timeout = portMAX_DELAY; - } - return acquireWithTickTimeout(timeoutType, timeout); -} - -ReturnValue_t BinarySemaphoreUsingTask::acquireWithTickTimeout( - TimeoutType timeoutType, TickType_t timeoutTicks) { - BaseType_t returncode = ulTaskNotifyTake(pdTRUE, timeoutTicks); - if (returncode == pdPASS) { - return HasReturnvaluesIF::RETURN_OK; - } - else { - return SemaphoreIF::SEMAPHORE_TIMEOUT; - } -} - -ReturnValue_t BinarySemaphoreUsingTask::release() { - return release(this->handle); -} - -ReturnValue_t BinarySemaphoreUsingTask::release( - TaskHandle_t taskHandle) { - if(getSemaphoreCounter(taskHandle) == 1) { - return SemaphoreIF::SEMAPHORE_NOT_OWNED; - } - BaseType_t returncode = xTaskNotifyGive(taskHandle); - if (returncode == pdPASS) { - return HasReturnvaluesIF::RETURN_OK; - } - else { - // This should never happen. - return HasReturnvaluesIF::RETURN_FAILED; - } -} - -TaskHandle_t BinarySemaphoreUsingTask::getTaskHandle() { - return handle; -} - -uint8_t BinarySemaphoreUsingTask::getSemaphoreCounter() const { - return getSemaphoreCounter(this->handle); -} - -uint8_t BinarySemaphoreUsingTask::getSemaphoreCounter( - TaskHandle_t taskHandle) { - uint32_t notificationValue; - xTaskNotifyAndQuery(taskHandle, 0, eNoAction, ¬ificationValue); - return notificationValue; -} - -// Be careful with the stack size here. This is called from an ISR! -ReturnValue_t BinarySemaphoreUsingTask::releaseFromISR( - TaskHandle_t taskHandle, BaseType_t * higherPriorityTaskWoken) { - if(getSemaphoreCounterFromISR(taskHandle, higherPriorityTaskWoken) == 1) { - return SemaphoreIF::SEMAPHORE_NOT_OWNED; - } - vTaskNotifyGiveFromISR(taskHandle, higherPriorityTaskWoken); - return HasReturnvaluesIF::RETURN_OK; -} - -uint8_t BinarySemaphoreUsingTask::getSemaphoreCounterFromISR( - TaskHandle_t taskHandle, BaseType_t* higherPriorityTaskWoken) { - uint32_t notificationValue = 0; - xTaskNotifyAndQueryFromISR(taskHandle, 0, eNoAction, ¬ificationValue, - higherPriorityTaskWoken); - return notificationValue; -} +#include "../../osal/FreeRTOS/BinSemaphUsingTask.h" +#include "../../osal/FreeRTOS/TaskManagement.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" + +BinarySemaphoreUsingTask::BinarySemaphoreUsingTask() { + handle = TaskManagement::getCurrentTaskHandle(); + if(handle == nullptr) { + sif::error << "Could not retrieve task handle. Please ensure the" + "constructor was called inside a task." << std::endl; + } + xTaskNotifyGive(handle); +} + +BinarySemaphoreUsingTask::~BinarySemaphoreUsingTask() { + // Clear notification value on destruction. + xTaskNotifyAndQuery(handle, 0, eSetValueWithOverwrite, nullptr); +} + +ReturnValue_t BinarySemaphoreUsingTask::acquire(TimeoutType timeoutType, + uint32_t timeoutMs) { + TickType_t timeout = 0; + if(timeoutType == TimeoutType::POLLING) { + timeout = 0; + } + else if(timeoutType == TimeoutType::WAITING){ + timeout = pdMS_TO_TICKS(timeoutMs); + } + else { + timeout = portMAX_DELAY; + } + return acquireWithTickTimeout(timeoutType, timeout); +} + +ReturnValue_t BinarySemaphoreUsingTask::acquireWithTickTimeout( + TimeoutType timeoutType, TickType_t timeoutTicks) { + BaseType_t returncode = ulTaskNotifyTake(pdTRUE, timeoutTicks); + if (returncode == pdPASS) { + return HasReturnvaluesIF::RETURN_OK; + } + else { + return SemaphoreIF::SEMAPHORE_TIMEOUT; + } +} + +ReturnValue_t BinarySemaphoreUsingTask::release() { + return release(this->handle); +} + +ReturnValue_t BinarySemaphoreUsingTask::release( + TaskHandle_t taskHandle) { + if(getSemaphoreCounter(taskHandle) == 1) { + return SemaphoreIF::SEMAPHORE_NOT_OWNED; + } + BaseType_t returncode = xTaskNotifyGive(taskHandle); + if (returncode == pdPASS) { + return HasReturnvaluesIF::RETURN_OK; + } + else { + // This should never happen. + return HasReturnvaluesIF::RETURN_FAILED; + } +} + +TaskHandle_t BinarySemaphoreUsingTask::getTaskHandle() { + return handle; +} + +uint8_t BinarySemaphoreUsingTask::getSemaphoreCounter() const { + return getSemaphoreCounter(this->handle); +} + +uint8_t BinarySemaphoreUsingTask::getSemaphoreCounter( + TaskHandle_t taskHandle) { + uint32_t notificationValue; + xTaskNotifyAndQuery(taskHandle, 0, eNoAction, ¬ificationValue); + return notificationValue; +} + +// Be careful with the stack size here. This is called from an ISR! +ReturnValue_t BinarySemaphoreUsingTask::releaseFromISR( + TaskHandle_t taskHandle, BaseType_t * higherPriorityTaskWoken) { + if(getSemaphoreCounterFromISR(taskHandle, higherPriorityTaskWoken) == 1) { + return SemaphoreIF::SEMAPHORE_NOT_OWNED; + } + vTaskNotifyGiveFromISR(taskHandle, higherPriorityTaskWoken); + return HasReturnvaluesIF::RETURN_OK; +} + +uint8_t BinarySemaphoreUsingTask::getSemaphoreCounterFromISR( + TaskHandle_t taskHandle, BaseType_t* higherPriorityTaskWoken) { + uint32_t notificationValue = 0; + xTaskNotifyAndQueryFromISR(taskHandle, 0, eNoAction, ¬ificationValue, + higherPriorityTaskWoken); + return notificationValue; +} diff --git a/osal/FreeRTOS/BinSemaphUsingTask.h b/osal/FreeRTOS/BinSemaphUsingTask.h index 2d2cf159..f3c0b0ea 100644 --- a/osal/FreeRTOS/BinSemaphUsingTask.h +++ b/osal/FreeRTOS/BinSemaphUsingTask.h @@ -1,76 +1,76 @@ -#ifndef FRAMEWORK_OSAL_FREERTOS_BINSEMAPHUSINGTASK_H_ -#define FRAMEWORK_OSAL_FREERTOS_BINSEMAPHUSINGTASK_H_ - -#include "../../returnvalues/HasReturnvaluesIF.h" -#include "../../tasks/SemaphoreIF.h" - -#include -#include - -/** - * @brief Binary Semaphore implementation using the task notification value. - * The notification value should therefore not be used - * for other purposes. - * @details - * Additional information: https://www.freertos.org/RTOS-task-notifications.html - * and general semaphore documentation. - */ -class BinarySemaphoreUsingTask: public SemaphoreIF, - public HasReturnvaluesIF { -public: - static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF; - - //! @brief Default ctor - BinarySemaphoreUsingTask(); - //! @brief Default dtor - virtual~ BinarySemaphoreUsingTask(); - - ReturnValue_t acquire(TimeoutType timeoutType = TimeoutType::BLOCKING, - uint32_t timeoutMs = portMAX_DELAY) override; - ReturnValue_t release() override; - uint8_t getSemaphoreCounter() const override; - static uint8_t getSemaphoreCounter(TaskHandle_t taskHandle); - static uint8_t getSemaphoreCounterFromISR(TaskHandle_t taskHandle, - BaseType_t* higherPriorityTaskWoken); - - /** - * Same as acquire() with timeout in FreeRTOS ticks. - * @param timeoutTicks - * @return - @c RETURN_OK on success - * - @c RETURN_FAILED on failure - */ - ReturnValue_t acquireWithTickTimeout( - TimeoutType timeoutType = TimeoutType::BLOCKING, - TickType_t timeoutTicks = portMAX_DELAY); - - /** - * Get handle to the task related to the semaphore. - * @return - */ - TaskHandle_t getTaskHandle(); - - /** - * Wrapper function to give back semaphore from handle - * @param semaphore - * @return - @c RETURN_OK on success - * - @c RETURN_FAILED on failure - */ - static ReturnValue_t release(TaskHandle_t taskToNotify); - - /** - * Wrapper function to give back semaphore from handle when called from an ISR - * @param semaphore - * @param higherPriorityTaskWoken This will be set to pdPASS if a task with - * a higher priority was unblocked. A context switch should be requested - * from an ISR if this is the case (see TaskManagement functions) - * @return - @c RETURN_OK on success - * - @c RETURN_FAILED on failure - */ - static ReturnValue_t releaseFromISR(TaskHandle_t taskToNotify, - BaseType_t * higherPriorityTaskWoken); - -protected: - TaskHandle_t handle; -}; - -#endif /* FRAMEWORK_OSAL_FREERTOS_BINSEMAPHUSINGTASK_H_ */ +#ifndef FRAMEWORK_OSAL_FREERTOS_BINSEMAPHUSINGTASK_H_ +#define FRAMEWORK_OSAL_FREERTOS_BINSEMAPHUSINGTASK_H_ + +#include "../../returnvalues/HasReturnvaluesIF.h" +#include "../../tasks/SemaphoreIF.h" + +#include +#include + +/** + * @brief Binary Semaphore implementation using the task notification value. + * The notification value should therefore not be used + * for other purposes. + * @details + * Additional information: https://www.freertos.org/RTOS-task-notifications.html + * and general semaphore documentation. + */ +class BinarySemaphoreUsingTask: public SemaphoreIF, + public HasReturnvaluesIF { +public: + static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF; + + //! @brief Default ctor + BinarySemaphoreUsingTask(); + //! @brief Default dtor + virtual~ BinarySemaphoreUsingTask(); + + ReturnValue_t acquire(TimeoutType timeoutType = TimeoutType::BLOCKING, + uint32_t timeoutMs = portMAX_DELAY) override; + ReturnValue_t release() override; + uint8_t getSemaphoreCounter() const override; + static uint8_t getSemaphoreCounter(TaskHandle_t taskHandle); + static uint8_t getSemaphoreCounterFromISR(TaskHandle_t taskHandle, + BaseType_t* higherPriorityTaskWoken); + + /** + * Same as acquire() with timeout in FreeRTOS ticks. + * @param timeoutTicks + * @return - @c RETURN_OK on success + * - @c RETURN_FAILED on failure + */ + ReturnValue_t acquireWithTickTimeout( + TimeoutType timeoutType = TimeoutType::BLOCKING, + TickType_t timeoutTicks = portMAX_DELAY); + + /** + * Get handle to the task related to the semaphore. + * @return + */ + TaskHandle_t getTaskHandle(); + + /** + * Wrapper function to give back semaphore from handle + * @param semaphore + * @return - @c RETURN_OK on success + * - @c RETURN_FAILED on failure + */ + static ReturnValue_t release(TaskHandle_t taskToNotify); + + /** + * Wrapper function to give back semaphore from handle when called from an ISR + * @param semaphore + * @param higherPriorityTaskWoken This will be set to pdPASS if a task with + * a higher priority was unblocked. A context switch should be requested + * from an ISR if this is the case (see TaskManagement functions) + * @return - @c RETURN_OK on success + * - @c RETURN_FAILED on failure + */ + static ReturnValue_t releaseFromISR(TaskHandle_t taskToNotify, + BaseType_t * higherPriorityTaskWoken); + +protected: + TaskHandle_t handle; +}; + +#endif /* FRAMEWORK_OSAL_FREERTOS_BINSEMAPHUSINGTASK_H_ */ diff --git a/osal/FreeRTOS/BinarySemaphore.cpp b/osal/FreeRTOS/BinarySemaphore.cpp index f1c78473..8cc3c495 100644 --- a/osal/FreeRTOS/BinarySemaphore.cpp +++ b/osal/FreeRTOS/BinarySemaphore.cpp @@ -1,108 +1,108 @@ -#include "../../osal/FreeRTOS/BinarySemaphore.h" -#include "../../osal/FreeRTOS/TaskManagement.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" - -BinarySemaphore::BinarySemaphore() { - handle = xSemaphoreCreateBinary(); - if(handle == nullptr) { - sif::error << "Semaphore: Binary semaph creation failure" << std::endl; - } - // Initiated semaphore must be given before it can be taken. - xSemaphoreGive(handle); -} - -BinarySemaphore::~BinarySemaphore() { - vSemaphoreDelete(handle); -} - -BinarySemaphore::BinarySemaphore(BinarySemaphore&& s) { - handle = xSemaphoreCreateBinary(); - if(handle == nullptr) { - sif::error << "Binary semaphore creation failure" << std::endl; - } - xSemaphoreGive(handle); -} - -BinarySemaphore& BinarySemaphore::operator =( - BinarySemaphore&& s) { - if(&s != this) { - handle = xSemaphoreCreateBinary(); - if(handle == nullptr) { - sif::error << "Binary semaphore creation failure" << std::endl; - } - xSemaphoreGive(handle); - } - return *this; -} - -ReturnValue_t BinarySemaphore::acquire(TimeoutType timeoutType, - uint32_t timeoutMs) { - TickType_t timeout = 0; - if(timeoutType == TimeoutType::POLLING) { - timeout = 0; - } - else if(timeoutType == TimeoutType::WAITING){ - timeout = pdMS_TO_TICKS(timeoutMs); - } - else { - timeout = portMAX_DELAY; - } - return acquireWithTickTimeout(timeoutType, timeout); -} - -ReturnValue_t BinarySemaphore::acquireWithTickTimeout(TimeoutType timeoutType, - TickType_t timeoutTicks) { - if(handle == nullptr) { - return SemaphoreIF::SEMAPHORE_INVALID; - } - - BaseType_t returncode = xSemaphoreTake(handle, timeoutTicks); - if (returncode == pdPASS) { - return HasReturnvaluesIF::RETURN_OK; - } - else { - return SemaphoreIF::SEMAPHORE_TIMEOUT; - } -} - -ReturnValue_t BinarySemaphore::release() { - return release(handle); -} - -ReturnValue_t BinarySemaphore::release(SemaphoreHandle_t semaphore) { - if (semaphore == nullptr) { - return SemaphoreIF::SEMAPHORE_INVALID; - } - BaseType_t returncode = xSemaphoreGive(semaphore); - if (returncode == pdPASS) { - return HasReturnvaluesIF::RETURN_OK; - } - else { - return SemaphoreIF::SEMAPHORE_NOT_OWNED; - } -} - -uint8_t BinarySemaphore::getSemaphoreCounter() const { - return uxSemaphoreGetCount(handle); -} - -SemaphoreHandle_t BinarySemaphore::getSemaphore() { - return handle; -} - - -// Be careful with the stack size here. This is called from an ISR! -ReturnValue_t BinarySemaphore::releaseFromISR( - SemaphoreHandle_t semaphore, BaseType_t * higherPriorityTaskWoken) { - if (semaphore == nullptr) { - return SemaphoreIF::SEMAPHORE_INVALID; - } - BaseType_t returncode = xSemaphoreGiveFromISR(semaphore, - higherPriorityTaskWoken); - if (returncode == pdPASS) { - return HasReturnvaluesIF::RETURN_OK; - } - else { - return SemaphoreIF::SEMAPHORE_NOT_OWNED; - } -} +#include "../../osal/FreeRTOS/BinarySemaphore.h" +#include "../../osal/FreeRTOS/TaskManagement.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" + +BinarySemaphore::BinarySemaphore() { + handle = xSemaphoreCreateBinary(); + if(handle == nullptr) { + sif::error << "Semaphore: Binary semaph creation failure" << std::endl; + } + // Initiated semaphore must be given before it can be taken. + xSemaphoreGive(handle); +} + +BinarySemaphore::~BinarySemaphore() { + vSemaphoreDelete(handle); +} + +BinarySemaphore::BinarySemaphore(BinarySemaphore&& s) { + handle = xSemaphoreCreateBinary(); + if(handle == nullptr) { + sif::error << "Binary semaphore creation failure" << std::endl; + } + xSemaphoreGive(handle); +} + +BinarySemaphore& BinarySemaphore::operator =( + BinarySemaphore&& s) { + if(&s != this) { + handle = xSemaphoreCreateBinary(); + if(handle == nullptr) { + sif::error << "Binary semaphore creation failure" << std::endl; + } + xSemaphoreGive(handle); + } + return *this; +} + +ReturnValue_t BinarySemaphore::acquire(TimeoutType timeoutType, + uint32_t timeoutMs) { + TickType_t timeout = 0; + if(timeoutType == TimeoutType::POLLING) { + timeout = 0; + } + else if(timeoutType == TimeoutType::WAITING){ + timeout = pdMS_TO_TICKS(timeoutMs); + } + else { + timeout = portMAX_DELAY; + } + return acquireWithTickTimeout(timeoutType, timeout); +} + +ReturnValue_t BinarySemaphore::acquireWithTickTimeout(TimeoutType timeoutType, + TickType_t timeoutTicks) { + if(handle == nullptr) { + return SemaphoreIF::SEMAPHORE_INVALID; + } + + BaseType_t returncode = xSemaphoreTake(handle, timeoutTicks); + if (returncode == pdPASS) { + return HasReturnvaluesIF::RETURN_OK; + } + else { + return SemaphoreIF::SEMAPHORE_TIMEOUT; + } +} + +ReturnValue_t BinarySemaphore::release() { + return release(handle); +} + +ReturnValue_t BinarySemaphore::release(SemaphoreHandle_t semaphore) { + if (semaphore == nullptr) { + return SemaphoreIF::SEMAPHORE_INVALID; + } + BaseType_t returncode = xSemaphoreGive(semaphore); + if (returncode == pdPASS) { + return HasReturnvaluesIF::RETURN_OK; + } + else { + return SemaphoreIF::SEMAPHORE_NOT_OWNED; + } +} + +uint8_t BinarySemaphore::getSemaphoreCounter() const { + return uxSemaphoreGetCount(handle); +} + +SemaphoreHandle_t BinarySemaphore::getSemaphore() { + return handle; +} + + +// Be careful with the stack size here. This is called from an ISR! +ReturnValue_t BinarySemaphore::releaseFromISR( + SemaphoreHandle_t semaphore, BaseType_t * higherPriorityTaskWoken) { + if (semaphore == nullptr) { + return SemaphoreIF::SEMAPHORE_INVALID; + } + BaseType_t returncode = xSemaphoreGiveFromISR(semaphore, + higherPriorityTaskWoken); + if (returncode == pdPASS) { + return HasReturnvaluesIF::RETURN_OK; + } + else { + return SemaphoreIF::SEMAPHORE_NOT_OWNED; + } +} diff --git a/osal/FreeRTOS/BinarySemaphore.h b/osal/FreeRTOS/BinarySemaphore.h index 5a32088a..c6cedc53 100644 --- a/osal/FreeRTOS/BinarySemaphore.h +++ b/osal/FreeRTOS/BinarySemaphore.h @@ -1,107 +1,107 @@ -#ifndef FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ -#define FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ - -#include "../../returnvalues/HasReturnvaluesIF.h" -#include "../../tasks/SemaphoreIF.h" - -#include -#include - -/** - * @brief OS Tool to achieve synchronization of between tasks or between - * task and ISR. The default semaphore implementation creates a - * binary semaphore, which can only be taken once. - * @details - * Documentation: https://www.freertos.org/Embedded-RTOS-Binary-Semaphores.html - * - * Please note that if the semaphore implementation is only related to - * the synchronization of one task, the new task notifications can be used, - * also see the BinSemaphUsingTask and CountingSemaphUsingTask classes. - * These use the task notification value instead of a queue and are - * faster and more efficient. - * - * @author R. Mueller - * @ingroup osal - */ -class BinarySemaphore: public SemaphoreIF, - public HasReturnvaluesIF { -public: - static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF; - - //! @brief Default ctor - BinarySemaphore(); - //! @brief Copy ctor, deleted explicitely. - BinarySemaphore(const BinarySemaphore&) = delete; - //! @brief Copy assignment, deleted explicitely. - BinarySemaphore& operator=(const BinarySemaphore&) = delete; - //! @brief Move ctor - BinarySemaphore (BinarySemaphore &&); - //! @brief Move assignment - BinarySemaphore & operator=(BinarySemaphore &&); - //! @brief Destructor - virtual ~BinarySemaphore(); - - uint8_t getSemaphoreCounter() const override; - - /** - * Take the binary semaphore. - * If the semaphore has already been taken, the task will be blocked - * for a maximum of #timeoutMs or until the semaphore is given back, - * for example by an ISR or another task. - * @param timeoutMs - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout - */ - ReturnValue_t acquire(TimeoutType timeoutType = - TimeoutType::BLOCKING, uint32_t timeoutMs = portMAX_DELAY) override; - - /** - * Same as lockBinarySemaphore() with timeout in FreeRTOS ticks. - * @param timeoutTicks - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout - */ - ReturnValue_t acquireWithTickTimeout(TimeoutType timeoutType = - TimeoutType::BLOCKING, TickType_t timeoutTicks = portMAX_DELAY); - - /** - * Release the binary semaphore. - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if the semaphores is - * already available. - */ - ReturnValue_t release() override; - - /** - * Get Handle to the semaphore. - * @return - */ - SemaphoreHandle_t getSemaphore(); - - /** - * Wrapper function to give back semaphore from handle - * @param semaphore - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if the semaphores is - * already available. - */ - static ReturnValue_t release(SemaphoreHandle_t semaphore); - - /** - * Wrapper function to give back semaphore from handle when called from an ISR - * @param semaphore - * @param higherPriorityTaskWoken This will be set to pdPASS if a task with - * a higher priority was unblocked. A context switch from an ISR should - * then be requested (see TaskManagement functions) - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if the semaphores is - * already available. - */ - static ReturnValue_t releaseFromISR(SemaphoreHandle_t semaphore, - BaseType_t * higherPriorityTaskWoken); - -protected: - SemaphoreHandle_t handle; -}; - -#endif /* FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ */ +#ifndef FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ +#define FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ + +#include "../../returnvalues/HasReturnvaluesIF.h" +#include "../../tasks/SemaphoreIF.h" + +#include +#include + +/** + * @brief OS Tool to achieve synchronization of between tasks or between + * task and ISR. The default semaphore implementation creates a + * binary semaphore, which can only be taken once. + * @details + * Documentation: https://www.freertos.org/Embedded-RTOS-Binary-Semaphores.html + * + * Please note that if the semaphore implementation is only related to + * the synchronization of one task, the new task notifications can be used, + * also see the BinSemaphUsingTask and CountingSemaphUsingTask classes. + * These use the task notification value instead of a queue and are + * faster and more efficient. + * + * @author R. Mueller + * @ingroup osal + */ +class BinarySemaphore: public SemaphoreIF, + public HasReturnvaluesIF { +public: + static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF; + + //! @brief Default ctor + BinarySemaphore(); + //! @brief Copy ctor, deleted explicitely. + BinarySemaphore(const BinarySemaphore&) = delete; + //! @brief Copy assignment, deleted explicitely. + BinarySemaphore& operator=(const BinarySemaphore&) = delete; + //! @brief Move ctor + BinarySemaphore (BinarySemaphore &&); + //! @brief Move assignment + BinarySemaphore & operator=(BinarySemaphore &&); + //! @brief Destructor + virtual ~BinarySemaphore(); + + uint8_t getSemaphoreCounter() const override; + + /** + * Take the binary semaphore. + * If the semaphore has already been taken, the task will be blocked + * for a maximum of #timeoutMs or until the semaphore is given back, + * for example by an ISR or another task. + * @param timeoutMs + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout + */ + ReturnValue_t acquire(TimeoutType timeoutType = + TimeoutType::BLOCKING, uint32_t timeoutMs = portMAX_DELAY) override; + + /** + * Same as lockBinarySemaphore() with timeout in FreeRTOS ticks. + * @param timeoutTicks + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout + */ + ReturnValue_t acquireWithTickTimeout(TimeoutType timeoutType = + TimeoutType::BLOCKING, TickType_t timeoutTicks = portMAX_DELAY); + + /** + * Release the binary semaphore. + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if the semaphores is + * already available. + */ + ReturnValue_t release() override; + + /** + * Get Handle to the semaphore. + * @return + */ + SemaphoreHandle_t getSemaphore(); + + /** + * Wrapper function to give back semaphore from handle + * @param semaphore + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if the semaphores is + * already available. + */ + static ReturnValue_t release(SemaphoreHandle_t semaphore); + + /** + * Wrapper function to give back semaphore from handle when called from an ISR + * @param semaphore + * @param higherPriorityTaskWoken This will be set to pdPASS if a task with + * a higher priority was unblocked. A context switch from an ISR should + * then be requested (see TaskManagement functions) + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if the semaphores is + * already available. + */ + static ReturnValue_t releaseFromISR(SemaphoreHandle_t semaphore, + BaseType_t * higherPriorityTaskWoken); + +protected: + SemaphoreHandle_t handle; +}; + +#endif /* FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ */ diff --git a/osal/FreeRTOS/CountingSemaphUsingTask.cpp b/osal/FreeRTOS/CountingSemaphUsingTask.cpp index e66671ac..a47341bc 100644 --- a/osal/FreeRTOS/CountingSemaphUsingTask.cpp +++ b/osal/FreeRTOS/CountingSemaphUsingTask.cpp @@ -1,114 +1,114 @@ -#include "../../osal/FreeRTOS/CountingSemaphUsingTask.h" -#include "../../osal/FreeRTOS/TaskManagement.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" - -CountingSemaphoreUsingTask::CountingSemaphoreUsingTask(const uint8_t maxCount, - uint8_t initCount): maxCount(maxCount) { - if(initCount > maxCount) { - sif::error << "CountingSemaphoreUsingTask: Max count bigger than " - "intial cout. Setting initial count to max count." << std::endl; - initCount = maxCount; - } - - handle = TaskManagement::getCurrentTaskHandle(); - if(handle == nullptr) { - sif::error << "CountingSemaphoreUsingTask: Could not retrieve task " - "handle. Please ensure the constructor was called inside a " - "task." << std::endl; - } - - uint32_t oldNotificationValue; - xTaskNotifyAndQuery(handle, 0, eSetValueWithOverwrite, - &oldNotificationValue); - if(oldNotificationValue != 0) { - sif::warning << "CountinSemaphoreUsingTask: Semaphore initiated but " - "current notification value is not 0. Please ensure the " - "notification value is not used for other purposes!" << std::endl; - } - for(int i = 0; i < initCount; i++) { - xTaskNotifyGive(handle); - } -} - -CountingSemaphoreUsingTask::~CountingSemaphoreUsingTask() { - // Clear notification value on destruction. - // If this is not desired, don't call the destructor - // (or implement a boolean which disables the reset) - xTaskNotifyAndQuery(handle, 0, eSetValueWithOverwrite, nullptr); -} - -ReturnValue_t CountingSemaphoreUsingTask::acquire(TimeoutType timeoutType, - uint32_t timeoutMs) { - TickType_t timeout = 0; - if(timeoutType == TimeoutType::POLLING) { - timeout = 0; - } - else if(timeoutType == TimeoutType::WAITING){ - timeout = pdMS_TO_TICKS(timeoutMs); - } - else { - timeout = portMAX_DELAY; - } - return acquireWithTickTimeout(timeoutType, timeout); - -} - -ReturnValue_t CountingSemaphoreUsingTask::acquireWithTickTimeout( - TimeoutType timeoutType, TickType_t timeoutTicks) { - // Decrement notfication value without resetting it. - BaseType_t oldCount = ulTaskNotifyTake(pdFALSE, timeoutTicks); - if (getSemaphoreCounter() == oldCount - 1) { - return HasReturnvaluesIF::RETURN_OK; - } - else { - return SemaphoreIF::SEMAPHORE_TIMEOUT; - } -} - -ReturnValue_t CountingSemaphoreUsingTask::release() { - if(getSemaphoreCounter() == maxCount) { - return SemaphoreIF::SEMAPHORE_NOT_OWNED; - } - return release(handle); -} - -ReturnValue_t CountingSemaphoreUsingTask::release( - TaskHandle_t taskToNotify) { - BaseType_t returncode = xTaskNotifyGive(taskToNotify); - if (returncode == pdPASS) { - return HasReturnvaluesIF::RETURN_OK; - } - else { - // This should never happen. - return HasReturnvaluesIF::RETURN_FAILED; - } -} - - -uint8_t CountingSemaphoreUsingTask::getSemaphoreCounter() const { - uint32_t notificationValue = 0; - xTaskNotifyAndQuery(handle, 0, eNoAction, ¬ificationValue); - return notificationValue; -} - -TaskHandle_t CountingSemaphoreUsingTask::getTaskHandle() { - return handle; -} - -ReturnValue_t CountingSemaphoreUsingTask::releaseFromISR( - TaskHandle_t taskToNotify, BaseType_t* higherPriorityTaskWoken) { - vTaskNotifyGiveFromISR(taskToNotify, higherPriorityTaskWoken); - return HasReturnvaluesIF::RETURN_OK; -} - -uint8_t CountingSemaphoreUsingTask::getSemaphoreCounterFromISR( - TaskHandle_t task, BaseType_t* higherPriorityTaskWoken) { - uint32_t notificationValue; - xTaskNotifyAndQueryFromISR(task, 0, eNoAction, ¬ificationValue, - higherPriorityTaskWoken); - return notificationValue; -} - -uint8_t CountingSemaphoreUsingTask::getMaxCount() const { - return maxCount; -} +#include "../../osal/FreeRTOS/CountingSemaphUsingTask.h" +#include "../../osal/FreeRTOS/TaskManagement.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" + +CountingSemaphoreUsingTask::CountingSemaphoreUsingTask(const uint8_t maxCount, + uint8_t initCount): maxCount(maxCount) { + if(initCount > maxCount) { + sif::error << "CountingSemaphoreUsingTask: Max count bigger than " + "intial cout. Setting initial count to max count." << std::endl; + initCount = maxCount; + } + + handle = TaskManagement::getCurrentTaskHandle(); + if(handle == nullptr) { + sif::error << "CountingSemaphoreUsingTask: Could not retrieve task " + "handle. Please ensure the constructor was called inside a " + "task." << std::endl; + } + + uint32_t oldNotificationValue; + xTaskNotifyAndQuery(handle, 0, eSetValueWithOverwrite, + &oldNotificationValue); + if(oldNotificationValue != 0) { + sif::warning << "CountinSemaphoreUsingTask: Semaphore initiated but " + "current notification value is not 0. Please ensure the " + "notification value is not used for other purposes!" << std::endl; + } + for(int i = 0; i < initCount; i++) { + xTaskNotifyGive(handle); + } +} + +CountingSemaphoreUsingTask::~CountingSemaphoreUsingTask() { + // Clear notification value on destruction. + // If this is not desired, don't call the destructor + // (or implement a boolean which disables the reset) + xTaskNotifyAndQuery(handle, 0, eSetValueWithOverwrite, nullptr); +} + +ReturnValue_t CountingSemaphoreUsingTask::acquire(TimeoutType timeoutType, + uint32_t timeoutMs) { + TickType_t timeout = 0; + if(timeoutType == TimeoutType::POLLING) { + timeout = 0; + } + else if(timeoutType == TimeoutType::WAITING){ + timeout = pdMS_TO_TICKS(timeoutMs); + } + else { + timeout = portMAX_DELAY; + } + return acquireWithTickTimeout(timeoutType, timeout); + +} + +ReturnValue_t CountingSemaphoreUsingTask::acquireWithTickTimeout( + TimeoutType timeoutType, TickType_t timeoutTicks) { + // Decrement notfication value without resetting it. + BaseType_t oldCount = ulTaskNotifyTake(pdFALSE, timeoutTicks); + if (getSemaphoreCounter() == oldCount - 1) { + return HasReturnvaluesIF::RETURN_OK; + } + else { + return SemaphoreIF::SEMAPHORE_TIMEOUT; + } +} + +ReturnValue_t CountingSemaphoreUsingTask::release() { + if(getSemaphoreCounter() == maxCount) { + return SemaphoreIF::SEMAPHORE_NOT_OWNED; + } + return release(handle); +} + +ReturnValue_t CountingSemaphoreUsingTask::release( + TaskHandle_t taskToNotify) { + BaseType_t returncode = xTaskNotifyGive(taskToNotify); + if (returncode == pdPASS) { + return HasReturnvaluesIF::RETURN_OK; + } + else { + // This should never happen. + return HasReturnvaluesIF::RETURN_FAILED; + } +} + + +uint8_t CountingSemaphoreUsingTask::getSemaphoreCounter() const { + uint32_t notificationValue = 0; + xTaskNotifyAndQuery(handle, 0, eNoAction, ¬ificationValue); + return notificationValue; +} + +TaskHandle_t CountingSemaphoreUsingTask::getTaskHandle() { + return handle; +} + +ReturnValue_t CountingSemaphoreUsingTask::releaseFromISR( + TaskHandle_t taskToNotify, BaseType_t* higherPriorityTaskWoken) { + vTaskNotifyGiveFromISR(taskToNotify, higherPriorityTaskWoken); + return HasReturnvaluesIF::RETURN_OK; +} + +uint8_t CountingSemaphoreUsingTask::getSemaphoreCounterFromISR( + TaskHandle_t task, BaseType_t* higherPriorityTaskWoken) { + uint32_t notificationValue; + xTaskNotifyAndQueryFromISR(task, 0, eNoAction, ¬ificationValue, + higherPriorityTaskWoken); + return notificationValue; +} + +uint8_t CountingSemaphoreUsingTask::getMaxCount() const { + return maxCount; +} diff --git a/osal/FreeRTOS/CountingSemaphUsingTask.h b/osal/FreeRTOS/CountingSemaphUsingTask.h index e3733382..8977258c 100644 --- a/osal/FreeRTOS/CountingSemaphUsingTask.h +++ b/osal/FreeRTOS/CountingSemaphUsingTask.h @@ -1,102 +1,102 @@ -#ifndef FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHUSINGTASK_H_ -#define FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHUSINGTASK_H_ - -#include "../../osal/FreeRTOS/CountingSemaphUsingTask.h" -#include "../../tasks/SemaphoreIF.h" - -extern "C" { -#include -#include -} - -/** - * @brief Couting Semaphore implementation which uses the notification value - * of the task. The notification value should therefore not be used - * for other purposes. - * @details - * Additional information: https://www.freertos.org/RTOS-task-notifications.html - * and general semaphore documentation. - */ -class CountingSemaphoreUsingTask: public SemaphoreIF { -public: - CountingSemaphoreUsingTask(const uint8_t maxCount, uint8_t initCount); - virtual ~CountingSemaphoreUsingTask(); - - /** - * Acquire the counting semaphore. - * If no semaphores are available, the task will be blocked - * for a maximum of #timeoutMs or until one is given back, - * for example by an ISR or another task. - * @param timeoutMs - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout - */ - ReturnValue_t acquire(TimeoutType timeoutType = TimeoutType::BLOCKING, - uint32_t timeoutMs = portMAX_DELAY) override; - - /** - * Release a semaphore, increasing the number of available counting - * semaphores up to the #maxCount value. - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if #maxCount semaphores are - * already available. - */ - ReturnValue_t release() override; - - uint8_t getSemaphoreCounter() const override; - /** - * Get the semaphore counter from an ISR. - * @param task - * @param higherPriorityTaskWoken This will be set to pdPASS if a task with - * a higher priority was unblocked. A context switch should be requested - * from an ISR if this is the case (see TaskManagement functions) - * @return - */ - static uint8_t getSemaphoreCounterFromISR(TaskHandle_t task, - BaseType_t* higherPriorityTaskWoken); - - /** - * Acquire with a timeout value in ticks - * @param timeoutTicks - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout - */ - ReturnValue_t acquireWithTickTimeout( - TimeoutType timeoutType = TimeoutType::BLOCKING, - TickType_t timeoutTicks = portMAX_DELAY); - - /** - * Get handle to the task related to the semaphore. - * @return - */ - TaskHandle_t getTaskHandle(); - - /** - * Release semaphore of task by supplying task handle - * @param taskToNotify - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if #maxCount semaphores are - * already available. - */ - static ReturnValue_t release(TaskHandle_t taskToNotify); - /** - * Release seamphore of a task from an ISR. - * @param taskToNotify - * @param higherPriorityTaskWoken This will be set to pdPASS if a task with - * a higher priority was unblocked. A context switch should be requested - * from an ISR if this is the case (see TaskManagement functions) - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if #maxCount semaphores are - * already available. - */ - static ReturnValue_t releaseFromISR(TaskHandle_t taskToNotify, - BaseType_t* higherPriorityTaskWoken); - - uint8_t getMaxCount() const; - -private: - TaskHandle_t handle; - const uint8_t maxCount; -}; - -#endif /* FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHUSINGTASK_H_ */ +#ifndef FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHUSINGTASK_H_ +#define FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHUSINGTASK_H_ + +#include "../../osal/FreeRTOS/CountingSemaphUsingTask.h" +#include "../../tasks/SemaphoreIF.h" + +extern "C" { +#include +#include +} + +/** + * @brief Couting Semaphore implementation which uses the notification value + * of the task. The notification value should therefore not be used + * for other purposes. + * @details + * Additional information: https://www.freertos.org/RTOS-task-notifications.html + * and general semaphore documentation. + */ +class CountingSemaphoreUsingTask: public SemaphoreIF { +public: + CountingSemaphoreUsingTask(const uint8_t maxCount, uint8_t initCount); + virtual ~CountingSemaphoreUsingTask(); + + /** + * Acquire the counting semaphore. + * If no semaphores are available, the task will be blocked + * for a maximum of #timeoutMs or until one is given back, + * for example by an ISR or another task. + * @param timeoutMs + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout + */ + ReturnValue_t acquire(TimeoutType timeoutType = TimeoutType::BLOCKING, + uint32_t timeoutMs = portMAX_DELAY) override; + + /** + * Release a semaphore, increasing the number of available counting + * semaphores up to the #maxCount value. + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if #maxCount semaphores are + * already available. + */ + ReturnValue_t release() override; + + uint8_t getSemaphoreCounter() const override; + /** + * Get the semaphore counter from an ISR. + * @param task + * @param higherPriorityTaskWoken This will be set to pdPASS if a task with + * a higher priority was unblocked. A context switch should be requested + * from an ISR if this is the case (see TaskManagement functions) + * @return + */ + static uint8_t getSemaphoreCounterFromISR(TaskHandle_t task, + BaseType_t* higherPriorityTaskWoken); + + /** + * Acquire with a timeout value in ticks + * @param timeoutTicks + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout + */ + ReturnValue_t acquireWithTickTimeout( + TimeoutType timeoutType = TimeoutType::BLOCKING, + TickType_t timeoutTicks = portMAX_DELAY); + + /** + * Get handle to the task related to the semaphore. + * @return + */ + TaskHandle_t getTaskHandle(); + + /** + * Release semaphore of task by supplying task handle + * @param taskToNotify + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if #maxCount semaphores are + * already available. + */ + static ReturnValue_t release(TaskHandle_t taskToNotify); + /** + * Release seamphore of a task from an ISR. + * @param taskToNotify + * @param higherPriorityTaskWoken This will be set to pdPASS if a task with + * a higher priority was unblocked. A context switch should be requested + * from an ISR if this is the case (see TaskManagement functions) + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if #maxCount semaphores are + * already available. + */ + static ReturnValue_t releaseFromISR(TaskHandle_t taskToNotify, + BaseType_t* higherPriorityTaskWoken); + + uint8_t getMaxCount() const; + +private: + TaskHandle_t handle; + const uint8_t maxCount; +}; + +#endif /* FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHUSINGTASK_H_ */ diff --git a/osal/FreeRTOS/CountingSemaphore.cpp b/osal/FreeRTOS/CountingSemaphore.cpp index 0b218e54..d1310a6a 100644 --- a/osal/FreeRTOS/CountingSemaphore.cpp +++ b/osal/FreeRTOS/CountingSemaphore.cpp @@ -1,43 +1,43 @@ -#include "../../osal/FreeRTOS/CountingSemaphore.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" -#include "../../osal/FreeRTOS/TaskManagement.h" - -#include - -// Make sure #define configUSE_COUNTING_SEMAPHORES 1 is set in -// free FreeRTOSConfig.h file. -CountingSemaphore::CountingSemaphore(const uint8_t maxCount, uint8_t initCount): - maxCount(maxCount), initCount(initCount) { - if(initCount > maxCount) { - sif::error << "CountingSemaphoreUsingTask: Max count bigger than " - "intial cout. Setting initial count to max count." << std::endl; - initCount = maxCount; - } - - handle = xSemaphoreCreateCounting(maxCount, initCount); - if(handle == nullptr) { - sif::error << "CountingSemaphore: Creation failure" << std::endl; - } -} - -CountingSemaphore::CountingSemaphore(CountingSemaphore&& other): - maxCount(other.maxCount), initCount(other.initCount) { - handle = xSemaphoreCreateCounting(other.maxCount, other.initCount); - if(handle == nullptr) { - sif::error << "CountingSemaphore: Creation failure" << std::endl; - } -} - -CountingSemaphore& CountingSemaphore::operator =( - CountingSemaphore&& other) { - handle = xSemaphoreCreateCounting(other.maxCount, other.initCount); - if(handle == nullptr) { - sif::error << "CountingSemaphore: Creation failure" << std::endl; - } - return * this; -} - - -uint8_t CountingSemaphore::getMaxCount() const { - return maxCount; -} +#include "../../osal/FreeRTOS/CountingSemaphore.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" +#include "../../osal/FreeRTOS/TaskManagement.h" + +#include + +// Make sure #define configUSE_COUNTING_SEMAPHORES 1 is set in +// free FreeRTOSConfig.h file. +CountingSemaphore::CountingSemaphore(const uint8_t maxCount, uint8_t initCount): + maxCount(maxCount), initCount(initCount) { + if(initCount > maxCount) { + sif::error << "CountingSemaphoreUsingTask: Max count bigger than " + "intial cout. Setting initial count to max count." << std::endl; + initCount = maxCount; + } + + handle = xSemaphoreCreateCounting(maxCount, initCount); + if(handle == nullptr) { + sif::error << "CountingSemaphore: Creation failure" << std::endl; + } +} + +CountingSemaphore::CountingSemaphore(CountingSemaphore&& other): + maxCount(other.maxCount), initCount(other.initCount) { + handle = xSemaphoreCreateCounting(other.maxCount, other.initCount); + if(handle == nullptr) { + sif::error << "CountingSemaphore: Creation failure" << std::endl; + } +} + +CountingSemaphore& CountingSemaphore::operator =( + CountingSemaphore&& other) { + handle = xSemaphoreCreateCounting(other.maxCount, other.initCount); + if(handle == nullptr) { + sif::error << "CountingSemaphore: Creation failure" << std::endl; + } + return * this; +} + + +uint8_t CountingSemaphore::getMaxCount() const { + return maxCount; +} diff --git a/osal/FreeRTOS/CountingSemaphore.h b/osal/FreeRTOS/CountingSemaphore.h index 9432ed81..ae2f62ae 100644 --- a/osal/FreeRTOS/CountingSemaphore.h +++ b/osal/FreeRTOS/CountingSemaphore.h @@ -1,34 +1,34 @@ -#ifndef FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_ -#define FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_ -#include "../../osal/FreeRTOS/BinarySemaphore.h" - -/** - * @brief Counting semaphores, which can be acquire more than once. - * @details - * See: https://www.freertos.org/CreateCounting.html - * API of counting semaphores is almost identical to binary semaphores, - * so we just inherit from binary semaphore and provide the respective - * constructors. - */ -class CountingSemaphore: public BinarySemaphore { -public: - CountingSemaphore(const uint8_t maxCount, uint8_t initCount); - //! @brief Copy ctor, disabled - CountingSemaphore(const CountingSemaphore&) = delete; - //! @brief Copy assignment, disabled - CountingSemaphore& operator=(const CountingSemaphore&) = delete; - //! @brief Move ctor - CountingSemaphore (CountingSemaphore &&); - //! @brief Move assignment - CountingSemaphore & operator=(CountingSemaphore &&); - - /* Same API as binary semaphore otherwise. acquire() can be called - * until there are not semaphores left and release() can be called - * until maxCount is reached. */ - uint8_t getMaxCount() const; -private: - const uint8_t maxCount; - uint8_t initCount = 0; -}; - -#endif /* FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_ */ +#ifndef FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_ +#define FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_ +#include "../../osal/FreeRTOS/BinarySemaphore.h" + +/** + * @brief Counting semaphores, which can be acquire more than once. + * @details + * See: https://www.freertos.org/CreateCounting.html + * API of counting semaphores is almost identical to binary semaphores, + * so we just inherit from binary semaphore and provide the respective + * constructors. + */ +class CountingSemaphore: public BinarySemaphore { +public: + CountingSemaphore(const uint8_t maxCount, uint8_t initCount); + //! @brief Copy ctor, disabled + CountingSemaphore(const CountingSemaphore&) = delete; + //! @brief Copy assignment, disabled + CountingSemaphore& operator=(const CountingSemaphore&) = delete; + //! @brief Move ctor + CountingSemaphore (CountingSemaphore &&); + //! @brief Move assignment + CountingSemaphore & operator=(CountingSemaphore &&); + + /* Same API as binary semaphore otherwise. acquire() can be called + * until there are not semaphores left and release() can be called + * until maxCount is reached. */ + uint8_t getMaxCount() const; +private: + const uint8_t maxCount; + uint8_t initCount = 0; +}; + +#endif /* FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_ */ diff --git a/osal/FreeRTOS/SemaphoreFactory.cpp b/osal/FreeRTOS/SemaphoreFactory.cpp index 8575cf4a..beb0d096 100644 --- a/osal/FreeRTOS/SemaphoreFactory.cpp +++ b/osal/FreeRTOS/SemaphoreFactory.cpp @@ -1,59 +1,59 @@ -#include "../../osal/FreeRTOS/BinarySemaphore.h" -#include "../../osal/FreeRTOS/BinSemaphUsingTask.h" -#include "../../osal/FreeRTOS/CountingSemaphore.h" -#include "../../osal/FreeRTOS/CountingSemaphUsingTask.h" -#include "../../tasks/SemaphoreFactory.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" - -SemaphoreFactory* SemaphoreFactory::factoryInstance = nullptr; - -static const uint32_t USE_REGULAR_SEMAPHORES = 0; -static const uint32_t USE_TASK_NOTIFICATIONS = 1; - -SemaphoreFactory::SemaphoreFactory() { -} - -SemaphoreFactory::~SemaphoreFactory() { - delete factoryInstance; -} - -SemaphoreFactory* SemaphoreFactory::instance() { - if (factoryInstance == nullptr){ - factoryInstance = new SemaphoreFactory(); - } - return SemaphoreFactory::factoryInstance; -} - -SemaphoreIF* SemaphoreFactory::createBinarySemaphore(uint32_t argument) { - if(argument == USE_REGULAR_SEMAPHORES) { - return new BinarySemaphore(); - } - else if(argument == USE_TASK_NOTIFICATIONS) { - return new BinarySemaphoreUsingTask(); - } - else { - sif::warning << "SemaphoreFactory: Invalid argument, return regular" - "binary semaphore" << std::endl; - return new BinarySemaphore(); - } -} - -SemaphoreIF* SemaphoreFactory::createCountingSemaphore(uint8_t maxCount, - uint8_t initCount, uint32_t argument) { - if(argument == USE_REGULAR_SEMAPHORES) { - return new CountingSemaphore(maxCount, initCount); - } - else if(argument == USE_TASK_NOTIFICATIONS) { - return new CountingSemaphoreUsingTask(maxCount, initCount); - } - else { - sif::warning << "SemaphoreFactory: Invalid argument, return regular" - "binary semaphore" << std::endl; - return new CountingSemaphore(maxCount, initCount); - } - -} - -void SemaphoreFactory::deleteSemaphore(SemaphoreIF* semaphore) { - delete semaphore; -} +#include "../../osal/FreeRTOS/BinarySemaphore.h" +#include "../../osal/FreeRTOS/BinSemaphUsingTask.h" +#include "../../osal/FreeRTOS/CountingSemaphore.h" +#include "../../osal/FreeRTOS/CountingSemaphUsingTask.h" +#include "../../tasks/SemaphoreFactory.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" + +SemaphoreFactory* SemaphoreFactory::factoryInstance = nullptr; + +static const uint32_t USE_REGULAR_SEMAPHORES = 0; +static const uint32_t USE_TASK_NOTIFICATIONS = 1; + +SemaphoreFactory::SemaphoreFactory() { +} + +SemaphoreFactory::~SemaphoreFactory() { + delete factoryInstance; +} + +SemaphoreFactory* SemaphoreFactory::instance() { + if (factoryInstance == nullptr){ + factoryInstance = new SemaphoreFactory(); + } + return SemaphoreFactory::factoryInstance; +} + +SemaphoreIF* SemaphoreFactory::createBinarySemaphore(uint32_t argument) { + if(argument == USE_REGULAR_SEMAPHORES) { + return new BinarySemaphore(); + } + else if(argument == USE_TASK_NOTIFICATIONS) { + return new BinarySemaphoreUsingTask(); + } + else { + sif::warning << "SemaphoreFactory: Invalid argument, return regular" + "binary semaphore" << std::endl; + return new BinarySemaphore(); + } +} + +SemaphoreIF* SemaphoreFactory::createCountingSemaphore(uint8_t maxCount, + uint8_t initCount, uint32_t argument) { + if(argument == USE_REGULAR_SEMAPHORES) { + return new CountingSemaphore(maxCount, initCount); + } + else if(argument == USE_TASK_NOTIFICATIONS) { + return new CountingSemaphoreUsingTask(maxCount, initCount); + } + else { + sif::warning << "SemaphoreFactory: Invalid argument, return regular" + "binary semaphore" << std::endl; + return new CountingSemaphore(maxCount, initCount); + } + +} + +void SemaphoreFactory::deleteSemaphore(SemaphoreIF* semaphore) { + delete semaphore; +} diff --git a/osal/FreeRTOS/TaskManagement.cpp b/osal/FreeRTOS/TaskManagement.cpp index 16682d36..b77f12a9 100644 --- a/osal/FreeRTOS/TaskManagement.cpp +++ b/osal/FreeRTOS/TaskManagement.cpp @@ -1,24 +1,24 @@ -#include "../../osal/FreeRTOS/TaskManagement.h" - -void TaskManagement::vRequestContextSwitchFromTask() { - vTaskDelay(0); -} - -void TaskManagement::requestContextSwitch( - CallContext callContext = CallContext::TASK) { - if(callContext == CallContext::ISR) { - // This function depends on the partmacro.h definition for the specific device - vRequestContextSwitchFromISR(); - } else { - vRequestContextSwitchFromTask(); - } -} - -TaskHandle_t TaskManagement::getCurrentTaskHandle() { - return xTaskGetCurrentTaskHandle(); -} - -size_t TaskManagement::getTaskStackHighWatermark( - TaskHandle_t task) { - return uxTaskGetStackHighWaterMark(task) * sizeof(StackType_t); -} +#include "../../osal/FreeRTOS/TaskManagement.h" + +void TaskManagement::vRequestContextSwitchFromTask() { + vTaskDelay(0); +} + +void TaskManagement::requestContextSwitch( + CallContext callContext = CallContext::TASK) { + if(callContext == CallContext::ISR) { + // This function depends on the partmacro.h definition for the specific device + vRequestContextSwitchFromISR(); + } else { + vRequestContextSwitchFromTask(); + } +} + +TaskHandle_t TaskManagement::getCurrentTaskHandle() { + return xTaskGetCurrentTaskHandle(); +} + +size_t TaskManagement::getTaskStackHighWatermark( + TaskHandle_t task) { + return uxTaskGetStackHighWaterMark(task) * sizeof(StackType_t); +} diff --git a/osal/FreeRTOS/TaskManagement.h b/osal/FreeRTOS/TaskManagement.h index 43003d76..4b7fe3eb 100644 --- a/osal/FreeRTOS/TaskManagement.h +++ b/osal/FreeRTOS/TaskManagement.h @@ -1,64 +1,64 @@ -#ifndef FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ -#define FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ - -#include "../../returnvalues/HasReturnvaluesIF.h" - -extern "C" { -#include -#include -} -#include - -/** - * Architecture dependant portmacro.h function call. - * Should be implemented in bsp. - */ -extern void vRequestContextSwitchFromISR(); - -/*! - * Used by functions to tell if they are being called from - * within an ISR or from a regular task. This is required because FreeRTOS - * has different functions for handling semaphores and messages from within - * an ISR and task. - */ -enum class CallContext { - TASK = 0x00,//!< task_context - ISR = 0xFF //!< isr_context -}; - - -class TaskManagement { -public: - /** - * @brief In this function, a function dependant on the portmacro.h header - * function calls to request a context switch can be specified. - * This can be used if sending to the queue from an ISR caused a task - * to unblock and a context switch is required. - */ - static void requestContextSwitch(CallContext callContext); - - /** - * If task preemption in FreeRTOS is disabled, a context switch - * can be requested manually by calling this function. - */ - static void vRequestContextSwitchFromTask(void); - - /** - * @return The current task handle - */ - static TaskHandle_t getCurrentTaskHandle(); - - /** - * Get returns the minimum amount of remaining stack space in words - * that was a available to the task since the task started executing. - * Please note that the actual value in bytes depends - * on the stack depth type. - * E.g. on a 32 bit machine, a value of 200 means 800 bytes. - * @return Smallest value of stack remaining since the task was started in - * words. - */ - static size_t getTaskStackHighWatermark( - TaskHandle_t task = nullptr); -}; - -#endif /* FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ */ +#ifndef FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ +#define FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ + +#include "../../returnvalues/HasReturnvaluesIF.h" + +extern "C" { +#include +#include +} +#include + +/** + * Architecture dependant portmacro.h function call. + * Should be implemented in bsp. + */ +extern void vRequestContextSwitchFromISR(); + +/*! + * Used by functions to tell if they are being called from + * within an ISR or from a regular task. This is required because FreeRTOS + * has different functions for handling semaphores and messages from within + * an ISR and task. + */ +enum class CallContext { + TASK = 0x00,//!< task_context + ISR = 0xFF //!< isr_context +}; + + +class TaskManagement { +public: + /** + * @brief In this function, a function dependant on the portmacro.h header + * function calls to request a context switch can be specified. + * This can be used if sending to the queue from an ISR caused a task + * to unblock and a context switch is required. + */ + static void requestContextSwitch(CallContext callContext); + + /** + * If task preemption in FreeRTOS is disabled, a context switch + * can be requested manually by calling this function. + */ + static void vRequestContextSwitchFromTask(void); + + /** + * @return The current task handle + */ + static TaskHandle_t getCurrentTaskHandle(); + + /** + * Get returns the minimum amount of remaining stack space in words + * that was a available to the task since the task started executing. + * Please note that the actual value in bytes depends + * on the stack depth type. + * E.g. on a 32 bit machine, a value of 200 means 800 bytes. + * @return Smallest value of stack remaining since the task was started in + * words. + */ + static size_t getTaskStackHighWatermark( + TaskHandle_t task = nullptr); +}; + +#endif /* FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ */ diff --git a/osal/linux/BinarySemaphore.cpp b/osal/linux/BinarySemaphore.cpp index 5216ff37..8c0eeae7 100644 --- a/osal/linux/BinarySemaphore.cpp +++ b/osal/linux/BinarySemaphore.cpp @@ -1,149 +1,149 @@ -#include "../../osal/linux/BinarySemaphore.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" - -extern "C" { -#include -#include -} - -BinarySemaphore::BinarySemaphore() { - // Using unnamed semaphores for now - initSemaphore(); -} - -BinarySemaphore::~BinarySemaphore() { - sem_destroy(&handle); -} - -BinarySemaphore::BinarySemaphore(BinarySemaphore&& s) { - initSemaphore(); -} - -BinarySemaphore& BinarySemaphore::operator =( - BinarySemaphore&& s) { - initSemaphore(); - return * this; -} - -ReturnValue_t BinarySemaphore::acquire(TimeoutType timeoutType, - uint32_t timeoutMs) { - int result = 0; - if(timeoutType == TimeoutType::POLLING) { - result = sem_trywait(&handle); - } - else if(timeoutType == TimeoutType::BLOCKING) { - result = sem_wait(&handle); - } - else if(timeoutType == TimeoutType::WAITING){ - timespec timeOut; - clock_gettime(CLOCK_REALTIME, &timeOut); - uint64_t nseconds = timeOut.tv_sec * 1000000000 + timeOut.tv_nsec; - nseconds += timeoutMs * 1000000; - timeOut.tv_sec = nseconds / 1000000000; - timeOut.tv_nsec = nseconds - timeOut.tv_sec * 1000000000; - result = sem_timedwait(&handle, &timeOut); - if(result != 0 and errno == EINVAL) { - sif::debug << "BinarySemaphore::acquire: Invalid time value possible" - << std::endl; - } - } - if(result == 0) { - return HasReturnvaluesIF::RETURN_OK; - } - - switch(errno) { - case(EAGAIN): - // Operation could not be performed without blocking (for sem_trywait) - case(ETIMEDOUT): - // Semaphore is 0 - return SemaphoreIF::SEMAPHORE_TIMEOUT; - case(EINVAL): - // Semaphore invalid - return SemaphoreIF::SEMAPHORE_INVALID; - case(EINTR): - // Call was interrupted by signal handler - sif::debug << "BinarySemaphore::acquire: Signal handler interrupted." - "Code " << strerror(errno) << std::endl; - /* No break */ - default: - return HasReturnvaluesIF::RETURN_FAILED; - } -} - -ReturnValue_t BinarySemaphore::release() { - return BinarySemaphore::release(&this->handle); -} - -ReturnValue_t BinarySemaphore::release(sem_t *handle) { - ReturnValue_t countResult = checkCount(handle, 1); - if(countResult != HasReturnvaluesIF::RETURN_OK) { - return countResult; - } - - int result = sem_post(handle); - if(result == 0) { - return HasReturnvaluesIF::RETURN_OK; - } - - switch(errno) { - case(EINVAL): - // Semaphore invalid - return SemaphoreIF::SEMAPHORE_INVALID; - case(EOVERFLOW): - // SEM_MAX_VALUE overflow. This should never happen - default: - return HasReturnvaluesIF::RETURN_FAILED; - } -} - -uint8_t BinarySemaphore::getSemaphoreCounter() const { - // And another ugly cast :-D - return getSemaphoreCounter(const_cast(&this->handle)); -} - -uint8_t BinarySemaphore::getSemaphoreCounter(sem_t *handle) { - int value = 0; - int result = sem_getvalue(handle, &value); - if (result == 0) { - return value; - } - else if(result != 0 and errno == EINVAL) { - // Could be called from interrupt, use lightweight printf - printf("BinarySemaphore::getSemaphoreCounter: Invalid semaphore\n"); - return 0; - } - else { - // This should never happen. - return 0; - } -} - -void BinarySemaphore::initSemaphore(uint8_t initCount) { - auto result = sem_init(&handle, true, initCount); - if(result == -1) { - switch(errno) { - case(EINVAL): - // Value exceeds SEM_VALUE_MAX - case(ENOSYS): - // System does not support process-shared semaphores - sif::error << "BinarySemaphore: Init failed with" << strerror(errno) - << std::endl; - } - } -} - -ReturnValue_t BinarySemaphore::checkCount(sem_t* handle, uint8_t maxCount) { - int value = getSemaphoreCounter(handle); - if(value >= maxCount) { - if(maxCount == 1 and value > 1) { - // Binary Semaphore special case. - // This is a config error use lightweight printf is this is called - // from an interrupt - printf("BinarySemaphore::release: Value of binary semaphore greater" - " than 1!\n"); - return HasReturnvaluesIF::RETURN_FAILED; - } - return SemaphoreIF::SEMAPHORE_NOT_OWNED; - } - return HasReturnvaluesIF::RETURN_OK; -} +#include "../../osal/linux/BinarySemaphore.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" + +extern "C" { +#include +#include +} + +BinarySemaphore::BinarySemaphore() { + // Using unnamed semaphores for now + initSemaphore(); +} + +BinarySemaphore::~BinarySemaphore() { + sem_destroy(&handle); +} + +BinarySemaphore::BinarySemaphore(BinarySemaphore&& s) { + initSemaphore(); +} + +BinarySemaphore& BinarySemaphore::operator =( + BinarySemaphore&& s) { + initSemaphore(); + return * this; +} + +ReturnValue_t BinarySemaphore::acquire(TimeoutType timeoutType, + uint32_t timeoutMs) { + int result = 0; + if(timeoutType == TimeoutType::POLLING) { + result = sem_trywait(&handle); + } + else if(timeoutType == TimeoutType::BLOCKING) { + result = sem_wait(&handle); + } + else if(timeoutType == TimeoutType::WAITING){ + timespec timeOut; + clock_gettime(CLOCK_REALTIME, &timeOut); + uint64_t nseconds = timeOut.tv_sec * 1000000000 + timeOut.tv_nsec; + nseconds += timeoutMs * 1000000; + timeOut.tv_sec = nseconds / 1000000000; + timeOut.tv_nsec = nseconds - timeOut.tv_sec * 1000000000; + result = sem_timedwait(&handle, &timeOut); + if(result != 0 and errno == EINVAL) { + sif::debug << "BinarySemaphore::acquire: Invalid time value possible" + << std::endl; + } + } + if(result == 0) { + return HasReturnvaluesIF::RETURN_OK; + } + + switch(errno) { + case(EAGAIN): + // Operation could not be performed without blocking (for sem_trywait) + case(ETIMEDOUT): + // Semaphore is 0 + return SemaphoreIF::SEMAPHORE_TIMEOUT; + case(EINVAL): + // Semaphore invalid + return SemaphoreIF::SEMAPHORE_INVALID; + case(EINTR): + // Call was interrupted by signal handler + sif::debug << "BinarySemaphore::acquire: Signal handler interrupted." + "Code " << strerror(errno) << std::endl; + /* No break */ + default: + return HasReturnvaluesIF::RETURN_FAILED; + } +} + +ReturnValue_t BinarySemaphore::release() { + return BinarySemaphore::release(&this->handle); +} + +ReturnValue_t BinarySemaphore::release(sem_t *handle) { + ReturnValue_t countResult = checkCount(handle, 1); + if(countResult != HasReturnvaluesIF::RETURN_OK) { + return countResult; + } + + int result = sem_post(handle); + if(result == 0) { + return HasReturnvaluesIF::RETURN_OK; + } + + switch(errno) { + case(EINVAL): + // Semaphore invalid + return SemaphoreIF::SEMAPHORE_INVALID; + case(EOVERFLOW): + // SEM_MAX_VALUE overflow. This should never happen + default: + return HasReturnvaluesIF::RETURN_FAILED; + } +} + +uint8_t BinarySemaphore::getSemaphoreCounter() const { + // And another ugly cast :-D + return getSemaphoreCounter(const_cast(&this->handle)); +} + +uint8_t BinarySemaphore::getSemaphoreCounter(sem_t *handle) { + int value = 0; + int result = sem_getvalue(handle, &value); + if (result == 0) { + return value; + } + else if(result != 0 and errno == EINVAL) { + // Could be called from interrupt, use lightweight printf + printf("BinarySemaphore::getSemaphoreCounter: Invalid semaphore\n"); + return 0; + } + else { + // This should never happen. + return 0; + } +} + +void BinarySemaphore::initSemaphore(uint8_t initCount) { + auto result = sem_init(&handle, true, initCount); + if(result == -1) { + switch(errno) { + case(EINVAL): + // Value exceeds SEM_VALUE_MAX + case(ENOSYS): + // System does not support process-shared semaphores + sif::error << "BinarySemaphore: Init failed with" << strerror(errno) + << std::endl; + } + } +} + +ReturnValue_t BinarySemaphore::checkCount(sem_t* handle, uint8_t maxCount) { + int value = getSemaphoreCounter(handle); + if(value >= maxCount) { + if(maxCount == 1 and value > 1) { + // Binary Semaphore special case. + // This is a config error use lightweight printf is this is called + // from an interrupt + printf("BinarySemaphore::release: Value of binary semaphore greater" + " than 1!\n"); + return HasReturnvaluesIF::RETURN_FAILED; + } + return SemaphoreIF::SEMAPHORE_NOT_OWNED; + } + return HasReturnvaluesIF::RETURN_OK; +} diff --git a/osal/linux/BinarySemaphore.h b/osal/linux/BinarySemaphore.h index 4181de85..e9bb8bb6 100644 --- a/osal/linux/BinarySemaphore.h +++ b/osal/linux/BinarySemaphore.h @@ -1,81 +1,81 @@ -#ifndef FRAMEWORK_OSAL_LINUX_BINARYSEMPAHORE_H_ -#define FRAMEWORK_OSAL_LINUX_BINARYSEMPAHORE_H_ - -#include "../../returnvalues/HasReturnvaluesIF.h" -#include "../../tasks/SemaphoreIF.h" - -extern "C" { -#include -} - -/** - * @brief OS Tool to achieve synchronization of between tasks or between - * task and ISR. The default semaphore implementation creates a - * binary semaphore, which can only be taken once. - * @details - * See: http://www.man7.org/linux/man-pages/man7/sem_overview.7.html - * @author R. Mueller - * @ingroup osal - */ -class BinarySemaphore: public SemaphoreIF, - public HasReturnvaluesIF { -public: - static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF; - - //! @brief Default ctor - BinarySemaphore(); - //! @brief Copy ctor, deleted explicitely. - BinarySemaphore(const BinarySemaphore&) = delete; - //! @brief Copy assignment, deleted explicitely. - BinarySemaphore& operator=(const BinarySemaphore&) = delete; - //! @brief Move ctor - BinarySemaphore (BinarySemaphore &&); - //! @brief Move assignment - BinarySemaphore & operator=(BinarySemaphore &&); - //! @brief Destructor - virtual ~BinarySemaphore(); - - void initSemaphore(uint8_t initCount = 1); - - uint8_t getSemaphoreCounter() const override; - static uint8_t getSemaphoreCounter(sem_t* handle); - - /** - * Take the binary semaphore. - * If the semaphore has already been taken, the task will be blocked - * for a maximum of #timeoutMs or until the semaphore is given back, - * for example by an ISR or another task. - * @param timeoutMs - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout - */ - ReturnValue_t acquire(TimeoutType timeoutType = TimeoutType::BLOCKING, - uint32_t timeoutMs = 0) override; - - /** - * Release the binary semaphore. - * @return -@c RETURN_OK on success - * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if the semaphores is - * already available. - */ - virtual ReturnValue_t release() override; - /** - * This static function can be used to release a semaphore by providing - * its handle. - * @param handle - * @return - */ - static ReturnValue_t release(sem_t* handle); - - /** Checks the validity of the semaphore count against a specified - * known maxCount - * @param handle - * @param maxCount - * @return - */ - static ReturnValue_t checkCount(sem_t* handle, uint8_t maxCount); -protected: - sem_t handle; -}; - -#endif /* FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ */ +#ifndef FRAMEWORK_OSAL_LINUX_BINARYSEMPAHORE_H_ +#define FRAMEWORK_OSAL_LINUX_BINARYSEMPAHORE_H_ + +#include "../../returnvalues/HasReturnvaluesIF.h" +#include "../../tasks/SemaphoreIF.h" + +extern "C" { +#include +} + +/** + * @brief OS Tool to achieve synchronization of between tasks or between + * task and ISR. The default semaphore implementation creates a + * binary semaphore, which can only be taken once. + * @details + * See: http://www.man7.org/linux/man-pages/man7/sem_overview.7.html + * @author R. Mueller + * @ingroup osal + */ +class BinarySemaphore: public SemaphoreIF, + public HasReturnvaluesIF { +public: + static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF; + + //! @brief Default ctor + BinarySemaphore(); + //! @brief Copy ctor, deleted explicitely. + BinarySemaphore(const BinarySemaphore&) = delete; + //! @brief Copy assignment, deleted explicitely. + BinarySemaphore& operator=(const BinarySemaphore&) = delete; + //! @brief Move ctor + BinarySemaphore (BinarySemaphore &&); + //! @brief Move assignment + BinarySemaphore & operator=(BinarySemaphore &&); + //! @brief Destructor + virtual ~BinarySemaphore(); + + void initSemaphore(uint8_t initCount = 1); + + uint8_t getSemaphoreCounter() const override; + static uint8_t getSemaphoreCounter(sem_t* handle); + + /** + * Take the binary semaphore. + * If the semaphore has already been taken, the task will be blocked + * for a maximum of #timeoutMs or until the semaphore is given back, + * for example by an ISR or another task. + * @param timeoutMs + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout + */ + ReturnValue_t acquire(TimeoutType timeoutType = TimeoutType::BLOCKING, + uint32_t timeoutMs = 0) override; + + /** + * Release the binary semaphore. + * @return -@c RETURN_OK on success + * -@c SemaphoreIF::SEMAPHORE_NOT_OWNED if the semaphores is + * already available. + */ + virtual ReturnValue_t release() override; + /** + * This static function can be used to release a semaphore by providing + * its handle. + * @param handle + * @return + */ + static ReturnValue_t release(sem_t* handle); + + /** Checks the validity of the semaphore count against a specified + * known maxCount + * @param handle + * @param maxCount + * @return + */ + static ReturnValue_t checkCount(sem_t* handle, uint8_t maxCount); +protected: + sem_t handle; +}; + +#endif /* FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ */ diff --git a/osal/linux/CountingSemaphore.cpp b/osal/linux/CountingSemaphore.cpp index ef32539b..18339399 100644 --- a/osal/linux/CountingSemaphore.cpp +++ b/osal/linux/CountingSemaphore.cpp @@ -1,54 +1,54 @@ -#include "../../osal/linux/CountingSemaphore.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" - -CountingSemaphore::CountingSemaphore(const uint8_t maxCount, uint8_t initCount): - maxCount(maxCount), initCount(initCount) { - if(initCount > maxCount) { - sif::error << "CountingSemaphoreUsingTask: Max count bigger than " - "intial cout. Setting initial count to max count." << std::endl; - initCount = maxCount; - } - - initSemaphore(initCount); -} - -CountingSemaphore::CountingSemaphore(CountingSemaphore&& other): - maxCount(other.maxCount), initCount(other.initCount) { - initSemaphore(initCount); -} - -CountingSemaphore& CountingSemaphore::operator =( - CountingSemaphore&& other) { - initSemaphore(other.initCount); - return * this; -} - -ReturnValue_t CountingSemaphore::release() { - ReturnValue_t result = checkCount(&handle, maxCount); - if(result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - return CountingSemaphore::release(&this->handle); -} - -ReturnValue_t CountingSemaphore::release(sem_t* handle) { - int result = sem_post(handle); - if(result == 0) { - return HasReturnvaluesIF::RETURN_OK; - } - - switch(errno) { - case(EINVAL): - // Semaphore invalid - return SemaphoreIF::SEMAPHORE_INVALID; - case(EOVERFLOW): - // SEM_MAX_VALUE overflow. This should never happen - default: - return HasReturnvaluesIF::RETURN_FAILED; - } -} - -uint8_t CountingSemaphore::getMaxCount() const { - return maxCount; -} - +#include "../../osal/linux/CountingSemaphore.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" + +CountingSemaphore::CountingSemaphore(const uint8_t maxCount, uint8_t initCount): + maxCount(maxCount), initCount(initCount) { + if(initCount > maxCount) { + sif::error << "CountingSemaphoreUsingTask: Max count bigger than " + "intial cout. Setting initial count to max count." << std::endl; + initCount = maxCount; + } + + initSemaphore(initCount); +} + +CountingSemaphore::CountingSemaphore(CountingSemaphore&& other): + maxCount(other.maxCount), initCount(other.initCount) { + initSemaphore(initCount); +} + +CountingSemaphore& CountingSemaphore::operator =( + CountingSemaphore&& other) { + initSemaphore(other.initCount); + return * this; +} + +ReturnValue_t CountingSemaphore::release() { + ReturnValue_t result = checkCount(&handle, maxCount); + if(result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + return CountingSemaphore::release(&this->handle); +} + +ReturnValue_t CountingSemaphore::release(sem_t* handle) { + int result = sem_post(handle); + if(result == 0) { + return HasReturnvaluesIF::RETURN_OK; + } + + switch(errno) { + case(EINVAL): + // Semaphore invalid + return SemaphoreIF::SEMAPHORE_INVALID; + case(EOVERFLOW): + // SEM_MAX_VALUE overflow. This should never happen + default: + return HasReturnvaluesIF::RETURN_FAILED; + } +} + +uint8_t CountingSemaphore::getMaxCount() const { + return maxCount; +} + diff --git a/osal/linux/CountingSemaphore.h b/osal/linux/CountingSemaphore.h index afe21a61..e0fbb992 100644 --- a/osal/linux/CountingSemaphore.h +++ b/osal/linux/CountingSemaphore.h @@ -1,37 +1,37 @@ -#ifndef FRAMEWORK_OSAL_LINUX_COUNTINGSEMAPHORE_H_ -#define FRAMEWORK_OSAL_LINUX_COUNTINGSEMAPHORE_H_ -#include "../../osal/linux/BinarySemaphore.h" - -/** - * @brief Counting semaphores, which can be acquired more than once. - * @details - * See: https://www.freertos.org/CreateCounting.html - * API of counting semaphores is almost identical to binary semaphores, - * so we just inherit from binary semaphore and provide the respective - * constructors. - */ -class CountingSemaphore: public BinarySemaphore { -public: - CountingSemaphore(const uint8_t maxCount, uint8_t initCount); - //! @brief Copy ctor, disabled - CountingSemaphore(const CountingSemaphore&) = delete; - //! @brief Copy assignment, disabled - CountingSemaphore& operator=(const CountingSemaphore&) = delete; - //! @brief Move ctor - CountingSemaphore (CountingSemaphore &&); - //! @brief Move assignment - CountingSemaphore & operator=(CountingSemaphore &&); - - ReturnValue_t release() override; - static ReturnValue_t release(sem_t* sem); - /* Same API as binary semaphore otherwise. acquire() can be called - * until there are not semaphores left and release() can be called - * until maxCount is reached. */ - - uint8_t getMaxCount() const; -private: - const uint8_t maxCount; - uint8_t initCount = 0; -}; - -#endif /* FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_ */ +#ifndef FRAMEWORK_OSAL_LINUX_COUNTINGSEMAPHORE_H_ +#define FRAMEWORK_OSAL_LINUX_COUNTINGSEMAPHORE_H_ +#include "../../osal/linux/BinarySemaphore.h" + +/** + * @brief Counting semaphores, which can be acquired more than once. + * @details + * See: https://www.freertos.org/CreateCounting.html + * API of counting semaphores is almost identical to binary semaphores, + * so we just inherit from binary semaphore and provide the respective + * constructors. + */ +class CountingSemaphore: public BinarySemaphore { +public: + CountingSemaphore(const uint8_t maxCount, uint8_t initCount); + //! @brief Copy ctor, disabled + CountingSemaphore(const CountingSemaphore&) = delete; + //! @brief Copy assignment, disabled + CountingSemaphore& operator=(const CountingSemaphore&) = delete; + //! @brief Move ctor + CountingSemaphore (CountingSemaphore &&); + //! @brief Move assignment + CountingSemaphore & operator=(CountingSemaphore &&); + + ReturnValue_t release() override; + static ReturnValue_t release(sem_t* sem); + /* Same API as binary semaphore otherwise. acquire() can be called + * until there are not semaphores left and release() can be called + * until maxCount is reached. */ + + uint8_t getMaxCount() const; +private: + const uint8_t maxCount; + uint8_t initCount = 0; +}; + +#endif /* FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_ */ diff --git a/osal/linux/SemaphoreFactory.cpp b/osal/linux/SemaphoreFactory.cpp index bcb5d16f..e4710933 100644 --- a/osal/linux/SemaphoreFactory.cpp +++ b/osal/linux/SemaphoreFactory.cpp @@ -1,33 +1,33 @@ -#include "../../tasks/SemaphoreFactory.h" -#include "BinarySemaphore.h" -#include "CountingSemaphore.h" -#include "../../serviceinterface/ServiceInterfaceStream.h" - -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) { - return new BinarySemaphore(); -} - -SemaphoreIF* SemaphoreFactory::createCountingSemaphore(const uint8_t maxCount, - uint8_t initCount, uint32_t arguments) { - return new CountingSemaphore(maxCount, initCount); -} - -void SemaphoreFactory::deleteSemaphore(SemaphoreIF* semaphore) { - delete semaphore; -} +#include "../../tasks/SemaphoreFactory.h" +#include "BinarySemaphore.h" +#include "CountingSemaphore.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" + +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) { + return new BinarySemaphore(); +} + +SemaphoreIF* SemaphoreFactory::createCountingSemaphore(const uint8_t maxCount, + uint8_t initCount, uint32_t arguments) { + return new CountingSemaphore(maxCount, initCount); +} + +void SemaphoreFactory::deleteSemaphore(SemaphoreIF* semaphore) { + delete semaphore; +} diff --git a/serialize/SerialBufferAdapter.cpp b/serialize/SerialBufferAdapter.cpp index 812cd34b..1c11afd4 100644 --- a/serialize/SerialBufferAdapter.cpp +++ b/serialize/SerialBufferAdapter.cpp @@ -1,129 +1,129 @@ -#include "../serialize/SerialBufferAdapter.h" -#include "../serviceinterface/ServiceInterfaceStream.h" -#include - -template -SerialBufferAdapter::SerialBufferAdapter(const uint8_t* buffer, - count_t bufferLength, bool serializeLength) : - serializeLength(serializeLength), - constBuffer(buffer), buffer(nullptr), - bufferLength(bufferLength) {} - -template -SerialBufferAdapter::SerialBufferAdapter(uint8_t* buffer, - count_t bufferLength, bool serializeLength) : - serializeLength(serializeLength), constBuffer(buffer), buffer(buffer), - bufferLength(bufferLength) {} - - -template -SerialBufferAdapter::~SerialBufferAdapter() { -} - -template -ReturnValue_t SerialBufferAdapter::serialize(uint8_t** buffer, - size_t* size, size_t maxSize, Endianness streamEndianness) const { - if (serializeLength) { - ReturnValue_t result = SerializeAdapter::serialize(&bufferLength, - buffer, size, maxSize, streamEndianness); - if(result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - } - - if (*size + bufferLength > maxSize) { - return BUFFER_TOO_SHORT; - } - - if (this->constBuffer != nullptr) { - std::memcpy(*buffer, this->constBuffer, bufferLength); - } - else if (this->buffer != nullptr) { - // This will propably be never reached, constBuffer should always be - // set if non-const buffer is set. - std::memcpy(*buffer, this->buffer, bufferLength); - } - else { - return HasReturnvaluesIF::RETURN_FAILED; - } - *size += bufferLength; - (*buffer) += bufferLength; - return HasReturnvaluesIF::RETURN_OK; - -} - -template -size_t SerialBufferAdapter::getSerializedSize() const { - if (serializeLength) { - return bufferLength + SerializeAdapter::getSerializedSize(&bufferLength); - } else { - return bufferLength; - } -} - -template -ReturnValue_t SerialBufferAdapter::deSerialize(const uint8_t** buffer, - size_t* size, Endianness streamEndianness) { - if (this->buffer == nullptr) { - return HasReturnvaluesIF::RETURN_FAILED; - } - - if(serializeLength){ - count_t lengthField = 0; - ReturnValue_t result = SerializeAdapter::deSerialize(&lengthField, - buffer, size, streamEndianness); - if(result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - if(lengthField > bufferLength) { - return TOO_MANY_ELEMENTS; - } - bufferLength = lengthField; - } - - if (bufferLength <= *size) { - *size -= bufferLength; - std::memcpy(this->buffer, *buffer, bufferLength); - (*buffer) += bufferLength; - return HasReturnvaluesIF::RETURN_OK; - } - else { - return STREAM_TOO_SHORT; - } -} - -template -uint8_t * SerialBufferAdapter::getBuffer() { - if(buffer == nullptr) { - sif::error << "Wrong access function for stored type !" - " Use getConstBuffer()." << std::endl; - return nullptr; - } - return buffer; -} - -template -const uint8_t * SerialBufferAdapter::getConstBuffer() { - if(constBuffer == nullptr) { - sif::error << "SerialBufferAdapter::getConstBuffer:" - " Buffers are unitialized!" << std::endl; - return nullptr; - } - return constBuffer; -} - -template -void SerialBufferAdapter::setBuffer(uint8_t* buffer, - count_t bufferLength) { - this->buffer = buffer; - this->constBuffer = buffer; - this->bufferLength = bufferLength; -} - - -//forward Template declaration for linker -template class SerialBufferAdapter; -template class SerialBufferAdapter; -template class SerialBufferAdapter; -template class SerialBufferAdapter; - +#include "../serialize/SerialBufferAdapter.h" +#include "../serviceinterface/ServiceInterfaceStream.h" +#include + +template +SerialBufferAdapter::SerialBufferAdapter(const uint8_t* buffer, + count_t bufferLength, bool serializeLength) : + serializeLength(serializeLength), + constBuffer(buffer), buffer(nullptr), + bufferLength(bufferLength) {} + +template +SerialBufferAdapter::SerialBufferAdapter(uint8_t* buffer, + count_t bufferLength, bool serializeLength) : + serializeLength(serializeLength), constBuffer(buffer), buffer(buffer), + bufferLength(bufferLength) {} + + +template +SerialBufferAdapter::~SerialBufferAdapter() { +} + +template +ReturnValue_t SerialBufferAdapter::serialize(uint8_t** buffer, + size_t* size, size_t maxSize, Endianness streamEndianness) const { + if (serializeLength) { + ReturnValue_t result = SerializeAdapter::serialize(&bufferLength, + buffer, size, maxSize, streamEndianness); + if(result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + } + + if (*size + bufferLength > maxSize) { + return BUFFER_TOO_SHORT; + } + + if (this->constBuffer != nullptr) { + std::memcpy(*buffer, this->constBuffer, bufferLength); + } + else if (this->buffer != nullptr) { + // This will propably be never reached, constBuffer should always be + // set if non-const buffer is set. + std::memcpy(*buffer, this->buffer, bufferLength); + } + else { + return HasReturnvaluesIF::RETURN_FAILED; + } + *size += bufferLength; + (*buffer) += bufferLength; + return HasReturnvaluesIF::RETURN_OK; + +} + +template +size_t SerialBufferAdapter::getSerializedSize() const { + if (serializeLength) { + return bufferLength + SerializeAdapter::getSerializedSize(&bufferLength); + } else { + return bufferLength; + } +} + +template +ReturnValue_t SerialBufferAdapter::deSerialize(const uint8_t** buffer, + size_t* size, Endianness streamEndianness) { + if (this->buffer == nullptr) { + return HasReturnvaluesIF::RETURN_FAILED; + } + + if(serializeLength){ + count_t lengthField = 0; + ReturnValue_t result = SerializeAdapter::deSerialize(&lengthField, + buffer, size, streamEndianness); + if(result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + if(lengthField > bufferLength) { + return TOO_MANY_ELEMENTS; + } + bufferLength = lengthField; + } + + if (bufferLength <= *size) { + *size -= bufferLength; + std::memcpy(this->buffer, *buffer, bufferLength); + (*buffer) += bufferLength; + return HasReturnvaluesIF::RETURN_OK; + } + else { + return STREAM_TOO_SHORT; + } +} + +template +uint8_t * SerialBufferAdapter::getBuffer() { + if(buffer == nullptr) { + sif::error << "Wrong access function for stored type !" + " Use getConstBuffer()." << std::endl; + return nullptr; + } + return buffer; +} + +template +const uint8_t * SerialBufferAdapter::getConstBuffer() { + if(constBuffer == nullptr) { + sif::error << "SerialBufferAdapter::getConstBuffer:" + " Buffers are unitialized!" << std::endl; + return nullptr; + } + return constBuffer; +} + +template +void SerialBufferAdapter::setBuffer(uint8_t* buffer, + count_t bufferLength) { + this->buffer = buffer; + this->constBuffer = buffer; + this->bufferLength = bufferLength; +} + + +//forward Template declaration for linker +template class SerialBufferAdapter; +template class SerialBufferAdapter; +template class SerialBufferAdapter; +template class SerialBufferAdapter; + diff --git a/serialize/SerialBufferAdapter.h b/serialize/SerialBufferAdapter.h index c3dfcd8f..9a89e18b 100644 --- a/serialize/SerialBufferAdapter.h +++ b/serialize/SerialBufferAdapter.h @@ -1,78 +1,78 @@ -#ifndef SERIALBUFFERADAPTER_H_ -#define SERIALBUFFERADAPTER_H_ - -#include "../serialize/SerializeIF.h" -#include "../serialize/SerializeAdapter.h" - -/** - * This adapter provides an interface for SerializeIF to serialize or deserialize - * buffers with no length header but a known size. - * - * Additionally, the buffer length can be serialized too and will be put in - * front of the serialized buffer. - * - * Can be used with SerialLinkedListAdapter by declaring a SerializeElement with - * SerialElement>. - * Right now, the SerialBufferAdapter must always - * be initialized with the buffer and size ! - * - * \ingroup serialize - */ -template -class SerialBufferAdapter: public SerializeIF { -public: - - /** - * Constructor for constant uint8_t buffer. Length field can be serialized optionally. - * Type of length can be supplied as template type. - * @param buffer - * @param bufferLength - * @param serializeLength - */ - SerialBufferAdapter(const uint8_t* buffer, count_t bufferLength, - bool serializeLength = false); - - /** - * Constructor for non-constant uint8_t buffer. - * Length field can be serialized optionally. - * Type of length can be supplied as template type. - * @param buffer - * @param bufferLength - * @param serializeLength Length field will be serialized with size count_t - */ - SerialBufferAdapter(uint8_t* buffer, count_t bufferLength, - bool serializeLength = false); - - virtual ~SerialBufferAdapter(); - - virtual ReturnValue_t serialize(uint8_t** buffer, size_t* size, - size_t maxSize, Endianness streamEndianness) const override; - - virtual size_t getSerializedSize() const override; - - /** - * @brief This function deserializes a buffer into the member buffer. - * @details - * If a length field is present, it is ignored, as the size should have - * been set in the constructor. If the size is not known beforehand, - * consider using SerialFixedArrayListAdapter instead. - * @param buffer [out] Resulting buffer - * @param size remaining size to deserialize, should be larger than buffer - * + size field size - * @param bigEndian - * @return - */ - virtual ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size, - Endianness streamEndianness) override; - - uint8_t * getBuffer(); - const uint8_t * getConstBuffer(); - void setBuffer(uint8_t* buffer, count_t bufferLength); -private: - bool serializeLength = false; - const uint8_t *constBuffer = nullptr; - uint8_t *buffer = nullptr; - count_t bufferLength = 0; -}; - -#endif /* SERIALBUFFERADAPTER_H_ */ +#ifndef SERIALBUFFERADAPTER_H_ +#define SERIALBUFFERADAPTER_H_ + +#include "../serialize/SerializeIF.h" +#include "../serialize/SerializeAdapter.h" + +/** + * This adapter provides an interface for SerializeIF to serialize or deserialize + * buffers with no length header but a known size. + * + * Additionally, the buffer length can be serialized too and will be put in + * front of the serialized buffer. + * + * Can be used with SerialLinkedListAdapter by declaring a SerializeElement with + * SerialElement>. + * Right now, the SerialBufferAdapter must always + * be initialized with the buffer and size ! + * + * \ingroup serialize + */ +template +class SerialBufferAdapter: public SerializeIF { +public: + + /** + * Constructor for constant uint8_t buffer. Length field can be serialized optionally. + * Type of length can be supplied as template type. + * @param buffer + * @param bufferLength + * @param serializeLength + */ + SerialBufferAdapter(const uint8_t* buffer, count_t bufferLength, + bool serializeLength = false); + + /** + * Constructor for non-constant uint8_t buffer. + * Length field can be serialized optionally. + * Type of length can be supplied as template type. + * @param buffer + * @param bufferLength + * @param serializeLength Length field will be serialized with size count_t + */ + SerialBufferAdapter(uint8_t* buffer, count_t bufferLength, + bool serializeLength = false); + + virtual ~SerialBufferAdapter(); + + virtual ReturnValue_t serialize(uint8_t** buffer, size_t* size, + size_t maxSize, Endianness streamEndianness) const override; + + virtual size_t getSerializedSize() const override; + + /** + * @brief This function deserializes a buffer into the member buffer. + * @details + * If a length field is present, it is ignored, as the size should have + * been set in the constructor. If the size is not known beforehand, + * consider using SerialFixedArrayListAdapter instead. + * @param buffer [out] Resulting buffer + * @param size remaining size to deserialize, should be larger than buffer + * + size field size + * @param bigEndian + * @return + */ + virtual ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size, + Endianness streamEndianness) override; + + uint8_t * getBuffer(); + const uint8_t * getConstBuffer(); + void setBuffer(uint8_t* buffer, count_t bufferLength); +private: + bool serializeLength = false; + const uint8_t *constBuffer = nullptr; + uint8_t *buffer = nullptr; + count_t bufferLength = 0; +}; + +#endif /* SERIALBUFFERADAPTER_H_ */ diff --git a/tasks/SemaphoreFactory.h b/tasks/SemaphoreFactory.h index 2c41a135..01c09d1b 100644 --- a/tasks/SemaphoreFactory.h +++ b/tasks/SemaphoreFactory.h @@ -1,50 +1,50 @@ -#ifndef FSFW_TASKS_SEMAPHOREFACTORY_H_ -#define FSFW_TASKS_SEMAPHOREFACTORY_H_ - -#include "../tasks/SemaphoreIF.h" - -/** - * Creates Semaphore. - * This class is a "singleton" interface, i.e. it provides an - * interface, but also is the base class for a singleton. - */ -class SemaphoreFactory { -public: - virtual ~SemaphoreFactory(); - /** - * Returns the single instance of SemaphoreFactory. - * The implementation of #instance is found in its subclasses. - * Thus, we choose link-time variability of the instance. - */ - static SemaphoreFactory* instance(); - - /** - * Create a binary semaphore. - * Creator function for a binary semaphore which may only be acquired once - * @param argument Can be used to pass implementation specific information. - * @return Pointer to newly created semaphore class instance. - */ - SemaphoreIF* createBinarySemaphore(uint32_t arguments = 0); - /** - * Create a counting semaphore. - * Creator functons for a counting semaphore which may be acquired multiple - * times. - * @param count Semaphore can be taken count times. - * @param initCount Initial count value. - * @param argument Can be used to pass implementation specific information. - * @return - */ - SemaphoreIF* createCountingSemaphore(const uint8_t maxCount, - uint8_t initCount, uint32_t arguments = 0); - - void deleteSemaphore(SemaphoreIF* semaphore); - -private: - /** - * External instantiation is not allowed. - */ - SemaphoreFactory(); - static SemaphoreFactory* factoryInstance; -}; - -#endif /* FSFW_TASKS_SEMAPHOREFACTORY_H_ */ +#ifndef FSFW_TASKS_SEMAPHOREFACTORY_H_ +#define FSFW_TASKS_SEMAPHOREFACTORY_H_ + +#include "../tasks/SemaphoreIF.h" + +/** + * Creates Semaphore. + * This class is a "singleton" interface, i.e. it provides an + * interface, but also is the base class for a singleton. + */ +class SemaphoreFactory { +public: + virtual ~SemaphoreFactory(); + /** + * Returns the single instance of SemaphoreFactory. + * The implementation of #instance is found in its subclasses. + * Thus, we choose link-time variability of the instance. + */ + static SemaphoreFactory* instance(); + + /** + * Create a binary semaphore. + * Creator function for a binary semaphore which may only be acquired once + * @param argument Can be used to pass implementation specific information. + * @return Pointer to newly created semaphore class instance. + */ + SemaphoreIF* createBinarySemaphore(uint32_t arguments = 0); + /** + * Create a counting semaphore. + * Creator functons for a counting semaphore which may be acquired multiple + * times. + * @param count Semaphore can be taken count times. + * @param initCount Initial count value. + * @param argument Can be used to pass implementation specific information. + * @return + */ + SemaphoreIF* createCountingSemaphore(const uint8_t maxCount, + uint8_t initCount, uint32_t arguments = 0); + + void deleteSemaphore(SemaphoreIF* semaphore); + +private: + /** + * External instantiation is not allowed. + */ + SemaphoreFactory(); + static SemaphoreFactory* factoryInstance; +}; + +#endif /* FSFW_TASKS_SEMAPHOREFACTORY_H_ */ diff --git a/tasks/SemaphoreIF.h b/tasks/SemaphoreIF.h index 30d4ed88..dd327e23 100644 --- a/tasks/SemaphoreIF.h +++ b/tasks/SemaphoreIF.h @@ -1,68 +1,68 @@ -#ifndef FRAMEWORK_TASKS_SEMAPHOREIF_H_ -#define FRAMEWORK_TASKS_SEMAPHOREIF_H_ -#include "../returnvalues/FwClassIds.h" -#include "../returnvalues/HasReturnvaluesIF.h" -#include - -/** - * @brief Generic interface for semaphores, which can be used to achieve - * task synchronization. This is a generic interface which can be - * used for both binary semaphores and counting semaphores. - * @details - * A semaphore is a synchronization primitive. - * See: https://en.wikipedia.org/wiki/Semaphore_(programming) - * A semaphore can be used to achieve task synchonization and track the - * availability of resources by using either the binary or the counting - * semaphore types. - * - * If mutual exlcusion of a resource is desired, a mutex should be used, - * which is a special form of a semaphore and has an own interface. - */ -class SemaphoreIF { -public: - /** - * Different types of timeout for the mutex lock. - */ - enum TimeoutType { - POLLING, //!< If mutex is not available, return immediately - WAITING, //!< Wait a specified time for the mutex to become available - BLOCKING //!< Block indefinitely until the mutex becomes available. - }; - - virtual~ SemaphoreIF() {}; - - static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF; - //! Semaphore timeout - static constexpr ReturnValue_t SEMAPHORE_TIMEOUT = MAKE_RETURN_CODE(1); - //! The current semaphore can not be given, because it is not owned - static constexpr ReturnValue_t SEMAPHORE_NOT_OWNED = MAKE_RETURN_CODE(2); - static constexpr ReturnValue_t SEMAPHORE_INVALID = MAKE_RETURN_CODE(3); - - /** - * Generic call to acquire a semaphore. - * If there are no more semaphores to be taken (for a counting semaphore, - * a semaphore may be taken more than once), the taks will block - * for a maximum of timeoutMs while trying to acquire the semaphore. - * This can be used to achieve task synchrnization. - * @param timeoutMs - * @return - c RETURN_OK for successfull acquisition - */ - virtual ReturnValue_t acquire(TimeoutType timeoutType = - TimeoutType::BLOCKING, uint32_t timeoutMs = 0) = 0; - - /** - * Corrensponding call to release a semaphore. - * @return -@c RETURN_OK for successfull release - */ - virtual ReturnValue_t release() = 0; - - /** - * If the semaphore is a counting semaphore then the semaphores current - * count value is returned. If the semaphore is a binary semaphore then 1 - * is returned if the semaphore is available, and 0 is returned if the - * semaphore is not available. - */ - virtual uint8_t getSemaphoreCounter() const = 0; -}; - -#endif /* FRAMEWORK_TASKS_SEMAPHOREIF_H_ */ +#ifndef FRAMEWORK_TASKS_SEMAPHOREIF_H_ +#define FRAMEWORK_TASKS_SEMAPHOREIF_H_ +#include "../returnvalues/FwClassIds.h" +#include "../returnvalues/HasReturnvaluesIF.h" +#include + +/** + * @brief Generic interface for semaphores, which can be used to achieve + * task synchronization. This is a generic interface which can be + * used for both binary semaphores and counting semaphores. + * @details + * A semaphore is a synchronization primitive. + * See: https://en.wikipedia.org/wiki/Semaphore_(programming) + * A semaphore can be used to achieve task synchonization and track the + * availability of resources by using either the binary or the counting + * semaphore types. + * + * If mutual exlcusion of a resource is desired, a mutex should be used, + * which is a special form of a semaphore and has an own interface. + */ +class SemaphoreIF { +public: + /** + * Different types of timeout for the mutex lock. + */ + enum TimeoutType { + POLLING, //!< If mutex is not available, return immediately + WAITING, //!< Wait a specified time for the mutex to become available + BLOCKING //!< Block indefinitely until the mutex becomes available. + }; + + virtual~ SemaphoreIF() {}; + + static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF; + //! Semaphore timeout + static constexpr ReturnValue_t SEMAPHORE_TIMEOUT = MAKE_RETURN_CODE(1); + //! The current semaphore can not be given, because it is not owned + static constexpr ReturnValue_t SEMAPHORE_NOT_OWNED = MAKE_RETURN_CODE(2); + static constexpr ReturnValue_t SEMAPHORE_INVALID = MAKE_RETURN_CODE(3); + + /** + * Generic call to acquire a semaphore. + * If there are no more semaphores to be taken (for a counting semaphore, + * a semaphore may be taken more than once), the taks will block + * for a maximum of timeoutMs while trying to acquire the semaphore. + * This can be used to achieve task synchrnization. + * @param timeoutMs + * @return - c RETURN_OK for successfull acquisition + */ + virtual ReturnValue_t acquire(TimeoutType timeoutType = + TimeoutType::BLOCKING, uint32_t timeoutMs = 0) = 0; + + /** + * Corrensponding call to release a semaphore. + * @return -@c RETURN_OK for successfull release + */ + virtual ReturnValue_t release() = 0; + + /** + * If the semaphore is a counting semaphore then the semaphores current + * count value is returned. If the semaphore is a binary semaphore then 1 + * is returned if the semaphore is available, and 0 is returned if the + * semaphore is not available. + */ + virtual uint8_t getSemaphoreCounter() const = 0; +}; + +#endif /* FRAMEWORK_TASKS_SEMAPHOREIF_H_ */ From 85f21b05165efeabb626fcb5f7d8c7d3bc8a18ea Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Fri, 18 Sep 2020 13:32:53 +0200 Subject: [PATCH 48/53] object manager convergence --- objectmanager/ObjectManager.h | 23 +++++-------- objectmanager/ObjectManagerIF.h | 8 ++--- objectmanager/SystemObject.cpp | 2 +- objectmanager/SystemObject.h | 30 ++++++++--------- objectmanager/SystemObjectIF.h | 56 +++++++++++++++----------------- objectmanager/frameworkObjects.h | 14 ++++++-- 6 files changed, 65 insertions(+), 68 deletions(-) diff --git a/objectmanager/ObjectManager.h b/objectmanager/ObjectManager.h index 0d2b3c4a..69a74f73 100644 --- a/objectmanager/ObjectManager.h +++ b/objectmanager/ObjectManager.h @@ -1,12 +1,5 @@ -/** - * @file ObjectManager.h - * @brief This file contains the implementation of the ObjectManager class - * @date 18.09.2012 - * @author Bastian Baetz - */ - -#ifndef OBJECTMANAGER_H_ -#define OBJECTMANAGER_H_ +#ifndef FSFW_OBJECTMANAGER_OBJECTMANAGER_H_ +#define FSFW_OBJECTMANAGER_OBJECTMANAGER_H_ #include "ObjectManagerIF.h" #include "SystemObjectIF.h" @@ -22,14 +15,15 @@ * most of the system initialization. * As the system is static after initialization, no new objects are * created or inserted into the list after startup. - * \ingroup system_objects + * @ingroup system_objects + * @author Bastian Baetz */ class ObjectManager : public ObjectManagerIF { private: //comparison? /** - * \brief This is the map of all initialized objects in the manager. - * \details Objects in the List must inherit the SystemObjectIF. + * @brief This is the map of all initialized objects in the manager. + * @details Objects in the List must inherit the SystemObjectIF. */ std::map objectList; protected: @@ -54,7 +48,8 @@ public: /** * @brief In the class's destructor, all objects in the list are deleted. */ - //SHOULDDO: If, for some reason, deleting an ObjectManager instance is required, check if this works. + // SHOULDDO: If, for some reason, deleting an ObjectManager instance is + // required, check if this works. virtual ~ObjectManager( void ); ReturnValue_t insert( object_id_t id, SystemObjectIF* object ); ReturnValue_t remove( object_id_t id ); @@ -64,4 +59,4 @@ public: -#endif /* OBJECTMANAGER_H_ */ +#endif /* FSFW_OBJECTMANAGER_OBJECTMANAGER_H_ */ diff --git a/objectmanager/ObjectManagerIF.h b/objectmanager/ObjectManagerIF.h index e0162af9..1f462509 100644 --- a/objectmanager/ObjectManagerIF.h +++ b/objectmanager/ObjectManagerIF.h @@ -1,5 +1,5 @@ -#ifndef FRAMEWORK_OBJECTMANAGER_OBJECTMANAGERIF_H_ -#define FRAMEWORK_OBJECTMANAGER_OBJECTMANAGERIF_H_ +#ifndef FSFW_OBJECTMANAGER_OBJECTMANAGERIF_H_ +#define FSFW_OBJECTMANAGER_OBJECTMANAGERIF_H_ #include "frameworkObjects.h" #include "SystemObjectIF.h" @@ -21,8 +21,7 @@ public: static constexpr uint8_t INTERFACE_ID = CLASS_ID::OBJECT_MANAGER_IF; static constexpr ReturnValue_t INSERTION_FAILED = MAKE_RETURN_CODE( 1 ); static constexpr ReturnValue_t NOT_FOUND = MAKE_RETURN_CODE( 2 ); - - static constexpr ReturnValue_t CHILD_INIT_FAILED = MAKE_RETURN_CODE( 3 ); //!< Can be used if the initialization of a SystemObject failed. + static constexpr ReturnValue_t CHILD_INIT_FAILED = MAKE_RETURN_CODE( 3 ); static constexpr ReturnValue_t INTERNAL_ERR_REPORTER_UNINIT = MAKE_RETURN_CODE( 4 ); protected: @@ -80,6 +79,7 @@ public: /** * @brief This is the forward declaration of the global objectManager instance. */ +// maybe but this in the glob namespace to explicitely mark it global? extern ObjectManagerIF *objectManager; /*Documentation can be found in the class method declaration above.*/ diff --git a/objectmanager/SystemObject.cpp b/objectmanager/SystemObject.cpp index 53aac599..64330fbc 100644 --- a/objectmanager/SystemObject.cpp +++ b/objectmanager/SystemObject.cpp @@ -1,6 +1,6 @@ -#include "../events/EventManagerIF.h" #include "ObjectManager.h" #include "SystemObject.h" +#include "../events/EventManagerIF.h" SystemObject::SystemObject(object_id_t setObjectId, bool doRegister) : objectId(setObjectId), registered(doRegister) { diff --git a/objectmanager/SystemObject.h b/objectmanager/SystemObject.h index 8e9f20d5..9188693e 100644 --- a/objectmanager/SystemObject.h +++ b/objectmanager/SystemObject.h @@ -1,16 +1,9 @@ -/** - * @file SystemObject.h - * @brief This file contains the definition of the SystemObject class. - * @date 07.11.2012 - * @author Ulrich Mohr - */ - -#ifndef SYSTEMOBJECT_H_ -#define SYSTEMOBJECT_H_ +#ifndef FSFW_OBJECTMANAGER_SYSTEMOBJECT_H_ +#define FSFW_OBJECTMANAGER_SYSTEMOBJECT_H_ +#include "SystemObjectIF.h" #include "../events/Event.h" #include "../events/EventReportingProxyIF.h" -#include "SystemObjectIF.h" #include "../timemanager/Clock.h" /** @@ -20,7 +13,7 @@ * class that is announced to ObjectManager. It automatically includes * itself (and therefore the inheriting class) in the object manager's * list. - * \ingroup system_objects + * @ingroup system_objects */ class SystemObject: public SystemObjectIF { private: @@ -37,25 +30,28 @@ public: * @param parameter1 * @param parameter2 */ - virtual void triggerEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0); + virtual void triggerEvent(Event event, uint32_t parameter1 = 0, + uint32_t parameter2 = 0); /** * @brief The class's constructor. * @details In the constructor, the object id is set and the class is * inserted in the object manager. * @param setObjectId The id the object shall have. - * @param doRegister Determines if the object is registered in the global object manager. + * @param doRegister Determines if the object is registered in + * the global object manager. */ SystemObject(object_id_t setObjectId, bool doRegister = true); /** * @brief On destruction, the object removes itself from the list. */ virtual ~SystemObject(); - object_id_t getObjectId() const; - virtual ReturnValue_t initialize(); + object_id_t getObjectId() const override; + virtual ReturnValue_t initialize() override; virtual ReturnValue_t checkObjectConnections(); - virtual void forwardEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0) const; + virtual void forwardEvent(Event event, uint32_t parameter1 = 0, + uint32_t parameter2 = 0) const; }; -#endif /* SYSTEMOBJECT_H_ */ +#endif /* FSFW_OBJECTMANAGER_SYSTEMOBJECT_H_ */ diff --git a/objectmanager/SystemObjectIF.h b/objectmanager/SystemObjectIF.h index c5a92196..315fde9b 100644 --- a/objectmanager/SystemObjectIF.h +++ b/objectmanager/SystemObjectIF.h @@ -1,26 +1,19 @@ -/** - * @file SystemObjectIF.h - * @brief This file contains the definition of the SystemObjectIF interface. - * @date 18.09.2012 - * @author Bastian Baetz - */ - -#ifndef SYSTEMOBJECTIF_H_ -#define SYSTEMOBJECTIF_H_ +#ifndef FSFW_OBJECTMANAGER_SYSTEMOBJECTIF_H_ +#define FSFW_OBJECTMANAGER_SYSTEMOBJECTIF_H_ #include "../events/EventReportingProxyIF.h" #include "../returnvalues/HasReturnvaluesIF.h" -#include +#include /** - * \defgroup system_objects Software System Object Management - * The classes to create System Objects and classes to manage these are contained in this group. - * System Objects are software elements that can be controlled externally. They all have a unique - * object identifier. + * @defgroup system_objects Software System Object Management + * The classes to create System Objects and classes to manage these are + * contained in this group. System Objects are software elements that can be + * controlled externally. They all have a unique object identifier. */ /** * This is the typedef for object identifiers. - * \ingroup system_objects + * @ingroup system_objects */ typedef uint32_t object_id_t; @@ -29,7 +22,8 @@ typedef uint32_t object_id_t; * list. * It does not provide any method definitions, still it is required to * perform a type check with dynamic_cast. - * \ingroup system_objects + * @author Bastian Baetz + * @ingroup system_objects */ class SystemObjectIF : public EventReportingProxyIF { public: @@ -41,24 +35,28 @@ public: /** * The empty virtual destructor as required for C++ interfaces. */ - virtual ~SystemObjectIF() { - } + virtual ~SystemObjectIF() {} /** - * Initializes all inter-object dependencies. - * This is necessary to avoid circular dependencies of not-fully - * initialized objects on start up. - * @return - \c RETURN_OK in case the initialization was successful - * - \c RETURN_FAILED otherwise + * @brief Initializes the object. + * There are initialization steps which can also be done in the constructor. + * However, there is no clean way to get a returnvalue from a constructor. + * Furthermore some components require other system object to be created + * which might not have been built yet. + * Therefore, a two-step initialization resolves this problem and prevents + * circular dependencies of not-fully initialized objects on start up. + * @return - @c RETURN_OK in case the initialization was successful + * - @c RETURN_FAILED otherwise */ virtual ReturnValue_t initialize() = 0; /** - * Checks, if all object-object interconnections are satisfying for operation. - * Some objects need certain other objects (or a certain number), to be registered as children. - * These checks can be done in this method. - * @return - \c RETURN_OK in case the check was successful - * - \c any other code otherwise + * @brief Checks if all object-object interconnections are satisfying + * for operation. + * Some objects need certain other objects (or a certain number), to be + * registered as children. These checks can be done in this method. + * @return - @c RETURN_OK in case the check was successful + * - @c any other code otherwise */ virtual ReturnValue_t checkObjectConnections() = 0; }; -#endif /* SYSTEMOBJECTIF_H_ */ +#endif /* #ifndef FSFW_OBJECTMANAGER_SYSTEMOBJECTIF_H_ */ diff --git a/objectmanager/frameworkObjects.h b/objectmanager/frameworkObjects.h index c3deafc4..4a30e70a 100644 --- a/objectmanager/frameworkObjects.h +++ b/objectmanager/frameworkObjects.h @@ -1,8 +1,15 @@ -#ifndef FRAMEWORK_OBJECTMANAGER_FRAMEWORKOBJECTS_H_ -#define FRAMEWORK_OBJECTMANAGER_FRAMEWORKOBJECTS_H_ +#ifndef FSFW_OBJECTMANAGER_FRAMEWORKOBJECTS_H_ +#define FSFW_OBJECTMANAGER_FRAMEWORKOBJECTS_H_ namespace objects { enum framework_objects { + // Default verification reporter. + PUS_SERVICE_1 = 0x53000001, + PUS_SERVICE_2 = 0x53000002, + PUS_SERVICE_5 = 0x53000005, + PUS_SERVICE_8 = 0x53000008, + PUS_SERVICE_200 = 0x53000200, + //Generic IDs for IPC, modes, health, events HEALTH_TABLE = 0x53010000, // MODE_STORE = 0x53010100, @@ -12,10 +19,11 @@ enum framework_objects { //IDs for PUS Packet Communication TC_STORE = 0x534f0100, TM_STORE = 0x534f0200, + NO_OBJECT = 0xFFFFFFFF }; } -#endif /* FRAMEWORK_OBJECTMANAGER_FRAMEWORKOBJECTS_H_ */ +#endif /* FSFW_OBJECTMANAGER_FRAMEWORKOBJECTS_H_ */ From 6062192bf0a5865fe3c1edb23aa2bc1e1b19d122 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Fri, 18 Sep 2020 13:37:06 +0200 Subject: [PATCH 49/53] dded back retval expalanation --- objectmanager/ObjectManagerIF.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/objectmanager/ObjectManagerIF.h b/objectmanager/ObjectManagerIF.h index 1f462509..8bae800b 100644 --- a/objectmanager/ObjectManagerIF.h +++ b/objectmanager/ObjectManagerIF.h @@ -21,7 +21,7 @@ public: static constexpr uint8_t INTERFACE_ID = CLASS_ID::OBJECT_MANAGER_IF; static constexpr ReturnValue_t INSERTION_FAILED = MAKE_RETURN_CODE( 1 ); static constexpr ReturnValue_t NOT_FOUND = MAKE_RETURN_CODE( 2 ); - static constexpr ReturnValue_t CHILD_INIT_FAILED = MAKE_RETURN_CODE( 3 ); + static constexpr ReturnValue_t CHILD_INIT_FAILED = MAKE_RETURN_CODE( 3 ); //!< Can be used if the initialization of a SystemObject failed. static constexpr ReturnValue_t INTERNAL_ERR_REPORTER_UNINIT = MAKE_RETURN_CODE( 4 ); protected: From 8ea0a38658b1322a6bee67b7c656544d1f4d3467 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Fri, 18 Sep 2020 13:37:38 +0200 Subject: [PATCH 50/53] shoulddo fix --- objectmanager/ObjectManagerIF.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/objectmanager/ObjectManagerIF.h b/objectmanager/ObjectManagerIF.h index 8bae800b..4bd1d915 100644 --- a/objectmanager/ObjectManagerIF.h +++ b/objectmanager/ObjectManagerIF.h @@ -79,7 +79,7 @@ public: /** * @brief This is the forward declaration of the global objectManager instance. */ -// maybe but this in the glob namespace to explicitely mark it global? +// SHOULDDO: maybe put this in the glob namespace to explicitely mark it global? extern ObjectManagerIF *objectManager; /*Documentation can be found in the class method declaration above.*/ From 721793a0582174e2edfa1e624bcf67377c8ce981 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Fri, 18 Sep 2020 13:38:53 +0200 Subject: [PATCH 51/53] added author tag back --- objectmanager/SystemObject.h | 1 + 1 file changed, 1 insertion(+) diff --git a/objectmanager/SystemObject.h b/objectmanager/SystemObject.h index 9188693e..d9c4236d 100644 --- a/objectmanager/SystemObject.h +++ b/objectmanager/SystemObject.h @@ -13,6 +13,7 @@ * class that is announced to ObjectManager. It automatically includes * itself (and therefore the inheriting class) in the object manager's * list. + * @author Ulrich Mohr * @ingroup system_objects */ class SystemObject: public SystemObjectIF { From 7851a71a8eb96931fbdc5f24ef40b5326665a892 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Tue, 22 Sep 2020 14:00:18 +0200 Subject: [PATCH 52/53] commanding service base hotfix --- tmtcservices/CommandingServiceBase.cpp | 2 +- tmtcservices/CommandingServiceBase.h | 10 +++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/tmtcservices/CommandingServiceBase.cpp b/tmtcservices/CommandingServiceBase.cpp index 4cead0e9..0ebc3944 100644 --- a/tmtcservices/CommandingServiceBase.cpp +++ b/tmtcservices/CommandingServiceBase.cpp @@ -384,7 +384,7 @@ void CommandingServiceBase::acceptPacket(uint8_t reportId, } -void CommandingServiceBase::checkAndExecuteFifo(CommandMapIter iter) { +void CommandingServiceBase::checkAndExecuteFifo(CommandMapIter& iter) { store_address_t address; if (iter->second.fifo.retrieve(&address) != RETURN_OK) { commandMap.erase(&iter); diff --git a/tmtcservices/CommandingServiceBase.h b/tmtcservices/CommandingServiceBase.h index 23b08acf..252b6943 100644 --- a/tmtcservices/CommandingServiceBase.h +++ b/tmtcservices/CommandingServiceBase.h @@ -39,7 +39,11 @@ class CommandingServiceBase: public SystemObject, public HasReturnvaluesIF { friend void (Factory::setStaticFrameworkObjectIds)(); public: + // We could make this configurable via preprocessor and the FSFWConfig file. + static constexpr uint8_t COMMAND_INFO_FIFO_DEPTH = 3; + static const uint8_t INTERFACE_ID = CLASS_ID::COMMAND_SERVICE_BASE; + static const ReturnValue_t EXECUTION_COMPLETE = MAKE_RETURN_CODE(1); static const ReturnValue_t NO_STEP_MESSAGE = MAKE_RETURN_CODE(2); static const ReturnValue_t OBJECT_BUSY = MAKE_RETURN_CODE(3); @@ -223,7 +227,7 @@ protected: uint32_t state; Command_t command; object_id_t objectId; - FIFO fifo; + FIFO fifo; virtual ReturnValue_t serialize(uint8_t **buffer, size_t *size, size_t maxSize, Endianness streamEndianness) const override{ @@ -235,7 +239,7 @@ protected: }; virtual ReturnValue_t deSerialize(const uint8_t **buffer, size_t *size, - Endianness streamEndianness) override{ + Endianness streamEndianness) override { return HasReturnvaluesIF::RETURN_FAILED; }; }; @@ -312,7 +316,7 @@ protected: ReturnValue_t sendTmPacket(uint8_t subservice, SerializeIF* content, SerializeIF* header = nullptr); - void checkAndExecuteFifo(CommandMapIter iter); + void checkAndExecuteFifo(CommandMapIter& iter); private: /** From 4587fbb76d1bc356227229dcb1ad44d389d4a5c0 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Tue, 22 Sep 2020 14:52:24 +0200 Subject: [PATCH 53/53] srv9 func removed --- pus/Service9TimeManagement.h | 1 - 1 file changed, 1 deletion(-) diff --git a/pus/Service9TimeManagement.h b/pus/Service9TimeManagement.h index 9eed12ca..4802cdec 100644 --- a/pus/Service9TimeManagement.h +++ b/pus/Service9TimeManagement.h @@ -34,7 +34,6 @@ private: SET_TIME = 128 //!< [EXPORT] : [COMMAND] Time command in ASCII, CUC or CDS format }; - void setIsisClock(Clock::TimeOfDay_t& timeOfDay); };