Feature/sm/scaffolding (#60)

Co-authored-by: RonaldYou <[email protected]>

Author: HardyYu

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,27 +16,34 @@
 #define SBUS_MAX 100
 
 class SystemManager {
-public:
-    /* Constructors and Destructors */
-    SystemManager();
-    ~SystemManager();
-
-    /* Other managers*/
-    TelemetryManager *telemetryManager;
-
-    /* Class Functions */
-    void flyManually();
-
-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;
+
+        /* 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

@@ -11,11 +11,21 @@
 #include "sbus_defines.h"
 #include "sbus_receiver.hpp"
 #include "tim.h"
+#include "cmsis_os.h"
+#include "FreeRTOS.h"
+#include <iostream>
 
+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;
 float prevyaw;
@@ -60,10 +70,30 @@ SystemManager::SystemManager()
 }
 
 
-SystemManager::~SystemManager() {}
+//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
 
-void SystemManager::flyManually() {
-    for (;;) {
         this->rcInputs_ = rcController_->GetRCControl();
 
         // TO-DO: need to implement it using is_Data_New;
@@ -103,5 +133,64 @@ 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();
+
 }