Merge pull request #19 from RoboMaster-Club/18-daemon-watchdog-implem…

…entation

daemon + remote + bsp_uart

Makefile

@@ -116,6 +116,7 @@ src/algo/src/wheel_legged_locomotion.c \
 src/algo/src/leg.c \
 src/bsp/src/bsp_can.c \
 src/bsp/src/bsp_delay.c \
+src/bsp/src/bsp_daemon.c \
 src/bsp/src/bsp_pwm.c \
 src/bsp/src/bsp_spi.c \
 src/bsp/src/bsp_serial.c \

src/app/inc/robot_tasks.h

@@ -9,6 +9,7 @@
 #include "debug_task.h"
 #include "jetson_orin.h"
 #include "bsp_serial.h"
+#include "bsp_daemon.h"
 
 extern void IMU_Task(void const *pvParameters);
 
@@ -18,13 +19,15 @@ osThreadId motor_task_handle;
 osThreadId ui_task_handle;
 osThreadId debug_task_handle;
 osThreadId jetson_orin_task_handle;
+osThreadId daemon_task_handle;
 
 void Robot_Tasks_Robot_Control(void const *argument);
 void Robot_Tasks_Motor(void const *argument);
 void Robot_Tasks_IMU(void const *argument);
 void Robot_Tasks_UI(void const *argument);
 void Robot_Tasks_Debug(void const *argument);
 void Robot_Tasks_Jetson_Orin(void const *argument);
+void Robot_Tasks_Daemon(void const *argument);
 
 void Robot_Tasks_Start()
 {
@@ -45,6 +48,9 @@ void Robot_Tasks_Start()
 
     osThreadDef(jetson_orin_task, Robot_Tasks_Jetson_Orin, osPriorityAboveNormal, 0, 256);
     jetson_orin_task_handle = osThreadCreate(osThread(jetson_orin_task), NULL);
+
+    osThreadDef(daemon_task, Robot_Tasks_Daemon, osPriorityAboveNormal, 0, 256);
+    daemon_task_handle = osThreadCreate(osThread(daemon_task), NULL);
 }
 
 void Robot_Tasks_Robot_Control(void const *argument)
@@ -111,3 +117,15 @@ void Robot_Tasks_Jetson_Orin(void const *argument)
         vTaskDelayUntil(&xLastWakeTime, TimeIncrement);
     }
 }
+
+void Robot_Tasks_Daemon(void const *argument)
+{
+    portTickType xLastWakeTime;
+    xLastWakeTime = xTaskGetTickCount();
+    const TickType_t TimeIncrement = pdMS_TO_TICKS(DAEMON_PERIOD);
+    while (1)
+    {
+        Daemon_Task_Loop();
+        vTaskDelayUntil(&xLastWakeTime, TimeIncrement);
+    }
+}

src/app/src/robot.c

@@ -36,7 +36,7 @@ void Robot_Init()
     Chassis_Task_Init();
     Gimbal_Task_Init();
     Launch_Task_Init();
-    Remote_Init();
+    Remote_Init(&huart3);
     CAN_Service_Init();
     Referee_System_Init(&huart1);
     Jetson_Orin_Init(&huart6);

src/bsp/Inc/bsp_daemon.h

@@ -0,0 +1,22 @@
+#ifndef BSP_DAEMON_H
+#define BSP_DAEMON_H
+
+#include <stdint.h>
+
+#define DAEMON_PERIOD (10) // unit: ms
+
+typedef struct
+{
+    uint8_t id;
+    uint16_t counter;
+    uint16_t reload_value;
+
+    void (*callback)(void);
+} Daemon_Instance_t;
+
+void Daemon_Init(void);
+Daemon_Instance_t* Daemon_Register(uint16_t reload_value, uint16_t initial_counter, void (*callback)(void));
+void Daemon_Reload(Daemon_Instance_t *daemon);
+void Daemon_Task_Loop(void);
+
+#endif // BSP_DAEMON_H

src/bsp/Inc/bsp_uart.h

@@ -3,4 +3,15 @@
 
 #include "usart.h"
 
+typedef struct _UART_Instance
+{
+    UART_HandleTypeDef *uart_handle;
+    uint8_t *rx_buffer;
+    uint16_t rx_buffer_size;
+    void (*callback)(struct _UART_Instance *uart_instance);
+} UART_Instance_t;
+
+void UART_Service_Init(UART_Instance_t *uart_insatce);
+UART_Instance_t *UART_Register(UART_HandleTypeDef *huart, uint8_t *rx_buffer, uint8_t rx_buffer_size, void (*callback)(UART_Instance_t *uart_instance)); 
+
 #endif // BSP_UART_H

src/bsp/Src/bsp_daemon.c

@@ -0,0 +1,36 @@
+#include "bsp_daemon.h"
+#include <stdlib.h>
+
+#define DAEMON_INSTANCE_MAX (3)
+Daemon_Instance_t *g_daemon_instances[DAEMON_INSTANCE_MAX];
+uint8_t g_daemon_instance_count = 0;
+
+Daemon_Instance_t *Daemon_Register(uint16_t reload_value, uint16_t initial_counter, void (*callback)(void))
+{
+    Daemon_Instance_t *daemon = (Daemon_Instance_t *)malloc(sizeof(Daemon_Instance_t));
+
+    daemon->counter = initial_counter;
+    daemon->reload_value = reload_value;
+    daemon->callback = callback;
+
+    g_daemon_instances[g_daemon_instance_count++] = daemon;
+    return daemon;
+}
+
+void Daemon_Reload(Daemon_Instance_t *daemon)
+{
+    daemon->counter = daemon->reload_value;
+}
+
+void Daemon_Task_Loop()
+{
+    for (int i = 0; i < g_daemon_instance_count; i++)
+    {
+        g_daemon_instances[i]->counter--;
+        if (g_daemon_instances[i]->counter == 0)
+        {
+            g_daemon_instances[i]->callback();
+            g_daemon_instances[i]->counter = g_daemon_instances[i]->reload_value;
+        }
+    }
+}

