From dfce61c82106fbfa26cc69f77d3df46d2ed1b645 Mon Sep 17 00:00:00 2001 From: ydzulynsky Date: Mon, 11 Mar 2024 11:30:09 -0400 Subject: [PATCH] Small Stuff --- TelemetryManager/Inc/TM.hpp | 60 ++++++++++++++--------- TelemetryManager/Src/TM.cpp | 65 ++++++++++++++----------- TelemetryManager/Src/TimerInterrupt.cpp | 6 +-- 3 files changed, 76 insertions(+), 55 deletions(-) diff --git a/TelemetryManager/Inc/TM.hpp b/TelemetryManager/Inc/TM.hpp index a211078e..c504ec43 100644 --- a/TelemetryManager/Inc/TM.hpp +++ b/TelemetryManager/Inc/TM.hpp @@ -14,18 +14,18 @@ #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) { \ +/** + * @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(); \ + tmInstance->memberFunction(); \ } #include "GroundStationComms.hpp" @@ -33,15 +33,10 @@ #include "TimerInterrupt.hpp" class TelemetryManager { - -/** - * @brief Construct a new Telemetry Manager object. Does not initialize the threads or timer interrupts. - * To do so call the init() method. - * - * @param GSC - The GroundStationComms object. - * @param MT - The MavlinkTranslator object. - * @param TI - - */ + /** + * @brief Construct a new Telemetry Manager object. Does not initialize the threads or timer + * interrupts. To do so call the init() method. + */ TelemetryManager(); ~TelemetryManager(); @@ -61,7 +56,11 @@ class TelemetryManager { * message. */ void translateToMavlinkThread(); - // Create a static version of the translateToMavlinkThread function to be used as a callback for the FreeRTOS task. + + /* + Create a static version of the translateToMavlinkThread function to be used as a callback for + the FreeRTOS task. + */ TRANSLATE_TO_STATIC(translateToMavlinkThread); /** @@ -69,9 +68,12 @@ class TelemetryManager { * 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); + /* + 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 @@ -81,7 +83,17 @@ class TelemetryManager { */ void sendLowPriorityData(); - void createTimerInterrupts(); + /** + * @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/Src/TM.cpp b/TelemetryManager/Src/TM.cpp index 78231a0e..8ca57730 100644 --- a/TelemetryManager/Src/TM.cpp +++ b/TelemetryManager/Src/TM.cpp @@ -1,7 +1,7 @@ +#include "FreeRTOS.h" #include "GroundStationComms.hpp" #include "MavlinkTranslator.hpp" #include "TimerInterrupt.hpp" -#include "FreeRTOS.h" #include "task.h" TelemetryManager::TelemetryManager() { @@ -9,14 +9,49 @@ TelemetryManager::TelemetryManager() { this->GSC = GroundStationComms(); } +TelemetryManager::~TelemetryManager() { + // Destructor +} + /** * @brief Initialize TM threads and timer interrupts. * */ void TelemetryManager::init() { // initialize the timer interrupts - createTimerInterrupts(); + initTimerInterrupts(); + + // Init TM tasks + initTasks(); + + // Start the scheduler + vTaskStartScheduler(); // should this be in system manager instead? +} + +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 @@ -34,9 +69,6 @@ void TelemetryManager::init() { tskIDLE_PRIORITY + 1, // Task priority NULL // Task handle ); - - // Start the scheduler - vTaskStartScheduler(); // should this be in system manager instead? } /** @@ -76,26 +108,3 @@ void TelemetryManager::sendLowPriorityData() { // transmit low priority the data via GSC.sendToGroundStation(); function GSC.sendToGroundStation(GSC.lowPriorityTransmitBuffer); } - -void TelemetryManager::createTimerInterrupts() { - /** - * @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); - }); -} \ No newline at end of file diff --git a/TelemetryManager/Src/TimerInterrupt.cpp b/TelemetryManager/Src/TimerInterrupt.cpp index 9d70d5cd..e68429df 100644 --- a/TelemetryManager/Src/TimerInterrupt.cpp +++ b/TelemetryManager/Src/TimerInterrupt.cpp @@ -4,8 +4,9 @@ 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. + + // 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 @@ -18,4 +19,3 @@ TimerInterrupt::~TimerInterrupt() { vTaskDelete(xHandle); } } -