From f388bd374c8db334b5d7b0b9207a360d712d5fde Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Fri, 29 May 2020 13:29:29 +0200 Subject: [PATCH 01/50] added new factory and interface --- tasks/SemaphoreFactory.h | 49 ++++++++++++++++++++++++++++++++ tasks/SemaphoreIF.h | 61 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 110 insertions(+) create mode 100644 tasks/SemaphoreFactory.h create mode 100644 tasks/SemaphoreIF.h diff --git a/tasks/SemaphoreFactory.h b/tasks/SemaphoreFactory.h new file mode 100644 index 00000000..7f8edaf1 --- /dev/null +++ b/tasks/SemaphoreFactory.h @@ -0,0 +1,49 @@ +#ifndef FRAMEWORK_TASKS_SEMAPHOREFACTORY_H_ +#define FRAMEWORK_TASKS_SEMAPHOREFACTORY_H_ +#include + +/** + * 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 /* FRAMEWORK_TASKS_SEMAPHOREFACTORY_H_ */ diff --git a/tasks/SemaphoreIF.h b/tasks/SemaphoreIF.h new file mode 100644 index 00000000..a7c5a97b --- /dev/null +++ b/tasks/SemaphoreIF.h @@ -0,0 +1,61 @@ +#ifndef FRAMEWORK_TASKS_SEMAPHOREIF_H_ +#define FRAMEWORK_TASKS_SEMAPHOREIF_H_ +#include +#include +#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: + virtual~ SemaphoreIF() {}; + //! Needs to be defined in implementation. No blocking time + static const uint32_t NO_TIMEOUT; + //! Needs to be defined in implementation. Blocks indefinitely. + static const uint32_t MAX_TIMEOUT; + 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(uint32_t timeoutMs) = 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 d909fcb6a17448a3f2ccd3c7c9512a666a429e33 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Fri, 29 May 2020 13:31:19 +0200 Subject: [PATCH 02/50] added semaphore implementations --- osal/FreeRTOS/BinSemaphUsingTask.cpp | 91 ++++++++++++++++++ osal/FreeRTOS/BinSemaphUsingTask.h | 75 +++++++++++++++ osal/FreeRTOS/BinarySemaphore.cpp | 103 ++++++++++++++++++++ osal/FreeRTOS/BinarySemaphore.h | 107 +++++++++++++++++++++ osal/FreeRTOS/CountingSemaphUsingTask.cpp | 110 ++++++++++++++++++++++ osal/FreeRTOS/CountingSemaphUsingTask.h | 100 ++++++++++++++++++++ osal/FreeRTOS/CountingSemaphore.cpp | 43 +++++++++ osal/FreeRTOS/CountingSemaphore.h | 34 +++++++ 8 files changed, 663 insertions(+) create mode 100644 osal/FreeRTOS/BinSemaphUsingTask.cpp create mode 100644 osal/FreeRTOS/BinSemaphUsingTask.h create mode 100644 osal/FreeRTOS/BinarySemaphore.cpp create mode 100644 osal/FreeRTOS/BinarySemaphore.h create mode 100644 osal/FreeRTOS/CountingSemaphUsingTask.cpp create mode 100644 osal/FreeRTOS/CountingSemaphUsingTask.h create mode 100644 osal/FreeRTOS/CountingSemaphore.cpp create mode 100644 osal/FreeRTOS/CountingSemaphore.h diff --git a/osal/FreeRTOS/BinSemaphUsingTask.cpp b/osal/FreeRTOS/BinSemaphUsingTask.cpp new file mode 100644 index 00000000..8f5fd4d8 --- /dev/null +++ b/osal/FreeRTOS/BinSemaphUsingTask.cpp @@ -0,0 +1,91 @@ +#include +#include +#include + +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(uint32_t timeoutMs) { + TickType_t timeout = SemaphoreIF::NO_TIMEOUT; + if(timeoutMs == SemaphoreIF::MAX_TIMEOUT) { + timeout = SemaphoreIF::MAX_TIMEOUT; + } + else if(timeoutMs > SemaphoreIF::NO_TIMEOUT){ + timeout = pdMS_TO_TICKS(timeoutMs); + } + return acquireWithTickTimeout(timeout); +} + +ReturnValue_t BinarySemaphoreUsingTask::acquireWithTickTimeout( + 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 new file mode 100644 index 00000000..48c1cd12 --- /dev/null +++ b/osal/FreeRTOS/BinSemaphUsingTask.h @@ -0,0 +1,75 @@ +#ifndef FRAMEWORK_OSAL_FREERTOS_BINSEMAPHUSINGTASK_H_ +#define FRAMEWORK_OSAL_FREERTOS_BINSEMAPHUSINGTASK_H_ + +#include +#include + +#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(uint32_t timeoutMs = + SemaphoreIF::NO_TIMEOUT) 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(TickType_t timeoutTicks = + SemaphoreIF::NO_TIMEOUT); + + /** + * 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 new file mode 100644 index 00000000..b6687bb7 --- /dev/null +++ b/osal/FreeRTOS/BinarySemaphore.cpp @@ -0,0 +1,103 @@ +#include +#include +#include + +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(uint32_t timeoutMs) { + TickType_t timeout = SemaphoreIF::NO_TIMEOUT; + if(timeoutMs == SemaphoreIF::MAX_TIMEOUT) { + timeout = SemaphoreIF::MAX_TIMEOUT; + } + else if(timeoutMs > SemaphoreIF::NO_TIMEOUT){ + timeout = pdMS_TO_TICKS(timeoutMs); + } + return acquireWithTickTimeout(timeout); +} + +ReturnValue_t BinarySemaphore::acquireWithTickTimeout(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 new file mode 100644 index 00000000..0938dee6 --- /dev/null +++ b/osal/FreeRTOS/BinarySemaphore.h @@ -0,0 +1,107 @@ +#ifndef FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ +#define FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ + +#include +#include + +#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(uint32_t timeoutMs = + SemaphoreIF::NO_TIMEOUT) 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(TickType_t timeoutTicks = + BinarySemaphore::NO_TIMEOUT); + + /** + * 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 new file mode 100644 index 00000000..d0f63f3f --- /dev/null +++ b/osal/FreeRTOS/CountingSemaphUsingTask.cpp @@ -0,0 +1,110 @@ +#include +#include +#include + +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(uint32_t timeoutMs) { + TickType_t timeout = SemaphoreIF::NO_TIMEOUT; + if(timeoutMs == SemaphoreIF::MAX_TIMEOUT) { + timeout = SemaphoreIF::MAX_TIMEOUT; + } + else if(timeoutMs > SemaphoreIF::NO_TIMEOUT){ + timeout = pdMS_TO_TICKS(timeoutMs); + } + return acquireWithTickTimeout(timeout); + +} + +ReturnValue_t CountingSemaphoreUsingTask::acquireWithTickTimeout( + 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 new file mode 100644 index 00000000..797e864b --- /dev/null +++ b/osal/FreeRTOS/CountingSemaphUsingTask.h @@ -0,0 +1,100 @@ +#ifndef FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHUSINGTASK_H_ +#define FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHUSINGTASK_H_ + +#include +#include + +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(uint32_t timeoutMs = SemaphoreIF::NO_TIMEOUT) 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( + TickType_t timeoutTicks = SemaphoreIF::NO_TIMEOUT); + + /** + * 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 new file mode 100644 index 00000000..957f6a31 --- /dev/null +++ b/osal/FreeRTOS/CountingSemaphore.cpp @@ -0,0 +1,43 @@ +#include +#include +#include + +#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 new file mode 100644 index 00000000..dca3ab0e --- /dev/null +++ b/osal/FreeRTOS/CountingSemaphore.h @@ -0,0 +1,34 @@ +#ifndef FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_ +#define FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHORE_H_ +#include + +/** + * @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_ */ From f829d063cee5383abfdc9dc951a4f82cbea7d36f Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Fri, 29 May 2020 13:31:55 +0200 Subject: [PATCH 03/50] added task management functions required for freeRTOS --- osal/FreeRTOS/TaskManagement.cpp | 23 ++++++++++++ osal/FreeRTOS/TaskManagement.h | 63 ++++++++++++++++++++++++++++++++ 2 files changed, 86 insertions(+) create mode 100644 osal/FreeRTOS/TaskManagement.cpp create mode 100644 osal/FreeRTOS/TaskManagement.h diff --git a/osal/FreeRTOS/TaskManagement.cpp b/osal/FreeRTOS/TaskManagement.cpp new file mode 100644 index 00000000..7871ab2e --- /dev/null +++ b/osal/FreeRTOS/TaskManagement.cpp @@ -0,0 +1,23 @@ +#include + +void TaskManagement::requestContextSwitchFromTask() { + 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 + requestContextSwitchFromISR(); + } else { + requestContextSwitchFromTask(); + } +} + +TaskHandle_t TaskManagement::getCurrentTaskHandle() { + return xTaskGetCurrentTaskHandle(); +} + +configSTACK_DEPTH_TYPE TaskManagement::getTaskStackHighWatermark() { + return uxTaskGetStackHighWaterMark(TaskManagement::getCurrentTaskHandle()); +} diff --git a/osal/FreeRTOS/TaskManagement.h b/osal/FreeRTOS/TaskManagement.h new file mode 100644 index 00000000..39c24377 --- /dev/null +++ b/osal/FreeRTOS/TaskManagement.h @@ -0,0 +1,63 @@ +#ifndef FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ +#define FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ + +#include + +extern "C" { +#include +#include +} +#include + +/** + * Architecture dependant portmacro.h function call. + * Should be implemented in bsp. + */ +extern "C" void requestContextSwitchFromISR(); + +/*! + * 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 requestContextSwitchFromTask(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 configSTACK_DEPTH_TYPE getTaskStackHighWatermark(); +}; + +#endif /* FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ */ From adb80a68fc0fb8fd7a206ad842565083df18a59d Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Fri, 29 May 2020 13:32:38 +0200 Subject: [PATCH 04/50] addd freertos semaphore factory --- osal/FreeRTOS/SemaphoreFactory.cpp | 61 ++++++++++++++++++++++++++++++ 1 file changed, 61 insertions(+) create mode 100644 osal/FreeRTOS/SemaphoreFactory.cpp diff --git a/osal/FreeRTOS/SemaphoreFactory.cpp b/osal/FreeRTOS/SemaphoreFactory.cpp new file mode 100644 index 00000000..05a898cd --- /dev/null +++ b/osal/FreeRTOS/SemaphoreFactory.cpp @@ -0,0 +1,61 @@ +#include +#include +#include +#include +#include +#include + +SemaphoreFactory* SemaphoreFactory::factoryInstance = nullptr; +const uint32_t SemaphoreIF::NO_TIMEOUT = 0; +const uint32_t SemaphoreIF::MAX_TIMEOUT = portMAX_DELAY; + +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; +} From b4c6a04d4a9969727f2602fee1fb1fff8c4bc832 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Fri, 29 May 2020 13:33:44 +0200 Subject: [PATCH 05/50] added linux implementations --- osal/linux/BinarySemaphore.cpp | 148 +++++++++++++++++++++++++++++++ osal/linux/BinarySemaphore.h | 81 +++++++++++++++++ osal/linux/CountingSemaphore.cpp | 54 +++++++++++ osal/linux/CountingSemaphore.h | 37 ++++++++ osal/linux/SemaphoreFactory.cpp | 36 ++++++++ 5 files changed, 356 insertions(+) create mode 100644 osal/linux/BinarySemaphore.cpp create mode 100644 osal/linux/BinarySemaphore.h create mode 100644 osal/linux/CountingSemaphore.cpp create mode 100644 osal/linux/CountingSemaphore.h create mode 100644 osal/linux/SemaphoreFactory.cpp diff --git a/osal/linux/BinarySemaphore.cpp b/osal/linux/BinarySemaphore.cpp new file mode 100644 index 00000000..7c76a5c4 --- /dev/null +++ b/osal/linux/BinarySemaphore.cpp @@ -0,0 +1,148 @@ +#include +#include + +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(uint32_t timeoutMs) { + int result = 0; + if(timeoutMs == SemaphoreIF::NO_TIMEOUT) { + result = sem_trywait(&handle); + } + else if(timeoutMs == SemaphoreIF::MAX_TIMEOUT) { + result = sem_wait(&handle); + } + else if(timeoutMs > SemaphoreIF::NO_TIMEOUT){ + 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 new file mode 100644 index 00000000..7836cd41 --- /dev/null +++ b/osal/linux/BinarySemaphore.h @@ -0,0 +1,81 @@ +#ifndef FRAMEWORK_OSAL_LINUX_BINARYSEMPAHORE_H_ +#define FRAMEWORK_OSAL_LINUX_BINARYSEMPAHORE_H_ + +#include +#include + +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(uint32_t timeoutMs = + SemaphoreIF::NO_TIMEOUT) 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 new file mode 100644 index 00000000..dfc4d801 --- /dev/null +++ b/osal/linux/CountingSemaphore.cpp @@ -0,0 +1,54 @@ +#include +#include + +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 new file mode 100644 index 00000000..ba606595 --- /dev/null +++ b/osal/linux/CountingSemaphore.h @@ -0,0 +1,37 @@ +#ifndef FRAMEWORK_OSAL_LINUX_COUNTINGSEMAPHORE_H_ +#define FRAMEWORK_OSAL_LINUX_COUNTINGSEMAPHORE_H_ +#include + +/** + * @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 new file mode 100644 index 00000000..5aec84ea --- /dev/null +++ b/osal/linux/SemaphoreFactory.cpp @@ -0,0 +1,36 @@ +#include +#include +#include +#include + +const uint32_t SemaphoreIF::NO_TIMEOUT = 0; +const uint32_t SemaphoreIF::MAX_TIMEOUT = 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) { + 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; +} From 6c9f157d2c657874990c579bea920bc2c0a6c3e5 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Fri, 29 May 2020 13:34:17 +0200 Subject: [PATCH 06/50] added fw class ids --- returnvalues/FwClassIds.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/returnvalues/FwClassIds.h b/returnvalues/FwClassIds.h index d21861c0..120d8b8c 100644 --- a/returnvalues/FwClassIds.h +++ b/returnvalues/FwClassIds.h @@ -24,6 +24,7 @@ enum { MEMORY_HELPER, //MH SERIALIZE_IF, //SE FIXED_MAP, //FM + FIXED_MULTIMAP, //FMM HAS_HEALTH_IF, //HHI FIFO_CLASS, //FF MESSAGE_PROXY, //MQP @@ -59,6 +60,7 @@ enum { SGP4PROPAGATOR_CLASS, //SGP4 53 MUTEX_IF, //MUX 54 MESSAGE_QUEUE_IF,//MQI 55 + SEMAPHORE_IF, //SPH 56 FW_CLASS_ID_COUNT //is actually count + 1 ! }; From 73183b39efaa3de9e7a295100dace8fb82aa069f Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Fri, 29 May 2020 13:37:13 +0200 Subject: [PATCH 07/50] added new casts otherwise i cant compile with new freertos version --- osal/FreeRTOS/MessageQueue.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/osal/FreeRTOS/MessageQueue.cpp b/osal/FreeRTOS/MessageQueue.cpp index e5da0442..18e7aa3d 100644 --- a/osal/FreeRTOS/MessageQueue.cpp +++ b/osal/FreeRTOS/MessageQueue.cpp @@ -97,7 +97,8 @@ ReturnValue_t MessageQueue::sendMessageFromMessageQueue(MessageQueueId_t sendTo, bool ignoreFault) { message->setSender(sentFrom); - BaseType_t result = xQueueSendToBack(reinterpret_cast(sendTo),reinterpret_cast(message->getBuffer()), 0); + BaseType_t result = xQueueSendToBack(reinterpret_cast(sendTo), + reinterpret_cast(message->getBuffer()), 0); if (result != pdPASS) { if (!ignoreFault) { InternalErrorReporterIF* internalErrorReporter = objectManager->get( From d4f69633f0e45c7c379d7cc9738af2859d6a8ad3 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Tue, 2 Jun 2020 22:36:42 +0200 Subject: [PATCH 08/50] renamed timeout values --- osal/FreeRTOS/BinSemaphUsingTask.cpp | 8 ++++---- osal/FreeRTOS/BinSemaphUsingTask.h | 4 ++-- osal/FreeRTOS/BinarySemaphore.cpp | 8 ++++---- osal/FreeRTOS/BinarySemaphore.h | 4 ++-- osal/FreeRTOS/CountingSemaphUsingTask.cpp | 8 ++++---- osal/FreeRTOS/CountingSemaphUsingTask.h | 4 ++-- osal/FreeRTOS/SemaphoreFactory.cpp | 4 ++-- osal/linux/BinarySemaphore.cpp | 6 +++--- osal/linux/BinarySemaphore.h | 2 +- osal/linux/SemaphoreFactory.cpp | 4 ++-- tasks/SemaphoreIF.h | 19 +++++++++++++++---- 11 files changed, 41 insertions(+), 30 deletions(-) diff --git a/osal/FreeRTOS/BinSemaphUsingTask.cpp b/osal/FreeRTOS/BinSemaphUsingTask.cpp index 8f5fd4d8..d3351888 100644 --- a/osal/FreeRTOS/BinSemaphUsingTask.cpp +++ b/osal/FreeRTOS/BinSemaphUsingTask.cpp @@ -17,11 +17,11 @@ BinarySemaphoreUsingTask::~BinarySemaphoreUsingTask() { } ReturnValue_t BinarySemaphoreUsingTask::acquire(uint32_t timeoutMs) { - TickType_t timeout = SemaphoreIF::NO_TIMEOUT; - if(timeoutMs == SemaphoreIF::MAX_TIMEOUT) { - timeout = SemaphoreIF::MAX_TIMEOUT; + TickType_t timeout = SemaphoreIF::POLLING; + if(timeoutMs == SemaphoreIF::BLOCKING) { + timeout = SemaphoreIF::BLOCKING; } - else if(timeoutMs > SemaphoreIF::NO_TIMEOUT){ + else if(timeoutMs > SemaphoreIF::POLLING){ timeout = pdMS_TO_TICKS(timeoutMs); } return acquireWithTickTimeout(timeout); diff --git a/osal/FreeRTOS/BinSemaphUsingTask.h b/osal/FreeRTOS/BinSemaphUsingTask.h index 48c1cd12..43772e5a 100644 --- a/osal/FreeRTOS/BinSemaphUsingTask.h +++ b/osal/FreeRTOS/BinSemaphUsingTask.h @@ -26,7 +26,7 @@ public: virtual~ BinarySemaphoreUsingTask(); ReturnValue_t acquire(uint32_t timeoutMs = - SemaphoreIF::NO_TIMEOUT) override; + SemaphoreIF::BLOCKING) override; ReturnValue_t release() override; uint8_t getSemaphoreCounter() const override; static uint8_t getSemaphoreCounter(TaskHandle_t taskHandle); @@ -40,7 +40,7 @@ public: * - @c RETURN_FAILED on failure */ ReturnValue_t acquireWithTickTimeout(TickType_t timeoutTicks = - SemaphoreIF::NO_TIMEOUT); + SemaphoreIF::BLOCKING); /** * Get handle to the task related to the semaphore. diff --git a/osal/FreeRTOS/BinarySemaphore.cpp b/osal/FreeRTOS/BinarySemaphore.cpp index b6687bb7..e6d2f92f 100644 --- a/osal/FreeRTOS/BinarySemaphore.cpp +++ b/osal/FreeRTOS/BinarySemaphore.cpp @@ -36,11 +36,11 @@ BinarySemaphore& BinarySemaphore::operator =( } ReturnValue_t BinarySemaphore::acquire(uint32_t timeoutMs) { - TickType_t timeout = SemaphoreIF::NO_TIMEOUT; - if(timeoutMs == SemaphoreIF::MAX_TIMEOUT) { - timeout = SemaphoreIF::MAX_TIMEOUT; + TickType_t timeout = SemaphoreIF::POLLING; + if(timeoutMs == SemaphoreIF::BLOCKING) { + timeout = SemaphoreIF::BLOCKING; } - else if(timeoutMs > SemaphoreIF::NO_TIMEOUT){ + else if(timeoutMs > SemaphoreIF::POLLING){ timeout = pdMS_TO_TICKS(timeoutMs); } return acquireWithTickTimeout(timeout); diff --git a/osal/FreeRTOS/BinarySemaphore.h b/osal/FreeRTOS/BinarySemaphore.h index 0938dee6..2a372cd2 100644 --- a/osal/FreeRTOS/BinarySemaphore.h +++ b/osal/FreeRTOS/BinarySemaphore.h @@ -53,7 +53,7 @@ public: * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout */ ReturnValue_t acquire(uint32_t timeoutMs = - SemaphoreIF::NO_TIMEOUT) override; + SemaphoreIF::BLOCKING) override; /** * Same as lockBinarySemaphore() with timeout in FreeRTOS ticks. @@ -62,7 +62,7 @@ public: * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout */ ReturnValue_t acquireWithTickTimeout(TickType_t timeoutTicks = - BinarySemaphore::NO_TIMEOUT); + SemaphoreIF::BLOCKING); /** * Release the binary semaphore. diff --git a/osal/FreeRTOS/CountingSemaphUsingTask.cpp b/osal/FreeRTOS/CountingSemaphUsingTask.cpp index d0f63f3f..f33c7a0d 100644 --- a/osal/FreeRTOS/CountingSemaphUsingTask.cpp +++ b/osal/FreeRTOS/CountingSemaphUsingTask.cpp @@ -38,11 +38,11 @@ CountingSemaphoreUsingTask::~CountingSemaphoreUsingTask() { } ReturnValue_t CountingSemaphoreUsingTask::acquire(uint32_t timeoutMs) { - TickType_t timeout = SemaphoreIF::NO_TIMEOUT; - if(timeoutMs == SemaphoreIF::MAX_TIMEOUT) { - timeout = SemaphoreIF::MAX_TIMEOUT; + TickType_t timeout = SemaphoreIF::POLLING; + if(timeoutMs == SemaphoreIF::BLOCKING) { + timeout = SemaphoreIF::BLOCKING; } - else if(timeoutMs > SemaphoreIF::NO_TIMEOUT){ + else if(timeoutMs > SemaphoreIF::POLLING){ timeout = pdMS_TO_TICKS(timeoutMs); } return acquireWithTickTimeout(timeout); diff --git a/osal/FreeRTOS/CountingSemaphUsingTask.h b/osal/FreeRTOS/CountingSemaphUsingTask.h index 797e864b..f1d82c1d 100644 --- a/osal/FreeRTOS/CountingSemaphUsingTask.h +++ b/osal/FreeRTOS/CountingSemaphUsingTask.h @@ -31,7 +31,7 @@ public: * @return -@c RETURN_OK on success * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout */ - ReturnValue_t acquire(uint32_t timeoutMs = SemaphoreIF::NO_TIMEOUT) override; + ReturnValue_t acquire(uint32_t timeoutMs = SemaphoreIF::BLOCKING) override; /** * Release a semaphore, increasing the number of available counting @@ -61,7 +61,7 @@ public: * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout */ ReturnValue_t acquireWithTickTimeout( - TickType_t timeoutTicks = SemaphoreIF::NO_TIMEOUT); + TickType_t timeoutTicks = SemaphoreIF::BLOCKING); /** * Get handle to the task related to the semaphore. diff --git a/osal/FreeRTOS/SemaphoreFactory.cpp b/osal/FreeRTOS/SemaphoreFactory.cpp index 05a898cd..78427f6c 100644 --- a/osal/FreeRTOS/SemaphoreFactory.cpp +++ b/osal/FreeRTOS/SemaphoreFactory.cpp @@ -6,8 +6,8 @@ #include SemaphoreFactory* SemaphoreFactory::factoryInstance = nullptr; -const uint32_t SemaphoreIF::NO_TIMEOUT = 0; -const uint32_t SemaphoreIF::MAX_TIMEOUT = portMAX_DELAY; +const uint32_t SemaphoreIF::POLLING = 0; +const uint32_t SemaphoreIF::BLOCKING = portMAX_DELAY; static const uint32_t USE_REGULAR_SEMAPHORES = 0; static const uint32_t USE_TASK_NOTIFICATIONS = 1; diff --git a/osal/linux/BinarySemaphore.cpp b/osal/linux/BinarySemaphore.cpp index 7c76a5c4..e2ad9b58 100644 --- a/osal/linux/BinarySemaphore.cpp +++ b/osal/linux/BinarySemaphore.cpp @@ -27,13 +27,13 @@ BinarySemaphore& BinarySemaphore::operator =( ReturnValue_t BinarySemaphore::acquire(uint32_t timeoutMs) { int result = 0; - if(timeoutMs == SemaphoreIF::NO_TIMEOUT) { + if(timeoutMs == SemaphoreIF::POLLING) { result = sem_trywait(&handle); } - else if(timeoutMs == SemaphoreIF::MAX_TIMEOUT) { + else if(timeoutMs == SemaphoreIF::BLOCKING) { result = sem_wait(&handle); } - else if(timeoutMs > SemaphoreIF::NO_TIMEOUT){ + else if(timeoutMs > SemaphoreIF::POLLING){ timespec timeOut; clock_gettime(CLOCK_REALTIME, &timeOut); uint64_t nseconds = timeOut.tv_sec * 1000000000 + timeOut.tv_nsec; diff --git a/osal/linux/BinarySemaphore.h b/osal/linux/BinarySemaphore.h index 7836cd41..9d4ed1cd 100644 --- a/osal/linux/BinarySemaphore.h +++ b/osal/linux/BinarySemaphore.h @@ -50,7 +50,7 @@ public: * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout */ ReturnValue_t acquire(uint32_t timeoutMs = - SemaphoreIF::NO_TIMEOUT) override; + SemaphoreIF::BLOCKING) override; /** * Release the binary semaphore. diff --git a/osal/linux/SemaphoreFactory.cpp b/osal/linux/SemaphoreFactory.cpp index 5aec84ea..4fbd60d9 100644 --- a/osal/linux/SemaphoreFactory.cpp +++ b/osal/linux/SemaphoreFactory.cpp @@ -3,8 +3,8 @@ #include #include -const uint32_t SemaphoreIF::NO_TIMEOUT = 0; -const uint32_t SemaphoreIF::MAX_TIMEOUT = 0xFFFFFFFF; +const uint32_t SemaphoreIF::POLLING = 0; +const uint32_t SemaphoreIF::BLOCKING = 0xffffffff; SemaphoreFactory* SemaphoreFactory::factoryInstance = nullptr; diff --git a/tasks/SemaphoreIF.h b/tasks/SemaphoreIF.h index a7c5a97b..4fc92ebd 100644 --- a/tasks/SemaphoreIF.h +++ b/tasks/SemaphoreIF.h @@ -21,10 +21,21 @@ class SemaphoreIF { public: virtual~ SemaphoreIF() {}; - //! Needs to be defined in implementation. No blocking time - static const uint32_t NO_TIMEOUT; - //! Needs to be defined in implementation. Blocks indefinitely. - static const uint32_t MAX_TIMEOUT; + /** + * @brief Timeout value used for polling lock attempt. + * @details + * If the lock is not successfull, MUTEX_TIMEOUT will be returned + * immediately. Value needs to be defined in implementation. + */ + static const uint32_t POLLING; + /** + * @brief Timeout value used for permanent blocking lock attempt. + * @details + * The task will be blocked (indefinitely) until the mutex is unlocked. + * Value needs to be defined in implementation. + */ + static const uint32_t BLOCKING; + static const uint8_t INTERFACE_ID = CLASS_ID::SEMAPHORE_IF; //! Semaphore timeout static constexpr ReturnValue_t SEMAPHORE_TIMEOUT = MAKE_RETURN_CODE(1); From ec212d9fcf45410507110d5b87f00af71846d037 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Mon, 6 Jul 2020 13:26:58 +0200 Subject: [PATCH 09/50] new fifo init --- container/FIFO.h | 88 ++++++++---------------------------------- container/FIFOBase.h | 59 ++++++++++++++++++++++++++++ container/FIFOBase.tpp | 76 ++++++++++++++++++++++++++++++++++++ container/StaticFIFO.h | 23 +++++++++++ 4 files changed, 174 insertions(+), 72 deletions(-) create mode 100644 container/FIFOBase.h create mode 100644 container/FIFOBase.tpp create mode 100644 container/StaticFIFO.h diff --git a/container/FIFO.h b/container/FIFO.h index f70c78b0..217ddbca 100644 --- a/container/FIFO.h +++ b/container/FIFO.h @@ -1,82 +1,26 @@ -#ifndef FIFO_H_ -#define FIFO_H_ +#ifndef FRAMEWORK_CONTAINER_FIFO_H_ +#define FRAMEWORK_CONTAINER_FIFO_H_ -#include +#include +#include /** - * @brief Simple First-In-First-Out data structure + * @brief Simple First-In-First-Out data structure. The maximum size + * can be set in the constructor. THe public interface of + * FIFOBase exposes the user interface for the FIFO. * @tparam T Entry Type * @tparam capacity Maximum capacity */ -template -class FIFO { -private: - uint8_t readIndex, writeIndex, currentSize; - T data[capacity]; - - uint8_t next(uint8_t current) { - ++current; - if (current == capacity) { - current = 0; - } - return current; - } +template +class FIFO: public FIFOBase { public: - FIFO() : - readIndex(0), writeIndex(0), currentSize(0) { - } + FIFO(size_t maxCapacity): FIFOBase(values.data(), maxCapacity) { + values.reserve(maxCapacity); + values.resize(maxCapacity); + }; - bool empty() { - return (currentSize == 0); - } - - bool full() { - return (currentSize == capacity); - } - - uint8_t size(){ - return currentSize; - } - - ReturnValue_t insert(T value) { - if (full()) { - return FULL; - } else { - data[writeIndex] = value; - writeIndex = next(writeIndex); - ++currentSize; - return HasReturnvaluesIF::RETURN_OK; - } - } - - ReturnValue_t retrieve(T *value) { - if (empty()) { - return EMPTY; - } else { - *value = data[readIndex]; - readIndex = next(readIndex); - --currentSize; - return HasReturnvaluesIF::RETURN_OK; - } - } - - ReturnValue_t peek(T * value) { - if(empty()) { - return EMPTY; - } else { - *value = data[readIndex]; - return HasReturnvaluesIF::RETURN_OK; - } - } - - ReturnValue_t pop() { - T value; - return this->retrieve(&value); - } - - static const uint8_t INTERFACE_ID = CLASS_ID::FIFO_CLASS; - static const ReturnValue_t FULL = MAKE_RETURN_CODE(1); - static const ReturnValue_t EMPTY = MAKE_RETURN_CODE(2); +private: + std::vector values; }; -#endif /* FIFO_H_ */ +#endif /* FRAMEWORK_CONTAINER_FIFO_H_ */ diff --git a/container/FIFOBase.h b/container/FIFOBase.h new file mode 100644 index 00000000..c6a74d2f --- /dev/null +++ b/container/FIFOBase.h @@ -0,0 +1,59 @@ +#ifndef FRAMEWORK_CONTAINER_FIFOBASE_H_ +#define FRAMEWORK_CONTAINER_FIFOBASE_H_ + +#include +#include + +template +class FIFOBase { +public: + static const uint8_t INTERFACE_ID = CLASS_ID::FIFO_CLASS; + static const ReturnValue_t FULL = MAKE_RETURN_CODE(1); + static const ReturnValue_t EMPTY = MAKE_RETURN_CODE(2); + + + /** Default ctor, no input arguments required. */ + FIFOBase(T* values, const size_t maxCapacity); + + /** + * Insert value into FIFO + * @param value + * @return + */ + ReturnValue_t insert(T value); + /** + * Retrieve item from FIFO. This removes the item from the FIFO. + * @param value + * @return + */ + ReturnValue_t retrieve(T *value); + /** + * Retrieve item from FIFO without removing it from FIFO. + * @param value + * @return + */ + ReturnValue_t peek(T * value); + /** + * Remove item from FIFO. + * @return + */ + ReturnValue_t pop(); + + bool empty(); + bool full(); + size_t size(); + +private: + T* values; + size_t maxCapacity; + + size_t readIndex = 0; + size_t writeIndex = 0; + size_t currentSize = 0; + + size_t next(size_t current); +}; + +#include + +#endif /* FRAMEWORK_CONTAINER_FIFOBASE_H_ */ diff --git a/container/FIFOBase.tpp b/container/FIFOBase.tpp new file mode 100644 index 00000000..011ce6de --- /dev/null +++ b/container/FIFOBase.tpp @@ -0,0 +1,76 @@ +#ifndef FRAMEWORK_CONTAINER_FIFOBASE_TPP_ +#define FRAMEWORK_CONTAINER_FIFOBASE_TPP_ + +#ifndef FRAMEWORK_CONTAINER_FIFOBASE_H_ +#error Include FIFOBase.h before FIFOBase.tpp! +#endif + +template +inline FIFOBase::FIFOBase(T* values, const size_t maxCapacity): + values(values), maxCapacity(maxCapacity) {}; + +template +inline ReturnValue_t FIFOBase::insert(T value) { + if (full()) { + return FULL; + } else { + values[writeIndex] = value; + writeIndex = next(writeIndex); + ++currentSize; + return HasReturnvaluesIF::RETURN_OK; + } +}; + +template +inline ReturnValue_t FIFOBase::retrieve(T* value) { + if (empty()) { + return EMPTY; + } else { + *value = values[readIndex]; + readIndex = next(readIndex); + --currentSize; + return HasReturnvaluesIF::RETURN_OK; + } +}; + +template +inline ReturnValue_t FIFOBase::peek(T* value) { + if(empty()) { + return EMPTY; + } else { + *value = values[readIndex]; + return HasReturnvaluesIF::RETURN_OK; + } +}; + +template +inline ReturnValue_t FIFOBase::pop() { + T value; + return this->retrieve(&value); +}; + +template +inline bool FIFOBase::empty() { + return (currentSize == 0); +}; + +template +inline bool FIFOBase::full() { + return (currentSize == maxCapacity); +} + +template +inline size_t FIFOBase::size() { + return currentSize; +} + +template +inline size_t FIFOBase::next(size_t current) { + ++current; + if (current == maxCapacity) { + current = 0; + } + return current; +} + +#endif diff --git a/container/StaticFIFO.h b/container/StaticFIFO.h new file mode 100644 index 00000000..651c4989 --- /dev/null +++ b/container/StaticFIFO.h @@ -0,0 +1,23 @@ +#ifndef FRAMEWORK_CONTAINER_STATICFIFO_H_ +#define FRAMEWORK_CONTAINER_STATICFIFO_H_ + +#include +#include + +/** + * @brief Simple First-In-First-Out data structure with size fixed at + * compile time. The public interface of FIFOBase exposes + * the user interface for the FIFO. + * @tparam T Entry Type + * @tparam capacity Maximum capacity + */ +template +class StaticFIFO: public FIFOBase { +public: + StaticFIFO(): FIFOBase(values.data(), capacity) {}; + +private: + std::array values; +}; + +#endif /* FRAMEWORK_CONTAINERS_STATICFIFO_H_ */ From 991385de6576583e4a45aa8c059c23f964094434 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Mon, 6 Jul 2020 13:29:56 +0200 Subject: [PATCH 10/50] added missing include, CSB uses static fifo now --- container/StaticFIFO.h | 1 + tmtcservices/CommandingServiceBase.h | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/container/StaticFIFO.h b/container/StaticFIFO.h index 651c4989..780b0383 100644 --- a/container/StaticFIFO.h +++ b/container/StaticFIFO.h @@ -3,6 +3,7 @@ #include #include +#include /** * @brief Simple First-In-First-Out data structure with size fixed at diff --git a/tmtcservices/CommandingServiceBase.h b/tmtcservices/CommandingServiceBase.h index bf2e8242..b3132be5 100644 --- a/tmtcservices/CommandingServiceBase.h +++ b/tmtcservices/CommandingServiceBase.h @@ -2,7 +2,7 @@ #define COMMANDINGSERVICEBASE_H_ #include -#include +#include #include #include #include @@ -179,7 +179,7 @@ protected: uint32_t state; Command_t command; object_id_t objectId; - FIFO fifo; + StaticFIFO fifo; }; const uint16_t apid; From b699c8b2b3be6446502c107f893c8b53201f5bc9 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Mon, 6 Jul 2020 13:38:11 +0200 Subject: [PATCH 11/50] put FIFO in namespace, fixed doc --- container/FIFO.h | 4 ++++ container/FIFOBase.h | 8 ++++++-- container/StaticFIFO.h | 10 ++++++++-- tmtcservices/CommandingServiceBase.h | 2 +- 4 files changed, 19 insertions(+), 5 deletions(-) diff --git a/container/FIFO.h b/container/FIFO.h index 217ddbca..801df6a2 100644 --- a/container/FIFO.h +++ b/container/FIFO.h @@ -4,6 +4,8 @@ #include #include +namespace fifo { + /** * @brief Simple First-In-First-Out data structure. The maximum size * can be set in the constructor. THe public interface of @@ -23,4 +25,6 @@ private: std::vector values; }; +} + #endif /* FRAMEWORK_CONTAINER_FIFO_H_ */ diff --git a/container/FIFOBase.h b/container/FIFOBase.h index c6a74d2f..ac5c6e2c 100644 --- a/container/FIFOBase.h +++ b/container/FIFOBase.h @@ -4,6 +4,8 @@ #include #include +namespace fsfw { + template class FIFOBase { public: @@ -11,8 +13,8 @@ public: static const ReturnValue_t FULL = MAKE_RETURN_CODE(1); static const ReturnValue_t EMPTY = MAKE_RETURN_CODE(2); - - /** Default ctor, no input arguments required. */ + /** Default ctor, takes pointer to first entry of underlying container + * and maximum capacity */ FIFOBase(T* values, const size_t maxCapacity); /** @@ -56,4 +58,6 @@ private: #include +} + #endif /* FRAMEWORK_CONTAINER_FIFOBASE_H_ */ diff --git a/container/StaticFIFO.h b/container/StaticFIFO.h index 780b0383..f3128200 100644 --- a/container/StaticFIFO.h +++ b/container/StaticFIFO.h @@ -5,10 +5,14 @@ #include #include +namespace fsfw { + /** * @brief Simple First-In-First-Out data structure with size fixed at - * compile time. The public interface of FIFOBase exposes - * the user interface for the FIFO. + * compile time + * @details + * Performs no dynamic memory allocation. + * The public interface of FIFOBase exposes the user interface for the FIFO. * @tparam T Entry Type * @tparam capacity Maximum capacity */ @@ -21,4 +25,6 @@ private: std::array values; }; +} + #endif /* FRAMEWORK_CONTAINERS_STATICFIFO_H_ */ diff --git a/tmtcservices/CommandingServiceBase.h b/tmtcservices/CommandingServiceBase.h index b3132be5..a4343065 100644 --- a/tmtcservices/CommandingServiceBase.h +++ b/tmtcservices/CommandingServiceBase.h @@ -179,7 +179,7 @@ protected: uint32_t state; Command_t command; object_id_t objectId; - StaticFIFO fifo; + fsfw::StaticFIFO fifo; }; const uint16_t apid; From 8f6c3b50af92e9e5619a79e48414a8e252e29577 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Mon, 6 Jul 2020 13:40:13 +0200 Subject: [PATCH 12/50] namespace fix --- container/FIFO.h | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/container/FIFO.h b/container/FIFO.h index 801df6a2..7251321f 100644 --- a/container/FIFO.h +++ b/container/FIFO.h @@ -4,12 +4,15 @@ #include #include -namespace fifo { +namespace fsfw { /** * @brief Simple First-In-First-Out data structure. The maximum size - * can be set in the constructor. THe public interface of - * FIFOBase exposes the user interface for the FIFO. + * can be set in the constructor. + * @details + * The maximum capacity can be determined at run-time, so this container + * performs dynamic memory allocation! + * The public interface of FIFOBase exposes the user interface for the FIFO. * @tparam T Entry Type * @tparam capacity Maximum capacity */ From 2fccc4fef77537a62f39297f1c5bf39fbc9ea08f Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Mon, 6 Jul 2020 23:08:31 +0200 Subject: [PATCH 13/50] getter function for capacity --- container/FIFO.h | 6 ++---- container/FIFOBase.h | 4 +++- container/FIFOBase.tpp | 5 +++++ 3 files changed, 10 insertions(+), 5 deletions(-) diff --git a/container/FIFO.h b/container/FIFO.h index 7251321f..19daaf1b 100644 --- a/container/FIFO.h +++ b/container/FIFO.h @@ -19,10 +19,8 @@ namespace fsfw { template class FIFO: public FIFOBase { public: - FIFO(size_t maxCapacity): FIFOBase(values.data(), maxCapacity) { - values.reserve(maxCapacity); - values.resize(maxCapacity); - }; + FIFO(size_t maxCapacity): FIFOBase(values.data(), maxCapacity), + values(maxCapacity) {}; private: std::vector values; diff --git a/container/FIFOBase.h b/container/FIFOBase.h index ac5c6e2c..6ed7d924 100644 --- a/container/FIFOBase.h +++ b/container/FIFOBase.h @@ -45,9 +45,11 @@ public: bool full(); size_t size(); + size_t getMaxCapacity() const; + private: T* values; - size_t maxCapacity; + size_t maxCapacity = 0; size_t readIndex = 0; size_t writeIndex = 0; diff --git a/container/FIFOBase.tpp b/container/FIFOBase.tpp index 011ce6de..48a060ff 100644 --- a/container/FIFOBase.tpp +++ b/container/FIFOBase.tpp @@ -73,4 +73,9 @@ inline size_t FIFOBase::next(size_t current) { return current; } +template +inline size_t FIFOBase::getMaxCapacity() const { + return maxCapacity; +} + #endif From 5f76b03f3ab29b5f401124ad2a0a137a6828d285 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Tue, 7 Jul 2020 17:44:15 +0200 Subject: [PATCH 14/50] fifo tweaks --- container/DynamicFIFO.h | 27 +++++++++++++++++++++++++++ container/FIFO.h | 23 +++++++++-------------- container/FIFOBase.h | 4 ---- container/StaticFIFO.h | 30 ------------------------------ 4 files changed, 36 insertions(+), 48 deletions(-) create mode 100644 container/DynamicFIFO.h delete mode 100644 container/StaticFIFO.h diff --git a/container/DynamicFIFO.h b/container/DynamicFIFO.h new file mode 100644 index 00000000..3a5242c7 --- /dev/null +++ b/container/DynamicFIFO.h @@ -0,0 +1,27 @@ +#ifndef FRAMEWORK_CONTAINER_DYNAMICFIFO_H_ +#define FRAMEWORK_CONTAINER_DYNAMICFIFO_H_ + +#include +#include + +/** + * @brief Simple First-In-First-Out data structure. The maximum size + * can be set in the constructor. + * @details + * The maximum capacity can be determined at run-time, so this container + * performs dynamic memory allocation! + * The public interface of FIFOBase exposes the user interface for the FIFO. + * @tparam T Entry Type + * @tparam capacity Maximum capacity + */ +template +class DynamicFIFO: public FIFOBase { +public: + DynamicFIFO(size_t maxCapacity): FIFOBase(values.data(), maxCapacity), + values(maxCapacity) {}; + +private: + std::vector values; +}; + +#endif /* FRAMEWORK_CONTAINER_DYNAMICFIFO_H_ */ diff --git a/container/FIFO.h b/container/FIFO.h index 19daaf1b..ecb218fd 100644 --- a/container/FIFO.h +++ b/container/FIFO.h @@ -1,31 +1,26 @@ #ifndef FRAMEWORK_CONTAINER_FIFO_H_ #define FRAMEWORK_CONTAINER_FIFO_H_ +#include #include -#include - -namespace fsfw { +#include /** - * @brief Simple First-In-First-Out data structure. The maximum size - * can be set in the constructor. + * @brief Simple First-In-First-Out data structure with size fixed at + * compile time * @details - * The maximum capacity can be determined at run-time, so this container - * performs dynamic memory allocation! + * Performs no dynamic memory allocation. * The public interface of FIFOBase exposes the user interface for the FIFO. * @tparam T Entry Type * @tparam capacity Maximum capacity */ -template +template class FIFO: public FIFOBase { public: - FIFO(size_t maxCapacity): FIFOBase(values.data(), maxCapacity), - values(maxCapacity) {}; + FIFO(): FIFOBase(values.data(), capacity) {}; private: - std::vector values; + std::array values; }; -} - -#endif /* FRAMEWORK_CONTAINER_FIFO_H_ */ +#endif /* FRAMEWORK_CONTAINERS_STATICFIFO_H_ */ diff --git a/container/FIFOBase.h b/container/FIFOBase.h index 6ed7d924..8bdb333f 100644 --- a/container/FIFOBase.h +++ b/container/FIFOBase.h @@ -4,8 +4,6 @@ #include #include -namespace fsfw { - template class FIFOBase { public: @@ -60,6 +58,4 @@ private: #include -} - #endif /* FRAMEWORK_CONTAINER_FIFOBASE_H_ */ diff --git a/container/StaticFIFO.h b/container/StaticFIFO.h deleted file mode 100644 index f3128200..00000000 --- a/container/StaticFIFO.h +++ /dev/null @@ -1,30 +0,0 @@ -#ifndef FRAMEWORK_CONTAINER_STATICFIFO_H_ -#define FRAMEWORK_CONTAINER_STATICFIFO_H_ - -#include -#include -#include - -namespace fsfw { - -/** - * @brief Simple First-In-First-Out data structure with size fixed at - * compile time - * @details - * Performs no dynamic memory allocation. - * The public interface of FIFOBase exposes the user interface for the FIFO. - * @tparam T Entry Type - * @tparam capacity Maximum capacity - */ -template -class StaticFIFO: public FIFOBase { -public: - StaticFIFO(): FIFOBase(values.data(), capacity) {}; - -private: - std::array values; -}; - -} - -#endif /* FRAMEWORK_CONTAINERS_STATICFIFO_H_ */ From d795892d578772cc04ac159a228f9501ab39d8db Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Tue, 7 Jul 2020 17:54:09 +0200 Subject: [PATCH 15/50] csb fifo tweak --- tmtcservices/CommandingServiceBase.h | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/tmtcservices/CommandingServiceBase.h b/tmtcservices/CommandingServiceBase.h index a4343065..e7702860 100644 --- a/tmtcservices/CommandingServiceBase.h +++ b/tmtcservices/CommandingServiceBase.h @@ -1,8 +1,8 @@ -#ifndef COMMANDINGSERVICEBASE_H_ -#define COMMANDINGSERVICEBASE_H_ +#ifndef FRAMEWORK_TMTCSERVICES_COMMANDINGSERVICEBASE_H_ +#define FRAMEWORK_TMTCSERVICES_COMMANDINGSERVICEBASE_H_ #include -#include +#include #include #include #include @@ -179,7 +179,7 @@ protected: uint32_t state; Command_t command; object_id_t objectId; - fsfw::StaticFIFO fifo; + FIFO fifo; }; const uint16_t apid; @@ -286,4 +286,4 @@ private: void checkTimeout(); }; -#endif /* COMMANDINGSERVICEBASE_H_ */ +#endif /* FRAMEWORK_TMTCSERVICES_COMMANDINGSERVICEBASE_H_ */ From 65d71b1c65b2841a94d0fa1cb95b849afa2cff47 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Tue, 7 Jul 2020 18:11:36 +0200 Subject: [PATCH 16/50] added new ctor, non-allocating --- container/SimpleRingBuffer.cpp | 18 +++++++---- container/SimpleRingBuffer.h | 57 +++++++++++++++++++++++++++++++--- 2 files changed, 64 insertions(+), 11 deletions(-) diff --git a/container/SimpleRingBuffer.cpp b/container/SimpleRingBuffer.cpp index 63dc3514..731c9dc9 100644 --- a/container/SimpleRingBuffer.cpp +++ b/container/SimpleRingBuffer.cpp @@ -1,18 +1,23 @@ #include -#include +#include -SimpleRingBuffer::SimpleRingBuffer(uint32_t size, bool overwriteOld) : - RingBufferBase<>(0, size, overwriteOld), buffer(NULL) { +SimpleRingBuffer::SimpleRingBuffer(const size_t size, bool overwriteOld) : + RingBufferBase<>(0, size, overwriteOld) { buffer = new uint8_t[size]; } +SimpleRingBuffer::SimpleRingBuffer(uint8_t *buffer, const size_t size, + bool overwriteOld): + RingBufferBase<>(0, size, overwriteOld), buffer(buffer) {} + + SimpleRingBuffer::~SimpleRingBuffer() { delete[] buffer; } ReturnValue_t SimpleRingBuffer::writeData(const uint8_t* data, uint32_t amount) { - if (availableWriteSpace() >= amount || overwriteOld) { + if (availableWriteSpace() >= amount or overwriteOld) { uint32_t amountTillWrap = writeTillWrap(); if (amountTillWrap >= amount) { memcpy(&buffer[write], data, amount); @@ -38,7 +43,7 @@ ReturnValue_t SimpleRingBuffer::readData(uint8_t* data, uint32_t amount, return HasReturnvaluesIF::RETURN_FAILED; } } - if (trueAmount != NULL) { + if (trueAmount != nullptr) { *trueAmount = amount; } if (amountTillWrap >= amount) { @@ -60,9 +65,10 @@ ReturnValue_t SimpleRingBuffer::deleteData(uint32_t amount, return HasReturnvaluesIF::RETURN_FAILED; } } - if (trueAmount != NULL) { + if (trueAmount != nullptr) { *trueAmount = amount; } incrementRead(amount, READ_PTR); return HasReturnvaluesIF::RETURN_OK; } + diff --git a/container/SimpleRingBuffer.h b/container/SimpleRingBuffer.h index 6d7d67e1..30dd184c 100644 --- a/container/SimpleRingBuffer.h +++ b/container/SimpleRingBuffer.h @@ -2,19 +2,66 @@ #define FRAMEWORK_CONTAINER_SIMPLERINGBUFFER_H_ #include -#include +#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: - SimpleRingBuffer(uint32_t size, bool overwriteOld); + /** + * This constructor allocates a new internal buffer with the supplied size. + * @param size + * @param overwriteOld + */ + SimpleRingBuffer(const size_t size, bool overwriteOld); + /** + * This constructor takes an external buffer with the specified size. + * @param buffer + * @param size + * @param overwriteOld + */ + SimpleRingBuffer(uint8_t* buffer, const size_t size, bool overwriteOld); + virtual ~SimpleRingBuffer(); + + /** + * Write to circular buffer and increment write pointer by amount + * @param data + * @param amount + * @return + */ 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); + + /** + * Read from circular buffer at read pointer + * @param data + * @param amount + * @param readRemaining + * @param trueAmount + * @return + */ + ReturnValue_t readData(uint8_t* data, uint32_t amount, + bool readRemaining = false, uint32_t* trueAmount = nullptr); + + /** + * Delete data starting by incrementing read pointer + * @param amount + * @param deleteRemaining + * @param trueAmount + * @return + */ + ReturnValue_t deleteData(uint32_t amount, bool deleteRemaining = false, + uint32_t* trueAmount = nullptr); private: // static const uint8_t TEMP_READ_PTR = 1; static const uint8_t READ_PTR = 0; - uint8_t* buffer; + uint8_t* buffer = nullptr; }; #endif /* FRAMEWORK_CONTAINER_SIMPLERINGBUFFER_H_ */ From 646e86ea85d8bd29f89c2568efb42e2a49e497aa Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 16 Jul 2020 11:56:48 +0200 Subject: [PATCH 17/50] dhb update init --- devicehandlers/DeviceHandlerBase.cpp | 517 +++++++++++++++++---------- devicehandlers/DeviceHandlerBase.h | 310 +++++++++------- 2 files changed, 504 insertions(+), 323 deletions(-) diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp index 872310a1..cc97cd42 100644 --- a/devicehandlers/DeviceHandlerBase.cpp +++ b/devicehandlers/DeviceHandlerBase.cpp @@ -4,55 +4,69 @@ #include #include -#include -#include +#include +#include #include #include +#include +#include #include #include #include #include -object_id_t DeviceHandlerBase::powerSwitcherId = 0; -object_id_t DeviceHandlerBase::rawDataReceiverId = 0; -object_id_t DeviceHandlerBase::defaultFDIRParentId = 0; +object_id_t DeviceHandlerBase::powerSwitcherId = objects::NO_OBJECT; +object_id_t DeviceHandlerBase::rawDataReceiverId = objects::NO_OBJECT; +object_id_t DeviceHandlerBase::defaultFdirParentId = objects::NO_OBJECT; +object_id_t DeviceHandlerBase::defaultHkDestination = objects::NO_OBJECT; DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, CookieIF * comCookie, - uint8_t setDeviceSwitch, uint32_t thermalStatePoolId, - uint32_t thermalRequestPoolId, FailureIsolationBase* fdirInstance, - size_t cmdQueueSize) : + FailureIsolationBase* fdirInstance, size_t cmdQueueSize) : SystemObject(setObjectId), mode(MODE_OFF), submode(SUBMODE_NONE), wiretappingMode(OFF), storedRawData(StorageManagerIF::INVALID_ADDRESS), deviceCommunicationId(deviceCommunication), comCookie(comCookie), - deviceThermalStatePoolId(thermalStatePoolId), - deviceThermalRequestPoolId(thermalRequestPoolId), healthHelper(this,setObjectId), modeHelper(this), parameterHelper(this), + actionHelper(this, nullptr), hkManager(this, nullptr), childTransitionFailure(RETURN_OK), fdirInstance(fdirInstance), hkSwitcher(this), defaultFDIRUsed(fdirInstance == nullptr), - switchOffWasReported(false), actionHelper(this, nullptr), - childTransitionDelay(5000), - transitionSourceMode(_MODE_POWER_DOWN), transitionSourceSubMode( - SUBMODE_NONE), deviceSwitch(setDeviceSwitch) { + switchOffWasReported(false), childTransitionDelay(5000), + transitionSourceMode(_MODE_POWER_DOWN), + transitionSourceSubMode(SUBMODE_NONE) { commandQueue = QueueFactory::instance()->createMessageQueue(cmdQueueSize, - CommandMessage::MAX_MESSAGE_SIZE); + MessageQueueMessage::MAX_MESSAGE_SIZE); insertInCommandMap(RAW_COMMAND_ID); cookieInfo.state = COOKIE_UNUSED; cookieInfo.pendingCommand = deviceCommandMap.end(); if (comCookie == nullptr) { - sif::error << "DeviceHandlerBase: ObjectID 0x" << std::hex << - std::setw(8) << std::setfill('0') << this->getObjectId() << - std::dec << ": Do not pass nullptr as a cookie, consider " - << std::setfill(' ') << "passing a dummy cookie instead!" << - std::endl; + sif::error << "DeviceHandlerBase: ObjectID 0x" << std::hex + << std::setw(8) << std::setfill('0') << this->getObjectId() + << std::dec << ": Do not pass nullptr as a cookie, consider " + << std::setfill(' ') << "passing a dummy cookie instead!" + << std::endl; } if (this->fdirInstance == nullptr) { this->fdirInstance = new DeviceHandlerFailureIsolation(setObjectId, - defaultFDIRParentId); + defaultFdirParentId); } } +void DeviceHandlerBase::setHkDestination(object_id_t hkDestination) { + this->hkDestination = hkDestination; +} + +void DeviceHandlerBase::setThermalStateRequestPoolIds( + uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId) { + this->deviceThermalRequestPoolId = thermalStatePoolId; + this->deviceThermalRequestPoolId = thermalRequestPoolId; +} + + +void DeviceHandlerBase::setDeviceSwitch(uint8_t deviceSwitch) { + this->deviceSwitch = deviceSwitch; +} + DeviceHandlerBase::~DeviceHandlerBase() { delete comCookie; if (defaultFDIRUsed) { @@ -108,8 +122,12 @@ ReturnValue_t DeviceHandlerBase::initialize() { communicationInterface = objectManager->get( deviceCommunicationId); - if (communicationInterface == NULL) { - return RETURN_FAILED; + if (communicationInterface == nullptr) { + sif::error << "DeviceHandlerBase::initialize: Communication interface " + "invalid." << std::endl; + sif::error << "Make sure it is set up properly and implements" + " DeviceCommunicationIF" << std::endl; + return ObjectManagerIF::CHILD_INIT_FAILED; } result = communicationInterface->initializeInterface(comCookie); @@ -118,27 +136,44 @@ ReturnValue_t DeviceHandlerBase::initialize() { } IPCStore = objectManager->get(objects::IPC_STORE); - if (IPCStore == NULL) { - return RETURN_FAILED; + if (IPCStore == nullptr) { + sif::error << "DeviceHandlerBase::initialize: IPC store not set up in " + "factory." << std::endl; + return ObjectManagerIF::CHILD_INIT_FAILED; } - AcceptsDeviceResponsesIF *rawReceiver = objectManager->get< - AcceptsDeviceResponsesIF>(rawDataReceiverId); + if(rawDataReceiverId != objects::NO_OBJECT) { + AcceptsDeviceResponsesIF *rawReceiver = objectManager->get< + AcceptsDeviceResponsesIF>(rawDataReceiverId); - if (rawReceiver == NULL) { - return RETURN_FAILED; + if (rawReceiver == nullptr) { + sif::error << "DeviceHandlerBase::initialize: Raw receiver object " + "ID set but no valid object found." << std::endl; + sif::error << "Make sure the raw receiver object is set up properly" + " and implements AcceptsDeviceResponsesIF" << std::endl; + return ObjectManagerIF::CHILD_INIT_FAILED; + } + defaultRawReceiver = rawReceiver->getDeviceQueue(); } - defaultRawReceiver = rawReceiver->getDeviceQueue(); - - powerSwitcher = objectManager->get(powerSwitcherId); - if (powerSwitcher == NULL) { - return RETURN_FAILED; + if(powerSwitcherId != objects::NO_OBJECT) { + powerSwitcher = objectManager->get(powerSwitcherId); + if (powerSwitcher == nullptr) { + sif::error << "DeviceHandlerBase::initialize: Power switcher " + << "object ID set but no valid object found." << std::endl; + sif::error << "Make sure the raw receiver object is set up properly" + << " and implements PowerSwitchIF" << std::endl; + return ObjectManagerIF::CHILD_INIT_FAILED; + } } result = healthHelper.initialize(); - if (result != RETURN_OK) { - return result; + if (result == RETURN_OK) { + healthHelperActive = true; + } + else { + sif::warning << "DeviceHandlerBase::initialize: Health Helper " + "initialization failure." << std::endl; } result = modeHelper.initialize(); @@ -164,11 +199,20 @@ ReturnValue_t DeviceHandlerBase::initialize() { return result; } + if(hkDestination == objects::NO_OBJECT) { + hkDestination = defaultHkDestination; + } + + result = hkManager.initialize(commandQueue, hkDestination); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + fillCommandAndReplyMap(); //Set temperature target state to NON_OP. - DataSet mySet; - PoolVariable thermalRequest(deviceThermalRequestPoolId, &mySet, + GlobDataSet mySet; + gp_uint8_t thermalRequest(deviceThermalRequestPoolId, &mySet, PoolVariableIF::VAR_WRITE); mySet.read(); thermalRequest = ThermalComponentIF::STATE_REQUEST_NON_OPERATIONAL; @@ -200,43 +244,50 @@ void DeviceHandlerBase::readCommandQueue() { return; } - CommandMessage message; - ReturnValue_t result = commandQueue->receiveMessage(&message); + CommandMessage command; + ReturnValue_t result = commandQueue->receiveMessage(&command); if (result != RETURN_OK) { return; } - result = healthHelper.handleHealthCommand(&message); + if(healthHelperActive) { + result = healthHelper.handleHealthCommand(&command); + if (result == RETURN_OK) { + return; + } + } + + result = modeHelper.handleModeCommand(&command); if (result == RETURN_OK) { return; } - result = modeHelper.handleModeCommand(&message); + result = actionHelper.handleActionMessage(&command); if (result == RETURN_OK) { return; } - result = actionHelper.handleActionMessage(&message); + result = parameterHelper.handleParameterMessage(&command); if (result == RETURN_OK) { return; } - result = parameterHelper.handleParameterMessage(&message); + result = hkManager.handleHousekeepingMessage(&command); if (result == RETURN_OK) { return; } - result = handleDeviceHandlerMessage(&message); + result = handleDeviceHandlerMessage(&command); if (result == RETURN_OK) { return; } - result = letChildHandleMessage(&message); + result = letChildHandleMessage(&command); if (result == RETURN_OK) { return; } - replyReturnvalueToCommand(CommandMessage::UNKNOW_COMMAND); + replyReturnvalueToCommand(CommandMessage::UNKNOWN_COMMAND); } @@ -273,7 +324,8 @@ void DeviceHandlerBase::doStateMachine() { case _MODE_WAIT_ON: { uint32_t currentUptime; Clock::getUptime(¤tUptime); - if (currentUptime - timeoutStart >= powerSwitcher->getSwitchDelayMs()) { + if (powerSwitcher != nullptr and currentUptime - timeoutStart >= + powerSwitcher->getSwitchDelayMs()) { triggerEvent(MODE_TRANSITION_FAILED, PowerSwitchIF::SWITCH_TIMEOUT, 0); setMode(_MODE_POWER_DOWN); @@ -293,6 +345,12 @@ void DeviceHandlerBase::doStateMachine() { case _MODE_WAIT_OFF: { uint32_t currentUptime; Clock::getUptime(¤tUptime); + + if(powerSwitcher == nullptr) { + setMode(MODE_OFF); + break; + } + if (currentUptime - timeoutStart >= powerSwitcher->getSwitchDelayMs()) { triggerEvent(MODE_TRANSITION_FAILED, PowerSwitchIF::SWITCH_TIMEOUT, 0); @@ -343,9 +401,10 @@ ReturnValue_t DeviceHandlerBase::isModeCombinationValid(Mode_t mode, } } -ReturnValue_t DeviceHandlerBase::insertInCommandAndReplyMap(DeviceCommandId_t deviceCommand, - uint16_t maxDelayCycles, size_t replyLen, bool periodic, - bool hasDifferentReplyId, DeviceCommandId_t replyId) { +ReturnValue_t DeviceHandlerBase::insertInCommandAndReplyMap( + DeviceCommandId_t deviceCommand, uint16_t maxDelayCycles, + size_t replyLen, bool periodic, bool hasDifferentReplyId, + DeviceCommandId_t replyId) { //No need to check, as we may try to insert multiple times. insertInCommandMap(deviceCommand); if (hasDifferentReplyId) { @@ -371,7 +430,8 @@ ReturnValue_t DeviceHandlerBase::insertInReplyMap(DeviceCommandId_t replyId, } } -ReturnValue_t DeviceHandlerBase::insertInCommandMap(DeviceCommandId_t deviceCommand) { +ReturnValue_t DeviceHandlerBase::insertInCommandMap( + DeviceCommandId_t deviceCommand) { DeviceCommandInfo info; info.expectedReplies = 0; info.isExecuting = false; @@ -419,7 +479,7 @@ void DeviceHandlerBase::setTransition(Mode_t modeTo, Submode_t submodeTo) { transitionSourceSubMode = submode; childTransitionFailure = CHILD_TIMEOUT; -//transitionTargetMode is set by setMode + // transitionTargetMode is set by setMode setMode((modeTo | TRANSITION_MODE_CHILD_ACTION_MASK), submodeTo); } @@ -436,8 +496,8 @@ void DeviceHandlerBase::setMode(Mode_t newMode, uint8_t newSubmode) { Clock::getUptime(&timeoutStart); if (mode == MODE_OFF) { - DataSet mySet; - PoolVariable thermalRequest(deviceThermalRequestPoolId, &mySet, + GlobDataSet mySet; + gp_uint8_t thermalRequest(deviceThermalRequestPoolId, &mySet, PoolVariableIF::VAR_READ_WRITE); mySet.read(); if (thermalRequest != ThermalComponentIF::STATE_REQUEST_IGNORE) { @@ -578,11 +638,8 @@ void DeviceHandlerBase::doSendRead() { } void DeviceHandlerBase::doGetRead() { - size_t receivedDataLen; - uint8_t *receivedData; - DeviceCommandId_t foundId = 0xFFFFFFFF; - size_t foundLen = 0; - ReturnValue_t result; + size_t receivedDataLen = 0; + uint8_t *receivedData = nullptr; if (cookieInfo.state != COOKIE_READ_SENT) { cookieInfo.state = COOKIE_UNUSED; @@ -591,8 +648,8 @@ void DeviceHandlerBase::doGetRead() { cookieInfo.state = COOKIE_UNUSED; - result = communicationInterface->readReceivedMessage(comCookie, - &receivedData, &receivedDataLen); + ReturnValue_t result = communicationInterface->readReceivedMessage( + comCookie, &receivedData, &receivedDataLen); if (result != RETURN_OK) { triggerEvent(DEVICE_REQUESTING_REPLY_FAILED, result); @@ -608,51 +665,101 @@ void DeviceHandlerBase::doGetRead() { replyRawData(receivedData, receivedDataLen, requestedRawTraffic); } - if (mode == MODE_RAW) { + if (mode == MODE_RAW and defaultRawReceiver != MessageQueueIF::NO_QUEUE) { replyRawReplyIfnotWiretapped(receivedData, receivedDataLen); - } else { - //The loop may not execute more often than the number of received bytes (worst case). - //This approach avoids infinite loops due to buggy scanForReply routines (seen in bug 1077). - uint32_t remainingLength = receivedDataLen; - for (uint32_t count = 0; count < receivedDataLen; count++) { - result = scanForReply(receivedData, remainingLength, &foundId, - &foundLen); - switch (result) { - case RETURN_OK: - handleReply(receivedData, foundId, foundLen); - break; - case APERIODIC_REPLY: { - result = interpretDeviceReply(foundId, receivedData); - if (result != RETURN_OK) { - replyRawReplyIfnotWiretapped(receivedData, foundLen); - triggerEvent(DEVICE_INTERPRETING_REPLY_FAILED, result, - foundId); - } - } - break; - case IGNORE_REPLY_DATA: - break; - case IGNORE_FULL_PACKET: - return; - default: - //We need to wait for timeout.. don't know what command failed and who sent it. + } + else { + parseReply(receivedData, receivedDataLen); + } +} + +void DeviceHandlerBase::parseReply(const uint8_t* receivedData, + size_t receivedDataLen) { + ReturnValue_t result = HasReturnvaluesIF::RETURN_FAILED; + DeviceCommandId_t foundId = 0xFFFFFFFF; + size_t foundLen = 0; + // The loop may not execute more often than the number of received bytes + // (worst case). This approach avoids infinite loops due to buggy + // scanForReply routines. + uint32_t remainingLength = receivedDataLen; + for (uint32_t count = 0; count < receivedDataLen; count++) { + result = scanForReply(receivedData, remainingLength, &foundId, + &foundLen); + switch (result) { + case RETURN_OK: + handleReply(receivedData, foundId, foundLen); + break; + case APERIODIC_REPLY: { + result = interpretDeviceReply(foundId, receivedData); + if (result != RETURN_OK) { replyRawReplyIfnotWiretapped(receivedData, foundLen); - triggerEvent(DEVICE_READING_REPLY_FAILED, result, foundLen); - break; - } - receivedData += foundLen; - if (remainingLength > foundLen) { - remainingLength -= foundLen; - } else { - return; + triggerEvent(DEVICE_INTERPRETING_REPLY_FAILED, result, + foundId); } } + break; + case IGNORE_REPLY_DATA: + break; + case IGNORE_FULL_PACKET: + return; + default: + //We need to wait for timeout.. don't know what command failed and who sent it. + replyRawReplyIfnotWiretapped(receivedData, foundLen); + triggerEvent(DEVICE_READING_REPLY_FAILED, result, foundLen); + break; + } + receivedData += foundLen; + if (remainingLength > foundLen) { + remainingLength -= foundLen; + } else { + return; + } + } +} +void DeviceHandlerBase::handleReply(const uint8_t* receivedData, + DeviceCommandId_t foundId, uint32_t foundLen) { + ReturnValue_t result; + DeviceReplyMap::iterator iter = deviceReplyMap.find(foundId); + + if (iter == deviceReplyMap.end()) { + replyRawReplyIfnotWiretapped(receivedData, foundLen); + triggerEvent(DEVICE_UNKNOWN_REPLY, foundId); + return; + } + + DeviceReplyInfo *info = &(iter->second); + + if (info->delayCycles != 0) { + + if (info->periodic != false) { + info->delayCycles = info->maxDelayCycles; + } + else { + info->delayCycles = 0; + } + + result = interpretDeviceReply(foundId, receivedData); + + if (result != RETURN_OK) { + // Report failed interpretation to FDIR. + replyRawReplyIfnotWiretapped(receivedData, foundLen); + triggerEvent(DEVICE_INTERPRETING_REPLY_FAILED, result, foundId); + } + replyToReply(iter, result); + } + else { + // Other completion failure messages are created by timeout. + // Powering down the device might take some time during which periodic + // replies may still come in. + if (mode != _MODE_WAIT_OFF) { + triggerEvent(DEVICE_UNREQUESTED_REPLY, foundId); + } } } ReturnValue_t DeviceHandlerBase::getStorageData(store_address_t storageAddress, - uint8_t * *data, uint32_t * len) { + uint8_t** data, uint32_t * len) { size_t lenTmp; if (IPCStore == nullptr) { @@ -675,7 +782,7 @@ ReturnValue_t DeviceHandlerBase::getStorageData(store_address_t storageAddress, void DeviceHandlerBase::replyRawData(const uint8_t *data, size_t len, MessageQueueId_t sendTo, bool isCommand) { - if (IPCStore == NULL || len == 0) { + if (IPCStore == nullptr or len == 0 or sendTo == MessageQueueIF::NO_QUEUE) { return; } store_address_t address; @@ -686,18 +793,17 @@ void DeviceHandlerBase::replyRawData(const uint8_t *data, size_t len, return; } - CommandMessage message; + CommandMessage command; - DeviceHandlerMessage::setDeviceHandlerRawReplyMessage(&message, + DeviceHandlerMessage::setDeviceHandlerRawReplyMessage(&command, getObjectId(), address, isCommand); -// this->DeviceHandlerCommand = CommandMessage::CMD_NONE; - - result = commandQueue->sendMessage(sendTo, &message); + result = commandQueue->sendMessage(sendTo, &command); if (result != RETURN_OK) { IPCStore->deleteData(address); - //Silently discard data, this indicates heavy TM traffic which should not be increased by additional events. + // Silently discard data, this indicates heavy TM traffic which + // should not be increased by additional events. } } @@ -726,57 +832,6 @@ MessageQueueId_t DeviceHandlerBase::getCommandQueue() const { return commandQueue->getId(); } -void DeviceHandlerBase::handleReply(const uint8_t* receivedData, - DeviceCommandId_t foundId, uint32_t foundLen) { - ReturnValue_t result; - DeviceReplyMap::iterator iter = deviceReplyMap.find(foundId); - - if (iter == deviceReplyMap.end()) { - replyRawReplyIfnotWiretapped(receivedData, foundLen); - triggerEvent(DEVICE_UNKNOWN_REPLY, foundId); - return; - } - - DeviceReplyInfo *info = &(iter->second); - - if (info->delayCycles != 0) { - - if (info->periodic) { - info->delayCycles = info->maxDelayCycles; - } else { - info->delayCycles = 0; - } - result = interpretDeviceReply(foundId, receivedData); - if (result != RETURN_OK) { - //Report failed interpretation to FDIR. - replyRawReplyIfnotWiretapped(receivedData, foundLen); - triggerEvent(DEVICE_INTERPRETING_REPLY_FAILED, result, foundId); - } - replyToReply(iter, result); - } else { - //Other completion failure messages are created by timeout. - //Powering down the device might take some time during which periodic replies may still come in. - if (mode != _MODE_WAIT_OFF) { - triggerEvent(DEVICE_UNREQUESTED_REPLY, foundId); - } - } -} - -//ReturnValue_t DeviceHandlerBase::switchCookieChannel(object_id_t newChannelId) { -// DeviceCommunicationIF *newCommunication = objectManager->get< -// DeviceCommunicationIF>(newChannelId); -// -// if (newCommunication != NULL) { -// ReturnValue_t result = newCommunication->reOpen(cookie, ioBoardAddress, -// maxDeviceReplyLen); -// if (result != RETURN_OK) { -// return result; -// } -// return RETURN_OK; -// } -// return RETURN_FAILED; -//} - void DeviceHandlerBase::buildRawDeviceCommand(CommandMessage* commandMessage) { storedRawData = DeviceHandlerMessage::getStoreAddress(commandMessage); ReturnValue_t result = getStorageData(storedRawData, &rawPacket, @@ -793,6 +848,9 @@ void DeviceHandlerBase::buildRawDeviceCommand(CommandMessage* commandMessage) { } void DeviceHandlerBase::commandSwitch(ReturnValue_t onOff) { + if(powerSwitcher == nullptr) { + return; + } const uint8_t *switches; uint8_t numberOfSwitches = 0; ReturnValue_t result = getSwitches(&switches, &numberOfSwitches); @@ -807,9 +865,7 @@ void DeviceHandlerBase::commandSwitch(ReturnValue_t onOff) { ReturnValue_t DeviceHandlerBase::getSwitches(const uint8_t **switches, uint8_t *numberOfSwitches) { - *switches = &deviceSwitch; - *numberOfSwitches = 1; - return RETURN_OK; + return DeviceHandlerBase::NO_SWITCH; } void DeviceHandlerBase::modeChanged(void) { @@ -845,6 +901,9 @@ uint32_t DeviceHandlerBase::getTransitionDelayMs(Mode_t modeFrom, } ReturnValue_t DeviceHandlerBase::getStateOfSwitches(void) { + if(powerSwitcher == nullptr) { + return NO_SWITCH; + } uint8_t numberOfSwitches = 0; const uint8_t *switches; @@ -894,10 +953,10 @@ ReturnValue_t DeviceHandlerBase::checkModeCommand(Mode_t commandedMode, if ((commandedMode == MODE_ON) && (mode == MODE_OFF) && (deviceThermalStatePoolId != PoolVariableIF::NO_PARAMETER)) { - DataSet mySet; - PoolVariable thermalState(deviceThermalStatePoolId, &mySet, + GlobDataSet mySet; + gp_uint8_t thermalState(deviceThermalStatePoolId, &mySet, PoolVariableIF::VAR_READ); - PoolVariable thermalRequest(deviceThermalRequestPoolId, &mySet, + gp_uint8_t thermalRequest(deviceThermalRequestPoolId, &mySet, PoolVariableIF::VAR_READ); mySet.read(); if (thermalRequest != ThermalComponentIF::STATE_REQUEST_IGNORE) { @@ -924,8 +983,8 @@ void DeviceHandlerBase::startTransition(Mode_t commandedMode, childTransitionDelay = getTransitionDelayMs(_MODE_START_UP, MODE_ON); triggerEvent(CHANGING_MODE, commandedMode, commandedSubmode); - DataSet mySet; - PoolVariable thermalRequest(deviceThermalRequestPoolId, + GlobDataSet mySet; + gp_int8_t thermalRequest(deviceThermalRequestPoolId, &mySet, PoolVariableIF::VAR_READ_WRITE); mySet.read(); if (thermalRequest != ThermalComponentIF::STATE_REQUEST_IGNORE) { @@ -973,7 +1032,9 @@ void DeviceHandlerBase::getMode(Mode_t* mode, Submode_t* submode) { } void DeviceHandlerBase::setToExternalControl() { - healthHelper.setHealth(EXTERNAL_CONTROL); + if(healthHelperActive) { + healthHelper.setHealth(EXTERNAL_CONTROL); + } } void DeviceHandlerBase::announceMode(bool recursive) { @@ -993,11 +1054,24 @@ void DeviceHandlerBase::missedReply(DeviceCommandId_t id) { } HasHealthIF::HealthState DeviceHandlerBase::getHealth() { - return healthHelper.getHealth(); + if(healthHelperActive) { + return healthHelper.getHealth(); + } + else { + sif::warning << "DeviceHandlerBase::getHealth: Health helper not active" + << std::endl; + return HasHealthIF::HEALTHY; + } } ReturnValue_t DeviceHandlerBase::setHealth(HealthState health) { - healthHelper.setHealth(health); + if(healthHelperActive) { + healthHelper.setHealth(health); + } + else { + sif::warning << "DeviceHandlerBase::getHealth: Health helper not active" + << std::endl; + } return HasReturnvaluesIF::RETURN_OK; } @@ -1085,7 +1159,7 @@ ReturnValue_t DeviceHandlerBase::handleDeviceHandlerMessage( void DeviceHandlerBase::setParentQueue(MessageQueueId_t parentQueueId) { modeHelper.setParentQueue(parentQueueId); - healthHelper.setParentQueue(parentQueueId); + healthHelper.setParentQeueue(parentQueueId); } bool DeviceHandlerBase::isAwaitingReply() { @@ -1111,37 +1185,49 @@ void DeviceHandlerBase::handleDeviceTM(SerializeIF* data, return; } DeviceTmReportingWrapper wrapper(getObjectId(), replyId, data); - if (iter->second.command != deviceCommandMap.end()) {//replies to a command + //replies to a command + if (iter->second.command != deviceCommandMap.end()) + { MessageQueueId_t queueId = iter->second.command->second.sendReplyTo; if (queueId != NO_COMMANDER) { //This may fail, but we'll ignore the fault. actionHelper.reportData(queueId, replyId, data); } + //This check should make sure we get any TM but don't get anything doubled. if (wiretappingMode == TM && (requestedRawTraffic != queueId)) { actionHelper.reportData(requestedRawTraffic, replyId, &wrapper); - } else if (forceDirectTm && (defaultRawReceiver != queueId)) { - - // hiding of sender needed so the service will handle it as unexpected Data, no matter what state - //(progress or completed) it is in - actionHelper.reportData(defaultRawReceiver, replyId, &wrapper, - true); - } - } else { //unrequested/aperiodic replies - if (wiretappingMode == TM) { - actionHelper.reportData(requestedRawTraffic, replyId, &wrapper); - } else if (forceDirectTm) { - // hiding of sender needed so the service will handle it as unexpected Data, no matter what state - //(progress or completed) it is in + else if (forceDirectTm and (defaultRawReceiver != queueId) and + (defaultRawReceiver != MessageQueueIF::NO_QUEUE)) + { + // hiding of sender needed so the service will handle it as + // unexpected Data, no matter what state (progress or completed) + // it is in actionHelper.reportData(defaultRawReceiver, replyId, &wrapper, - true); + true); } } -//Try to cast to DataSet and commit data. + //unrequested/aperiodic replies + else + { + if (wiretappingMode == TM) { + actionHelper.reportData(requestedRawTraffic, replyId, &wrapper); + } + else if (forceDirectTm and defaultRawReceiver != + MessageQueueIF::NO_QUEUE) + { + // hiding of sender needed so the service will handle it as + // unexpected Data, no matter what state (progress or completed) + // it is in + actionHelper.reportData(defaultRawReceiver, replyId, &wrapper, + true); + } + } + //Try to cast to GlobDataSet and commit data. if (!neverInDataPool) { - DataSet* dataSet = dynamic_cast(data); + GlobDataSet* dataSet = dynamic_cast(data); if (dataSet != NULL) { dataSet->commit(PoolVariableIF::VALID); } @@ -1149,7 +1235,7 @@ void DeviceHandlerBase::handleDeviceTM(SerializeIF* data, } ReturnValue_t DeviceHandlerBase::executeAction(ActionId_t actionId, - MessageQueueId_t commandedBy, const uint8_t* data, uint32_t size) { + MessageQueueId_t commandedBy, const uint8_t* data, size_t size) { ReturnValue_t result = acceptExternalDeviceCommands(); if (result != HasReturnvaluesIF::RETURN_OK) { return result; @@ -1178,18 +1264,23 @@ void DeviceHandlerBase::buildInternalCommand(void) { if (mode == MODE_NORMAL) { result = buildNormalDeviceCommand(&deviceCommandId); if (result == BUSY) { + //so we can track misconfigurations sif::debug << std::hex << getObjectId() - << ": DHB::buildInternalCommand busy" << std::endl; //so we can track misconfigurations + << ": DHB::buildInternalCommand: Busy" << std::endl; result = NOTHING_TO_SEND; //no need to report this } - } else if (mode == MODE_RAW) { + } + else if (mode == MODE_RAW) { result = buildChildRawCommand(); deviceCommandId = RAW_COMMAND_ID; - } else if (mode & TRANSITION_MODE_CHILD_ACTION_MASK) { + } + else if (mode & TRANSITION_MODE_CHILD_ACTION_MASK) { result = buildTransitionDeviceCommand(&deviceCommandId); - } else { + } + else { return; } + if (result == NOTHING_TO_SEND) { return; } @@ -1281,11 +1372,53 @@ void DeviceHandlerBase::changeHK(Mode_t mode, Submode_t submode, bool enable) { } void DeviceHandlerBase::setTaskIF(PeriodicTaskIF* task_){ - executingTask = task_; + executingTask = task_; } // Default implementations empty. void DeviceHandlerBase::debugInterface(uint8_t positionTracker, object_id_t objectId, uint32_t parameter) {} -void DeviceHandlerBase::performOperationHook() {} +void DeviceHandlerBase::performOperationHook() { +} + +ReturnValue_t DeviceHandlerBase::initializePoolEntries( + LocalDataPool &localDataPoolMap) { + return RETURN_OK; +} + +LocalDataPoolManager* DeviceHandlerBase::getHkManagerHandle() { + return &hkManager; +} + +ReturnValue_t DeviceHandlerBase::addDataSet(sid_t sid) { + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t DeviceHandlerBase::removeDataSet(sid_t sid) { + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t DeviceHandlerBase::changeCollectionInterval(sid_t sid, + dur_seconds_t newInterval) { + return HasReturnvaluesIF::RETURN_OK; +} + +ReturnValue_t DeviceHandlerBase::initializeAfterTaskCreation() { + // In this function, the task handle should be valid if the task + // was implemented correctly. We still check to be 1000 % sure :-) + if(executingTask != nullptr) { + pstIntervalMs = executingTask->getPeriodMs(); + } + return HasReturnvaluesIF::RETURN_OK; +} + +DataSetIF* DeviceHandlerBase::getDataSetHandle(sid_t sid) { + auto iter = deviceReplyMap.find(sid.ownerSetId); + if(iter != deviceReplyMap.end()) { + return iter->second.dataSet; + } + else { + return nullptr; + } +} diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h index 435f8e06..61fb701d 100644 --- a/devicehandlers/DeviceHandlerBase.h +++ b/devicehandlers/DeviceHandlerBase.h @@ -1,5 +1,5 @@ -#ifndef DEVICEHANDLERBASE_H_ -#define DEVICEHANDLERBASE_H_ +#ifndef FRAMEWORK_DEVICEHANDLERS_DEVICEHANDLERBASE_H_ +#define FRAMEWORK_DEVICEHANDLERS_DEVICEHANDLERBASE_H_ #include #include @@ -11,13 +11,15 @@ #include #include #include +#include #include #include #include #include +#include +#include #include - #include namespace Factory{ @@ -46,14 +48,16 @@ class StorageManagerIF; * If data has been received (GET_READ), the data will be interpreted. * The action for each step can be defined by the child class but as most * device handlers share a 4-call (sendRead-getRead-sendWrite-getWrite) structure, - * a default implementation is provided. NOTE: RMAP is a standard which is used for FLP. + * a default implementation is provided. + * NOTE: RMAP is a standard which is used for FLP. * RMAP communication is not mandatory for projects implementing the FSFW. * However, the communication principles are similar to RMAP as there are * two write and two send calls involved. * - * Device handler instances should extend this class and implement the abstract functions. - * Components and drivers can send so called cookies which are used for communication - * and contain information about the communcation (e.g. slave address for I2C or RMAP structs). + * Device handler instances should extend this class and implement the abstract + * functions. Components and drivers can send so called cookies which are used + * for communication and contain information about the communcation (e.g. slave + * address for I2C or RMAP structs). * The following abstract methods must be implemented by a device handler: * 1. doStartUp() * 2. doShutDown() @@ -82,7 +86,8 @@ class DeviceHandlerBase: public DeviceHandlerIF, public HasModesIF, public HasHealthIF, public HasActionsIF, - public ReceivesParameterMessagesIF { + public ReceivesParameterMessagesIF, + public HasLocalDataPoolIF { friend void (Factory::setStaticFrameworkObjectIds)(); public: /** @@ -100,12 +105,14 @@ public: * @param cmdQueueSize */ DeviceHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, - CookieIF * comCookie, uint8_t setDeviceSwitch, - uint32_t thermalStatePoolId = PoolVariableIF::NO_PARAMETER, - uint32_t thermalRequestPoolId = PoolVariableIF::NO_PARAMETER, - FailureIsolationBase* fdirInstance = nullptr, + CookieIF * comCookie, FailureIsolationBase* fdirInstance = nullptr, size_t cmdQueueSize = 20); + void setDeviceSwitch(uint8_t deviceSwitch); + void setHkDestination(object_id_t hkDestination); + void setThermalStateRequestPoolIds(uint32_t thermalStatePoolId, + uint32_t thermalRequestPoolId); + /** * @brief This function is the device handler base core component and is * called periodically. @@ -150,11 +157,9 @@ public: * @return */ virtual ReturnValue_t initialize(); - - /** - * Destructor. - */ + /** Destructor. */ virtual ~DeviceHandlerBase(); + protected: /** * @brief This is used to let the child class handle the transition from @@ -232,8 +237,9 @@ protected: * Build the device command to send for a transitional mode. * * This is only called in @c _MODE_TO_NORMAL, @c _MODE_TO_ON, @c _MODE_TO_RAW, - * @c _MODE_START_UP and @c _MODE_TO_POWER_DOWN. So it is used by doStartUp() - * and doShutDown() as well as doTransition() + * @c _MODE_START_UP and @c _MODE_SHUT_DOWN. So it is used by doStartUp() + * and doShutDown() as well as doTransition(), by setting those + * modes in the respective functions. * * A good idea is to implement a flag indicating a command has to be built * and a variable containing the command number to be built @@ -321,12 +327,11 @@ protected: * - @c RETURN_FAILED when the reply could not be interpreted, * e.g. logical errors or range violations occurred */ - virtual ReturnValue_t interpretDeviceReply(DeviceCommandId_t id, const uint8_t *packet) = 0; /** - * @brief fill the #deviceCommandMap + * @brief fill the #DeviceCommandMap and #DeviceReplyMap * called by the initialize() of the base class * @details * This is used to let the base class know which replies are expected. @@ -470,6 +475,23 @@ protected: virtual ReturnValue_t getSwitches(const uint8_t **switches, uint8_t *numberOfSwitches); + /** + * This function is used to initialize the local housekeeping pool + * entries. The default implementation leaves the pool empty. + * @param localDataPoolMap + * @return + */ + virtual ReturnValue_t initializePoolEntries( + LocalDataPool& localDataPoolMap) override; + + /** Get the HK manager object handle */ + virtual LocalDataPoolManager* getHkManagerHandle() override; + + virtual ReturnValue_t addDataSet(sid_t sid) override; + virtual ReturnValue_t removeDataSet(sid_t sid) override; + virtual ReturnValue_t changeCollectionInterval(sid_t sid, + dur_seconds_t newInterval) override; + /** * @brief Hook function for child handlers which is called once per * performOperation(). Default implementation is empty. @@ -484,7 +506,7 @@ public: /** @brief Implementation required for HasActionIF */ ReturnValue_t executeAction(ActionId_t actionId, MessageQueueId_t commandedBy, const uint8_t* data, - uint32_t size) override; + size_t size) override; Mode_t getTransitionSourceMode() const; Submode_t getTransitionSourceSubMode() const; @@ -493,7 +515,7 @@ public: ReturnValue_t setHealth(HealthState health); virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId, ParameterWrapper *parameterWrapper, - const ParameterWrapper *newValues, uint16_t startAtIndex); + const ParameterWrapper *newValues, uint16_t startAtIndex) override; /** * Implementation of ExecutableObjectIF function * @@ -505,7 +527,7 @@ public: protected: /** - * The Returnvalues ID of this class, required by HasReturnvaluesIF + * The Returnvalues id of this class, required by HasReturnvaluesIF */ static const uint8_t INTERFACE_ID = CLASS_ID::DEVICE_HANDLER_BASE; @@ -527,114 +549,141 @@ protected: static const DeviceCommandId_t NO_COMMAND_ID = -2; static const MessageQueueId_t NO_COMMANDER = 0; - /** - * Pointer to the raw packet that will be sent. - */ + /** Pointer to the raw packet that will be sent.*/ uint8_t *rawPacket = nullptr; - /** - * Size of the #rawPacket. - */ + /** Size of the #rawPacket. */ uint32_t rawPacketLen = 0; /** * The mode the device handler is currently in. - * * This should never be changed directly but only with setMode() */ Mode_t mode; - /** * The submode the device handler is currently in. - * * This should never be changed directly but only with setMode() */ Submode_t submode; - /** - * This is the counter value from performOperation(). - */ + /** This is the counter value from performOperation(). */ uint8_t pstStep = 0; + uint32_t pstIntervalMs = 0; /** - * wiretapping flag: + * Wiretapping flag: * - * indicates either that all raw messages to and from the device should be sent to #theOneWhoWantsToReadRawTraffic - * or that all device TM should be downlinked to #theOneWhoWantsToReadRawTraffic + * indicates either that all raw messages to and from the device should be + * sent to #defaultRawReceiver + * or that all device TM should be downlinked to #defaultRawReceiver. */ enum WiretappingMode { OFF = 0, RAW = 1, TM = 2 } wiretappingMode; - /** - * A message queue that accepts raw replies + * @brief A message queue that accepts raw replies * - * Statically initialized in initialize() to a configurable object. Used when there is no method - * of finding a recipient, ie raw mode and reporting erreonous replies + * Statically initialized in initialize() to a configurable object. + * Used when there is no method of finding a recipient, ie raw mode and + * reporting erroneous replies */ - MessageQueueId_t defaultRawReceiver = 0; - + MessageQueueId_t defaultRawReceiver = MessageQueueIF::NO_QUEUE; store_address_t storedRawData; /** - * the message queue which wants to read all raw traffic - * - * if #isWiretappingActive all raw communication from and to the device will be sent to this queue + * @brief The message queue which wants to read all raw traffic + * If #isWiretappingActive all raw communication from and to the device + * will be sent to this queue */ MessageQueueId_t requestedRawTraffic = 0; - /** - * the object used to set power switches - */ - PowerSwitchIF *powerSwitcher = nullptr; - /** * Pointer to the IPCStore. - * * This caches the pointer received from the objectManager in the constructor. */ StorageManagerIF *IPCStore = nullptr; - - /** - * cached for init - */ + /** The comIF object ID is cached for the intialize() function */ object_id_t deviceCommunicationId; - - /** - * Communication object used for device communication - */ + /** Communication object used for device communication */ DeviceCommunicationIF * communicationInterface = nullptr; - - /** - * Cookie used for communication - */ + /** Cookie used for communication */ CookieIF * comCookie; + /** Health helper for HasHealthIF */ + HealthHelper healthHelper; + bool healthHelperActive = false; + /** Mode helper for HasModesIF */ + ModeHelper modeHelper; + /** Parameter helper for ReceivesParameterMessagesIF */ + ParameterHelper parameterHelper; + /** Action helper for HasActionsIF */ + ActionHelper actionHelper; + /** Housekeeping Manager */ + LocalDataPoolManager hkManager; + + /** + * @brief Information about commands + */ struct DeviceCommandInfo { - bool isExecuting; //!< Indicates if the command is already executing. - uint8_t expectedReplies; //!< Dynamic value to indicate how many replies are expected. Inititated with 0. - MessageQueueId_t sendReplyTo; //!< if this is != NO_COMMANDER, DHB was commanded externally and shall report everything to commander. + //! Indicates if the command is already executing. + bool isExecuting; + //! Dynamic value to indicate how many replies are expected. + //! Inititated with 0. + uint8_t expectedReplies; + //! if this is != NO_COMMANDER, DHB was commanded externally and shall + //! report everything to commander. + MessageQueueId_t sendReplyTo; }; using DeviceCommandMap = std::map ; + /** + * Information about commands + */ + DeviceCommandMap deviceCommandMap; /** * @brief Information about expected replies - * - * This is used to keep track of pending replies + * This is used to keep track of pending replies. */ struct DeviceReplyInfo { - uint16_t maxDelayCycles; //!< The maximum number of cycles the handler should wait for a reply to this command. - uint16_t delayCycles; //!< The currently remaining cycles the handler should wait for a reply, 0 means there is no reply expected + //! The maximum number of cycles the handler should wait for a reply + //! to this command. + uint16_t maxDelayCycles; + //! The currently remaining cycles the handler should wait for a reply, + //! 0 means there is no reply expected + uint16_t delayCycles; size_t replyLen = 0; //!< Expected size of the reply. - bool periodic; //!< if this is !=0, the delayCycles will not be reset to 0 but to maxDelayCycles - DeviceCommandMap::iterator command; //!< The command that expects this reply. + //! if this is !=0, the delayCycles will not be reset to 0 but to + //! maxDelayCycles + bool periodic = false; + //! The dataset used to access housekeeping data related to the + //! respective device reply. Will point to a dataset held by + //! the child handler (if one is specified) + DataSetIF* dataSet = nullptr; + float collectionInterval = 0.0; + uint32_t intervalCounter = 0; + //! The command that expects this reply. + DeviceCommandMap::iterator command; }; using DeviceReplyMap = std::map ; using DeviceReplyIter = DeviceReplyMap::iterator; - /** - * The MessageQueue used to receive device handler commands and to send replies. + * This map is used to check and track correct reception of all replies. + * + * It has multiple use: + * - It stores the information on pending replies. If a command is sent, + * the DeviceCommandInfo.count is incremented. + * - It is used to time-out missing replies. If a command is sent, the + * DeviceCommandInfo.DelayCycles is set to MaxDelayCycles. + * - It is queried to check if a reply from the device can be interpreted. + * scanForReply() returns the id of the command a reply was found for. + * The reply is ignored in the following cases: + * - No entry for the returned id was found + * - The deviceReplyInfo.delayCycles is == 0 */ + DeviceReplyMap deviceReplyMap; + + //! The MessageQueue used to receive device handler commands + //! and to send replies. MessageQueueIF* commandQueue = nullptr; /** @@ -642,23 +691,14 @@ protected: * * can be set to PoolVariableIF::NO_PARAMETER to deactivate thermal checking */ - uint32_t deviceThermalStatePoolId; + uint32_t deviceThermalStatePoolId = PoolVariableIF::NO_PARAMETER; /** * this is the datapool variable with the thermal request of the device * * can be set to PoolVariableIF::NO_PARAMETER to deactivate thermal checking */ - uint32_t deviceThermalRequestPoolId; - - /** - * Taking care of the health - */ - HealthHelper healthHelper; - - ModeHelper modeHelper; - - ParameterHelper parameterHelper; + uint32_t deviceThermalRequestPoolId = PoolVariableIF::NO_PARAMETER; /** * Optional Error code @@ -676,13 +716,15 @@ protected: bool switchOffWasReported; //!< Indicates if SWITCH_WENT_OFF was already thrown. - PeriodicTaskIF* executingTask = nullptr;//!< Pointer to the task which executes this component, is invalid before setTaskIF was called. + //! Pointer to the task which executes this component, is invalid + //! before setTaskIF was called. + PeriodicTaskIF* executingTask = nullptr; static object_id_t powerSwitcherId; //!< Object which switches power on and off. static object_id_t rawDataReceiverId; //!< Object which receives RAW data by default. - static object_id_t defaultFDIRParentId; //!< Object which may be the root cause of an identified fault. + static object_id_t defaultFdirParentId; //!< Object which may be the root cause of an identified fault. /** * Helper function to report a missed reply * @@ -730,28 +772,40 @@ protected: /** * Do the transition to the main modes (MODE_ON, MODE_NORMAL and MODE_RAW). * - * If the transition is complete, the mode should be set to the target mode, which can be deduced from the current mode which is + * If the transition is complete, the mode should be set to the target mode, + * which can be deduced from the current mode which is * [_MODE_TO_ON, _MODE_TO_NORMAL, _MODE_TO_RAW] * - * The intended target submode is already set. The origin submode can be read in subModeFrom. + * The intended target submode is already set. + * The origin submode can be read in subModeFrom. * - * If the transition can not be completed, the child class can try to reach an working mode by setting the mode either directly - * or setting the mode to an transitional mode (TO_ON, TO_NORMAL, TO_RAW) if the device needs to be reconfigured. + * If the transition can not be completed, the child class can try to reach + * an working mode by setting the mode either directly + * or setting the mode to an transitional mode (TO_ON, TO_NORMAL, TO_RAW) + * if the device needs to be reconfigured. * - * If nothing works, the child class can wait for the timeout and the base class will reset the mode to the mode where the transition + * If nothing works, the child class can wait for the timeout and the base + * class will reset the mode to the mode where the transition * originated from (the child should report the reason for the failed transition). * - * The intended way to send commands is to set a flag (enum) indicating which command is to be sent here - * and then to check in buildTransitionCommand() for the flag. This flag can also be used by doStartUp() and - * doShutDown() to get a nice and clean implementation of buildTransitionCommand() without switching through modes. + * The intended way to send commands is to set a flag (enum) indicating + * which command is to be sent here and then to check in + * buildTransitionCommand() for the flag. This flag can also be used by + * doStartUp() and doShutDown() to get a nice and clean implementation of + * buildTransitionCommand() without switching through modes. * - * When the the condition for the completion of the transition is met, the mode can be set, for example in the parseReply() function. + * When the the condition for the completion of the transition is met, the + * mode can be set, for example in the scanForReply() function. * - * The default implementation goes into the target mode; + * The default implementation goes into the target mode directly. * - * #transitionFailure can be set to a failure code indicating the reason for a failed transition + * #transitionFailure can be set to a failure code indicating the reason + * for a failed transition * - * @param modeFrom the mode the transition originated from: [MODE_ON, MODE_NORMAL, MODE_RAW and _MODE_POWER_DOWN (if the mode changed from _MODE_START_UP to _MODE_TO_ON)] + * @param modeFrom + * The mode the transition originated from: + * [MODE_ON, MODE_NORMAL, MODE_RAW and _MODE_POWER_DOWN (if the mode changed + * from _MODE_START_UP to _MODE_TO_ON)] * @param subModeFrom the subMode of modeFrom */ virtual void doTransition(Mode_t modeFrom, Submode_t subModeFrom); @@ -953,24 +1007,11 @@ protected: bool commandIsExecuting(DeviceCommandId_t commandId); /** - * This map is used to check and track correct reception of all replies. + * set all switches returned by getSwitches() * - * It has multiple use: - * - it stores the information on pending replies. If a command is sent, the DeviceCommandInfo.count is incremented. - * - it is used to time-out missing replies. If a command is sent, the DeviceCommandInfo.DelayCycles is set to MaxDelayCycles. - * - it is queried to check if a reply from the device can be interpreted. scanForReply() returns the id of the command a reply was found for. - * The reply is ignored in the following cases: - * - No entry for the returned id was found - * - The deviceReplyInfo.delayCycles is == 0 + * @param onOff on == @c SWITCH_ON; off != @c SWITCH_ON */ - DeviceReplyMap deviceReplyMap; - - /** - * Information about commands - */ - DeviceCommandMap deviceCommandMap; - - ActionHelper actionHelper; + void commandSwitch(ReturnValue_t onOff); private: /** @@ -997,15 +1038,21 @@ private: }; /** - * Info about the #cookie - * + * @brief Info about the #cookie * Used to track the state of the communication */ CookieInfo cookieInfo; + /** the object used to set power switches */ + PowerSwitchIF *powerSwitcher = nullptr; + + /** Cached for initialize() */ + static object_id_t defaultHkDestination; + /** HK destination can also be set individually */ + object_id_t hkDestination = objects::NO_OBJECT; + /** - * Used for timing out mode transitions. - * + * @brief Used for timing out mode transitions. * Set when setMode() is called. */ uint32_t timeoutStart = 0; @@ -1016,11 +1063,12 @@ private: uint32_t childTransitionDelay; /** - * The mode the current transition originated from + * @brief The mode the current transition originated from * * This is private so the child can not change it and fuck up the timeouts * - * IMPORTANT: This is not valid during _MODE_SHUT_DOWN and _MODE_START_UP!! (it is _MODE_POWER_DOWN during this modes) + * IMPORTANT: This is not valid during _MODE_SHUT_DOWN and _MODE_START_UP!! + * (it is _MODE_POWER_DOWN during this modes) * * is element of [MODE_ON, MODE_NORMAL, MODE_RAW] */ @@ -1036,7 +1084,7 @@ private: * * for devices using two switches override getSwitches() */ - const uint8_t deviceSwitch; + uint8_t deviceSwitch; /** * read the command queue @@ -1135,12 +1183,6 @@ private: ReturnValue_t getStorageData(store_address_t storageAddress, uint8_t **data, uint32_t *len); - /** - * set all switches returned by getSwitches() - * - * @param onOff on == @c SWITCH_ON; off != @c SWITCH_ON - */ - void commandSwitch(ReturnValue_t onOff); /** * @param modeTo either @c MODE_ON, MODE_NORMAL or MODE_RAW NOTHING ELSE!!! @@ -1165,7 +1207,13 @@ private: ReturnValue_t switchCookieChannel(object_id_t newChannelId); ReturnValue_t handleDeviceHandlerMessage(CommandMessage *message); + + virtual ReturnValue_t initializeAfterTaskCreation() override; + DataSetIF* getDataSetHandle(sid_t sid) override; + + void parseReply(const uint8_t* receivedData, + size_t receivedDataLen); }; -#endif /* DEVICEHANDLERBASE_H_ */ +#endif /* FRAMEWORK_DEVICEHANDLERS_DEVICEHANDLERBASE_H_ */ From 36f7cf2caf2c1c1a4153389a9f3598fdf85fa5fa Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 16 Jul 2020 12:54:30 +0200 Subject: [PATCH 18/50] updated DHB changes --- devicehandlers/DeviceHandlerBase.cpp | 55 +++++----------------------- devicehandlers/DeviceHandlerBase.h | 14 ------- 2 files changed, 9 insertions(+), 60 deletions(-) diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp index cc97cd42..282f20b2 100644 --- a/devicehandlers/DeviceHandlerBase.cpp +++ b/devicehandlers/DeviceHandlerBase.cpp @@ -63,10 +63,6 @@ void DeviceHandlerBase::setThermalStateRequestPoolIds( } -void DeviceHandlerBase::setDeviceSwitch(uint8_t deviceSwitch) { - this->deviceSwitch = deviceSwitch; -} - DeviceHandlerBase::~DeviceHandlerBase() { delete comCookie; if (defaultFDIRUsed) { @@ -168,12 +164,8 @@ ReturnValue_t DeviceHandlerBase::initialize() { } result = healthHelper.initialize(); - if (result == RETURN_OK) { - healthHelperActive = true; - } - else { - sif::warning << "DeviceHandlerBase::initialize: Health Helper " - "initialization failure." << std::endl; + if (result != RETURN_OK) { + return result; } result = modeHelper.initialize(); @@ -250,11 +242,9 @@ void DeviceHandlerBase::readCommandQueue() { return; } - if(healthHelperActive) { - result = healthHelper.handleHealthCommand(&command); - if (result == RETURN_OK) { - return; - } + result = healthHelper.handleHealthCommand(&command); + if (result == RETURN_OK) { + return; } result = modeHelper.handleModeCommand(&command); @@ -1032,9 +1022,7 @@ void DeviceHandlerBase::getMode(Mode_t* mode, Submode_t* submode) { } void DeviceHandlerBase::setToExternalControl() { - if(healthHelperActive) { - healthHelper.setHealth(EXTERNAL_CONTROL); - } + healthHelper.setHealth(EXTERNAL_CONTROL); } void DeviceHandlerBase::announceMode(bool recursive) { @@ -1054,25 +1042,12 @@ void DeviceHandlerBase::missedReply(DeviceCommandId_t id) { } HasHealthIF::HealthState DeviceHandlerBase::getHealth() { - if(healthHelperActive) { - return healthHelper.getHealth(); - } - else { - sif::warning << "DeviceHandlerBase::getHealth: Health helper not active" - << std::endl; - return HasHealthIF::HEALTHY; - } + return healthHelper.getHealth(); } ReturnValue_t DeviceHandlerBase::setHealth(HealthState health) { - if(healthHelperActive) { - healthHelper.setHealth(health); - } - else { - sif::warning << "DeviceHandlerBase::getHealth: Health helper not active" - << std::endl; - } - return HasReturnvaluesIF::RETURN_OK; + healthHelper.setHealth(health); + return HasReturnvaluesIF::RETURN_OK; } void DeviceHandlerBase::checkSwitchState() { @@ -1391,18 +1366,6 @@ LocalDataPoolManager* DeviceHandlerBase::getHkManagerHandle() { return &hkManager; } -ReturnValue_t DeviceHandlerBase::addDataSet(sid_t sid) { - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t DeviceHandlerBase::removeDataSet(sid_t sid) { - return HasReturnvaluesIF::RETURN_OK; -} - -ReturnValue_t DeviceHandlerBase::changeCollectionInterval(sid_t sid, - dur_seconds_t newInterval) { - return HasReturnvaluesIF::RETURN_OK; -} ReturnValue_t DeviceHandlerBase::initializeAfterTaskCreation() { // In this function, the task handle should be valid if the task diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h index 61fb701d..ddbf6041 100644 --- a/devicehandlers/DeviceHandlerBase.h +++ b/devicehandlers/DeviceHandlerBase.h @@ -108,7 +108,6 @@ public: CookieIF * comCookie, FailureIsolationBase* fdirInstance = nullptr, size_t cmdQueueSize = 20); - void setDeviceSwitch(uint8_t deviceSwitch); void setHkDestination(object_id_t hkDestination); void setThermalStateRequestPoolIds(uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId); @@ -487,11 +486,6 @@ protected: /** Get the HK manager object handle */ virtual LocalDataPoolManager* getHkManagerHandle() override; - virtual ReturnValue_t addDataSet(sid_t sid) override; - virtual ReturnValue_t removeDataSet(sid_t sid) override; - virtual ReturnValue_t changeCollectionInterval(sid_t sid, - dur_seconds_t newInterval) override; - /** * @brief Hook function for child handlers which is called once per * performOperation(). Default implementation is empty. @@ -610,7 +604,6 @@ protected: /** Health helper for HasHealthIF */ HealthHelper healthHelper; - bool healthHelperActive = false; /** Mode helper for HasModesIF */ ModeHelper modeHelper; /** Parameter helper for ReceivesParameterMessagesIF */ @@ -1079,13 +1072,6 @@ private: */ Submode_t transitionSourceSubMode; - /** - * the switch of the device - * - * for devices using two switches override getSwitches() - */ - uint8_t deviceSwitch; - /** * read the command queue */ From c9bc022941251ad24ddcd2b3e4fff40904c18c1c Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 16 Jul 2020 12:57:29 +0200 Subject: [PATCH 19/50] commented out all new hk manager components --- devicehandlers/DeviceHandlerBase.cpp | 24 ++++++++++++------------ devicehandlers/DeviceHandlerBase.h | 16 ++++++++-------- 2 files changed, 20 insertions(+), 20 deletions(-) diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp index 282f20b2..6b7dad1e 100644 --- a/devicehandlers/DeviceHandlerBase.cpp +++ b/devicehandlers/DeviceHandlerBase.cpp @@ -191,14 +191,14 @@ ReturnValue_t DeviceHandlerBase::initialize() { return result; } - if(hkDestination == objects::NO_OBJECT) { - hkDestination = defaultHkDestination; - } - - result = hkManager.initialize(commandQueue, hkDestination); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } +// if(hkDestination == objects::NO_OBJECT) { +// hkDestination = defaultHkDestination; +// } +// +// result = hkManager.initialize(commandQueue, hkDestination); +// if (result != HasReturnvaluesIF::RETURN_OK) { +// return result; +// } fillCommandAndReplyMap(); @@ -262,10 +262,10 @@ void DeviceHandlerBase::readCommandQueue() { return; } - result = hkManager.handleHousekeepingMessage(&command); - if (result == RETURN_OK) { - return; - } +// result = hkManager.handleHousekeepingMessage(&command); +// if (result == RETURN_OK) { +// return; +// } result = handleDeviceHandlerMessage(&command); if (result == RETURN_OK) { diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h index ddbf6041..a041dbc7 100644 --- a/devicehandlers/DeviceHandlerBase.h +++ b/devicehandlers/DeviceHandlerBase.h @@ -17,8 +17,8 @@ #include #include #include -#include -#include +//#include +//#include #include #include @@ -86,8 +86,8 @@ class DeviceHandlerBase: public DeviceHandlerIF, public HasModesIF, public HasHealthIF, public HasActionsIF, - public ReceivesParameterMessagesIF, - public HasLocalDataPoolIF { + public ReceivesParameterMessagesIF + /* public HasLocalDataPoolIF */ { friend void (Factory::setStaticFrameworkObjectIds)(); public: /** @@ -480,11 +480,11 @@ protected: * @param localDataPoolMap * @return */ - virtual ReturnValue_t initializePoolEntries( - LocalDataPool& localDataPoolMap) override; + //virtual ReturnValue_t initializePoolEntries( + // LocalDataPool& localDataPoolMap) override; /** Get the HK manager object handle */ - virtual LocalDataPoolManager* getHkManagerHandle() override; + //virtual LocalDataPoolManager* getHkManagerHandle() override; /** * @brief Hook function for child handlers which is called once per @@ -611,7 +611,7 @@ protected: /** Action helper for HasActionsIF */ ActionHelper actionHelper; /** Housekeeping Manager */ - LocalDataPoolManager hkManager; + //LocalDataPoolManager hkManager; /** * @brief Information about commands From 6b03a1c03df0ee70c31810b0faafaf507646965b Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 16 Jul 2020 13:00:22 +0200 Subject: [PATCH 20/50] commented out some more components --- devicehandlers/DeviceHandlerBase.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h index a041dbc7..66c10778 100644 --- a/devicehandlers/DeviceHandlerBase.h +++ b/devicehandlers/DeviceHandlerBase.h @@ -650,9 +650,7 @@ protected: //! The dataset used to access housekeeping data related to the //! respective device reply. Will point to a dataset held by //! the child handler (if one is specified) - DataSetIF* dataSet = nullptr; - float collectionInterval = 0.0; - uint32_t intervalCounter = 0; + // DataSetIF* dataSet = nullptr; //! The command that expects this reply. DeviceCommandMap::iterator command; }; @@ -1195,7 +1193,8 @@ private: ReturnValue_t handleDeviceHandlerMessage(CommandMessage *message); virtual ReturnValue_t initializeAfterTaskCreation() override; - DataSetIF* getDataSetHandle(sid_t sid) override; + + //DataSetIF* getDataSetHandle(sid_t sid) override; void parseReply(const uint8_t* receivedData, size_t receivedDataLen); From dc4db6e031cb1ccb58b89d8a055b54208a939799 Mon Sep 17 00:00:00 2001 From: Steffen Gaisser Date: Thu, 25 Jun 2020 18:09:32 +0200 Subject: [PATCH 21/50] Fixed spelling mistake in HealthHelper --- devicehandlers/DeviceHandlerBase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp index 6b7dad1e..dc90d812 100644 --- a/devicehandlers/DeviceHandlerBase.cpp +++ b/devicehandlers/DeviceHandlerBase.cpp @@ -1134,7 +1134,7 @@ ReturnValue_t DeviceHandlerBase::handleDeviceHandlerMessage( void DeviceHandlerBase::setParentQueue(MessageQueueId_t parentQueueId) { modeHelper.setParentQueue(parentQueueId); - healthHelper.setParentQeueue(parentQueueId); + healthHelper.setParentQueue(parentQueueId); } bool DeviceHandlerBase::isAwaitingReply() { From 0449c63225de411920dba5a3b7bb88a9eb6c72d1 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Tue, 28 Jul 2020 13:13:40 +0200 Subject: [PATCH 22/50] bugfixes --- container/DynamicFIFO.h | 21 ++++++++++++++++++--- container/FIFO.h | 12 ++++++++++-- container/FIFOBase.h | 8 ++++++-- container/FIFOBase.tpp | 8 +++++++- 4 files changed, 41 insertions(+), 8 deletions(-) diff --git a/container/DynamicFIFO.h b/container/DynamicFIFO.h index 3a5242c7..59adfb3a 100644 --- a/container/DynamicFIFO.h +++ b/container/DynamicFIFO.h @@ -17,11 +17,26 @@ template class DynamicFIFO: public FIFOBase { public: - DynamicFIFO(size_t maxCapacity): FIFOBase(values.data(), maxCapacity), - values(maxCapacity) {}; + DynamicFIFO(size_t maxCapacity): FIFOBase(nullptr, maxCapacity), + fifoVector(maxCapacity) { + // trying to pass the pointer of the uninitialized vector + // to the FIFOBase constructor directly lead to a super evil bug. + // So we do it like this now. + this->setData(fifoVector.data()); + }; + + /** + * @brief Custom copy constructor which prevents setting the + * underlying pointer wrong. + */ + DynamicFIFO(const DynamicFIFO& other): FIFOBase(other), + fifoVector(other.maxCapacity) { + this->setData(fifoVector.data()); + } + private: - std::vector values; + std::vector fifoVector; }; #endif /* FRAMEWORK_CONTAINER_DYNAMICFIFO_H_ */ diff --git a/container/FIFO.h b/container/FIFO.h index ecb218fd..2e332dc2 100644 --- a/container/FIFO.h +++ b/container/FIFO.h @@ -17,10 +17,18 @@ template class FIFO: public FIFOBase { public: - FIFO(): FIFOBase(values.data(), capacity) {}; + FIFO(): FIFOBase(fifoArray.data(), capacity) {}; + + /** + * @brief Custom copy constructor to set pointer correctly. + * @param other + */ + FIFO(const FIFO& other): FIFOBase(other) { + this->setData(fifoArray.data()); + } private: - std::array values; + std::array fifoArray; }; #endif /* FRAMEWORK_CONTAINERS_STATICFIFO_H_ */ diff --git a/container/FIFOBase.h b/container/FIFOBase.h index 8bdb333f..7f8bde96 100644 --- a/container/FIFOBase.h +++ b/container/FIFOBase.h @@ -3,6 +3,7 @@ #include #include +#include template class FIFOBase { @@ -43,12 +44,15 @@ public: bool full(); size_t size(); + size_t getMaxCapacity() const; -private: - T* values; +protected: + void setData(T* data); size_t maxCapacity = 0; + T* values; + size_t readIndex = 0; size_t writeIndex = 0; size_t currentSize = 0; diff --git a/container/FIFOBase.tpp b/container/FIFOBase.tpp index 48a060ff..c73e9d59 100644 --- a/container/FIFOBase.tpp +++ b/container/FIFOBase.tpp @@ -7,7 +7,7 @@ template inline FIFOBase::FIFOBase(T* values, const size_t maxCapacity): - values(values), maxCapacity(maxCapacity) {}; + maxCapacity(maxCapacity), values(values){}; template inline ReturnValue_t FIFOBase::insert(T value) { @@ -78,4 +78,10 @@ inline size_t FIFOBase::getMaxCapacity() const { return maxCapacity; } + +template +inline void FIFOBase::setData(T *data) { + this->values = data; +} + #endif From a0ff1e0019824feb8da8183ce2051b5890bb6cb9 Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 2 Aug 2020 15:46:25 +0200 Subject: [PATCH 23/50] reverted datapool changes --- devicehandlers/DeviceHandlerBase.cpp | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp index dc90d812..8e256bcf 100644 --- a/devicehandlers/DeviceHandlerBase.cpp +++ b/devicehandlers/DeviceHandlerBase.cpp @@ -4,11 +4,11 @@ #include #include -#include -#include +#include +#include #include #include -#include +//#include #include #include #include @@ -203,8 +203,8 @@ ReturnValue_t DeviceHandlerBase::initialize() { fillCommandAndReplyMap(); //Set temperature target state to NON_OP. - GlobDataSet mySet; - gp_uint8_t thermalRequest(deviceThermalRequestPoolId, &mySet, + DataSet mySet; + dp_int8_t thermalRequest(deviceThermalRequestPoolId, &mySet, PoolVariableIF::VAR_WRITE); mySet.read(); thermalRequest = ThermalComponentIF::STATE_REQUEST_NON_OPERATIONAL; @@ -486,8 +486,8 @@ void DeviceHandlerBase::setMode(Mode_t newMode, uint8_t newSubmode) { Clock::getUptime(&timeoutStart); if (mode == MODE_OFF) { - GlobDataSet mySet; - gp_uint8_t thermalRequest(deviceThermalRequestPoolId, &mySet, + DataSet mySet; + db_int8_t thermalRequest(deviceThermalRequestPoolId, &mySet, PoolVariableIF::VAR_READ_WRITE); mySet.read(); if (thermalRequest != ThermalComponentIF::STATE_REQUEST_IGNORE) { @@ -943,10 +943,10 @@ ReturnValue_t DeviceHandlerBase::checkModeCommand(Mode_t commandedMode, if ((commandedMode == MODE_ON) && (mode == MODE_OFF) && (deviceThermalStatePoolId != PoolVariableIF::NO_PARAMETER)) { - GlobDataSet mySet; - gp_uint8_t thermalState(deviceThermalStatePoolId, &mySet, + DataSet mySet; + db_int8_t thermalState(deviceThermalStatePoolId, &mySet, PoolVariableIF::VAR_READ); - gp_uint8_t thermalRequest(deviceThermalRequestPoolId, &mySet, + db_int8_t thermalRequest(deviceThermalRequestPoolId, &mySet, PoolVariableIF::VAR_READ); mySet.read(); if (thermalRequest != ThermalComponentIF::STATE_REQUEST_IGNORE) { @@ -973,8 +973,8 @@ void DeviceHandlerBase::startTransition(Mode_t commandedMode, childTransitionDelay = getTransitionDelayMs(_MODE_START_UP, MODE_ON); triggerEvent(CHANGING_MODE, commandedMode, commandedSubmode); - GlobDataSet mySet; - gp_int8_t thermalRequest(deviceThermalRequestPoolId, + DataSet mySet; + db_int8_t thermalRequest(deviceThermalRequestPoolId, &mySet, PoolVariableIF::VAR_READ_WRITE); mySet.read(); if (thermalRequest != ThermalComponentIF::STATE_REQUEST_IGNORE) { From 2e08037e75f1457d67ef13e14c5b87765ee4a03d Mon Sep 17 00:00:00 2001 From: Robin Mueller Date: Sun, 2 Aug 2020 15:48:09 +0200 Subject: [PATCH 24/50] revertiing pool changes --- devicehandlers/DeviceHandlerBase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp index 8e256bcf..c09d913e 100644 --- a/devicehandlers/DeviceHandlerBase.cpp +++ b/devicehandlers/DeviceHandlerBase.cpp @@ -204,7 +204,7 @@ ReturnValue_t DeviceHandlerBase::initialize() { //Set temperature target state to NON_OP. DataSet mySet; - dp_int8_t thermalRequest(deviceThermalRequestPoolId, &mySet, + db_int8_t thermalRequest(deviceThermalRequestPoolId, &mySet, PoolVariableIF::VAR_WRITE); mySet.read(); thermalRequest = ThermalComponentIF::STATE_REQUEST_NON_OPERATIONAL; From 6b4fa0cc0443802d2d51db18dc32a800578afd65 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Sat, 8 Aug 2020 13:28:59 +0200 Subject: [PATCH 25/50] fdir update --- .../DeviceHandlerFailureIsolation.cpp | 38 +++++++++++-------- .../DeviceHandlerFailureIsolation.h | 2 + fdir/FailureIsolationBase.cpp | 35 +++++++++++------ fdir/FailureIsolationBase.h | 17 ++++++--- 4 files changed, 60 insertions(+), 32 deletions(-) diff --git a/devicehandlers/DeviceHandlerFailureIsolation.cpp b/devicehandlers/DeviceHandlerFailureIsolation.cpp index 65c40219..46e73cd8 100644 --- a/devicehandlers/DeviceHandlerFailureIsolation.cpp +++ b/devicehandlers/DeviceHandlerFailureIsolation.cpp @@ -7,13 +7,15 @@ object_id_t DeviceHandlerFailureIsolation::powerConfirmationId = 0; -DeviceHandlerFailureIsolation::DeviceHandlerFailureIsolation(object_id_t owner, object_id_t parent) : - FailureIsolationBase(owner, parent), strangeReplyCount(MAX_STRANGE_REPLIES, - STRANGE_REPLIES_TIME_MS, parameterDomainBase++), missedReplyCount( - MAX_MISSED_REPLY_COUNT, MISSED_REPLY_TIME_MS, - parameterDomainBase++), recoveryCounter(MAX_REBOOT, - REBOOT_TIME_MS, parameterDomainBase++), fdirState(NONE), powerConfirmation( - 0) { +DeviceHandlerFailureIsolation::DeviceHandlerFailureIsolation(object_id_t owner, + object_id_t parent) : + FailureIsolationBase(owner, parent), + strangeReplyCount(MAX_STRANGE_REPLIES, STRANGE_REPLIES_TIME_MS, + parameterDomainBase++), + missedReplyCount( MAX_MISSED_REPLY_COUNT, MISSED_REPLY_TIME_MS, + parameterDomainBase++), + recoveryCounter(MAX_REBOOT, REBOOT_TIME_MS, parameterDomainBase++), + fdirState(NONE), powerConfirmation(0) { } DeviceHandlerFailureIsolation::~DeviceHandlerFailureIsolation() { @@ -68,9 +70,11 @@ ReturnValue_t DeviceHandlerFailureIsolation::eventReceived(EventMessage* event) break; //****Power***** case PowerSwitchIF::SWITCH_WENT_OFF: - result = sendConfirmationRequest(event, powerConfirmation); - if (result == RETURN_OK) { - setFdirState(DEVICE_MIGHT_BE_OFF); + if(hasPowerConfirmation) { + result = sendConfirmationRequest(event, powerConfirmation); + if (result == RETURN_OK) { + setFdirState(DEVICE_MIGHT_BE_OFF); + } } break; case Fuse::FUSE_WENT_OFF: @@ -133,7 +137,7 @@ void DeviceHandlerFailureIsolation::decrementFaultCounters() { void DeviceHandlerFailureIsolation::handleRecovery(Event reason) { clearFaultCounters(); - if (!recoveryCounter.incrementAndCheck()) { + if (not recoveryCounter.incrementAndCheck()) { startRecovery(reason); } else { setFaulty(reason); @@ -142,7 +146,8 @@ void DeviceHandlerFailureIsolation::handleRecovery(Event reason) { void DeviceHandlerFailureIsolation::wasParentsFault(EventMessage* event) { //We'll better ignore the SWITCH_WENT_OFF event and await a system-wide reset. - //This means, no fault message will come through until a MODE_ or HEALTH_INFO message comes through -> Is that ok? + //This means, no fault message will come through until a MODE_ or + //HEALTH_INFO message comes through -> Is that ok? //Same issue in TxFailureIsolation! // if ((event->getEvent() == PowerSwitchIF::SWITCH_WENT_OFF) // && (fdirState != RECOVERY_ONGOING)) { @@ -158,14 +163,17 @@ void DeviceHandlerFailureIsolation::clearFaultCounters() { ReturnValue_t DeviceHandlerFailureIsolation::initialize() { ReturnValue_t result = FailureIsolationBase::initialize(); if (result != HasReturnvaluesIF::RETURN_OK) { + sif::error << "DeviceHandlerFailureIsolation::initialize: Could not" + " initialize FailureIsolationBase." << std::endl; return result; } ConfirmsFailuresIF* power = objectManager->get( powerConfirmationId); - if (power == NULL) { - return RETURN_FAILED; + if (power != nullptr) { + powerConfirmation = power->getEventReceptionQueue(); + hasPowerConfirmation = true; } - powerConfirmation = power->getEventReceptionQueue(); + return RETURN_OK; } diff --git a/devicehandlers/DeviceHandlerFailureIsolation.h b/devicehandlers/DeviceHandlerFailureIsolation.h index 91f6ef64..4343d88b 100644 --- a/devicehandlers/DeviceHandlerFailureIsolation.h +++ b/devicehandlers/DeviceHandlerFailureIsolation.h @@ -28,8 +28,10 @@ protected: NONE, RECOVERY_ONGOING, DEVICE_MIGHT_BE_OFF, AWAIT_SHUTDOWN }; FDIRState fdirState; + bool hasPowerConfirmation = false; MessageQueueId_t powerConfirmation; static object_id_t powerConfirmationId; + // TODO: Are those hardcoded value? How can they be changed. static const uint32_t MAX_REBOOT = 1; static const uint32_t REBOOT_TIME_MS = 180000; static const uint32_t MAX_STRANGE_REPLIES = 10; diff --git a/fdir/FailureIsolationBase.cpp b/fdir/FailureIsolationBase.cpp index 4b7caac5..ad4493ff 100644 --- a/fdir/FailureIsolationBase.cpp +++ b/fdir/FailureIsolationBase.cpp @@ -5,10 +5,12 @@ #include #include -FailureIsolationBase::FailureIsolationBase(object_id_t owner, object_id_t parent, uint8_t messageDepth, uint8_t parameterDomainBase) : - eventQueue(NULL), ownerId( - owner), owner(NULL), faultTreeParent(parent), parameterDomainBase(parameterDomainBase) { - eventQueue = QueueFactory::instance()->createMessageQueue(messageDepth, EventMessage::EVENT_MESSAGE_SIZE); +FailureIsolationBase::FailureIsolationBase(object_id_t owner, + object_id_t parent, uint8_t messageDepth, uint8_t parameterDomainBase) : + ownerId(owner), faultTreeParent(parent), + parameterDomainBase(parameterDomainBase) { + eventQueue = QueueFactory::instance()->createMessageQueue(messageDepth, + EventMessage::EVENT_MESSAGE_SIZE); } FailureIsolationBase::~FailureIsolationBase() { @@ -18,27 +20,36 @@ FailureIsolationBase::~FailureIsolationBase() { ReturnValue_t FailureIsolationBase::initialize() { EventManagerIF* manager = objectManager->get( objects::EVENT_MANAGER); - if (manager == NULL) { + if (manager == nullptr) { + sif::error << "FailureIsolationBase::initialize: Event Manager has not" + " been initialized!" << std::endl; return RETURN_FAILED; } ReturnValue_t result = manager->registerListener(eventQueue->getId()); if (result != HasReturnvaluesIF::RETURN_OK) { return result; } - if (ownerId != 0) { + if (ownerId != objects::NO_OBJECT) { result = manager->subscribeToAllEventsFrom(eventQueue->getId(), ownerId); if (result != HasReturnvaluesIF::RETURN_OK) { return result; } owner = objectManager->get(ownerId); - if (owner == NULL) { - return RETURN_FAILED; + if (owner == nullptr) { + sif::error << "FailureIsolationBase::intialize: Owner object " + "invalid. Make sure it implements HasHealthIF" << std::endl; + return ObjectManagerIF::CHILD_INIT_FAILED; } } - if (faultTreeParent != 0) { + if (faultTreeParent != objects::NO_OBJECT) { ConfirmsFailuresIF* parentIF = objectManager->get( faultTreeParent); - if (parentIF == NULL) { + if (parentIF == nullptr) { + sif::error << "FailureIsolationBase::intialize: Parent object" + << "invalid." << std::endl; + sif::error << "Make sure it implements ConfirmsFailuresIF." + << std::endl; + return ObjectManagerIF::CHILD_INIT_FAILED; return RETURN_FAILED; } eventQueue->setDefaultDestination(parentIF->getEventReceptionQueue()); @@ -93,9 +104,9 @@ MessageQueueId_t FailureIsolationBase::getEventReceptionQueue() { ReturnValue_t FailureIsolationBase::sendConfirmationRequest(EventMessage* event, MessageQueueId_t destination) { event->setMessageId(EventMessage::CONFIRMATION_REQUEST); - if (destination != 0) { + if (destination != MessageQueueIF::NO_QUEUE) { return eventQueue->sendMessage(destination, event); - } else if (faultTreeParent != 0) { + } else if (faultTreeParent != objects::NO_OBJECT) { return eventQueue->sendToDefault(event); } return RETURN_FAILED; diff --git a/fdir/FailureIsolationBase.h b/fdir/FailureIsolationBase.h index cd582e39..c0fe6041 100644 --- a/fdir/FailureIsolationBase.h +++ b/fdir/FailureIsolationBase.h @@ -17,18 +17,25 @@ public: static const Event FDIR_CHANGED_STATE = MAKE_EVENT(1, SEVERITY::INFO); //!< FDIR has an internal state, which changed from par2 (oldState) to par1 (newState). static const Event FDIR_STARTS_RECOVERY = MAKE_EVENT(2, SEVERITY::MEDIUM); //!< FDIR tries to restart device. Par1: event that caused recovery. static const Event FDIR_TURNS_OFF_DEVICE = MAKE_EVENT(3, SEVERITY::MEDIUM); //!< FDIR turns off device. Par1: event that caused recovery. - FailureIsolationBase(object_id_t owner, object_id_t parent = 0, + + FailureIsolationBase(object_id_t owner, + object_id_t parent = objects::NO_OBJECT, uint8_t messageDepth = 10, uint8_t parameterDomainBase = 0xF0); + virtual ~FailureIsolationBase(); virtual ReturnValue_t initialize(); + + /** + * This is called by the DHB in performOperation() + */ void checkForFailures(); - MessageQueueId_t getEventReceptionQueue(); + MessageQueueId_t getEventReceptionQueue() override; virtual void triggerEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0); protected: - MessageQueueIF* eventQueue; + MessageQueueIF* eventQueue = nullptr; object_id_t ownerId; - HasHealthIF* owner; + HasHealthIF* owner = nullptr; object_id_t faultTreeParent; uint8_t parameterDomainBase; void setOwnerHealth(HasHealthIF::HealthState health); @@ -38,7 +45,7 @@ protected: virtual ReturnValue_t confirmFault(EventMessage* event); virtual void decrementFaultCounters() = 0; ReturnValue_t sendConfirmationRequest(EventMessage* event, - MessageQueueId_t destination = 0); + MessageQueueId_t destination = MessageQueueIF::NO_QUEUE); void throwFdirEvent(Event event, uint32_t parameter1 = 0, uint32_t parameter2 = 0); private: From 4a2ef6d97f51418864856c53ac596652aa002bb9 Mon Sep 17 00:00:00 2001 From: Steffen Gaisser Date: Thu, 13 Aug 2020 17:58:42 +0200 Subject: [PATCH 26/50] Fixed a shadowing issue of with moving to inner scope --- subsystem/Subsystem.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/subsystem/Subsystem.cpp b/subsystem/Subsystem.cpp index fcf2e189..2f0cb04e 100644 --- a/subsystem/Subsystem.cpp +++ b/subsystem/Subsystem.cpp @@ -151,7 +151,6 @@ HybridIterator Subsystem::getTable(Mode_t id) { } ReturnValue_t Subsystem::handleCommandMessage(CommandMessage *message) { - ReturnValue_t result; switch (message->getCommand()) { case HealthMessage::HEALTH_INFO: { HealthState health = HealthMessage::getHealth(message); @@ -166,7 +165,7 @@ ReturnValue_t Subsystem::handleCommandMessage(CommandMessage *message) { FixedArrayList sequence; const uint8_t *pointer; size_t sizeRead; - result = IPCStore->getData( + ReturnValue_t result = IPCStore->getData( ModeSequenceMessage::getStoreAddress(message), &pointer, &sizeRead); if (result == RETURN_OK) { @@ -193,7 +192,7 @@ ReturnValue_t Subsystem::handleCommandMessage(CommandMessage *message) { FixedArrayList table; const uint8_t *pointer; size_t sizeRead; - result = IPCStore->getData( + ReturnValue_t result = IPCStore->getData( ModeSequenceMessage::getStoreAddress(message), &pointer, &sizeRead); if (result == RETURN_OK) { @@ -210,21 +209,23 @@ ReturnValue_t Subsystem::handleCommandMessage(CommandMessage *message) { } break; - case ModeSequenceMessage::DELETE_SEQUENCE: + case ModeSequenceMessage::DELETE_SEQUENCE:{ if (isInTransition) { replyToCommand(IN_TRANSITION, 0); break; } - result = deleteSequence(ModeSequenceMessage::getSequenceId(message)); + ReturnValue_t result = deleteSequence(ModeSequenceMessage::getSequenceId(message)); replyToCommand(result, 0); + } break; - case ModeSequenceMessage::DELETE_TABLE: + case ModeSequenceMessage::DELETE_TABLE:{ if (isInTransition) { replyToCommand(IN_TRANSITION, 0); break; } - result = deleteTable(ModeSequenceMessage::getTableId(message)); + ReturnValue_t result = deleteTable(ModeSequenceMessage::getTableId(message)); replyToCommand(result, 0); + } break; case ModeSequenceMessage::LIST_SEQUENCES: { SerialFixedArrayListAdapter sequences; From 3d89bc83e5a872a781fd6e1b1af6b39b20e06ddc Mon Sep 17 00:00:00 2001 From: Steffen Gaisser Date: Thu, 13 Aug 2020 18:08:17 +0200 Subject: [PATCH 27/50] Another shadowing, altough again no problem --- timemanager/CCSDSTime.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/timemanager/CCSDSTime.cpp b/timemanager/CCSDSTime.cpp index 71b2539d..cdb14d9a 100644 --- a/timemanager/CCSDSTime.cpp +++ b/timemanager/CCSDSTime.cpp @@ -119,7 +119,7 @@ ReturnValue_t CCSDSTime::convertFromCCS(Clock::TimeOfDay_t* to, const uint8_t* f if (temp->pField & (1 << 3)) { //day of year variation uint16_t tempDay = (temp->month << 8) + temp->day; - ReturnValue_t result = convertDaysOfYear(tempDay, to->year, + result = convertDaysOfYear(tempDay, to->year, &(temp->month), &(temp->day)); if (result != RETURN_OK) { return result; From 287a83c54c54da467b339fbb32fda52ce8946a67 Mon Sep 17 00:00:00 2001 From: Steffen Gaisser Date: Thu, 13 Aug 2020 19:11:23 +0200 Subject: [PATCH 28/50] Another shadowing error --- objectmanager/ObjectManager.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/objectmanager/ObjectManager.cpp b/objectmanager/ObjectManager.cpp index 1c54355a..71769d68 100644 --- a/objectmanager/ObjectManager.cpp +++ b/objectmanager/ObjectManager.cpp @@ -100,7 +100,6 @@ void ObjectManager::initialize() { } void ObjectManager::printList() { - std::map::iterator it; sif::debug << "ObjectManager: Object List contains:" << std::endl; for (auto const& it : objectList) { sif::debug << std::hex << it.first << " | " << it.second << std::endl; From af52d1ffdaa3a8fa09a34a4c0e39785d9ade9ff9 Mon Sep 17 00:00:00 2001 From: Steffen Gaisser Date: Tue, 25 Aug 2020 14:14:58 +0200 Subject: [PATCH 29/50] Added some documentation in SerializeIF --- serialize/SerializeIF.h | 55 ++++++++++++++++++++++++++++++++++------- 1 file changed, 46 insertions(+), 9 deletions(-) diff --git a/serialize/SerializeIF.h b/serialize/SerializeIF.h index 9ed52d29..1f31d91e 100644 --- a/serialize/SerializeIF.h +++ b/serialize/SerializeIF.h @@ -1,16 +1,16 @@ -#ifndef SERIALIZEIF_H_ -#define SERIALIZEIF_H_ +#ifndef FSFW_SERIALIZE_SERIALIZEIF_H_ +#define FSFW_SERIALIZE_SERIALIZEIF_H_ #include "../returnvalues/HasReturnvaluesIF.h" #include /** * \defgroup serialize Serialization - * Contains serialisation services. + * Contains serialization services. */ /** - * Translation of objects into data streams. + * Translation of objects into data streams and from data streams. * \ingroup serialize */ class SerializeIF { @@ -20,21 +20,58 @@ public: }; static const uint8_t INTERFACE_ID = CLASS_ID::SERIALIZE_IF; - static const ReturnValue_t BUFFER_TOO_SHORT = MAKE_RETURN_CODE(1); - static const ReturnValue_t STREAM_TOO_SHORT = MAKE_RETURN_CODE(2); - static const ReturnValue_t TOO_MANY_ELEMENTS = MAKE_RETURN_CODE(3); + static const ReturnValue_t BUFFER_TOO_SHORT = MAKE_RETURN_CODE(1); // !< The given buffer in serialize is too short + static const ReturnValue_t STREAM_TOO_SHORT = MAKE_RETURN_CODE(2); // !< The input stream in deSerialize is too short + static const ReturnValue_t TOO_MANY_ELEMENTS = MAKE_RETURN_CODE(3);// !< There are too many elements to be deSerialized virtual ~SerializeIF() { } - + /** + * Function to serialize the object into a buffer with maxSize at the current size. + * + * Implementations of this function must increase the size variable and move the buffer pointer. + * + * + * Custom implementations might use additional return values. + * + * @param buffer Buffer to serialize into, will be set to the current write location + * @param size Size that has been used in the buffer already, will be increase by the function + * @param maxSize Max size of the buffer. + * @param streamEndianness Endianness of the serialized data according to SerializeIF::Endianness + * @return + * - @¢ BUFFER_TOO_SHORT The given buffer in is too short + * - @c RETURN_FAILED Generic error + * - @c RETURN_OK Successful serialization + */ virtual ReturnValue_t serialize(uint8_t **buffer, size_t *size, size_t maxSize, Endianness streamEndianness) const = 0; + /** + * Gets the size of a object if it would be serialized in a buffer + * @return Size of serialized object + */ virtual size_t getSerializedSize() const = 0; + /** + * DeSerializes a object from a given buffer of given size. + * + * Buffer must be moved to the current read location by the implementation + * of this function. Size must be decreased by the implementation. + * + * Custom implementations might use additional return values. + * + * @param buffer Buffer to deSerialize from. Will be moved by the function. + * @param size Remaining Size of the buffer to read from. Will be decreased by function. + * @param streamEndianness Endianness as in according to SerializeIF::Endianness + * @return + * - @c STREAM_TOO_SHORT The input stream is too short to deSerialize the object + * - @c TOO_MANY_ELEMENTS The buffer has more inputs than expected + * - @c RETURN_FAILED Generic Error + * - @c RETURN_OK Successful deserialization + */ virtual ReturnValue_t deSerialize(const uint8_t **buffer, size_t *size, Endianness streamEndianness) = 0; }; -#endif /* SERIALIZEIF_H_ */ +#endif /* FSFW_SERIALIZE_SERIALIZEIF_H_ */ From 6903a761ff3a185c1d79ada853cee975b77fdf57 Mon Sep 17 00:00:00 2001 From: Steffen Gaisser Date: Tue, 25 Aug 2020 14:23:59 +0200 Subject: [PATCH 30/50] Some corrections --- serialize/SerializeIF.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/serialize/SerializeIF.h b/serialize/SerializeIF.h index 1f31d91e..3fb8b264 100644 --- a/serialize/SerializeIF.h +++ b/serialize/SerializeIF.h @@ -21,8 +21,8 @@ public: static const uint8_t INTERFACE_ID = CLASS_ID::SERIALIZE_IF; static const ReturnValue_t BUFFER_TOO_SHORT = MAKE_RETURN_CODE(1); // !< The given buffer in serialize is too short - static const ReturnValue_t STREAM_TOO_SHORT = MAKE_RETURN_CODE(2); // !< The input stream in deSerialize is too short - static const ReturnValue_t TOO_MANY_ELEMENTS = MAKE_RETURN_CODE(3);// !< There are too many elements to be deSerialized + static const ReturnValue_t STREAM_TOO_SHORT = MAKE_RETURN_CODE(2); // !< The input stream in deserialize is too short + static const ReturnValue_t TOO_MANY_ELEMENTS = MAKE_RETURN_CODE(3);// !< There are too many elements to be deserialized virtual ~SerializeIF() { } @@ -53,7 +53,7 @@ public: virtual size_t getSerializedSize() const = 0; /** - * DeSerializes a object from a given buffer of given size. + * Deserializes a object from a given buffer of given size. * * Buffer must be moved to the current read location by the implementation * of this function. Size must be decreased by the implementation. @@ -61,7 +61,7 @@ public: * Custom implementations might use additional return values. * * @param buffer Buffer to deSerialize from. Will be moved by the function. - * @param size Remaining Size of the buffer to read from. Will be decreased by function. + * @param size Remaining size of the buffer to read from. Will be decreased by function. * @param streamEndianness Endianness as in according to SerializeIF::Endianness * @return * - @c STREAM_TOO_SHORT The input stream is too short to deSerialize the object From 1ae79ab99d53f324eccf7a715bb3aaa9ba665e66 Mon Sep 17 00:00:00 2001 From: Steffen Gaisser Date: Tue, 25 Aug 2020 15:06:53 +0200 Subject: [PATCH 31/50] Improvements of comments --- serialize/SerializeIF.h | 27 +++++++++++++++++---------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/serialize/SerializeIF.h b/serialize/SerializeIF.h index 3fb8b264..2bb17154 100644 --- a/serialize/SerializeIF.h +++ b/serialize/SerializeIF.h @@ -27,17 +27,21 @@ public: virtual ~SerializeIF() { } /** - * Function to serialize the object into a buffer with maxSize at the current size. + * @brief + * Function to serialize the object into a buffer with maxSize. Size represents the written amount. + * If a part of the buffer has been used already, size must be set to the used amount of bytes. * + * @details * Implementations of this function must increase the size variable and move the buffer pointer. - * + * MaxSize must be checked by implementations of this function + * and BUFFER_TOO_SHORT has to be returned if size would be larger than maxSize. * * Custom implementations might use additional return values. * - * @param buffer Buffer to serialize into, will be set to the current write location - * @param size Size that has been used in the buffer already, will be increase by the function - * @param maxSize Max size of the buffer. - * @param streamEndianness Endianness of the serialized data according to SerializeIF::Endianness + * @param[in/out] buffer Buffer to serialize into, will be set to the current write location + * @param[in/out] size Size that has been used in the buffer already, will be increased by the function + * @param[in] maxSize The size of the buffer that is allowed to be used for serialize. + * @param[in] streamEndianness Endianness of the serialized data according to SerializeIF::Endianness * @return * - @¢ BUFFER_TOO_SHORT The given buffer in is too short * - @c RETURN_FAILED Generic error @@ -47,22 +51,25 @@ public: size_t maxSize, Endianness streamEndianness) const = 0; /** - * Gets the size of a object if it would be serialized in a buffer + * Gets the size of a object if it would be serialized in a buffer.s * @return Size of serialized object */ virtual size_t getSerializedSize() const = 0; /** + * @brief * Deserializes a object from a given buffer of given size. * + * @details * Buffer must be moved to the current read location by the implementation * of this function. Size must be decreased by the implementation. + * Implementations are not allowed to alter the buffer as indicated by const pointer. * * Custom implementations might use additional return values. * - * @param buffer Buffer to deSerialize from. Will be moved by the function. - * @param size Remaining size of the buffer to read from. Will be decreased by function. - * @param streamEndianness Endianness as in according to SerializeIF::Endianness + * @param[in/out] buffer Buffer to deSerialize from. Will be moved by the function. + * @param[in/out] size Remaining size of the buffer to read from. Will be decreased by function. + * @param[in] streamEndianness Endianness as in according to SerializeIF::Endianness * @return * - @c STREAM_TOO_SHORT The input stream is too short to deSerialize the object * - @c TOO_MANY_ELEMENTS The buffer has more inputs than expected From aacda3afc242dad3c20b9d959a2417dbae5822ed Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 27 Aug 2020 16:26:00 +0200 Subject: [PATCH 32/50] interface adapted --- osal/FreeRTOS/BinSemaphUsingTask.cpp | 186 +++++++++--------- osal/FreeRTOS/BinSemaphUsingTask.h | 151 +++++++-------- osal/FreeRTOS/BinarySemaphore.cpp | 33 ++-- osal/FreeRTOS/BinarySemaphore.h | 12 +- osal/FreeRTOS/CountingSemaphUsingTask.cpp | 224 +++++++++++----------- osal/FreeRTOS/CountingSemaphUsingTask.h | 202 +++++++++---------- osal/FreeRTOS/CountingSemaphore.cpp | 86 ++++----- osal/FreeRTOS/CountingSemaphore.h | 68 +++---- osal/FreeRTOS/SemaphoreFactory.cpp | 120 ++++++------ tasks/SemaphoreIF.h | 140 +++++++------- 10 files changed, 616 insertions(+), 606 deletions(-) diff --git a/osal/FreeRTOS/BinSemaphUsingTask.cpp b/osal/FreeRTOS/BinSemaphUsingTask.cpp index d3351888..8c831bbe 100644 --- a/osal/FreeRTOS/BinSemaphUsingTask.cpp +++ b/osal/FreeRTOS/BinSemaphUsingTask.cpp @@ -1,91 +1,95 @@ -#include -#include -#include - -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(uint32_t timeoutMs) { - TickType_t timeout = SemaphoreIF::POLLING; - if(timeoutMs == SemaphoreIF::BLOCKING) { - timeout = SemaphoreIF::BLOCKING; - } - else if(timeoutMs > SemaphoreIF::POLLING){ - timeout = pdMS_TO_TICKS(timeoutMs); - } - return acquireWithTickTimeout(timeout); -} - -ReturnValue_t BinarySemaphoreUsingTask::acquireWithTickTimeout( - 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 43772e5a..2d2cf159 100644 --- a/osal/FreeRTOS/BinSemaphUsingTask.h +++ b/osal/FreeRTOS/BinSemaphUsingTask.h @@ -1,75 +1,76 @@ -#ifndef FRAMEWORK_OSAL_FREERTOS_BINSEMAPHUSINGTASK_H_ -#define FRAMEWORK_OSAL_FREERTOS_BINSEMAPHUSINGTASK_H_ - -#include -#include - -#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(uint32_t timeoutMs = - SemaphoreIF::BLOCKING) 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(TickType_t timeoutTicks = - SemaphoreIF::BLOCKING); - - /** - * 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 e6d2f92f..f1c78473 100644 --- a/osal/FreeRTOS/BinarySemaphore.cpp +++ b/osal/FreeRTOS/BinarySemaphore.cpp @@ -1,11 +1,11 @@ -#include -#include -#include +#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; + if(handle == nullptr) { + sif::error << "Semaphore: Binary semaph creation failure" << std::endl; } // Initiated semaphore must be given before it can be taken. xSemaphoreGive(handle); @@ -35,18 +35,23 @@ BinarySemaphore& BinarySemaphore::operator =( return *this; } -ReturnValue_t BinarySemaphore::acquire(uint32_t timeoutMs) { - TickType_t timeout = SemaphoreIF::POLLING; - if(timeoutMs == SemaphoreIF::BLOCKING) { - timeout = SemaphoreIF::BLOCKING; +ReturnValue_t BinarySemaphore::acquire(TimeoutType timeoutType, + uint32_t timeoutMs) { + TickType_t timeout = 0; + if(timeoutType == TimeoutType::POLLING) { + timeout = 0; } - else if(timeoutMs > SemaphoreIF::POLLING){ + else if(timeoutType == TimeoutType::WAITING){ timeout = pdMS_TO_TICKS(timeoutMs); } - return acquireWithTickTimeout(timeout); + else { + timeout = portMAX_DELAY; + } + return acquireWithTickTimeout(timeoutType, timeout); } -ReturnValue_t BinarySemaphore::acquireWithTickTimeout(TickType_t timeoutTicks) { +ReturnValue_t BinarySemaphore::acquireWithTickTimeout(TimeoutType timeoutType, + TickType_t timeoutTicks) { if(handle == nullptr) { return SemaphoreIF::SEMAPHORE_INVALID; } @@ -84,7 +89,7 @@ uint8_t BinarySemaphore::getSemaphoreCounter() const { SemaphoreHandle_t BinarySemaphore::getSemaphore() { return handle; } - + // Be careful with the stack size here. This is called from an ISR! ReturnValue_t BinarySemaphore::releaseFromISR( @@ -100,4 +105,4 @@ ReturnValue_t BinarySemaphore::releaseFromISR( else { return SemaphoreIF::SEMAPHORE_NOT_OWNED; } -} +} diff --git a/osal/FreeRTOS/BinarySemaphore.h b/osal/FreeRTOS/BinarySemaphore.h index 2a372cd2..5a32088a 100644 --- a/osal/FreeRTOS/BinarySemaphore.h +++ b/osal/FreeRTOS/BinarySemaphore.h @@ -1,8 +1,8 @@ #ifndef FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ #define FRAMEWORK_OSAL_FREERTOS_BINARYSEMPAHORE_H_ -#include -#include +#include "../../returnvalues/HasReturnvaluesIF.h" +#include "../../tasks/SemaphoreIF.h" #include #include @@ -52,8 +52,8 @@ public: * @return -@c RETURN_OK on success * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout */ - ReturnValue_t acquire(uint32_t timeoutMs = - SemaphoreIF::BLOCKING) override; + ReturnValue_t acquire(TimeoutType timeoutType = + TimeoutType::BLOCKING, uint32_t timeoutMs = portMAX_DELAY) override; /** * Same as lockBinarySemaphore() with timeout in FreeRTOS ticks. @@ -61,8 +61,8 @@ public: * @return -@c RETURN_OK on success * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout */ - ReturnValue_t acquireWithTickTimeout(TickType_t timeoutTicks = - SemaphoreIF::BLOCKING); + ReturnValue_t acquireWithTickTimeout(TimeoutType timeoutType = + TimeoutType::BLOCKING, TickType_t timeoutTicks = portMAX_DELAY); /** * Release the binary semaphore. diff --git a/osal/FreeRTOS/CountingSemaphUsingTask.cpp b/osal/FreeRTOS/CountingSemaphUsingTask.cpp index f33c7a0d..e66671ac 100644 --- a/osal/FreeRTOS/CountingSemaphUsingTask.cpp +++ b/osal/FreeRTOS/CountingSemaphUsingTask.cpp @@ -1,110 +1,114 @@ -#include -#include -#include - -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(uint32_t timeoutMs) { - TickType_t timeout = SemaphoreIF::POLLING; - if(timeoutMs == SemaphoreIF::BLOCKING) { - timeout = SemaphoreIF::BLOCKING; - } - else if(timeoutMs > SemaphoreIF::POLLING){ - timeout = pdMS_TO_TICKS(timeoutMs); - } - return acquireWithTickTimeout(timeout); - -} - -ReturnValue_t CountingSemaphoreUsingTask::acquireWithTickTimeout( - 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 f1d82c1d..e3733382 100644 --- a/osal/FreeRTOS/CountingSemaphUsingTask.h +++ b/osal/FreeRTOS/CountingSemaphUsingTask.h @@ -1,100 +1,102 @@ -#ifndef FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHUSINGTASK_H_ -#define FRAMEWORK_OSAL_FREERTOS_COUNTINGSEMAPHUSINGTASK_H_ - -#include -#include - -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(uint32_t timeoutMs = SemaphoreIF::BLOCKING) 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( - TickType_t timeoutTicks = SemaphoreIF::BLOCKING); - - /** - * 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 957f6a31..0b218e54 100644 --- a/osal/FreeRTOS/CountingSemaphore.cpp +++ b/osal/FreeRTOS/CountingSemaphore.cpp @@ -1,43 +1,43 @@ -#include -#include -#include - -#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 dca3ab0e..9432ed81 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 - -/** - * @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 78427f6c..8575cf4a 100644 --- a/osal/FreeRTOS/SemaphoreFactory.cpp +++ b/osal/FreeRTOS/SemaphoreFactory.cpp @@ -1,61 +1,59 @@ -#include -#include -#include -#include -#include -#include - -SemaphoreFactory* SemaphoreFactory::factoryInstance = nullptr; -const uint32_t SemaphoreIF::POLLING = 0; -const uint32_t SemaphoreIF::BLOCKING = portMAX_DELAY; - -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/tasks/SemaphoreIF.h b/tasks/SemaphoreIF.h index 4fc92ebd..30d4ed88 100644 --- a/tasks/SemaphoreIF.h +++ b/tasks/SemaphoreIF.h @@ -1,72 +1,68 @@ -#ifndef FRAMEWORK_TASKS_SEMAPHOREIF_H_ -#define FRAMEWORK_TASKS_SEMAPHOREIF_H_ -#include -#include -#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: - virtual~ SemaphoreIF() {}; - /** - * @brief Timeout value used for polling lock attempt. - * @details - * If the lock is not successfull, MUTEX_TIMEOUT will be returned - * immediately. Value needs to be defined in implementation. - */ - static const uint32_t POLLING; - /** - * @brief Timeout value used for permanent blocking lock attempt. - * @details - * The task will be blocked (indefinitely) until the mutex is unlocked. - * Value needs to be defined in implementation. - */ - static const uint32_t BLOCKING; - - 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(uint32_t timeoutMs) = 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 073f168b9507800d11ddcee8f63deb6025c513e7 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 27 Aug 2020 16:27:54 +0200 Subject: [PATCH 33/50] task management function taken over from master --- osal/FreeRTOS/TaskManagement.cpp | 23 ++++++++++++----------- osal/FreeRTOS/TaskManagement.h | 23 ++++++++++++----------- 2 files changed, 24 insertions(+), 22 deletions(-) diff --git a/osal/FreeRTOS/TaskManagement.cpp b/osal/FreeRTOS/TaskManagement.cpp index 7871ab2e..16682d36 100644 --- a/osal/FreeRTOS/TaskManagement.cpp +++ b/osal/FreeRTOS/TaskManagement.cpp @@ -1,23 +1,24 @@ -#include - -void TaskManagement::requestContextSwitchFromTask() { +#include "../../osal/FreeRTOS/TaskManagement.h" + +void TaskManagement::vRequestContextSwitchFromTask() { vTaskDelay(0); } - + void TaskManagement::requestContextSwitch( - CallContext callContext = CallContext::TASK) { + CallContext callContext = CallContext::TASK) { if(callContext == CallContext::ISR) { // This function depends on the partmacro.h definition for the specific device - requestContextSwitchFromISR(); + vRequestContextSwitchFromISR(); } else { - requestContextSwitchFromTask(); + vRequestContextSwitchFromTask(); } } - + TaskHandle_t TaskManagement::getCurrentTaskHandle() { return xTaskGetCurrentTaskHandle(); } -configSTACK_DEPTH_TYPE TaskManagement::getTaskStackHighWatermark() { - return uxTaskGetStackHighWaterMark(TaskManagement::getCurrentTaskHandle()); -} +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 39c24377..43003d76 100644 --- a/osal/FreeRTOS/TaskManagement.h +++ b/osal/FreeRTOS/TaskManagement.h @@ -1,25 +1,25 @@ #ifndef FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ #define FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ -#include +#include "../../returnvalues/HasReturnvaluesIF.h" extern "C" { #include #include } #include - + /** * Architecture dependant portmacro.h function call. * Should be implemented in bsp. */ -extern "C" void requestContextSwitchFromISR(); +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 + * 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. + * an ISR and task. */ enum class CallContext { TASK = 0x00,//!< task_context @@ -29,19 +29,19 @@ enum class CallContext { 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. + * 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 requestContextSwitchFromTask(void); + */ + static void vRequestContextSwitchFromTask(void); /** * @return The current task handle @@ -56,8 +56,9 @@ public: * 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 configSTACK_DEPTH_TYPE getTaskStackHighWatermark(); + */ + static size_t getTaskStackHighWatermark( + TaskHandle_t task = nullptr); }; #endif /* FRAMEWORK_OSAL_FREERTOS_TASKMANAGEMENT_H_ */ From 73db79a3fefb1d7153a9a6f9cbaceb55b725d494 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 27 Aug 2020 17:06:10 +0200 Subject: [PATCH 34/50] updated linux implementation --- osal/linux/BinarySemaphore.cpp | 13 ++-- osal/linux/BinarySemaphore.h | 8 +-- osal/linux/CountingSemaphore.cpp | 108 +++++++++++++++---------------- osal/linux/CountingSemaphore.h | 74 ++++++++++----------- osal/linux/SemaphoreFactory.cpp | 69 ++++++++++---------- 5 files changed, 135 insertions(+), 137 deletions(-) diff --git a/osal/linux/BinarySemaphore.cpp b/osal/linux/BinarySemaphore.cpp index e2ad9b58..5216ff37 100644 --- a/osal/linux/BinarySemaphore.cpp +++ b/osal/linux/BinarySemaphore.cpp @@ -1,5 +1,5 @@ -#include -#include +#include "../../osal/linux/BinarySemaphore.h" +#include "../../serviceinterface/ServiceInterfaceStream.h" extern "C" { #include @@ -25,15 +25,16 @@ BinarySemaphore& BinarySemaphore::operator =( return * this; } -ReturnValue_t BinarySemaphore::acquire(uint32_t timeoutMs) { +ReturnValue_t BinarySemaphore::acquire(TimeoutType timeoutType, + uint32_t timeoutMs) { int result = 0; - if(timeoutMs == SemaphoreIF::POLLING) { + if(timeoutType == TimeoutType::POLLING) { result = sem_trywait(&handle); } - else if(timeoutMs == SemaphoreIF::BLOCKING) { + else if(timeoutType == TimeoutType::BLOCKING) { result = sem_wait(&handle); } - else if(timeoutMs > SemaphoreIF::POLLING){ + else if(timeoutType == TimeoutType::WAITING){ timespec timeOut; clock_gettime(CLOCK_REALTIME, &timeOut); uint64_t nseconds = timeOut.tv_sec * 1000000000 + timeOut.tv_nsec; diff --git a/osal/linux/BinarySemaphore.h b/osal/linux/BinarySemaphore.h index 9d4ed1cd..4181de85 100644 --- a/osal/linux/BinarySemaphore.h +++ b/osal/linux/BinarySemaphore.h @@ -1,8 +1,8 @@ #ifndef FRAMEWORK_OSAL_LINUX_BINARYSEMPAHORE_H_ #define FRAMEWORK_OSAL_LINUX_BINARYSEMPAHORE_H_ -#include -#include +#include "../../returnvalues/HasReturnvaluesIF.h" +#include "../../tasks/SemaphoreIF.h" extern "C" { #include @@ -49,8 +49,8 @@ public: * @return -@c RETURN_OK on success * -@c SemaphoreIF::SEMAPHORE_TIMEOUT on timeout */ - ReturnValue_t acquire(uint32_t timeoutMs = - SemaphoreIF::BLOCKING) override; + ReturnValue_t acquire(TimeoutType timeoutType = TimeoutType::BLOCKING, + uint32_t timeoutMs = 0) override; /** * Release the binary semaphore. diff --git a/osal/linux/CountingSemaphore.cpp b/osal/linux/CountingSemaphore.cpp index dfc4d801..ef32539b 100644 --- a/osal/linux/CountingSemaphore.cpp +++ b/osal/linux/CountingSemaphore.cpp @@ -1,54 +1,54 @@ -#include -#include - -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 ba606595..afe21a61 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 - -/** - * @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 4fbd60d9..805865ca 100644 --- a/osal/linux/SemaphoreFactory.cpp +++ b/osal/linux/SemaphoreFactory.cpp @@ -1,36 +1,33 @@ -#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) { - 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 "../../osal/linux/BinarySemaphore.h" +#include "../../osal/linux/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; +} From 3be253efd62cae876f3df5fc9689380d3309cf5d Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 27 Aug 2020 17:07:42 +0200 Subject: [PATCH 35/50] include fix --- tasks/SemaphoreFactory.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tasks/SemaphoreFactory.h b/tasks/SemaphoreFactory.h index 7f8edaf1..bffa59c8 100644 --- a/tasks/SemaphoreFactory.h +++ b/tasks/SemaphoreFactory.h @@ -1,6 +1,6 @@ #ifndef FRAMEWORK_TASKS_SEMAPHOREFACTORY_H_ #define FRAMEWORK_TASKS_SEMAPHOREFACTORY_H_ -#include +#include "../tasks/SemaphoreIF.h" /** * Creates Semaphore. From 0fedad6da09816c8a06615f3903a8e34545001bc Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 27 Aug 2020 17:14:49 +0200 Subject: [PATCH 36/50] semaphore factory update --- osal/linux/SemaphoreFactory.cpp | 4 +- tasks/SemaphoreFactory.h | 99 +++++++++++++++++---------------- 2 files changed, 52 insertions(+), 51 deletions(-) diff --git a/osal/linux/SemaphoreFactory.cpp b/osal/linux/SemaphoreFactory.cpp index 805865ca..bcb5d16f 100644 --- a/osal/linux/SemaphoreFactory.cpp +++ b/osal/linux/SemaphoreFactory.cpp @@ -1,6 +1,6 @@ #include "../../tasks/SemaphoreFactory.h" -#include "../../osal/linux/BinarySemaphore.h" -#include "../../osal/linux/CountingSemaphore.h" +#include "BinarySemaphore.h" +#include "CountingSemaphore.h" #include "../../serviceinterface/ServiceInterfaceStream.h" SemaphoreFactory* SemaphoreFactory::factoryInstance = nullptr; diff --git a/tasks/SemaphoreFactory.h b/tasks/SemaphoreFactory.h index bffa59c8..2c41a135 100644 --- a/tasks/SemaphoreFactory.h +++ b/tasks/SemaphoreFactory.h @@ -1,49 +1,50 @@ -#ifndef FRAMEWORK_TASKS_SEMAPHOREFACTORY_H_ -#define FRAMEWORK_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 /* FRAMEWORK_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_ */ From dfe3658a851d4576738dcd1c973ce0b20672753a Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 27 Aug 2020 19:52:25 +0200 Subject: [PATCH 37/50] removed hk stuff --- devicehandlers/ChildHandlerBase.cpp | 8 +++----- devicehandlers/ChildHandlerBase.h | 5 ++--- devicehandlers/DeviceHandlerBase.cpp | 14 -------------- devicehandlers/DeviceHandlerBase.h | 9 +-------- 4 files changed, 6 insertions(+), 30 deletions(-) diff --git a/devicehandlers/ChildHandlerBase.cpp b/devicehandlers/ChildHandlerBase.cpp index acc1b5a0..f18c3812 100644 --- a/devicehandlers/ChildHandlerBase.cpp +++ b/devicehandlers/ChildHandlerBase.cpp @@ -4,15 +4,13 @@ ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, CookieIF * cookie, - object_id_t hkDestination, uint32_t thermalStatePoolId, - uint32_t thermalRequestPoolId, - object_id_t parent, - FailureIsolationBase* customFdir, size_t cmdQueueSize) : + uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, + object_id_t parent, FailureIsolationBase* customFdir, + size_t cmdQueueSize) : DeviceHandlerBase(setObjectId, deviceCommunication, cookie, (customFdir == nullptr? &childHandlerFdir : customFdir), cmdQueueSize), parentId(parent), childHandlerFdir(setObjectId) { - this->setHkDestination(hkDestination); this->setThermalStateRequestPoolIds(thermalStatePoolId, thermalRequestPoolId); diff --git a/devicehandlers/ChildHandlerBase.h b/devicehandlers/ChildHandlerBase.h index 625955f0..70c27952 100644 --- a/devicehandlers/ChildHandlerBase.h +++ b/devicehandlers/ChildHandlerBase.h @@ -7,9 +7,8 @@ class ChildHandlerBase: public DeviceHandlerBase { public: ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, - CookieIF * cookie, object_id_t hkDestination, - uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, - object_id_t parent = objects::NO_OBJECT, + CookieIF * cookie, uint32_t thermalStatePoolId, + uint32_t thermalRequestPoolId, object_id_t parent = objects::NO_OBJECT, FailureIsolationBase* customFdir = nullptr, size_t cmdQueueSize = 20); virtual ~ChildHandlerBase(); diff --git a/devicehandlers/DeviceHandlerBase.cpp b/devicehandlers/DeviceHandlerBase.cpp index ff6a5dbe..017ea8d2 100644 --- a/devicehandlers/DeviceHandlerBase.cpp +++ b/devicehandlers/DeviceHandlerBase.cpp @@ -17,7 +17,6 @@ object_id_t DeviceHandlerBase::powerSwitcherId = objects::NO_OBJECT; object_id_t DeviceHandlerBase::rawDataReceiverId = objects::NO_OBJECT; object_id_t DeviceHandlerBase::defaultFdirParentId = objects::NO_OBJECT; -object_id_t DeviceHandlerBase::defaultHkDestination = objects::NO_OBJECT; DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, CookieIF * comCookie, @@ -49,10 +48,6 @@ DeviceHandlerBase::DeviceHandlerBase(object_id_t setObjectId, } } -void DeviceHandlerBase::setHkDestination(object_id_t hkDestination) { - this->hkDestination = hkDestination; -} - void DeviceHandlerBase::setThermalStateRequestPoolIds( uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId) { this->deviceThermalRequestPoolId = thermalStatePoolId; @@ -188,15 +183,6 @@ ReturnValue_t DeviceHandlerBase::initialize() { return result; } -// if(hkDestination == objects::NO_OBJECT) { -// hkDestination = defaultHkDestination; -// } -// -// result = hkManager.initialize(commandQueue, hkDestination); -// if (result != HasReturnvaluesIF::RETURN_OK) { -// return result; -// } - fillCommandAndReplyMap(); //Set temperature target state to NON_OP. diff --git a/devicehandlers/DeviceHandlerBase.h b/devicehandlers/DeviceHandlerBase.h index d41a50e5..5666f35e 100644 --- a/devicehandlers/DeviceHandlerBase.h +++ b/devicehandlers/DeviceHandlerBase.h @@ -85,8 +85,7 @@ class DeviceHandlerBase: public DeviceHandlerIF, public HasModesIF, public HasHealthIF, public HasActionsIF, - public ReceivesParameterMessagesIF - /* public HasLocalDataPoolIF */ { + public ReceivesParameterMessagesIF { friend void (Factory::setStaticFrameworkObjectIds)(); public: /** @@ -107,7 +106,6 @@ public: CookieIF * comCookie, FailureIsolationBase* fdirInstance = nullptr, size_t cmdQueueSize = 20); - void setHkDestination(object_id_t hkDestination); void setThermalStateRequestPoolIds(uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId); @@ -1036,11 +1034,6 @@ private: /** the object used to set power switches */ PowerSwitchIF *powerSwitcher = nullptr; - /** Cached for initialize() */ - static object_id_t defaultHkDestination; - /** HK destination can also be set individually */ - object_id_t hkDestination = objects::NO_OBJECT; - /** * @brief Used for timing out mode transitions. * Set when setMode() is called. From 8f2d5a5528c7247a0f1cd60a6dc29ffd85eaeb45 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 27 Aug 2020 19:58:35 +0200 Subject: [PATCH 38/50] DH fdir improvements --- devicehandlers/DeviceHandlerFailureIsolation.cpp | 6 +++--- devicehandlers/DeviceHandlerFailureIsolation.h | 5 ++--- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/devicehandlers/DeviceHandlerFailureIsolation.cpp b/devicehandlers/DeviceHandlerFailureIsolation.cpp index 1f098d55..8831f9ae 100644 --- a/devicehandlers/DeviceHandlerFailureIsolation.cpp +++ b/devicehandlers/DeviceHandlerFailureIsolation.cpp @@ -5,7 +5,8 @@ #include "../serviceinterface/ServiceInterfaceStream.h" #include "../thermal/ThermalComponentIF.h" -object_id_t DeviceHandlerFailureIsolation::powerConfirmationId = 0; +object_id_t DeviceHandlerFailureIsolation::powerConfirmationId = + objects::NO_OBJECT; DeviceHandlerFailureIsolation::DeviceHandlerFailureIsolation(object_id_t owner, object_id_t parent) : @@ -70,7 +71,7 @@ ReturnValue_t DeviceHandlerFailureIsolation::eventReceived(EventMessage* event) break; //****Power***** case PowerSwitchIF::SWITCH_WENT_OFF: - if(hasPowerConfirmation) { + if(powerConfirmation != MessageQueueIF::NO_QUEUE) { result = sendConfirmationRequest(event, powerConfirmation); if (result == RETURN_OK) { setFdirState(DEVICE_MIGHT_BE_OFF); @@ -171,7 +172,6 @@ ReturnValue_t DeviceHandlerFailureIsolation::initialize() { powerConfirmationId); if (power != nullptr) { powerConfirmation = power->getEventReceptionQueue(); - hasPowerConfirmation = true; } return RETURN_OK; diff --git a/devicehandlers/DeviceHandlerFailureIsolation.h b/devicehandlers/DeviceHandlerFailureIsolation.h index 56649ee9..6ed53efd 100644 --- a/devicehandlers/DeviceHandlerFailureIsolation.h +++ b/devicehandlers/DeviceHandlerFailureIsolation.h @@ -3,11 +3,11 @@ #include "../fdir/FaultCounter.h" #include "../fdir/FailureIsolationBase.h" + namespace Factory{ void setStaticFrameworkObjectIds(); } - class DeviceHandlerFailureIsolation: public FailureIsolationBase { friend void (Factory::setStaticFrameworkObjectIds)(); friend class Heater; @@ -28,8 +28,7 @@ protected: NONE, RECOVERY_ONGOING, DEVICE_MIGHT_BE_OFF, AWAIT_SHUTDOWN }; FDIRState fdirState; - bool hasPowerConfirmation = false; - MessageQueueId_t powerConfirmation; + MessageQueueId_t powerConfirmation = MessageQueueIF::NO_QUEUE; static object_id_t powerConfirmationId; // TODO: Are those hardcoded value? How can they be changed. static const uint32_t MAX_REBOOT = 1; From c4c607776c38d9913be6bf2b4ada0d767d345c53 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 27 Aug 2020 20:07:03 +0200 Subject: [PATCH 39/50] important fix, 0 init removed --- .../DeviceHandlerFailureIsolation.cpp | 15 ++++++++---- .../DeviceHandlerFailureIsolation.h | 24 +++++++++++-------- 2 files changed, 24 insertions(+), 15 deletions(-) diff --git a/devicehandlers/DeviceHandlerFailureIsolation.cpp b/devicehandlers/DeviceHandlerFailureIsolation.cpp index 8831f9ae..9fbe71d8 100644 --- a/devicehandlers/DeviceHandlerFailureIsolation.cpp +++ b/devicehandlers/DeviceHandlerFailureIsolation.cpp @@ -1,5 +1,7 @@ -#include "DeviceHandlerBase.h" #include "DeviceHandlerFailureIsolation.h" + +#include "../devicehandlers/DeviceHandlerIF.h" +#include "../modes/HasModesIF.h" #include "../health/HealthTableIF.h" #include "../power/Fuse.h" #include "../serviceinterface/ServiceInterfaceStream.h" @@ -11,12 +13,15 @@ object_id_t DeviceHandlerFailureIsolation::powerConfirmationId = DeviceHandlerFailureIsolation::DeviceHandlerFailureIsolation(object_id_t owner, object_id_t parent) : FailureIsolationBase(owner, parent), - strangeReplyCount(MAX_STRANGE_REPLIES, STRANGE_REPLIES_TIME_MS, + strangeReplyCount(DEFAULT_MAX_STRANGE_REPLIES, + DEFAULT_STRANGE_REPLIES_TIME_MS, parameterDomainBase++), - missedReplyCount( MAX_MISSED_REPLY_COUNT, MISSED_REPLY_TIME_MS, + missedReplyCount( DEFAULT_MAX_MISSED_REPLY_COUNT, + DEFAULT_MISSED_REPLY_TIME_MS, parameterDomainBase++), - recoveryCounter(MAX_REBOOT, REBOOT_TIME_MS, parameterDomainBase++), - fdirState(NONE), powerConfirmation(0) { + recoveryCounter(DEFAULT_MAX_REBOOT, DEFAULT_REBOOT_TIME_MS, + parameterDomainBase++), + fdirState(NONE) { } DeviceHandlerFailureIsolation::~DeviceHandlerFailureIsolation() { diff --git a/devicehandlers/DeviceHandlerFailureIsolation.h b/devicehandlers/DeviceHandlerFailureIsolation.h index 6ed53efd..8a3fd9dd 100644 --- a/devicehandlers/DeviceHandlerFailureIsolation.h +++ b/devicehandlers/DeviceHandlerFailureIsolation.h @@ -1,5 +1,5 @@ -#ifndef FRAMEWORK_DEVICEHANDLERS_DEVICEHANDLERFAILUREISOLATION_H_ -#define FRAMEWORK_DEVICEHANDLERS_DEVICEHANDLERFAILUREISOLATION_H_ +#ifndef FSFW_DEVICEHANDLERS_DEVICEHANDLERFAILUREISOLATION_H_ +#define FSFW_DEVICEHANDLERS_DEVICEHANDLERFAILUREISOLATION_H_ #include "../fdir/FaultCounter.h" #include "../fdir/FailureIsolationBase.h" @@ -20,23 +20,27 @@ public: virtual ReturnValue_t getParameter(uint8_t domainId, uint16_t parameterId, ParameterWrapper *parameterWrapper, const ParameterWrapper *newValues, uint16_t startAtIndex); + protected: FaultCounter strangeReplyCount; FaultCounter missedReplyCount; FaultCounter recoveryCounter; + enum FDIRState { NONE, RECOVERY_ONGOING, DEVICE_MIGHT_BE_OFF, AWAIT_SHUTDOWN }; FDIRState fdirState; + MessageQueueId_t powerConfirmation = MessageQueueIF::NO_QUEUE; static object_id_t powerConfirmationId; - // TODO: Are those hardcoded value? How can they be changed. - static const uint32_t MAX_REBOOT = 1; - static const uint32_t REBOOT_TIME_MS = 180000; - static const uint32_t MAX_STRANGE_REPLIES = 10; - static const uint32_t STRANGE_REPLIES_TIME_MS = 10000; - static const uint32_t MAX_MISSED_REPLY_COUNT = 5; - static const uint32_t MISSED_REPLY_TIME_MS = 10000; + + static const uint32_t DEFAULT_MAX_REBOOT = 1; + static const uint32_t DEFAULT_REBOOT_TIME_MS = 180000; + static const uint32_t DEFAULT_MAX_STRANGE_REPLIES = 10; + static const uint32_t DEFAULT_STRANGE_REPLIES_TIME_MS = 10000; + static const uint32_t DEFAULT_MAX_MISSED_REPLY_COUNT = 5; + static const uint32_t DEFAULT_MISSED_REPLY_TIME_MS = 10000; + virtual ReturnValue_t eventReceived(EventMessage* event); virtual void eventConfirmed(EventMessage* event); void wasParentsFault(EventMessage* event); @@ -50,4 +54,4 @@ protected: bool isFdirInActionOrAreWeFaulty(EventMessage* event); }; -#endif /* FRAMEWORK_DEVICEHANDLERS_DEVICEHANDLERFAILUREISOLATION_H_ */ +#endif /* FSFW_DEVICEHANDLERS_DEVICEHANDLERFAILUREISOLATION_H_ */ From 54112acf625b3ca918313b3186a127521686008f Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 27 Aug 2020 20:11:36 +0200 Subject: [PATCH 40/50] chb update --- devicehandlers/ChildHandlerBase.h | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/devicehandlers/ChildHandlerBase.h b/devicehandlers/ChildHandlerBase.h index 70c27952..3e9652a3 100644 --- a/devicehandlers/ChildHandlerBase.h +++ b/devicehandlers/ChildHandlerBase.h @@ -1,15 +1,17 @@ -#ifndef PAYLOADHANDLERBASE_H_ -#define PAYLOADHANDLERBASE_H_ +#ifndef FSFW_DEVICES_CHILDHANDLERBASE_H_ +#define FSFW_DEVICES_CHILDHANDLERBASE_H_ -#include "../devicehandlers/ChildHandlerFDIR.h" -#include "../devicehandlers/DeviceHandlerBase.h" +#include "ChildHandlerFDIR.h" +#include "DeviceHandlerBase.h" class ChildHandlerBase: public DeviceHandlerBase { public: ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, CookieIF * cookie, uint32_t thermalStatePoolId, - uint32_t thermalRequestPoolId, object_id_t parent = objects::NO_OBJECT, - FailureIsolationBase* customFdir = nullptr, size_t cmdQueueSize = 20); + uint32_t thermalRequestPoolId, + object_id_t parent = objects::NO_OBJECT, + FailureIsolationBase* customFdir = nullptr, + size_t cmdQueueSize = 20); virtual ~ChildHandlerBase(); virtual ReturnValue_t initialize(); @@ -20,4 +22,5 @@ protected: }; -#endif /* PAYLOADHANDLERBASE_H_ */ +#endif /* FSFW_DEVICES_CHILDHANDLERBASE_H_ */ + From eb503ae030fdc5a8c7925f58cad85427d77251cf Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Thu, 27 Aug 2020 20:19:27 +0200 Subject: [PATCH 41/50] include guard fix --- container/DynamicFIFO.h | 6 +++--- container/FIFOBase.h | 6 +++--- container/FIFOBase.tpp | 6 +++--- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/container/DynamicFIFO.h b/container/DynamicFIFO.h index 5cdae81a..7fa0c32c 100644 --- a/container/DynamicFIFO.h +++ b/container/DynamicFIFO.h @@ -1,5 +1,5 @@ -#ifndef FRAMEWORK_CONTAINER_DYNAMICFIFO_H_ -#define FRAMEWORK_CONTAINER_DYNAMICFIFO_H_ +#ifndef FSFW_CONTAINER_DYNAMICFIFO_H_ +#define FSFW_CONTAINER_DYNAMICFIFO_H_ #include "FIFOBase.h" #include @@ -39,4 +39,4 @@ private: std::vector fifoVector; }; -#endif /* FRAMEWORK_CONTAINER_DYNAMICFIFO_H_ */ +#endif /* FSFW_CONTAINER_DYNAMICFIFO_H_ */ diff --git a/container/FIFOBase.h b/container/FIFOBase.h index acb38b82..bbabd67c 100644 --- a/container/FIFOBase.h +++ b/container/FIFOBase.h @@ -1,5 +1,5 @@ -#ifndef FRAMEWORK_CONTAINER_FIFOBASE_H_ -#define FRAMEWORK_CONTAINER_FIFOBASE_H_ +#ifndef FSFW_CONTAINER_FIFOBASE_H_ +#define FSFW_CONTAINER_FIFOBASE_H_ #include "../returnvalues/HasReturnvaluesIF.h" #include @@ -62,4 +62,4 @@ protected: #include "FIFOBase.tpp" -#endif /* FRAMEWORK_CONTAINER_FIFOBASE_H_ */ +#endif /* FSFW_CONTAINER_FIFOBASE_H_ */ diff --git a/container/FIFOBase.tpp b/container/FIFOBase.tpp index c73e9d59..e1287dcf 100644 --- a/container/FIFOBase.tpp +++ b/container/FIFOBase.tpp @@ -1,7 +1,7 @@ -#ifndef FRAMEWORK_CONTAINER_FIFOBASE_TPP_ -#define FRAMEWORK_CONTAINER_FIFOBASE_TPP_ +#ifndef FSFW_CONTAINER_FIFOBASE_TPP_ +#define FSFW_CONTAINER_FIFOBASE_TPP_ -#ifndef FRAMEWORK_CONTAINER_FIFOBASE_H_ +#ifndef FSFW_CONTAINER_FIFOBASE_H_ #error Include FIFOBase.h before FIFOBase.tpp! #endif From b54d63edcb6077572f7cd61f0b94c96990460b07 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Fri, 28 Aug 2020 18:10:02 +0200 Subject: [PATCH 42/50] event manager improvements --- events/EventManager.cpp | 59 +++++++++++++++++++++++------------------ events/EventManager.h | 10 +++++-- 2 files changed, 41 insertions(+), 28 deletions(-) diff --git a/events/EventManager.cpp b/events/EventManager.cpp index e58f251a..e71951e3 100644 --- a/events/EventManager.cpp +++ b/events/EventManager.cpp @@ -8,13 +8,16 @@ const uint16_t EventManager::POOL_SIZES[N_POOLS] = { sizeof(EventMatchTree::Node), sizeof(EventIdRangeMatcher), sizeof(ReporterRangeMatcher) }; -//If one checks registerListener calls, there are around 40 (to max 50) objects registering for certain events. -//Each listener requires 1 or 2 EventIdMatcher and 1 or 2 ReportRangeMatcher. So a good guess is 75 to a max of 100 pools required for each, which fits well. +// If one checks registerListener calls, there are around 40 (to max 50) +// objects registering for certain events. +// Each listener requires 1 or 2 EventIdMatcher and 1 or 2 ReportRangeMatcher. +// So a good guess is 75 to a max of 100 pools required for each, which fits well. +// SHOULDDO: Shouldn't this be in the config folder and passed via ctor? const uint16_t EventManager::N_ELEMENTS[N_POOLS] = { 240, 120, 120 }; EventManager::EventManager(object_id_t setObjectId) : - SystemObject(setObjectId), eventReportQueue(NULL), mutex(NULL), factoryBackend( - 0, POOL_SIZES, N_ELEMENTS, false, true) { + SystemObject(setObjectId), + factoryBackend(0, POOL_SIZES, N_ELEMENTS, false, true) { mutex = MutexFactory::instance()->createMutex(); eventReportQueue = QueueFactory::instance()->createMessageQueue( MAX_EVENTS_PER_CYCLE, EventMessage::EVENT_MESSAGE_SIZE); @@ -108,41 +111,45 @@ ReturnValue_t EventManager::unsubscribeFromEventRange(MessageQueueId_t listener, #ifdef DEBUG -//forward declaration, should be implemented by mission -const char* translateObject(object_id_t object); -const char * translateEvents(Event event); - void EventManager::printEvent(EventMessage* message) { const char *string = 0; switch (message->getSeverity()) { case SEVERITY::INFO: -// string = translateObject(message->getReporter()); -// sif::info << "EVENT: "; -// if (string != 0) { -// sif::info << string; -// } else { -// sif::info << "0x" << std::hex << message->getReporter() << std::dec; -// } -// sif::info << " reported " << translateEvents(message->getEvent()) << " (" -// << std::dec << message->getEventId() << std::hex << ") P1: 0x" -// << message->getParameter1() << " P2: 0x" -// << message->getParameter2() << std::dec << std::endl; - break; - default: +#ifdef DEBUG_INFO_EVENT string = translateObject(message->getReporter()); - sif::error << "EVENT: "; + sif::info << "EVENT: "; if (string != 0) { - sif::error << string; + sif::info << string; } else { - sif::error << "0x" << std::hex << message->getReporter() << std::dec; + sif::info << "0x" << std::hex << message->getReporter() << std::dec; } - sif::error << " reported " << translateEvents(message->getEvent()) << " (" + sif::info << " reported " << translateEvents(message->getEvent()) << " (" << std::dec << message->getEventId() << std::hex << ") P1: 0x" << message->getParameter1() << " P2: 0x" << message->getParameter2() << std::dec << std::endl; +#endif + break; + default: + string = translateObject(message->getReporter()); + sif::debug << "EventManager: "; + if (string != 0) { + sif::debug << string; + } + else { + sif::debug << "0x" << std::hex << message->getReporter() << std::dec; + } + sif::debug << " reported " << translateEvents(message->getEvent()) + << " (" << std::dec << message->getEventId() << ") " + << std::endl; + + sif::debug << std::hex << "P1 Hex: 0x" << message->getParameter1() + << ", P1 Dec: " << std::dec << message->getParameter1() + << std::endl; + sif::debug << std::hex << "P2 Hex: 0x" << message->getParameter2() + << ", P2 Dec: " << std::dec << message->getParameter2() + << std::endl; break; } - } #endif diff --git a/events/EventManager.h b/events/EventManager.h index 7badc3bf..2602aeb2 100644 --- a/events/EventManager.h +++ b/events/EventManager.h @@ -10,6 +10,12 @@ #include "../ipc/MutexIF.h" #include +#ifdef DEBUG +// forward declaration, should be implemented by mission +extern const char* translateObject(object_id_t object); +extern const char* translateEvents(Event event); +#endif + class EventManager: public EventManagerIF, public ExecutableObjectIF, public SystemObject { @@ -36,11 +42,11 @@ public: ReturnValue_t performOperation(uint8_t opCode); protected: - MessageQueueIF* eventReportQueue; + MessageQueueIF* eventReportQueue = nullptr; std::map listenerList; - MutexIF* mutex; + MutexIF* mutex = nullptr; static const uint8_t N_POOLS = 3; LocalPool factoryBackend; From 7a5c70e75366399082ac9d40a6f95c1c7245cf95 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Fri, 28 Aug 2020 18:46:47 +0200 Subject: [PATCH 43/50] typo fixes --- parameters/HasParametersIF.h | 27 +++++-- parameters/ParameterHelper.cpp | 16 ++-- parameters/ParameterHelper.h | 23 ++++-- parameters/ParameterMessage.cpp | 2 +- parameters/ParameterMessage.h | 8 +- parameters/ParameterWrapper.cpp | 31 ++++---- parameters/ParameterWrapper.h | 95 +++++++++++++++--------- parameters/ReceivesParameterMessagesIF.h | 6 +- 8 files changed, 126 insertions(+), 82 deletions(-) diff --git a/parameters/HasParametersIF.h b/parameters/HasParametersIF.h index 71212b57..005403fa 100644 --- a/parameters/HasParametersIF.h +++ b/parameters/HasParametersIF.h @@ -1,12 +1,27 @@ -#ifndef HASPARAMETERSIF_H_ -#define HASPARAMETERSIF_H_ +#ifndef FSFW_PARAMETERS_HASPARAMETERSIF_H_ +#define FSFW_PARAMETERS_HASPARAMETERSIF_H_ -#include "ParameterWrapper.h" +#include "../parameters/ParameterWrapper.h" #include "../returnvalues/HasReturnvaluesIF.h" #include +/** Each parameter is identified with a unique parameter ID */ typedef uint32_t ParameterId_t; +/** + * @brief This interface is used by components which have modifiable + * parameters, e.g. atittude controllers + * @details + * Each parameter has a unique parameter ID. The first byte of the parameter + * ID is the domain ID which can be used to identify unqiue spacecraft domains + * (e.g. control and sensor domain in the AOCS controller). + * + * The second and third byte represent the matrix ID, which can represent + * a 8-bit row and column number and the last byte... + * + * Yeah, it it matrix ID oder parameter ID now and is index a 16 bit number + * of a 8 bit number now? + */ class HasParametersIF { public: static const uint8_t INTERFACE_ID = CLASS_ID::HAS_PARAMETERS_IF; @@ -32,13 +47,11 @@ public: return (domainId << 24) + (parameterId << 8) + index; } - virtual ~HasParametersIF() { - } + virtual ~HasParametersIF() {} /** * Always set parameter before checking newValues! * - * * @param domainId * @param parameterId * @param parameterWrapper @@ -51,4 +64,4 @@ public: const ParameterWrapper *newValues, uint16_t startAtIndex) = 0; }; -#endif /* HASPARAMETERSIF_H_ */ +#endif /* FSFW_PARAMETERS_HASPARAMETERSIF_H_ */ diff --git a/parameters/ParameterHelper.cpp b/parameters/ParameterHelper.cpp index 8543515f..659b00de 100644 --- a/parameters/ParameterHelper.cpp +++ b/parameters/ParameterHelper.cpp @@ -1,11 +1,9 @@ -#include "../objectmanager/ObjectManagerIF.h" #include "ParameterHelper.h" #include "ParameterMessage.h" +#include "../objectmanager/ObjectManagerIF.h" ParameterHelper::ParameterHelper(ReceivesParameterMessagesIF* owner) : - owner(owner), storage(NULL) { - -} + owner(owner) {} ParameterHelper::~ParameterHelper() { } @@ -28,7 +26,6 @@ ReturnValue_t ParameterHelper::handleParameterMessage(CommandMessage *message) { } break; case ParameterMessage::CMD_PARAMETER_LOAD: { - uint8_t domain = HasParametersIF::getDomain( ParameterMessage::getParameterId(message)); uint16_t parameterId = HasParametersIF::getMatrixId( @@ -36,12 +33,14 @@ ReturnValue_t ParameterHelper::handleParameterMessage(CommandMessage *message) { uint8_t index = HasParametersIF::getIndex( ParameterMessage::getParameterId(message)); - const uint8_t *storedStream; - size_t storedStreamSize; + const uint8_t *storedStream = nullptr; + size_t storedStreamSize = 0; result = storage->getData( ParameterMessage::getStoreId(message), &storedStream, &storedStreamSize); if (result != HasReturnvaluesIF::RETURN_OK) { + sif::error << "ParameterHelper::handleParameterMessage: Getting" + " store data failed for load command." << std::endl; break; } @@ -125,7 +124,8 @@ ReturnValue_t ParameterHelper::initialize() { } } -void ParameterHelper::rejectCommand(MessageQueueId_t to, ReturnValue_t reason, Command_t initialCommand) { +void ParameterHelper::rejectCommand(MessageQueueId_t to, ReturnValue_t reason, + Command_t initialCommand) { CommandMessage reply; reply.setReplyRejected(reason, initialCommand); MessageQueueSenderIF::sendMessage(to, &reply, ownerQueueId); diff --git a/parameters/ParameterHelper.h b/parameters/ParameterHelper.h index d887bf1d..595b4bc5 100644 --- a/parameters/ParameterHelper.h +++ b/parameters/ParameterHelper.h @@ -1,9 +1,16 @@ -#ifndef PARAMETERHELPER_H_ -#define PARAMETERHELPER_H_ +#ifndef FSFW_PARAMETERS_PARAMETERHELPER_H_ +#define FSFW_PARAMETERS_PARAMETERHELPER_H_ #include "ParameterMessage.h" #include "ReceivesParameterMessagesIF.h" +#include "../ipc/MessageQueueIF.h" +/** + * @brief Helper class to handle parameter messages. + * @details + * This class simplfies handling of parameter messages, which are sent + * to a class which implements ReceivesParameterMessagesIF. + */ class ParameterHelper { public: ParameterHelper(ReceivesParameterMessagesIF *owner); @@ -15,13 +22,15 @@ public: private: ReceivesParameterMessagesIF *owner; - MessageQueueId_t ownerQueueId; + MessageQueueId_t ownerQueueId = MessageQueueIF::NO_QUEUE; - StorageManagerIF *storage; + StorageManagerIF *storage = nullptr; - ReturnValue_t sendParameter(MessageQueueId_t to, uint32_t id, const ParameterWrapper *description); + ReturnValue_t sendParameter(MessageQueueId_t to, uint32_t id, + const ParameterWrapper *description); - void rejectCommand(MessageQueueId_t to, ReturnValue_t reason, Command_t initialCommand); + void rejectCommand(MessageQueueId_t to, ReturnValue_t reason, + Command_t initialCommand); }; -#endif /* PARAMETERHELPER_H_ */ +#endif /* FSFW_PARAMETERS_PARAMETERHELPER_H_ */ diff --git a/parameters/ParameterMessage.cpp b/parameters/ParameterMessage.cpp index 7e0ed114..e9f3191f 100644 --- a/parameters/ParameterMessage.cpp +++ b/parameters/ParameterMessage.cpp @@ -1,4 +1,4 @@ -#include "ParameterMessage.h" +#include "../parameters/ParameterMessage.h" #include "../objectmanager/ObjectManagerIF.h" ParameterId_t ParameterMessage::getParameterId(const CommandMessage* message) { diff --git a/parameters/ParameterMessage.h b/parameters/ParameterMessage.h index f59af513..31d7fe0b 100644 --- a/parameters/ParameterMessage.h +++ b/parameters/ParameterMessage.h @@ -1,8 +1,8 @@ -#ifndef PARAMETERMESSAGE_H_ -#define PARAMETERMESSAGE_H_ +#ifndef FSFW_PARAMETERS_PARAMETERMESSAGE_H_ +#define FSFW_PARAMETERS_PARAMETERMESSAGE_H_ -#include "../ipc/CommandMessage.h" #include "HasParametersIF.h" +#include "../ipc/CommandMessage.h" #include "../storagemanager/StorageManagerIF.h" class ParameterMessage { @@ -26,4 +26,4 @@ public: }; -#endif /* PARAMETERMESSAGE_H_ */ +#endif /* FSFW_PARAMETERS_PARAMETERMESSAGE_H_ */ diff --git a/parameters/ParameterWrapper.cpp b/parameters/ParameterWrapper.cpp index 56f56426..d020da33 100644 --- a/parameters/ParameterWrapper.cpp +++ b/parameters/ParameterWrapper.cpp @@ -1,20 +1,19 @@ #include "ParameterWrapper.h" ParameterWrapper::ParameterWrapper() : - pointsToStream(false), type(Type::UNKNOWN_TYPE), rows(0), columns(0), data( - NULL), readonlyData(NULL) { + pointsToStream(false), type(Type::UNKNOWN_TYPE) { } ParameterWrapper::ParameterWrapper(Type type, uint8_t rows, uint8_t columns, void *data) : - pointsToStream(false), type(type), rows(rows), columns(columns), data( - data), readonlyData(data) { + pointsToStream(false), type(type), rows(rows), columns(columns), + data(data), readonlyData(data) { } ParameterWrapper::ParameterWrapper(Type type, uint8_t rows, uint8_t columns, const void *data) : - pointsToStream(false), type(type), rows(rows), columns(columns), data( - NULL), readonlyData(data) { + pointsToStream(false), type(type), rows(rows), columns(columns), + data(nullptr), readonlyData(data) { } ParameterWrapper::~ParameterWrapper() { @@ -141,6 +140,7 @@ ReturnValue_t ParameterWrapper::deSerializeData(uint8_t startingRow, } + ReturnValue_t ParameterWrapper::deSerialize(const uint8_t **buffer, size_t *size, Endianness streamEndianness) { return deSerialize(buffer, size, streamEndianness, 0); @@ -184,16 +184,16 @@ ReturnValue_t ParameterWrapper::set(const uint8_t *stream, size_t streamSize, return SerializeIF::STREAM_TOO_SHORT; } - data = NULL; + data = nullptr; readonlyData = stream; pointsToStream = true; stream += dataSize; - if (remainingStream != NULL) { + if (remainingStream != nullptr) { *remainingStream = stream; } streamSize -= dataSize; - if (remainingSize != NULL) { + if (remainingSize != nullptr) { *remainingSize = streamSize; } @@ -265,15 +265,14 @@ ReturnValue_t ParameterWrapper::copyFrom(const ParameterWrapper *from, result = UNKNOW_DATATYPE; break; } - } else { + } + else { //need a type to do arithmetic - uint8_t *toDataWithType = (uint8_t*) data; + uint8_t* typedData = static_cast(data); for (uint8_t fromRow = 0; fromRow < from->rows; fromRow++) { - memcpy( - toDataWithType - + (((startingRow + fromRow) * columns) - + startingColumn) * typeSize, - from->readonlyData, typeSize * from->columns); + uint8_t offset = (((startingRow + fromRow) * columns) + startingColumn) * typeSize; + std::memcpy(typedData + offset, from->readonlyData, + typeSize * from->columns); } } diff --git a/parameters/ParameterWrapper.h b/parameters/ParameterWrapper.h index b18fe2b9..f07205d4 100644 --- a/parameters/ParameterWrapper.h +++ b/parameters/ParameterWrapper.h @@ -1,12 +1,16 @@ -#ifndef PARAMETERWRAPPER_H_ -#define PARAMETERWRAPPER_H_ +#ifndef FSFW_PARAMETERS_PARAMETERWRAPPER_H_ +#define FSFW_PARAMETERS_PARAMETERWRAPPER_H_ #include "../returnvalues/HasReturnvaluesIF.h" #include "../serialize/SerializeAdapter.h" #include "../serialize/SerializeIF.h" -#include #include "../globalfunctions/Type.h" +#include +/** + * @brief + * @details + */ class ParameterWrapper: public SerializeIF { friend class DataPoolParameterWrapper; public: @@ -36,32 +40,21 @@ public: virtual ReturnValue_t deSerialize(const uint8_t** buffer, size_t* size, Endianness streamEndianness, uint16_t startWritingAtIndex = 0); + /** + * Get a specific parameter value by supplying the row and the column. + * @tparam T Type of target data + * @param value [out] Pointer to storage location + * @param row + * @param column + * @return + * -@c RETURN_OK if element was retrieved successfully + * -@c NOT_SET data has not been set yet + * -@c DATATYPE_MISSMATCH Invalid supplied type + * -@c OUT_OF_BOUNDS Invalid row and/or column. + */ template - ReturnValue_t getElement(T *value, uint8_t row = 0, uint8_t column = 0) const { - if (readonlyData == NULL){ - return NOT_SET; - } - - if (PodTypeConversion::type != type) { - return DATATYPE_MISSMATCH; - } - - if ((row >= rows) || (column >= columns)) { - return OUT_OF_BOUNDS; - } - - if (pointsToStream) { - const uint8_t *streamWithtype = (const uint8_t *) readonlyData; - streamWithtype += (row * columns + column) * type.getSize(); - int32_t size = type.getSize(); - return SerializeAdapter::deSerialize(value, &streamWithtype, - &size, true); - } else { - const T *dataWithType = (const T *) readonlyData; - *value = dataWithType[row * columns + column]; - return HasReturnvaluesIF::RETURN_OK; - } - } + ReturnValue_t getElement(T *value, uint8_t row = 0, + uint8_t column = 0) const; template void set(T *data, uint8_t rows, uint8_t columns) { @@ -111,21 +104,22 @@ public: void setMatrix(const T& member) { this->set(member[0], sizeof(member)/sizeof(member[0]), sizeof(member[0])/sizeof(member[0][0])); } + ReturnValue_t set(const uint8_t *stream, size_t streamSize, - const uint8_t **remainingStream = NULL, size_t *remainingSize = - NULL); + const uint8_t **remainingStream = nullptr, + size_t *remainingSize = nullptr); ReturnValue_t copyFrom(const ParameterWrapper *from, uint16_t startWritingAtIndex); private: - bool pointsToStream; + bool pointsToStream = false; Type type; - uint8_t rows; - uint8_t columns; - void *data; - const void *readonlyData; + uint8_t rows = 0; + uint8_t columns = 0; + void *data = nullptr; + const void *readonlyData = nullptr; template ReturnValue_t serializeData(uint8_t** buffer, size_t* size, @@ -136,4 +130,33 @@ private: const void *from, uint8_t fromRows, uint8_t fromColumns); }; -#endif /* PARAMETERWRAPPER_H_ */ +template +inline ReturnValue_t ParameterWrapper::getElement(T *value, uint8_t row, + uint8_t column) const { + if (readonlyData == nullptr){ + return NOT_SET; + } + + if (PodTypeConversion::type != type) { + return DATATYPE_MISSMATCH; + } + + if ((row >= rows) or (column >= columns)) { + return OUT_OF_BOUNDS; + } + + if (pointsToStream) { + const uint8_t *streamWithType = static_cast(readonlyData); + streamWithType += (row * columns + column) * type.getSize(); + int32_t size = type.getSize(); + return SerializeAdapter::deSerialize(value, &streamWithType, + &size, true); + } + else { + const T *dataWithType = static_cast(readonlyData); + *value = dataWithType[row * columns + column]; + return HasReturnvaluesIF::RETURN_OK; + } +} + +#endif /* FSFW_PARAMETERS_PARAMETERWRAPPER_H_ */ diff --git a/parameters/ReceivesParameterMessagesIF.h b/parameters/ReceivesParameterMessagesIF.h index 1fcbd4b3..e8c7fa69 100644 --- a/parameters/ReceivesParameterMessagesIF.h +++ b/parameters/ReceivesParameterMessagesIF.h @@ -1,5 +1,5 @@ -#ifndef RECEIVESPARAMETERMESSAGESIF_H_ -#define RECEIVESPARAMETERMESSAGESIF_H_ +#ifndef FSFW_PARAMETERS_RECEIVESPARAMETERMESSAGESIF_H_ +#define FSFW_PARAMETERS_RECEIVESPARAMETERMESSAGESIF_H_ #include "HasParametersIF.h" @@ -16,4 +16,4 @@ public: }; -#endif /* RECEIVESPARAMETERMESSAGESIF_H_ */ +#endif /* FSFW_PARAMETERS_RECEIVESPARAMETERMESSAGESIF_H_ */ From c0ed474373aef91fc12e2dc9b26bc9b8f4b8a4e4 Mon Sep 17 00:00:00 2001 From: gaisser Date: Tue, 1 Sep 2020 11:34:28 +0200 Subject: [PATCH 44/50] Update 'serialize/SerializeIF.h' --- serialize/SerializeIF.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/serialize/SerializeIF.h b/serialize/SerializeIF.h index 2bb17154..7f9ea9df 100644 --- a/serialize/SerializeIF.h +++ b/serialize/SerializeIF.h @@ -5,13 +5,13 @@ #include /** - * \defgroup serialize Serialization + * @defgroup serialize Serialization * Contains serialization services. */ /** * Translation of objects into data streams and from data streams. - * \ingroup serialize + * @ingroup serialize */ class SerializeIF { public: @@ -51,7 +51,7 @@ public: size_t maxSize, Endianness streamEndianness) const = 0; /** - * Gets the size of a object if it would be serialized in a buffer.s + * Gets the size of a object if it would be serialized in a buffer * @return Size of serialized object */ virtual size_t getSerializedSize() const = 0; From a4d9c761a78831a963c06c5969410daedd48e4dd Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Tue, 1 Sep 2020 11:49:43 +0200 Subject: [PATCH 45/50] renormalize CHB --- contrib/sgp4/sgp4unit.cpp | 4180 +++++++++++++-------------- contrib/sgp4/sgp4unit.h | 234 +- devicehandlers/ChildHandlerBase.cpp | 90 +- devicehandlers/ChildHandlerBase.h | 48 +- osal/FreeRTOS/TaskManagement.cpp | 48 +- osal/FreeRTOS/TaskManagement.h | 128 +- serialize/SerialBufferAdapter.cpp | 258 +- serialize/SerialBufferAdapter.h | 156 +- 8 files changed, 2571 insertions(+), 2571 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/devicehandlers/ChildHandlerBase.cpp b/devicehandlers/ChildHandlerBase.cpp index f18c3812..e800cd56 100644 --- a/devicehandlers/ChildHandlerBase.cpp +++ b/devicehandlers/ChildHandlerBase.cpp @@ -1,45 +1,45 @@ -#include "../subsystem/SubsystemBase.h" -#include "../devicehandlers/ChildHandlerBase.h" -#include "../subsystem/SubsystemBase.h" - -ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId, - object_id_t deviceCommunication, CookieIF * cookie, - uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, - object_id_t parent, FailureIsolationBase* customFdir, - size_t cmdQueueSize) : - DeviceHandlerBase(setObjectId, deviceCommunication, cookie, - (customFdir == nullptr? &childHandlerFdir : customFdir), - cmdQueueSize), - parentId(parent), childHandlerFdir(setObjectId) { - this->setThermalStateRequestPoolIds(thermalStatePoolId, - thermalRequestPoolId); - -} - -ChildHandlerBase::~ChildHandlerBase() { -} - -ReturnValue_t ChildHandlerBase::initialize() { - ReturnValue_t result = DeviceHandlerBase::initialize(); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - - MessageQueueId_t parentQueue = 0; - - if (parentId != objects::NO_OBJECT) { - SubsystemBase *parent = objectManager->get(parentId); - if (parent == NULL) { - return RETURN_FAILED; - } - parentQueue = parent->getCommandQueue(); - - parent->registerChild(getObjectId()); - } - - healthHelper.setParentQueue(parentQueue); - - modeHelper.setParentQueue(parentQueue); - - return RETURN_OK; -} +#include "../subsystem/SubsystemBase.h" +#include "../devicehandlers/ChildHandlerBase.h" +#include "../subsystem/SubsystemBase.h" + +ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId, + object_id_t deviceCommunication, CookieIF * cookie, + uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, + object_id_t parent, FailureIsolationBase* customFdir, + size_t cmdQueueSize) : + DeviceHandlerBase(setObjectId, deviceCommunication, cookie, + (customFdir == nullptr? &childHandlerFdir : customFdir), + cmdQueueSize), + parentId(parent), childHandlerFdir(setObjectId) { + this->setThermalStateRequestPoolIds(thermalStatePoolId, + thermalRequestPoolId); + +} + +ChildHandlerBase::~ChildHandlerBase() { +} + +ReturnValue_t ChildHandlerBase::initialize() { + ReturnValue_t result = DeviceHandlerBase::initialize(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + MessageQueueId_t parentQueue = 0; + + if (parentId != objects::NO_OBJECT) { + SubsystemBase *parent = objectManager->get(parentId); + if (parent == NULL) { + return RETURN_FAILED; + } + parentQueue = parent->getCommandQueue(); + + parent->registerChild(getObjectId()); + } + + healthHelper.setParentQueue(parentQueue); + + modeHelper.setParentQueue(parentQueue); + + return RETURN_OK; +} diff --git a/devicehandlers/ChildHandlerBase.h b/devicehandlers/ChildHandlerBase.h index 3e9652a3..f6bf318a 100644 --- a/devicehandlers/ChildHandlerBase.h +++ b/devicehandlers/ChildHandlerBase.h @@ -1,26 +1,26 @@ #ifndef FSFW_DEVICES_CHILDHANDLERBASE_H_ -#define FSFW_DEVICES_CHILDHANDLERBASE_H_ - -#include "ChildHandlerFDIR.h" -#include "DeviceHandlerBase.h" - -class ChildHandlerBase: public DeviceHandlerBase { -public: - ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, - CookieIF * cookie, uint32_t thermalStatePoolId, - uint32_t thermalRequestPoolId, - object_id_t parent = objects::NO_OBJECT, - FailureIsolationBase* customFdir = nullptr, - size_t cmdQueueSize = 20); - virtual ~ChildHandlerBase(); - - virtual ReturnValue_t initialize(); - -protected: - const uint32_t parentId; - ChildHandlerFDIR childHandlerFdir; - -}; - -#endif /* FSFW_DEVICES_CHILDHANDLERBASE_H_ */ +#define FSFW_DEVICES_CHILDHANDLERBASE_H_ + +#include "ChildHandlerFDIR.h" +#include "DeviceHandlerBase.h" + +class ChildHandlerBase: public DeviceHandlerBase { +public: + ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, + CookieIF * cookie, uint32_t thermalStatePoolId, + uint32_t thermalRequestPoolId, + object_id_t parent = objects::NO_OBJECT, + FailureIsolationBase* customFdir = nullptr, + size_t cmdQueueSize = 20); + virtual ~ChildHandlerBase(); + + virtual ReturnValue_t initialize(); + +protected: + const uint32_t parentId; + ChildHandlerFDIR childHandlerFdir; + +}; + +#endif /* FSFW_DEVICES_CHILDHANDLERBASE_H_ */ 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/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_ */ From 4abdc436fbd995ada2090bbb4ce4cc2b09c147bf Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Tue, 1 Sep 2020 11:51:43 +0200 Subject: [PATCH 46/50] Revert "renormalize CHB" This reverts commit a4d9c761a78831a963c06c5969410daedd48e4dd. --- contrib/sgp4/sgp4unit.cpp | 4180 +++++++++++++-------------- contrib/sgp4/sgp4unit.h | 234 +- devicehandlers/ChildHandlerBase.cpp | 90 +- devicehandlers/ChildHandlerBase.h | 44 +- osal/FreeRTOS/TaskManagement.cpp | 48 +- osal/FreeRTOS/TaskManagement.h | 128 +- serialize/SerialBufferAdapter.cpp | 258 +- serialize/SerialBufferAdapter.h | 156 +- 8 files changed, 2569 insertions(+), 2569 deletions(-) diff --git a/contrib/sgp4/sgp4unit.cpp b/contrib/sgp4/sgp4unit.cpp index b707a7a2..24d63aa9 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 f16acbd9..48c113d7 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/devicehandlers/ChildHandlerBase.cpp b/devicehandlers/ChildHandlerBase.cpp index e800cd56..f18c3812 100644 --- a/devicehandlers/ChildHandlerBase.cpp +++ b/devicehandlers/ChildHandlerBase.cpp @@ -1,45 +1,45 @@ -#include "../subsystem/SubsystemBase.h" -#include "../devicehandlers/ChildHandlerBase.h" -#include "../subsystem/SubsystemBase.h" - -ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId, - object_id_t deviceCommunication, CookieIF * cookie, - uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, - object_id_t parent, FailureIsolationBase* customFdir, - size_t cmdQueueSize) : - DeviceHandlerBase(setObjectId, deviceCommunication, cookie, - (customFdir == nullptr? &childHandlerFdir : customFdir), - cmdQueueSize), - parentId(parent), childHandlerFdir(setObjectId) { - this->setThermalStateRequestPoolIds(thermalStatePoolId, - thermalRequestPoolId); - -} - -ChildHandlerBase::~ChildHandlerBase() { -} - -ReturnValue_t ChildHandlerBase::initialize() { - ReturnValue_t result = DeviceHandlerBase::initialize(); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - - MessageQueueId_t parentQueue = 0; - - if (parentId != objects::NO_OBJECT) { - SubsystemBase *parent = objectManager->get(parentId); - if (parent == NULL) { - return RETURN_FAILED; - } - parentQueue = parent->getCommandQueue(); - - parent->registerChild(getObjectId()); - } - - healthHelper.setParentQueue(parentQueue); - - modeHelper.setParentQueue(parentQueue); - - return RETURN_OK; -} +#include "../subsystem/SubsystemBase.h" +#include "../devicehandlers/ChildHandlerBase.h" +#include "../subsystem/SubsystemBase.h" + +ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId, + object_id_t deviceCommunication, CookieIF * cookie, + uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, + object_id_t parent, FailureIsolationBase* customFdir, + size_t cmdQueueSize) : + DeviceHandlerBase(setObjectId, deviceCommunication, cookie, + (customFdir == nullptr? &childHandlerFdir : customFdir), + cmdQueueSize), + parentId(parent), childHandlerFdir(setObjectId) { + this->setThermalStateRequestPoolIds(thermalStatePoolId, + thermalRequestPoolId); + +} + +ChildHandlerBase::~ChildHandlerBase() { +} + +ReturnValue_t ChildHandlerBase::initialize() { + ReturnValue_t result = DeviceHandlerBase::initialize(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + MessageQueueId_t parentQueue = 0; + + if (parentId != objects::NO_OBJECT) { + SubsystemBase *parent = objectManager->get(parentId); + if (parent == NULL) { + return RETURN_FAILED; + } + parentQueue = parent->getCommandQueue(); + + parent->registerChild(getObjectId()); + } + + healthHelper.setParentQueue(parentQueue); + + modeHelper.setParentQueue(parentQueue); + + return RETURN_OK; +} diff --git a/devicehandlers/ChildHandlerBase.h b/devicehandlers/ChildHandlerBase.h index f6bf318a..3e9652a3 100644 --- a/devicehandlers/ChildHandlerBase.h +++ b/devicehandlers/ChildHandlerBase.h @@ -1,26 +1,26 @@ #ifndef FSFW_DEVICES_CHILDHANDLERBASE_H_ -#define FSFW_DEVICES_CHILDHANDLERBASE_H_ - -#include "ChildHandlerFDIR.h" -#include "DeviceHandlerBase.h" - -class ChildHandlerBase: public DeviceHandlerBase { -public: - ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, - CookieIF * cookie, uint32_t thermalStatePoolId, +#define FSFW_DEVICES_CHILDHANDLERBASE_H_ + +#include "ChildHandlerFDIR.h" +#include "DeviceHandlerBase.h" + +class ChildHandlerBase: public DeviceHandlerBase { +public: + ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, + CookieIF * cookie, uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, - object_id_t parent = objects::NO_OBJECT, + object_id_t parent = objects::NO_OBJECT, FailureIsolationBase* customFdir = nullptr, - size_t cmdQueueSize = 20); - virtual ~ChildHandlerBase(); - - virtual ReturnValue_t initialize(); - -protected: - const uint32_t parentId; - ChildHandlerFDIR childHandlerFdir; - -}; - -#endif /* FSFW_DEVICES_CHILDHANDLERBASE_H_ */ + size_t cmdQueueSize = 20); + virtual ~ChildHandlerBase(); + + virtual ReturnValue_t initialize(); + +protected: + const uint32_t parentId; + ChildHandlerFDIR childHandlerFdir; + +}; + +#endif /* FSFW_DEVICES_CHILDHANDLERBASE_H_ */ diff --git a/osal/FreeRTOS/TaskManagement.cpp b/osal/FreeRTOS/TaskManagement.cpp index b77f12a9..16682d36 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 4b7fe3eb..43003d76 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/serialize/SerialBufferAdapter.cpp b/serialize/SerialBufferAdapter.cpp index 1c11afd4..812cd34b 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 9a89e18b..c3dfcd8f 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_ */ From b07b19327cf1be8059e2c7635a22118ef2e1fd90 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Tue, 1 Sep 2020 11:52:18 +0200 Subject: [PATCH 47/50] renormalize CHB --- devicehandlers/ChildHandlerBase.cpp | 90 ++++++++++++++--------------- devicehandlers/ChildHandlerBase.h | 48 +++++++-------- 2 files changed, 69 insertions(+), 69 deletions(-) diff --git a/devicehandlers/ChildHandlerBase.cpp b/devicehandlers/ChildHandlerBase.cpp index f18c3812..e800cd56 100644 --- a/devicehandlers/ChildHandlerBase.cpp +++ b/devicehandlers/ChildHandlerBase.cpp @@ -1,45 +1,45 @@ -#include "../subsystem/SubsystemBase.h" -#include "../devicehandlers/ChildHandlerBase.h" -#include "../subsystem/SubsystemBase.h" - -ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId, - object_id_t deviceCommunication, CookieIF * cookie, - uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, - object_id_t parent, FailureIsolationBase* customFdir, - size_t cmdQueueSize) : - DeviceHandlerBase(setObjectId, deviceCommunication, cookie, - (customFdir == nullptr? &childHandlerFdir : customFdir), - cmdQueueSize), - parentId(parent), childHandlerFdir(setObjectId) { - this->setThermalStateRequestPoolIds(thermalStatePoolId, - thermalRequestPoolId); - -} - -ChildHandlerBase::~ChildHandlerBase() { -} - -ReturnValue_t ChildHandlerBase::initialize() { - ReturnValue_t result = DeviceHandlerBase::initialize(); - if (result != HasReturnvaluesIF::RETURN_OK) { - return result; - } - - MessageQueueId_t parentQueue = 0; - - if (parentId != objects::NO_OBJECT) { - SubsystemBase *parent = objectManager->get(parentId); - if (parent == NULL) { - return RETURN_FAILED; - } - parentQueue = parent->getCommandQueue(); - - parent->registerChild(getObjectId()); - } - - healthHelper.setParentQueue(parentQueue); - - modeHelper.setParentQueue(parentQueue); - - return RETURN_OK; -} +#include "../subsystem/SubsystemBase.h" +#include "../devicehandlers/ChildHandlerBase.h" +#include "../subsystem/SubsystemBase.h" + +ChildHandlerBase::ChildHandlerBase(object_id_t setObjectId, + object_id_t deviceCommunication, CookieIF * cookie, + uint32_t thermalStatePoolId, uint32_t thermalRequestPoolId, + object_id_t parent, FailureIsolationBase* customFdir, + size_t cmdQueueSize) : + DeviceHandlerBase(setObjectId, deviceCommunication, cookie, + (customFdir == nullptr? &childHandlerFdir : customFdir), + cmdQueueSize), + parentId(parent), childHandlerFdir(setObjectId) { + this->setThermalStateRequestPoolIds(thermalStatePoolId, + thermalRequestPoolId); + +} + +ChildHandlerBase::~ChildHandlerBase() { +} + +ReturnValue_t ChildHandlerBase::initialize() { + ReturnValue_t result = DeviceHandlerBase::initialize(); + if (result != HasReturnvaluesIF::RETURN_OK) { + return result; + } + + MessageQueueId_t parentQueue = 0; + + if (parentId != objects::NO_OBJECT) { + SubsystemBase *parent = objectManager->get(parentId); + if (parent == NULL) { + return RETURN_FAILED; + } + parentQueue = parent->getCommandQueue(); + + parent->registerChild(getObjectId()); + } + + healthHelper.setParentQueue(parentQueue); + + modeHelper.setParentQueue(parentQueue); + + return RETURN_OK; +} diff --git a/devicehandlers/ChildHandlerBase.h b/devicehandlers/ChildHandlerBase.h index 3e9652a3..f6bf318a 100644 --- a/devicehandlers/ChildHandlerBase.h +++ b/devicehandlers/ChildHandlerBase.h @@ -1,26 +1,26 @@ #ifndef FSFW_DEVICES_CHILDHANDLERBASE_H_ -#define FSFW_DEVICES_CHILDHANDLERBASE_H_ - -#include "ChildHandlerFDIR.h" -#include "DeviceHandlerBase.h" - -class ChildHandlerBase: public DeviceHandlerBase { -public: - ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, - CookieIF * cookie, uint32_t thermalStatePoolId, - uint32_t thermalRequestPoolId, - object_id_t parent = objects::NO_OBJECT, - FailureIsolationBase* customFdir = nullptr, - size_t cmdQueueSize = 20); - virtual ~ChildHandlerBase(); - - virtual ReturnValue_t initialize(); - -protected: - const uint32_t parentId; - ChildHandlerFDIR childHandlerFdir; - -}; - -#endif /* FSFW_DEVICES_CHILDHANDLERBASE_H_ */ +#define FSFW_DEVICES_CHILDHANDLERBASE_H_ + +#include "ChildHandlerFDIR.h" +#include "DeviceHandlerBase.h" + +class ChildHandlerBase: public DeviceHandlerBase { +public: + ChildHandlerBase(object_id_t setObjectId, object_id_t deviceCommunication, + CookieIF * cookie, uint32_t thermalStatePoolId, + uint32_t thermalRequestPoolId, + object_id_t parent = objects::NO_OBJECT, + FailureIsolationBase* customFdir = nullptr, + size_t cmdQueueSize = 20); + virtual ~ChildHandlerBase(); + + virtual ReturnValue_t initialize(); + +protected: + const uint32_t parentId; + ChildHandlerFDIR childHandlerFdir; + +}; + +#endif /* FSFW_DEVICES_CHILDHANDLERBASE_H_ */ From 695a4e7842f367d35c532f921cea1ad5ffb41dba Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Tue, 1 Sep 2020 12:42:52 +0200 Subject: [PATCH 48/50] offset is size_t now --- parameters/ParameterWrapper.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/parameters/ParameterWrapper.cpp b/parameters/ParameterWrapper.cpp index d020da33..6adc6857 100644 --- a/parameters/ParameterWrapper.cpp +++ b/parameters/ParameterWrapper.cpp @@ -270,7 +270,8 @@ ReturnValue_t ParameterWrapper::copyFrom(const ParameterWrapper *from, //need a type to do arithmetic uint8_t* typedData = static_cast(data); for (uint8_t fromRow = 0; fromRow < from->rows; fromRow++) { - uint8_t offset = (((startingRow + fromRow) * columns) + startingColumn) * typeSize; + size_t offset = (((startingRow + fromRow) * columns) + + startingColumn) * typeSize; std::memcpy(typedData + offset, from->readonlyData, typeSize * from->columns); } From b522b3c29c5d1490c8b8d0fd2dc4209fd92cd9b9 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Tue, 1 Sep 2020 12:53:53 +0200 Subject: [PATCH 49/50] fifo updates --- container/DynamicFIFO.h | 4 ++-- container/FIFO.h | 12 +++++++----- container/FIFOBase.h | 2 +- container/FIFOBase.tpp | 2 +- 4 files changed, 11 insertions(+), 9 deletions(-) diff --git a/container/DynamicFIFO.h b/container/DynamicFIFO.h index 7fa0c32c..abb53330 100644 --- a/container/DynamicFIFO.h +++ b/container/DynamicFIFO.h @@ -22,7 +22,7 @@ public: // trying to pass the pointer of the uninitialized vector // to the FIFOBase constructor directly lead to a super evil bug. // So we do it like this now. - this->setData(fifoVector.data()); + this->setContainer(fifoVector.data()); }; /** @@ -31,7 +31,7 @@ public: */ DynamicFIFO(const DynamicFIFO& other): FIFOBase(other), fifoVector(other.maxCapacity) { - this->setData(fifoVector.data()); + this->setContainer(fifoVector.data()); } diff --git a/container/FIFO.h b/container/FIFO.h index e60a4979..19f763fc 100644 --- a/container/FIFO.h +++ b/container/FIFO.h @@ -1,5 +1,5 @@ -#ifndef FRAMEWORK_CONTAINER_FIFO_H_ -#define FRAMEWORK_CONTAINER_FIFO_H_ +#ifndef FSFW_CONTAINER_FIFO_H_ +#define FSFW_CONTAINER_FIFO_H_ #include "FIFOBase.h" #include @@ -16,18 +16,20 @@ template class FIFO: public FIFOBase { public: - FIFO(): FIFOBase(fifoArray.data(), capacity) {}; + FIFO(): FIFOBase(nullptr, capacity) { + this->setContainer(fifoArray.data()); + }; /** * @brief Custom copy constructor to set pointer correctly. * @param other */ FIFO(const FIFO& other): FIFOBase(other) { - this->setData(fifoArray.data()); + this->setContainer(fifoArray.data()); } private: std::array fifoArray; }; -#endif /* FRAMEWORK_CONTAINERS_STATICFIFO_H_ */ +#endif /* FSFW_CONTAINER_FIFO_H_ */ diff --git a/container/FIFOBase.h b/container/FIFOBase.h index bbabd67c..b744706d 100644 --- a/container/FIFOBase.h +++ b/container/FIFOBase.h @@ -48,7 +48,7 @@ public: size_t getMaxCapacity() const; protected: - void setData(T* data); + void setContainer(T* data); size_t maxCapacity = 0; T* values; diff --git a/container/FIFOBase.tpp b/container/FIFOBase.tpp index e1287dcf..d54b3f8f 100644 --- a/container/FIFOBase.tpp +++ b/container/FIFOBase.tpp @@ -80,7 +80,7 @@ inline size_t FIFOBase::getMaxCapacity() const { template -inline void FIFOBase::setData(T *data) { +inline void FIFOBase::setContainer(T *data) { this->values = data; } From 03b2ca679d81d15d0c449e9c44b515647c6623c2 Mon Sep 17 00:00:00 2001 From: "Robin.Mueller" Date: Tue, 1 Sep 2020 12:58:29 +0200 Subject: [PATCH 50/50] include guard correction --- tmtcservices/CommandingServiceBase.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/tmtcservices/CommandingServiceBase.h b/tmtcservices/CommandingServiceBase.h index a774e7ae..864a0614 100644 --- a/tmtcservices/CommandingServiceBase.h +++ b/tmtcservices/CommandingServiceBase.h @@ -1,5 +1,5 @@ -#ifndef FRAMEWORK_TMTCSERVICES_COMMANDINGSERVICEBASE_H_ -#define FRAMEWORK_TMTCSERVICES_COMMANDINGSERVICEBASE_H_ +#ifndef FSFW_TMTCSERVICES_COMMANDINGSERVICEBASE_H_ +#define FSFW_TMTCSERVICES_COMMANDINGSERVICEBASE_H_ #include "../objectmanager/SystemObject.h" #include "../storagemanager/StorageManagerIF.h" @@ -345,4 +345,4 @@ private: void checkTimeout(); }; -#endif /* FRAMEWORK_TMTCSERVICES_COMMANDINGSERVICEBASE_H_ */ +#endif /* FSFW_TMTCSERVICES_COMMANDINGSERVICEBASE_H_ */