src/bsp/Src/bsp_uart.c

@@ -1,21 +1,46 @@
 #include "bsp_uart.h"
 
-#include "referee_system.h"
-#include "jetson_orin.h"
+#include <stdlib.h>
+#include "memory.h"
 
-/**
- * @brief  This is the callback function for the UART receive interrupt.
- * This function overrides the weak implementation in stm32f4xx_hal_uart.c.
-*/
-void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
-    // Check which UART is calling the callback
-    if (huart == Referee_System.huart) {
-        // Decode Referee system data
-        Referee_Get_Data();
-    }
-    if (huart == g_orin_data.huartx)
+#define UART_INSTANCE_MAX 3
+UART_Instance_t *g_uart_insatnces[UART_INSTANCE_MAX];
+uint8_t g_uart_instance_count = 0;
+
+void UART_Service_Init(UART_Instance_t *uart_insatce)
+{
+    HAL_UARTEx_ReceiveToIdle_DMA(uart_insatce->uart_handle, uart_insatce->rx_buffer, uart_insatce->rx_buffer_size);
+    __HAL_DMA_DISABLE_IT(uart_insatce->uart_handle->hdmarx, DMA_IT_HT); // disable half transfer interrupt
+}
+
+UART_Instance_t *UART_Register(UART_HandleTypeDef *huart, uint8_t *rx_buffer, uint8_t rx_buffer_size, void (*callback)(UART_Instance_t *uart_instance))
+{
+    UART_Instance_t *uart_instance = (UART_Instance_t *)malloc(sizeof(UART_Instance_t));
+    uart_instance->uart_handle = huart;
+    uart_instance->rx_buffer = rx_buffer;
+    uart_instance->rx_buffer_size = rx_buffer_size;
+    uart_instance->callback = callback;
+
+    UART_Service_Init(uart_instance);
+
+    g_uart_insatnces[g_uart_instance_count++] = uart_instance;
+    return uart_instance;
+}
+
+void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
+{
+    for (int i = 0; i < g_uart_instance_count; i++)
     {
-        // Decode Jetson Orin data
-        Jetson_Orin_Decode();
+        if (g_uart_insatnces[i]->uart_handle == huart)
+        {
+            if (g_uart_insatnces[i]->callback != NULL)
+            {
+                g_uart_insatnces[i]->callback(g_uart_insatnces[i]);
+
+                HAL_UARTEx_ReceiveToIdle_DMA(huart, g_uart_insatnces[i]->rx_buffer, g_uart_insatnces[i]->rx_buffer_size);
+                __HAL_DMA_DISABLE_IT(huart3.hdmarx, DMA_IT_HT);
+            }
+        }
     }
-}
+
+}

src/devices/inc/remote.h

@@ -8,6 +8,7 @@
 #define __REMOTE_H
 
 #include <stdint.h>
+#include "bsp_uart.h"
 
 #define REMOTE_STICK_MAX (660.0f)
 
@@ -83,7 +84,6 @@ typedef struct
 
 
 extern Remote_t g_remote;
-extern void Remote_Init(void);
-extern void Remote_BufferProcess(void);
+Remote_t* Remote_Init(UART_HandleTypeDef *huart);
 
 #endif /* __REMOTE_H */

src/devices/src/remote.c

@@ -5,18 +5,24 @@
   ******************************************************************************
   */
 #include "remote.h"
-#include "usart.h"
+#include "bsp_uart.h"
+#include "bsp_daemon.h"
+#include <memory.h>
 
 Remote_t g_remote;
-uint8_t remote_buffer[18];
+UART_Instance_t *g_remote_uart;
+Daemon_Instance_t *g_remote_daemon;
 
+uint8_t remote_buffer[18];
+volatile uint8_t debug_buffer[18];
 /*
  * Remote_BufferProcess()
  * 
  * Decode the buffer received from DR16 receiver to g_remote. @ref Remote_t
  */
-void Remote_BufferProcess()
+void Remote_Buffer_Process()
 {
+	memcpy((void*) debug_buffer, remote_buffer, 18);
 	// controller decode
 	g_remote.controller.right_stick.x = ((remote_buffer[0] | (remote_buffer[1] << 8)) & 0x07ff) - 1024;
 	g_remote.controller.right_stick.y = (((remote_buffer[1] >> 3) | (remote_buffer[2] << 5)) & 0x07ff) - 1024;
@@ -53,7 +59,21 @@ void Remote_BufferProcess()
 	g_remote.keyboard.B = (key_buffer >> 15) & 0x001;
 }
 
-void Remote_Init(void)
+void Remote_Rx_Callback(UART_Instance_t *uart_instance)
 {
-	HAL_UART_Receive_DMA(&huart3, remote_buffer, 18);
+	Remote_Buffer_Process();
+	Daemon_Reload(g_remote_daemon);
 }
+
+void Remote_Timeout_Callback()
+{
+	// reinitalize the remote uart transmission
+	UART_Service_Init(g_remote_uart);
+}
+
+Remote_t* Remote_Init(UART_HandleTypeDef *huart)
+{
+	g_remote_uart = UART_Register(huart, remote_buffer, 18, Remote_Rx_Callback);
+	g_remote_daemon = Daemon_Register(20, 20, Remote_Timeout_Callback);
+	return &g_remote;
+}