From 3fe46abf269ea3dea05b25dcb64898e5fb67e880 Mon Sep 17 00:00:00 2001 From: ydzulynsky Date: Mon, 18 Mar 2024 20:44:58 -0400 Subject: [PATCH] Worked on improving clarity via naming & small updates --- TelemetryManager/Inc/CircularBuffer.hpp | 11 +- ...mms.hpp => GroundStationCommunication.hpp} | 25 ++-- ...nagerComs.hpp => ManagerCommunication.hpp} | 0 TelemetryManager/Inc/MavlinkTranslator.hpp | 5 +- TelemetryManager/Inc/TM.hpp | 101 ----------------- TelemetryManager/Inc/TelemetryManager.hpp | 61 ++++++++++ .../{TimerInterrupt.hpp => TelemetryTask.hpp} | 34 +++--- ...mms.cpp => GroundStationCommunication.cpp} | 12 +- ...nagerComs.cpp => ManagerCommunication.cpp} | 0 TelemetryManager/Src/TM.cpp | 107 ------------------ TelemetryManager/Src/TelemetryManager.cpp | 106 +++++++++++++++++ TelemetryManager/Src/TelemetryTask.cpp | 18 +++ TelemetryManager/Src/TimerInterrupt.cpp | 21 ---- 13 files changed, 223 insertions(+), 278 deletions(-) rename TelemetryManager/Inc/{GroundStationComms.hpp => GroundStationCommunication.hpp} (75%) rename TelemetryManager/Inc/{ManagerComs.hpp => ManagerCommunication.hpp} (100%) delete mode 100644 TelemetryManager/Inc/TM.hpp create mode 100644 TelemetryManager/Inc/TelemetryManager.hpp rename TelemetryManager/Inc/{TimerInterrupt.hpp => TelemetryTask.hpp} (61%) rename TelemetryManager/Src/{GroundStationComms.cpp => GroundStationCommunication.cpp} (66%) rename TelemetryManager/Src/{ManagerComs.cpp => ManagerCommunication.cpp} (100%) delete mode 100644 TelemetryManager/Src/TM.cpp create mode 100644 TelemetryManager/Src/TelemetryManager.cpp create mode 100644 TelemetryManager/Src/TelemetryTask.cpp delete mode 100644 TelemetryManager/Src/TimerInterrupt.cpp diff --git a/TelemetryManager/Inc/CircularBuffer.hpp b/TelemetryManager/Inc/CircularBuffer.hpp index b49c53a2..11247644 100644 --- a/TelemetryManager/Inc/CircularBuffer.hpp +++ b/TelemetryManager/Inc/CircularBuffer.hpp @@ -32,27 +32,26 @@ class CircularBuffer { */ ~CircularBuffer(); - public: /** * @brief Provides the number of bytes available in the queue * * @return int The number of bytes available in the queue. */ - virtual int remainingMemory(); + int remainingMemory(); /** * @brief Dequeue a byte from the queue * * @return MAVLinkByte The byte that was dequeued */ - virtual MAVLinkByte dequeue(); + MAVLinkByte dequeue(); /** * @brief Enqueue a byte into the queue * * @param byte The byte to be enqueued */ - virtual void enqueue(MAVLinkByte byte); + void enqueue(MAVLinkByte byte); /** * @brief Get the index of the last full message in the queue determined by the end flag @@ -65,7 +64,7 @@ class CircularBuffer { * in the MAVLink message. * */ - virtual int lastFullMessageEndIndex(); + int lastFullMessageEndIndex(); /** * @brief Returns the index of the current byte in the queue. This is useful for when we want to @@ -75,7 +74,7 @@ class CircularBuffer { * * @return int The index of the current byte in the queue. */ - virtual int currentIndex(); + int currentIndex(); }; #endif diff --git a/TelemetryManager/Inc/GroundStationComms.hpp b/TelemetryManager/Inc/GroundStationCommunication.hpp similarity index 75% rename from TelemetryManager/Inc/GroundStationComms.hpp rename to TelemetryManager/Inc/GroundStationCommunication.hpp index 2548a3f0..0194553f 100644 --- a/TelemetryManager/Inc/GroundStationComms.hpp +++ b/TelemetryManager/Inc/GroundStationCommunication.hpp @@ -1,5 +1,5 @@ /** - * @file GroundStationComms.hpp + * @file GroundStationCommunication.hpp * @brief What does this file do? * * @note Anything future maintainers should know about this file? @@ -7,14 +7,14 @@ * @version 1.0 * @date 2023-08-24 * @author Yarema Dzulynsky: initial structure - * @author Hunter Adams: implementation + * @author Roni Kant: implementation * * @warning Any issues you think are important/foresee in the future? */ #include "CircularBuffer.hpp" -#ifndef GROUNDSTATIONCOMMS_H -#define GROUNDSTATIONCOMMS_H +#ifndef GROUNDSTATIONCOMMUNICATION_H +#define GROUNDSTATIONCOMMUNICATION_H /** * @brief This class is responsible for handling the communication between the ground station @@ -23,26 +23,25 @@ * because it allows us to define the behaviour of the RFD 900 send/receive function before its * implementation. */ -class GroundStationComms { +class GroundStationCommunication { public: /** - * @brief Construct a new Ground Station Comms object. Do whatever needs to be done here. + * @brief Construct a new Ground Station Communication object. Do whatever needs to be done here. * */ - GroundStationComms(); - ~GroundStationComms(); + GroundStationCommunication(); + ~GroundStationCommunication(); - public: /* * When the DMA interrupt is triggered the data should be stored in the DMAReceiveBuffer * IF there is space. */ CircularBuffer DMAReceiveBuffer; - // low priority Mavlink bytes to be sent to the ground station. + // low priority/Non routine Mavlink bytes to be sent to the ground station. CircularBuffer lowPriorityTransmitBuffer; - // high priority Mavlink bytes to be sent to the ground station. + // high priority/Routine Mavlink bytes to be sent to the ground station. CircularBuffer highPriorityTransmitBuffer; /** @@ -57,14 +56,14 @@ class GroundStationComms { * @param transmissionBuffer A CircularBuffer containing the data/MAVLink bytes to be sent * to the ground station */ - virtual void sendToGroundStation(CircularBuffer &transmissionBuffer); + void transmit(CircularBuffer &transmissionBuffer); /** * @brief This is the Interrupt Service Routine (ISR) for when the RFD 900 receives data from * the ground station and stores bytes from the transmission into the GSC.DMAReceiveBuffer if * there is space. Otherwise the data is discarded. */ - virtual void receiveFromGroundStationISR(); + void receiveInterruptServiceRoutine(); }; #endif // GROUNDSTATIONCOMMS_H diff --git a/TelemetryManager/Inc/ManagerComs.hpp b/TelemetryManager/Inc/ManagerCommunication.hpp similarity index 100% rename from TelemetryManager/Inc/ManagerComs.hpp rename to TelemetryManager/Inc/ManagerCommunication.hpp diff --git a/TelemetryManager/Inc/MavlinkTranslator.hpp b/TelemetryManager/Inc/MavlinkTranslator.hpp index 08f18d25..359e0bd8 100644 --- a/TelemetryManager/Inc/MavlinkTranslator.hpp +++ b/TelemetryManager/Inc/MavlinkTranslator.hpp @@ -31,7 +31,6 @@ class MavlinkTranslator { */ ~MavlinkTranslator(); - public: /** * @brief Ingests bytes from the rxFromGroundByteQueue and converts them to Mavlink messages. * The Mavlink message, associated callbacks are then triggered. @@ -39,7 +38,7 @@ class MavlinkTranslator { * @param rxFromGroundByteQueue The CircularBuffer containing the bytes received from the ground * station. waiting to be decoded. */ - virtual void bytesToMavlinkMsg(CircularBuffer &rxFromGroundByteQueue); + void bytesToMavlinkMsg(CircularBuffer &rxFromGroundByteQueue); /** * @brief Converts Mavlink messages to bytes and adds them to the txToGroundByteQueue. @@ -48,7 +47,7 @@ class MavlinkTranslator { * @param txToGroundByteQueue The CircularBuffer containing the bytes to be sent to the ground * station. */ - virtual void addMavlinkMsgToByteQueue(mavlink_message_t &msg, + void addMavlinkMsgToByteQueue(mavlink_message_t &msg, CircularBuffer &txToGroundByteQueue); }; diff --git a/TelemetryManager/Inc/TM.hpp b/TelemetryManager/Inc/TM.hpp deleted file mode 100644 index b934cc12..00000000 --- a/TelemetryManager/Inc/TM.hpp +++ /dev/null @@ -1,101 +0,0 @@ -/** - * @file TM.hpp - * @brief What does this file do? - * - * @note Anything future maintainers should know about this file? - * - * @version 1.0 - * @date 2023-08-24 - * @author Yarema Dzulynsky: initial structure & Implementation - * - * @warning Any issues you think are important/foresee in the future? - */ - -#ifndef TM_H -#define TM_H - -/** - * @brief This is essentially a compatibility/wrapper function that allows us to turn - * a member function into a static function. This is necessary because FreeRTOS tasks - * can only take static functions as arguments. - * @param memberFunction - The member function to be turned into a static function. - * Note that the Macro simply takes the name in ANSI text of the member function and - * appends "Static" to it. All you need to do is pass the name of the member function. - */ -#define TRANSLATE_TO_STATIC(memberFunction) \ - static void memberFunction##Static(void* pvParameters) { \ - auto* tmInstance = static_cast(pvParameters); \ - tmInstance->memberFunction(); \ - } - -#include "GroundStationComms.hpp" -#include "MavlinkTranslator.hpp" -#include "TimerInterrupt.hpp" - -class TelemetryManager { - - public: - /** - * @brief Construct a new Telemetry Manager object. Does not initialize the threads or timer - * interrupts. To do so call the init() method. - */ - TelemetryManager(); - ~TelemetryManager(); - - public: - GroundStationComms GSC; - MavlinkTranslator MT; - - /** - * @brief Initialize TM threads and timer interrupts. - * - */ - void init(); - - /** - * @brief This thread is responsible for taking the bytes from the GSC.DMAReceiveBuffer and - * converting them to Mavlink messages/triggering the callbacks associated with each Mavlink - * message. - */ - void translateToMavlinkThread(); - - /* - Create a static version of the translateToMavlinkThread function to be used as a callback for - the FreeRTOS task. - */ - TRANSLATE_TO_STATIC(translateToMavlinkThread); - - /** - * @brief This thread is responsible for taking data from other managers and converting - * them to Mavlink bytes, then putting them into GSC.lowPriorityTransmitBuffer. - */ - void mavlinkToBytesThread(); - - /* - Create a static version of the mavlinkToBytesThread function to be used as a callback for the - FreeRTOS task. - */ - TRANSLATE_TO_STATIC(mavlinkToBytesThread); - - /** - * @brief This method is responsible for - * sending non routine data to the ground station. Such as arm disarmed message status, - * fulfilling data requests from the ground station etc. This is the lowest priority data - * in the GSC.lowPriorityTransmitBuffer. - */ - void sendLowPriorityData(); - - /** - * @brief Create the timer interrupts for the TelemetryManager. - * - */ - void initTimerInterrupts(); - - /** - * @brief Create and configure the threads for the TelemetryManager. - * - */ - void initTasks(); -}; - -#endif diff --git a/TelemetryManager/Inc/TelemetryManager.hpp b/TelemetryManager/Inc/TelemetryManager.hpp new file mode 100644 index 00000000..6b12d398 --- /dev/null +++ b/TelemetryManager/Inc/TelemetryManager.hpp @@ -0,0 +1,61 @@ +/** + * @file TelemetryManager.hpp + * @brief What does this file do? + * + * @note Anything future maintainers should know about this file? + * + * @version 1.0 + * @date 2023-08-24 + * @author Yarema Dzulynsky: initial structure & Implementation + * + * @warning Any issues you think are important/foresee in the future? + */ + +#ifndef TELEMETRYMANAGER_H +#define TELEMETRYMANAGER_H + +#include "GroundStationCommunication.hpp" +#include "MavlinkTranslator.hpp" +#include "TelemetryTask.hpp" + +class TelemetryManager { + + public: + /** + * @brief Construct a new Telemetry Manager object. Does not initialize the tasks. + * To do so call the init() method. + */ + TelemetryManager(); + ~TelemetryManager(); + + GroundStationCommunication GSC; + MavlinkTranslator MT; + + /** + * @brief Initialize TM. + * + */ + void init(); + + /** + * @brief This method is responsible for + * sending non routine data to the ground station. Such as arm disarmed message status, + * fulfilling data requests from the ground station etc. This is the lowest priority data + * in the GSC.lowPriorityTransmitBuffer. + */ + void transmitNonRoutineData(); + + /** + * @brief Create and configure FreeRTOS tasks. + * + */ + void spinUpTasks(); + + /** + * @brief Destroy FreeRTOS tasks and free up memory. + * + */ + void teardownTasks(); +}; + +#endif diff --git a/TelemetryManager/Inc/TimerInterrupt.hpp b/TelemetryManager/Inc/TelemetryTask.hpp similarity index 61% rename from TelemetryManager/Inc/TimerInterrupt.hpp rename to TelemetryManager/Inc/TelemetryTask.hpp index b868c700..0d4e220f 100644 --- a/TelemetryManager/Inc/TimerInterrupt.hpp +++ b/TelemetryManager/Inc/TelemetryTask.hpp @@ -1,5 +1,5 @@ /** - * @file TimerInterrupt.hpp + * @file TelemetryTask.hpp * @brief What does this file do? * * @note Anything future maintainers should know about this file? @@ -8,21 +8,20 @@ * @version 1.0 * @date 2023-08-24 * @author Yarema Dzulynsky: initial structure & implementation - * @author Rahul Ramkumar: implementation * @author Derek Tang: implementation * * @warning Any issues you think are important/foresee in the future? */ -#include +#include "TelemetryManager.hpp" +#include #include "FreeRTOS.h" -#include "TM.hpp" #include "task.h" -#ifndef TIMERINTURUPT_H -#define TIMERINTURUPT_H +#ifndef TELEMETRYTASK_H +#define TELEMETRYTASK_H -class TimerInterrupt { +class TelemetryTask { // Simple alias for a lambda function that takes a TelemetryManager reference as an argument. using Callback = std::function; @@ -36,39 +35,32 @@ class TimerInterrupt { TaskHandle_t xHandle; /** - * @brief Construct a new Timer Interrupt object which will call the given lambda function - * at the given interval. This is an abstraction of the software timer interrupt functionality - * in FreeRTOS. This will allow easier scheduling of data transfer or any TM tasks which need - * to happen at a regular interval. + * @brief Construct a new Timer TelemetryTask object which will call the given lambda function + * at the given interval. This will allow easier scheduling of data transfer or any TM tasks which need + * to happen at a regular interval with FreeRTOS. * * @param taskName - The name of the task. * @param stackSize - The size of the stack for the task. * @param uxPriority - The priority of the task. - * @param intervalMs - The time interval in milliseconds at which the interrupt should be - * triggered. * @param tm - The TelemetryManager object. * @param cbLambda - The callback function to be called when the timer interrupt is triggered. */ - TimerInterrupt(const char* taskName, int stackSize, UBaseType_t uxPriority, - TickType_t intervalMs, TelemetryManager& tm, Callback cbLambda); + TelemetryTask(const char* taskName, int stackSize, UBaseType_t uxPriority, TelemetryManager& tm, Callback cbLambda); /** * @brief Destroy the Timer Interrupt object. * */ - ~TimerInterrupt(); + ~TelemetryTask(); /** * @brief This is essentially a compatibility/wrapper function that allows us to use a lambda * function which has a class instance as an argument as a callback for the timer interrupt. * This allows us to access TM within the lambda function. * - * @param pvParameters - The TimerInterrupt object. + * @param pvParameters - The TelemetryTask object. */ - static void TaskTrampoline(void* pvParameters) { - auto* context = static_cast(pvParameters); - context->cbLambda(context->tm); - } + static void TaskTrampoline(void* pvParameters); }; #endif diff --git a/TelemetryManager/Src/GroundStationComms.cpp b/TelemetryManager/Src/GroundStationCommunication.cpp similarity index 66% rename from TelemetryManager/Src/GroundStationComms.cpp rename to TelemetryManager/Src/GroundStationCommunication.cpp index ef76828f..4bbd7391 100644 --- a/TelemetryManager/Src/GroundStationComms.cpp +++ b/TelemetryManager/Src/GroundStationCommunication.cpp @@ -1,19 +1,19 @@ -#include "GroundStationComms.hpp" +#include "GroundStationCommunication.hpp" -GroundStationComms::GroundStationComms() { +GroundStationCommunication::GroundStationCommunication() { // Constructor } -GroundStationComms::~GroundStationComms() { +GroundStationCommunication::~GroundStationCommunication() { // Destructor } -// ** Implement sendToGroundStation first ** +// ** Implement transmit first ** -void GroundStationComms::sendToGroundStation(CircularBuffer &transmissionBuffer) { +void GroundStationCommunication::transmit(CircularBuffer &transmissionBuffer) { // START: Send the bytes in transmissionBuffer to the ground station via RFD900 // END: Send the bytes in transmissionBuffer to the ground station via RFD900 @@ -21,7 +21,7 @@ void GroundStationComms::sendToGroundStation(CircularBuffer &transmissionBuffer) return; } -void GroundStationComms::receiveFromGroundStationISR() { +void GroundStationCommunication::receiveInterruptServiceRoutine() { int bytesReceived = 0; // replace with actual number of bytes received diff --git a/TelemetryManager/Src/ManagerComs.cpp b/TelemetryManager/Src/ManagerCommunication.cpp similarity index 100% rename from TelemetryManager/Src/ManagerComs.cpp rename to TelemetryManager/Src/ManagerCommunication.cpp diff --git a/TelemetryManager/Src/TM.cpp b/TelemetryManager/Src/TM.cpp deleted file mode 100644 index 77c47718..00000000 --- a/TelemetryManager/Src/TM.cpp +++ /dev/null @@ -1,107 +0,0 @@ -#include "FreeRTOS.h" -#include "GroundStationComms.hpp" -#include "MavlinkTranslator.hpp" -#include "TimerInterrupt.hpp" -#include "task.h" - -TelemetryManager::TelemetryManager() { - this->MT = MavlinkTranslator(); - this->GSC = GroundStationComms(); -} - -TelemetryManager::~TelemetryManager() { - // Destructor -} - -/** - * @brief Initialize TM threads and timer interrupts. - * - */ -void TelemetryManager::init() { - // initialize the timer interrupts - initTimerInterrupts(); - - // Init TM tasks - initTasks(); -} - -void TelemetryManager::initTimerInterrupts() { - /** - * @brief This interrupt service routine is called every 1000ms. It is responsible for - * sending the highest priority drone "state" data to the ground station. Data such as - * heartbeat message, altitude, attitude, latitude, longitude... And anything else deemed - * important enough to be transmitted at a regular interval. This is the highest priority - * data in the GSC.highPriorityTransmitBuffer. - * - */ - TimerInterrupt dispatchHighPriorityInterrupt1000Ms( - "dispatchHighPriority1000Ms", configMINIMAL_STACK_SIZE, tskIDLE_PRIORITY, 500, *this, - [](TelemetryManager& tm) -> void { - auto GSC = tm.GSC; - - // START: ingest drone state data and pack bytes into GSC.highPriorityTransmitBuffer - - // END: ingest drone state data and pack bytes into GSC.highPriorityTransmitBuffer - - // transmit the data via GSC.sendToGroundStation(); function - GSC.sendToGroundStation(GSC.highPriorityTransmitBuffer); - }); -} - -void TelemetryManager::initTasks() { - // Initialize translateToMavlinkThread as a FreeRTOS task - xTaskCreate(TelemetryManager::translateToMavlinkThreadStatic, // Task function - "translateToMavlinkThread", // Task name - configMINIMAL_STACK_SIZE, // Stack size - this, // Task parameters - tskIDLE_PRIORITY + 1, // Task priority - NULL // Task handle - ); - - // Initialize mavlinkToBytesThread as a FreeRTOS task - xTaskCreate(TelemetryManager::mavlinkToBytesThreadStatic, // Task function - "mavlinkToBytesThread", // Task name - configMINIMAL_STACK_SIZE, // Stack size - this, // Task parameters - tskIDLE_PRIORITY + 1, // Task priority - NULL // Task handle - ); -} - -/** - * @brief This thread is responsible for taking the bytes from the GSC.DMAReceiveBuffer and - * converting them to Mavlink messages/triggering the callbacks associated with each Mavlink - * message. - */ -void TelemetryManager::translateToMavlinkThread() { - while (true) { - MT.bytesToMavlinkMsg(GSC.DMAReceiveBuffer); - - vTaskDelay(pdMS_TO_TICKS(10)); // Adjust the delay as necessary - } -} - -/** - * @brief This thread is responsible for taking data from other managers and converting - * them to Mavlink bytes, then putting them into GSC.lowPriorityTransmitBuffer. - */ -void TelemetryManager::mavlinkToBytesThread() { - while (true) { - // START: fill GSC.lowPriorityTransmitBuffer with data to transmit - - // END: fill GSC.lowPriorityTransmitBuffer with data to transmit - - vTaskDelay(pdMS_TO_TICKS(10)); // Adjust the delay as necessary - } -} - -/** - * @brief This function is responsible for - * sending non routine data to the ground station. Such as arm disarmed message status, - * fulfilling data requests from the ground station etc. This is the lowest priority data - * in the GSC.lowPriorityTransmitBuffer. - */ -void TelemetryManager::sendLowPriorityData() { - // transmit low priority the data via GSC.sendToGroundStation(); function - GSC.sendToGroundStation(GSC.lowPriorityTransmitBuffer); -} diff --git a/TelemetryManager/Src/TelemetryManager.cpp b/TelemetryManager/Src/TelemetryManager.cpp new file mode 100644 index 00000000..155d74b3 --- /dev/null +++ b/TelemetryManager/Src/TelemetryManager.cpp @@ -0,0 +1,106 @@ +#include "TelemetryManager.hpp" + +/** + * @brief This task is called every 500ms. It is responsible for + * sending the highest priority/routine drone "state" data to the ground station. Data such as + * heartbeat message, altitude, attitude, latitude, longitude... And anything else deemed + * important enough to be transmitted at a regular interval. This is the highest priority + * data in the GSC.highPriorityTransmitBuffer. + * + */ +TelemetryTask* routineDataTransmission; + +/** + * @brief This thread is responsible for taking data from other managers and converting + * them to Mavlink bytes, then putting them into GSC.lowPriorityTransmitBuffer. + */ +TelemetryTask* translateToMavlink; + +/** + * @brief This thread is responsible for taking the bytes from the GSC.DMAReceiveBuffer and + * converting them to Mavlink messages/triggering the callbacks associated with each Mavlink + * message. + */ +TelemetryTask* translateFromMavlink; + +TelemetryManager::TelemetryManager() { + this->MT = MavlinkTranslator(); + this->GSC = GroundStationCommunication(); +} + +TelemetryManager::~TelemetryManager() { + // Destructor + teardownTasks(); +} + +/** + * @brief Initialize TM threads and timer interrupts. + * + */ +void TelemetryManager::init() { + // register TM tasks with FreeRTOS + spinUpTasks(); +} + +void TelemetryManager::spinUpTasks() { + routineDataTransmission = + new TelemetryTask("routineDataTransmission", configMINIMAL_STACK_SIZE, tskIDLE_PRIORITY, + *this, [](TelemetryManager& tm) -> void { + auto GSC = tm.GSC; + while (true) { + // START: ingest drone state data and pack bytes into + // GSC.highPriorityTransmitBuffer + + // END: ingest drone state data and pack bytes into + // GSC.highPriorityTransmitBuffer + + // transmit the data via GSC.transmit(); function + GSC.transmit(GSC.highPriorityTransmitBuffer); + + // The interval at which the instructions in the lambda function + // are executed. + vTaskDelay(pdMS_TO_TICKS(500)); // Adjust the delay as necessary + } + }); + + translateToMavlink = + new TelemetryTask("translateToMavlink", configMINIMAL_STACK_SIZE, tskIDLE_PRIORITY, *this, + [](TelemetryManager& tm) -> void { + auto MT = tm.MT; + auto GSC = tm.GSC; + + while (true) { + MT.bytesToMavlinkMsg(GSC.DMAReceiveBuffer); + vTaskDelay(pdMS_TO_TICKS(10)); // Adjust the delay as necessary + } + }); + + translateFromMavlink = + new TelemetryTask("translateFromMavlink", configMINIMAL_STACK_SIZE, tskIDLE_PRIORITY, *this, + [](TelemetryManager& tm) -> void { + while (true) { + // START: fill GSC.lowPriorityTransmitBuffer with data to transmit + + // END: fill GSC.lowPriorityTransmitBuffer with data to transmit + + vTaskDelay(pdMS_TO_TICKS(10)); // Adjust the delay as necessary + } + }); +} + +void TelemetryManager::teardownTasks() { + delete routineDataTransmission; + delete translateToMavlink; + delete translateFromMavlink; +} + +/** + * @brief This function is responsible for + * sending non routine data to the ground station. Such as arm disarmed message status, + * fulfilling data requests from the ground station etc. This is the lowest priority data + * in the GSC.lowPriorityTransmitBuffer. + */ +void TelemetryManager::transmitNonRoutineData() { + // transmit non routine data via GSC.transmit(); function + GSC.transmit(GSC.lowPriorityTransmitBuffer); +} diff --git a/TelemetryManager/Src/TelemetryTask.cpp b/TelemetryManager/Src/TelemetryTask.cpp new file mode 100644 index 00000000..34bed4aa --- /dev/null +++ b/TelemetryManager/Src/TelemetryTask.cpp @@ -0,0 +1,18 @@ + +#include "TelemetryTask.hpp" + +TelemetryTask::TelemetryTask(const char *taskName, int stackSize, UBaseType_t uxPriority, TelemetryManager &tm, Callback cbLambda) + : tm(tm), cbLambda(cbLambda), xHandle(nullptr) { + xTaskCreate(&TelemetryTask::TaskTrampoline, taskName, stackSize, this, uxPriority, &xHandle); +} + +void TelemetryTask::TaskTrampoline(void *pvParameters) { + auto* context = static_cast(pvParameters); + context->cbLambda(context->tm); + } + +TelemetryTask::~TelemetryTask() { + if (eTaskGetState(xHandle) != eDeleted) { + vTaskDelete(xHandle); + } +} diff --git a/TelemetryManager/Src/TimerInterrupt.cpp b/TelemetryManager/Src/TimerInterrupt.cpp deleted file mode 100644 index e68429df..00000000 --- a/TelemetryManager/Src/TimerInterrupt.cpp +++ /dev/null @@ -1,21 +0,0 @@ - -#include "TimerInterrupt.hpp" - -TimerInterrupt::TimerInterrupt(const char *taskName, int stackSize, UBaseType_t uxPriority, - TickType_t intervalMs, TelemetryManager &tm, Callback cbLambda) - : tm(tm), cbLambda(cbLambda), xHandle(nullptr) { - - // Below is just an example of creating a task with the TaskTrampoline. This is not the actual - // implementation. Delete it and implement the actual timer interrupt. - xTaskCreate(&TimerInterrupt::TaskTrampoline, taskName, stackSize, this, uxPriority, &xHandle); - - // START: Implement the actual timer interrupt and call the callback function every intervalMs - - // END: Implement the actual timer interrupt and call the callback function every intervalMs -} - -TimerInterrupt::~TimerInterrupt() { - if (eTaskGetState(xHandle) != eDeleted) { - vTaskDelete(xHandle); - } -}