Merge branch 'Dependency-Injection' of github.com:UWARG/efs-zeropilot-3.5 into Dependency-Injection

Author: larry-pan

Boardfiles/nucleol552zeq/Core/Inc/FreeRTOSConfig.h

@@ -72,7 +72,7 @@
 #define configTICK_RATE_HZ                       ((TickType_t)1000)
 #define configMAX_PRIORITIES                     ( 56 )
 #define configMINIMAL_STACK_SIZE                 ((uint16_t)128)
-#define configTOTAL_HEAP_SIZE                    ((size_t)8192)
+#define configTOTAL_HEAP_SIZE                    ((size_t)65536)
 #define configMAX_TASK_NAME_LEN                  ( 16 )
 #define configUSE_TRACE_FACILITY                 1
 #define configUSE_16_BIT_TICKS                   0

Boardfiles/nucleol552zeq/Core/Src/main.cpp

@@ -32,17 +32,14 @@
 #include "ucpd.h"
 #include "usb.h"
 #include "gpio.h"
+#include "SystemManager.hpp"
 
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
 #include "SystemManager.hpp"
 #include "drivers_config.hpp"
 #include "independent_watchdog.h"
 
-extern "C" {
-  #include "app_fatfs.h"
-  #include "log_util.h"
-}
 /* USER CODE END Includes */
 
 /* Private typedef -----------------------------------------------------------*/
@@ -77,9 +74,9 @@ extern "C" {
 
 /* Private user code ---------------------------------------------------------*/
 /* USER CODE BEGIN 0 */
-void SMTask(void *pvParameters) {
-    SystemManager SM;
-    SM.flyManually();
+void SMTask(void * pvParameters){
+  SystemManager SM;
+  SM.startSystemManager();
 }
 /* USER CODE END 0 */
 
@@ -131,23 +128,12 @@ int main(void)
   MX_ICACHE_Init();
   MX_TIM3_Init();
   MX_SDMMC1_SD_Init();
-  if (MX_FATFS_Init() != APP_OK) {
-    Error_Handler();
-  }
   /* USER CODE BEGIN 2 */
-  logInit();
-
-  TaskHandle_t hSM = NULL;
-  xTaskCreate(SMTask, "SM", 500U, NULL, osPriorityNormal, &hSM);
 
   /* USER CODE END 2 */
 
-  /* Init scheduler */
-  osKernelInitialize();  /* Call init function for freertos objects (in freertos.c) */
-  MX_FREERTOS_Init();
-
-  /* Start scheduler */
-  osKernelStart();
+  xTaskCreate(SMTask, "SM Task", 1500U, NULL, osPriorityNormal, NULL);
+  vTaskStartScheduler();
   /* We should never get here as control is now taken by the scheduler */
   /* Infinite loop */
   /* USER CODE BEGIN WHILE */

Boardfiles/nucleol552zeq/Core/Src/usart.c

@@ -21,7 +21,11 @@
 #include "usart.h"
 
 /* USER CODE BEGIN 0 */
-
+int __io_putchar(int ch)
+{
+  HAL_UART_Transmit(&hlpuart1, (uint8_t *)&ch, 1, 100);
+  return ch;
+}
 /* USER CODE END 0 */
 
 UART_HandleTypeDef hlpuart1;
@@ -44,7 +48,7 @@ void MX_LPUART1_UART_Init(void)
 
   /* USER CODE END LPUART1_Init 1 */
   hlpuart1.Instance = LPUART1;
-  hlpuart1.Init.BaudRate = 209700;
+  hlpuart1.Init.BaudRate = 112500;
   hlpuart1.Init.WordLength = UART_WORDLENGTH_8B;
   hlpuart1.Init.StopBits = UART_STOPBITS_1;
   hlpuart1.Init.Parity = UART_PARITY_NONE;

Boardfiles/nucleol552zeq/nucleol552zeq.ioc

@@ -100,7 +100,7 @@ IWDG.IPParameters=Prescaler,Reload
 IWDG.Prescaler=IWDG_PRESCALER_8
 IWDG.Reload=3999
 KeepUserPlacement=false
-LPUART1.BaudRate=230400
+LPUART1.BaudRate=112500
 LPUART1.IPParameters=BaudRate
 Mcu.CPN=STM32L552ZET6Q
 Mcu.ContextProject=TrustZoneDisabled

SystemManager/Inc/SystemManager.hpp

@@ -16,28 +16,36 @@
 #define SBUS_MAX 100
 
 class SystemManager {
-public:
-    /* Constructors and Destructors */
-    SystemManager();
-    ~SystemManager();
-
-    /* Other managers*/
-    TelemetryManager *telemetryManager;
-
-    /* Class Functions */
-    void flyManually();
-    TelemetryManager* setupTM();
-
-private:
-    SBUSReceiver* rcController_;
-    RCControl rcInputs_;
-    PWMChannel throttleMotorChannel_;
-    PWMChannel yawMotorChannel_;
-    PWMChannel rollMotorChannel_;
-    PWMChannel invertedRollMotorChannel_;
-    PWMChannel pitchMotorChannel_;
-    IndependentWatchdog watchdog_;
+    public:
+        /* Constructors and Destructors */
+        SystemManager();
+
+        /* Other managers*/
+        TelemetryManager *telemetryManager;
+
+        TelemetryManager* setupTM();
+
+        /* Class Functions */
+        void startSystemManager(); 
+        
+    private:
+        SBUSReceiver* rcController_;
+        RCControl rcInputs_;
+        PWMChannel throttleMotorChannel_;
+        PWMChannel yawMotorChannel_;
+        PWMChannel rollMotorChannel_;
+        PWMChannel invertedRollMotorChannel_;
+        PWMChannel pitchMotorChannel_;
+        IndependentWatchdog watchdog_;
+
+        static void attitudeManagerTaskWrapper(void *pvParameters);
+        static void telemetryManagerTaskWrapper(void *pvParameters);
+        static void pathManagerTaskWrapper(void *pvParameters);
+        
+        void systemManagerTask();
+        void attitudeManagerTask();
+        void telemetryManagerTask();
+        void pathManagerTask();
 };
 
-
 #endif //ZEROPILOT_3_5_SYSTEMMANAGER_HPP

SystemManager/Src/SystemManager.cpp

@@ -5,19 +5,29 @@
 
 #include "SystemManager.hpp"
 
+#include <iostream>
+
+#include "FreeRTOS.h"
+#include "GroundStationCommunication.hpp"
+#include "TelemetryManager.hpp"
+#include "cmsis_os.h"
 #include "drivers_config.hpp"
 #include "independent_watchdog.h"
 #include "rcreceiver_datatypes.h"
 #include "sbus_defines.h"
 #include "sbus_receiver.hpp"
 #include "tim.h"
-#include "GroundStationCommunication.hpp"
-#include "TelemetryManager.hpp"
-#include "GroundStationCommunication.hpp"
-#include "TelemetryManager.hpp"
 
-#define TIMEOUT_CYCLES 250000 // 25k = 1 sec fro testing 10/14/2023 => 250k = 10 sec
-#define TIMOUT_MS      10000 // 10 sec
+extern "C" {
+#include "app_fatfs.h"
+#include "log_util.h"
+}
+
+#define TIMEOUT_CYCLES 250000  // 25k = 1 sec fro testing 10/14/2023 => 250k = 10 sec
+#define TIMOUT_MS 10000        // 10 sec
+
+// 0 - AM, 1 - TM, 2 - PM
+static TaskHandle_t taskHandles[3];
 
 static uint32_t DisconnectionCount = 0;
 float prevthrottle;
@@ -52,6 +62,7 @@ TelemetryManager* SystemManager::setupTM() {
     TMStateData stateData;
 
     // values to be assigned to stateData
+    // THIS IS JUST A PLACEHOLDER, THE MEMORY ADDRESSES NEED TO LIVE LONGER THAN THE SM CONSTRUCTOR
     int32_t alt = 0;
     int32_t lat = 0;
     int32_t lon = 0;
@@ -69,48 +80,71 @@ TelemetryManager* SystemManager::setupTM() {
     float yawspeed = 0;
 
     // use the memory address of the above, since stateData uses pointers
-    stateData.alt = &alt;
-    stateData.lat = &lat;
-    stateData.lon = &lon;
-    stateData.relative_alt = &relative_alt;
-    stateData.vx = &vx;
-    stateData.vy = &vy;
-    stateData.vz = &vz;
-    stateData.hdg = &hdg;
-    stateData.time_boot_ms = &time_boot_ms;
-    stateData.roll = &roll;
-    stateData.pitch = &pitch;
-    stateData.yaw = &yaw;
-    stateData.rollspeed = &rollspeed;
-    stateData.pitchspeed = &pitchspeed;
-    stateData.yawspeed = &yawspeed;
+    stateData.set_alt(&alt);
+    stateData.set_lat(&lat);
+    stateData.set_lon(&lon);
+    stateData.set_relative_alt(&relative_alt);
+    stateData.set_vx(&vx);
+    stateData.set_vy(&vy);
+    stateData.set_vz(&vz);
+    stateData.set_hdg(&hdg);
+    stateData.set_time_boot_ms(&time_boot_ms);
+    stateData.set_roll(&roll);
+    stateData.set_pitch(&pitch);
+    stateData.set_yaw(&yaw);
+    stateData.set_rollspeed(&rollspeed);
+    stateData.set_pitchspeed(&pitchspeed);
+    stateData.set_yawspeed(&yawspeed);
 
     MAV_STATE mavState = MAV_STATE::MAV_STATE_STANDBY;
     MAV_MODE_FLAG mavMode = MAV_MODE_FLAG::MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
 
-    // Creating parameters for the GroundStationCommunication that will be passed to telemetryManager
+    // Creating parameters for the GroundStationCommunication that will be passed to
+    // telemetryManager
     TMCircularBuffer DMAReceiveBuffer = *(new TMCircularBuffer(rfd900_circular_buffer));
 
-
     // the buffer that stores non_routine/low_priority bytes (ex. Battery Voltage) to be sent to the
     // ground station.
-    uint8_t* lowPriorityTransmitBuffer = new uint8_t[RFD900_BUF_SIZE];
+    uint8_t *lowPriorityTransmitBuffer = new uint8_t[RFD900_BUF_SIZE];
 
     // the buffer that stores routine/high_priority bytes (ex. heading, general state data) to be
     // sent to the ground station.
-    uint8_t* highPriorityTransmitBuffer = new uint8_t[RFD900_BUF_SIZE];
+    uint8_t *highPriorityTransmitBuffer = new uint8_t[RFD900_BUF_SIZE];
 
-    GroundStationCommunication GSC = *(new GroundStationCommunication(DMAReceiveBuffer, lowPriorityTransmitBuffer, 
-                                                                    highPriorityTransmitBuffer, RFD900_BUF_SIZE));
+    GroundStationCommunication GSC = *(new GroundStationCommunication(
+        DMAReceiveBuffer, lowPriorityTransmitBuffer, highPriorityTransmitBuffer, RFD900_BUF_SIZE));
 
-    // the buffer that stores the bytes received from the ground station.                                           
+    // the buffer that stores the bytes received from the ground station.
     MavlinkTranslator MT;
 
     return new TelemetryManager(stateData, mavState, mavMode, GSC, MT);
 }
 
-void SystemManager::flyManually() {
+// wrapper functions are needed as FreeRTOS xTaskCreate function does not accept functions that have
+// "this" pointers
+void SystemManager::attitudeManagerTaskWrapper(void *pvParameters) {
+    SystemManager *systemManagerInstance = static_cast<SystemManager *>(pvParameters);
+    systemManagerInstance->attitudeManagerTask();
+}
+
+void SystemManager::telemetryManagerTaskWrapper(void *pvParameters) {
+    SystemManager *systemManagerInstance = static_cast<SystemManager *>(pvParameters);
+    systemManagerInstance->telemetryManagerTask();
+}
+
+void SystemManager::pathManagerTaskWrapper(void *pvParameters) {
+    SystemManager *systemManagerInstance = static_cast<SystemManager *>(pvParameters);
+    systemManagerInstance->pathManagerTask();
+}
+
+void SystemManager::systemManagerTask() {
+    TickType_t xNextWakeTime = xTaskGetTickCount();
+    uint16_t frequency = 5;
+
     for (;;) {
+        printf("SM called\r\n");
+        HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_7);  // toggle led light for testing
+
         this->rcInputs_ = rcController_->GetRCControl();
 
         // TO-DO: need to implement it using is_Data_New;
@@ -150,5 +184,68 @@ void SystemManager::flyManually() {
             this->pitchMotorChannel_.set(SBUS_MAX / 2);
             this->invertedRollMotorChannel_.set(SBUS_MAX / 2);
         }
+
+        vTaskDelayUntil(&xNextWakeTime, 1000 / frequency);
+    }
+}
+
+void SystemManager::attitudeManagerTask() {
+    TickType_t xNextWakeTime = xTaskGetTickCount();
+    uint16_t frequency = 5;
+
+    for (;;) {
+        // call AM
+
+        printf("AM called\r\n");
+        HAL_GPIO_TogglePin(GPIOC, GPIO_PIN_7);  // toggle led light for testing
+        vTaskDelayUntil(&xNextWakeTime, 1000 / frequency);
+    }
+}
+
+void SystemManager::telemetryManagerTask() {
+    TickType_t xNextWakeTime = xTaskGetTickCount();
+    uint16_t frequency = 5;
+
+    for (;;) {
+        // call TM
+
+        printf("TM called\r\n");
+        HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_9);  // toggle led light for testing
+        vTaskDelayUntil(&xNextWakeTime, 1000 / frequency);
+    }
+}
+
+void SystemManager::pathManagerTask() {
+    TickType_t xNextWakeTime = xTaskGetTickCount();
+    uint16_t frequency = 5;
+
+    for (;;) {
+        // call PM
+
+        printf("PM called\r\n");
+        vTaskDelayUntil(&xNextWakeTime, 1000 / frequency);
+    }
+}
+
+void SystemManager::startSystemManager() {
+    printf("Initializing Tasks\r\n");
+
+    // enabling SD card logging
+    if (MX_FATFS_Init() != APP_OK) {
+        Error_Handler();
     }
+    logInit();
+
+    // BaseType_t xTaskCreate( TaskFunction_t pvTaskCode, const char * const pcName,
+    // configSTACK_DEPTH_TYPE usStackDepth, void * pvParameters, UBaseType_t uxPriority,
+    // TaskHandle_t * pxCreatedTask );
+    //                           function's name             description                 size of
+    //                           stack to allocate        parameters for task        priority
+    //                           handler
+    xTaskCreate(attitudeManagerTaskWrapper, "AM TASK", 800U, this, osPriorityNormal, taskHandles);
+    xTaskCreate(telemetryManagerTaskWrapper, "TM TASK", 800U, this, osPriorityNormal,
+                taskHandles + 1);
+    // xTaskCreate(pathManagerTaskWrapper, "PM TASK", 800U, this, osPriorityNormal, taskHandles +2);
+
+    systemManagerTask();
 }