修复

assets/User_code/bsp/bsp.h

@@ -0,0 +1,28 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+#define BSP_OK (0)
+#define BSP_ERR (-1)
+#define BSP_ERR_NULL (-2)
+#define BSP_ERR_INITED (-3)
+#define BSP_ERR_NO_DEV (-4)
+#define BSP_ERR_TIMEOUT (-5)
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/bsp/can.c

@@ -0,0 +1,659 @@
+/* Includes ----------------------------------------------------------------- */
+#include "bsp/can.h"
+#include "bsp/bsp.h"
+#include <can.h>
+#include <cmsis_os2.h>
+#include <string.h>
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private define ----------------------------------------------------------- */
+#define CAN_QUEUE_MUTEX_TIMEOUT         100   /* 队列互斥锁超时时间(ms) */
+#define CAN_TX_MAILBOX_NUM              3     /* CAN发送邮箱数量 */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+typedef struct BSP_CAN_QueueNode {
+    BSP_CAN_t can;         /* CAN通道 */
+    uint32_t can_id; /* 解析后的CAN ID */
+    osMessageQueueId_t queue; /* 消息队列ID */
+    uint8_t queue_size; /* 队列大小 */
+    struct BSP_CAN_QueueNode *next; /* 指向下一个节点的指针 */
+} BSP_CAN_QueueNode_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private variables -------------------------------------------------------- */
+static BSP_CAN_QueueNode_t *queue_list = NULL;
+static osMutexId_t queue_mutex = NULL;
+static void (*CAN_Callback[BSP_CAN_NUM][BSP_CAN_CB_NUM])(void);
+static bool inited = false;
+static BSP_CAN_IdParser_t id_parser = NULL; /* ID解析器 */
+static BSP_CAN_TxQueue_t tx_queues[BSP_CAN_NUM]; /* 每个CAN的发送队列 */
+
+/* Private function prototypes ---------------------------------------------- */
+static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan);
+static osMessageQueueId_t BSP_CAN_FindQueue(BSP_CAN_t can, uint32_t can_id);
+static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size);
+static void BSP_CAN_RxFifo0Callback(void);
+static void BSP_CAN_RxFifo1Callback(void);
+static void BSP_CAN_TxCompleteCallback(void);
+static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header);
+static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
+static void BSP_CAN_TxQueueInit(BSP_CAN_t can);
+static bool BSP_CAN_TxQueuePush(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg);
+static bool BSP_CAN_TxQueuePop(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg);
+static bool BSP_CAN_TxQueueIsEmpty(BSP_CAN_t can);
+
+/* Private functions -------------------------------------------------------- */
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+/**
+ * @brief 根据CAN句柄获取BSP_CAN实例
+ */
+static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan) {
+    if (hcan == NULL) return BSP_CAN_ERR;
+    
+/* AUTO GENERATED CAN_GET */
+    else
+        return BSP_CAN_ERR;
+}
+
+/**
+ * @brief 查找指定CAN ID的消息队列
+ * @note 调用前需要获取互斥锁
+ */
+static osMessageQueueId_t BSP_CAN_FindQueue(BSP_CAN_t can, uint32_t can_id) {
+    BSP_CAN_QueueNode_t *node = queue_list;
+    while (node != NULL) {
+        if (node->can == can && node->can_id == can_id) {
+            return node->queue;
+        }
+        node = node->next;
+    }
+    return NULL;
+}
+
+/**
+ * @brief 创建指定CAN ID的消息队列
+ * @note 内部函数，已包含互斥锁保护
+ */
+static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size) {
+    if (queue_size == 0) {
+        queue_size = BSP_CAN_DEFAULT_QUEUE_SIZE;
+    }
+    if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) {
+        return BSP_ERR_TIMEOUT;
+    }
+    BSP_CAN_QueueNode_t *node = queue_list;
+    while (node != NULL) {
+        if (node->can == can && node->can_id == can_id) {
+            osMutexRelease(queue_mutex);
+            return BSP_ERR;  // 已存在
+        }
+        node = node->next;
+    }
+    BSP_CAN_QueueNode_t *new_node = (BSP_CAN_QueueNode_t *)BSP_Malloc(sizeof(BSP_CAN_QueueNode_t));
+    if (new_node == NULL) {
+        osMutexRelease(queue_mutex);
+        return BSP_ERR_NULL;
+    }
+    new_node->queue = osMessageQueueNew(queue_size, sizeof(BSP_CAN_Message_t), NULL);
+    if (new_node->queue == NULL) {
+        BSP_Free(new_node);
+        osMutexRelease(queue_mutex);
+        return BSP_ERR;
+    }
+    new_node->can = can;
+    new_node->can_id = can_id;
+    new_node->queue_size = queue_size;
+    new_node->next = queue_list;
+    queue_list = new_node;
+    osMutexRelease(queue_mutex);
+    return BSP_OK;
+}
+
+
+/**
+ * @brief 获取帧类型
+ */
+static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header) {
+    if (header->RTR == CAN_RTR_REMOTE) {
+        return (header->IDE == CAN_ID_EXT) ? BSP_CAN_FRAME_EXT_REMOTE : BSP_CAN_FRAME_STD_REMOTE;
+    } else {
+        return (header->IDE == CAN_ID_EXT) ? BSP_CAN_FRAME_EXT_DATA : BSP_CAN_FRAME_STD_DATA;
+    }
+}
+
+/**
+ * @brief 默认ID解析器（直接返回原始ID）
+ */
+static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
+    (void)frame_type;  // 避免未使用参数警告
+    return original_id;
+}
+
+/**
+ * @brief 初始化发送队列
+ */
+static void BSP_CAN_TxQueueInit(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return;
+    
+    tx_queues[can].head = 0;
+    tx_queues[can].tail = 0;
+}
+
+/**
+ * @brief 向发送队列添加消息（无锁）
+ */
+static bool BSP_CAN_TxQueuePush(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg) {
+    if (can >= BSP_CAN_NUM || msg == NULL) return false;
+    
+    BSP_CAN_TxQueue_t *queue = &tx_queues[can];
+    uint32_t next_head = (queue->head + 1) % BSP_CAN_TX_QUEUE_SIZE;
+    
+    // 队列满
+    if (next_head == queue->tail) {
+        return false;
+    }
+    
+    // 复制消息
+    queue->buffer[queue->head] = *msg;
+    
+    // 更新头指针（原子操作）
+    queue->head = next_head;
+    
+    return true;
+}
+
+
+/**
+ * @brief 从发送队列取出消息（无锁）
+ */
+static bool BSP_CAN_TxQueuePop(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg) {
+    if (can >= BSP_CAN_NUM || msg == NULL) return false;
+    
+    BSP_CAN_TxQueue_t *queue = &tx_queues[can];
+    
+    // 队列空
+    if (queue->head == queue->tail) {
+        return false;
+    }
+    
+    // 复制消息
+    *msg = queue->buffer[queue->tail];
+    
+    // 更新尾指针（原子操作）
+    queue->tail = (queue->tail + 1) % BSP_CAN_TX_QUEUE_SIZE;
+    
+    return true;
+}
+
+/**
+ * @brief 检查发送队列是否为空
+ */
+static bool BSP_CAN_TxQueueIsEmpty(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return true;
+    
+    return tx_queues[can].head == tx_queues[can].tail;
+}
+
+/**
+ * @brief 处理所有CAN实例的发送队列
+ */
+static void BSP_CAN_TxCompleteCallback(void) {
+    // 处理所有CAN实例的发送队列
+    for (int i = 0; i < BSP_CAN_NUM; i++) {
+        BSP_CAN_t can = (BSP_CAN_t)i;
+        CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can);
+        if (hcan == NULL) continue;
+        
+        BSP_CAN_TxMessage_t msg;
+        uint32_t mailbox;
+        
+        // 尝试发送队列中的消息
+        while (!BSP_CAN_TxQueueIsEmpty(can)) {
+            // 检查是否有空闲邮箱
+            if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) == 0) {
+                break; // 没有空闲邮箱，等待下次中断
+            }
+            
+            // 从队列中取出消息
+            if (!BSP_CAN_TxQueuePop(can, &msg)) {
+                break;
+            }
+            
+            // 发送消息
+            if (HAL_CAN_AddTxMessage(hcan, &msg.header, msg.data, &mailbox) != HAL_OK) {
+                // 发送失败，消息已经从队列中移除，直接丢弃
+                break;
+            }
+        }
+    }
+}
+
+
+/**
+ * @brief FIFO0接收处理函数
+ */
+static void BSP_CAN_RxFifo0Callback(void) {
+    CAN_RxHeaderTypeDef rx_header;
+    uint8_t rx_data[BSP_CAN_MAX_DLC];
+    for (int can_idx = 0; can_idx < BSP_CAN_NUM; can_idx++) {
+        CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle((BSP_CAN_t)can_idx);
+        if (hcan == NULL) continue;
+        while (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) > 0) {
+            if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data) == HAL_OK) {
+                uint32_t original_id = (rx_header.IDE == CAN_ID_STD) ? rx_header.StdId : rx_header.ExtId;
+                BSP_CAN_FrameType_t frame_type = BSP_CAN_GetFrameType(&rx_header);
+                uint32_t parsed_id = BSP_CAN_ParseId(original_id, frame_type);
+                osMessageQueueId_t queue = BSP_CAN_FindQueue((BSP_CAN_t)can_idx, parsed_id);
+                if (queue != NULL) {
+                    BSP_CAN_Message_t msg = {0};
+                    msg.frame_type = frame_type;
+                    msg.original_id = original_id;
+                    msg.parsed_id = parsed_id;
+                    msg.dlc = rx_header.DLC;
+                    if (rx_header.RTR == CAN_RTR_DATA) {
+                        memcpy(msg.data, rx_data, rx_header.DLC);
+                    }
+                    msg.timestamp = HAL_GetTick();
+                    osMessageQueuePut(queue, &msg, 0, BSP_CAN_TIMEOUT_IMMEDIATE);
+                }
+            }
+        }
+    }
+}
+
+/**
+ * @brief FIFO1接收处理函数
+ */
+static void BSP_CAN_RxFifo1Callback(void) {
+    CAN_RxHeaderTypeDef rx_header;
+    uint8_t rx_data[BSP_CAN_MAX_DLC];
+    for (int can_idx = 0; can_idx < BSP_CAN_NUM; can_idx++) {
+        CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle((BSP_CAN_t)can_idx);
+        if (hcan == NULL) continue;
+        while (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO1) > 0) {
+            if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO1, &rx_header, rx_data) == HAL_OK) {
+                uint32_t original_id = (rx_header.IDE == CAN_ID_STD) ? rx_header.StdId : rx_header.ExtId;
+                BSP_CAN_FrameType_t frame_type = BSP_CAN_GetFrameType(&rx_header);
+                uint32_t parsed_id = BSP_CAN_ParseId(original_id, frame_type);
+                osMessageQueueId_t queue = BSP_CAN_FindQueue((BSP_CAN_t)can_idx, parsed_id);
+                if (queue != NULL) {
+                    BSP_CAN_Message_t msg = {0};
+                    msg.frame_type = frame_type;
+                    msg.original_id = original_id;
+                    msg.parsed_id = parsed_id;
+                    msg.dlc = rx_header.DLC;
+                    if (rx_header.RTR == CAN_RTR_DATA) {
+                        memcpy(msg.data, rx_data, rx_header.DLC);
+                    }
+                    msg.timestamp = HAL_GetTick();
+                    osMessageQueuePut(queue, &msg, 0, BSP_CAN_TIMEOUT_IMMEDIATE);
+                }
+            }
+        }
+    }
+}
+
+/* HAL Callback Functions --------------------------------------------------- */
+void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) {
+    BSP_CAN_t bsp_can = CAN_Get(hcan);
+    if (bsp_can != BSP_CAN_ERR) {
+        // 调用用户回调
+        if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_CPLT_CB])
+            CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_CPLT_CB]();
+    }
+}
+
+void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) {
+    BSP_CAN_t bsp_can = CAN_Get(hcan);
+    if (bsp_can != BSP_CAN_ERR) {
+        // 调用用户回调
+        if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_CPLT_CB])
+            CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_CPLT_CB]();
+    }
+}
+
+void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) {
+    BSP_CAN_t bsp_can = CAN_Get(hcan);
+    if (bsp_can != BSP_CAN_ERR) {
+        // 调用用户回调
+        if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_CPLT_CB])
+            CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_CPLT_CB]();
+    }
+}
+
+void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef *hcan) {
+    BSP_CAN_t bsp_can = CAN_Get(hcan);
+    if (bsp_can != BSP_CAN_ERR) {
+        // 调用用户回调
+        if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_ABORT_CB])
+            CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_ABORT_CB]();
+    }
+}
+
+void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef *hcan) {
+    BSP_CAN_t bsp_can = CAN_Get(hcan);
+    if (bsp_can != BSP_CAN_ERR) {
+        // 调用用户回调
+        if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_ABORT_CB])
+            CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_ABORT_CB]();
+    }
+}
+
+void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef *hcan) {
+    BSP_CAN_t bsp_can = CAN_Get(hcan);
+    if (bsp_can != BSP_CAN_ERR) {
+        // 调用用户回调
+        if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_ABORT_CB])
+            CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_ABORT_CB]();
+    }
+}
+
+void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) {
+  BSP_CAN_t bsp_can = CAN_Get(hcan);
+  if (bsp_can != BSP_CAN_ERR) {
+    if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_MSG_PENDING_CB])
+      CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_MSG_PENDING_CB]();
+  }
+}
+
+void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef *hcan) {
+  BSP_CAN_t bsp_can = CAN_Get(hcan);
+  if (bsp_can != BSP_CAN_ERR) {
+    if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_FULL_CB])
+      CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_FULL_CB]();
+  }
+}
+
+void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) {
+  BSP_CAN_t bsp_can = CAN_Get(hcan);
+  if (bsp_can != BSP_CAN_ERR) {
+    if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_MSG_PENDING_CB])
+      CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_MSG_PENDING_CB]();
+  }
+}
+
+void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef *hcan) {
+  BSP_CAN_t bsp_can = CAN_Get(hcan);
+  if (bsp_can != BSP_CAN_ERR) {
+    if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_FULL_CB])
+      CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_FULL_CB]();
+  }
+}
+
+void HAL_CAN_SleepCallback(CAN_HandleTypeDef *hcan) {
+  BSP_CAN_t bsp_can = CAN_Get(hcan);
+  if (bsp_can != BSP_CAN_ERR) {
+    if (CAN_Callback[bsp_can][HAL_CAN_SLEEP_CB])
+      CAN_Callback[bsp_can][HAL_CAN_SLEEP_CB]();
+  }
+}
+
+void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef *hcan) {
+  BSP_CAN_t bsp_can = CAN_Get(hcan);
+  if (bsp_can != BSP_CAN_ERR) {
+    if (CAN_Callback[bsp_can][HAL_CAN_WAKEUP_FROM_RX_MSG_CB])
+      CAN_Callback[bsp_can][HAL_CAN_WAKEUP_FROM_RX_MSG_CB]();
+  }
+}
+
+void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan) {
+  BSP_CAN_t bsp_can = CAN_Get(hcan);
+  if (bsp_can != BSP_CAN_ERR) {
+    if (CAN_Callback[bsp_can][HAL_CAN_ERROR_CB])
+      CAN_Callback[bsp_can][HAL_CAN_ERROR_CB]();
+  }
+}
+
+/* Exported functions ------------------------------------------------------- */
+
+int8_t BSP_CAN_Init(void) {
+    if (inited) {
+        return BSP_ERR_INITED;
+    }
+    
+    // 清零回调函数数组
+    memset(CAN_Callback, 0, sizeof(CAN_Callback));
+        
+    // 初始化发送队列
+    for (int i = 0; i < BSP_CAN_NUM; i++) {
+        BSP_CAN_TxQueueInit((BSP_CAN_t)i);
+    }
+
+    // 初始化ID解析器为默认解析器
+    id_parser = BSP_CAN_DefaultIdParser;
+    
+    // 创建互斥锁
+    queue_mutex = osMutexNew(NULL);
+    if (queue_mutex == NULL) {
+        return BSP_ERR;
+    }
+    
+/* AUTO GENERATED CAN_INIT */
+    
+    inited = true;
+    return BSP_OK;
+}
+
+
+CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) {
+        return NULL;
+    }
+    
+    switch (can) {
+/* AUTO GENERATED BSP_CAN_GET_HANDLE */
+        default:
+            return NULL;
+    }
+}
+
+int8_t BSP_CAN_RegisterCallback(BSP_CAN_t can, BSP_CAN_Callback_t type,
+                                void (*callback)(void)) {
+    if (!inited) {
+        return BSP_ERR_INITED;
+    }
+    if (callback == NULL) {
+        return BSP_ERR_NULL;
+    }
+    if (can >= BSP_CAN_NUM) {
+        return BSP_ERR;
+    }
+    if (type >= BSP_CAN_CB_NUM) {
+        return BSP_ERR;
+    }
+    
+    CAN_Callback[can][type] = callback;
+    return BSP_OK;
+}
+
+int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format,
+                        uint32_t id, uint8_t *data, uint8_t dlc) {
+    if (!inited) {
+        return BSP_ERR_INITED;
+    }
+    if (can >= BSP_CAN_NUM) {
+        return BSP_ERR;
+    }
+    if (data == NULL && format != BSP_CAN_FORMAT_STD_REMOTE && format != BSP_CAN_FORMAT_EXT_REMOTE) {
+        return BSP_ERR_NULL;
+    }
+    if (dlc > BSP_CAN_MAX_DLC) {
+        return BSP_ERR;
+    }
+    
+    CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can);
+    if (hcan == NULL) {
+        return BSP_ERR_NULL;
+    }
+    
+    // 准备发送消息
+    BSP_CAN_TxMessage_t tx_msg = {0};
+    
+    switch (format) {
+        case BSP_CAN_FORMAT_STD_DATA:
+            tx_msg.header.StdId = id;
+            tx_msg.header.IDE = CAN_ID_STD;
+            tx_msg.header.RTR = CAN_RTR_DATA;
+            break;
+        case BSP_CAN_FORMAT_EXT_DATA:
+            tx_msg.header.ExtId = id;
+            tx_msg.header.IDE = CAN_ID_EXT;
+            tx_msg.header.RTR = CAN_RTR_DATA;
+            break;
+        case BSP_CAN_FORMAT_STD_REMOTE:
+            tx_msg.header.StdId = id;
+            tx_msg.header.IDE = CAN_ID_STD;
+            tx_msg.header.RTR = CAN_RTR_REMOTE;
+            break;
+        case BSP_CAN_FORMAT_EXT_REMOTE:
+            tx_msg.header.ExtId = id;
+            tx_msg.header.IDE = CAN_ID_EXT;
+            tx_msg.header.RTR = CAN_RTR_REMOTE;
+            break;
+        default:
+            return BSP_ERR;
+    }
+    
+    tx_msg.header.DLC = dlc;
+    tx_msg.header.TransmitGlobalTime = DISABLE;
+    
+    // 复制数据
+    if (data != NULL && dlc > 0) {
+        memcpy(tx_msg.data, data, dlc);
+    }
+    
+    // 尝试直接发送到邮箱
+    uint32_t mailbox;
+    if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) > 0) {
+        HAL_StatusTypeDef result = HAL_CAN_AddTxMessage(hcan, &tx_msg.header, tx_msg.data, &mailbox);
+        if (result == HAL_OK) {
+            return BSP_OK; // 发送成功
+        }
+    }
+    
+    // 邮箱满，尝试放入队列
+    if (BSP_CAN_TxQueuePush(can, &tx_msg)) {
+        return BSP_OK; // 成功放入队列
+    }
+    
+    // 队列也满，丢弃数据
+    return BSP_ERR; // 数据丢弃
+}
+
+int8_t BSP_CAN_TransmitStdDataFrame(BSP_CAN_t can, BSP_CAN_StdDataFrame_t *frame) {
+    if (frame == NULL) {
+        return BSP_ERR_NULL;
+    }
+    return BSP_CAN_Transmit(can, BSP_CAN_FORMAT_STD_DATA, frame->id, frame->data, frame->dlc);
+}
+
+int8_t BSP_CAN_TransmitExtDataFrame(BSP_CAN_t can, BSP_CAN_ExtDataFrame_t *frame) {
+    if (frame == NULL) {
+        return BSP_ERR_NULL;
+    }
+    return BSP_CAN_Transmit(can, BSP_CAN_FORMAT_EXT_DATA, frame->id, frame->data, frame->dlc);
+}
+
+int8_t BSP_CAN_TransmitRemoteFrame(BSP_CAN_t can, BSP_CAN_RemoteFrame_t *frame) {
+    if (frame == NULL) {
+        return BSP_ERR_NULL;
+    }
+    BSP_CAN_Format_t format = frame->is_extended ? BSP_CAN_FORMAT_EXT_REMOTE : BSP_CAN_FORMAT_STD_REMOTE;
+    return BSP_CAN_Transmit(can, format, frame->id, NULL, frame->dlc);
+}
+
+int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size) {
+    if (!inited) {
+        return BSP_ERR_INITED;
+    }
+    return BSP_CAN_CreateIdQueue(can, can_id, queue_size);
+}
+
+
+int8_t BSP_CAN_GetMessage(BSP_CAN_t can, uint32_t can_id, BSP_CAN_Message_t *msg, uint32_t timeout) {
+    if (!inited) {
+        return BSP_ERR_INITED;
+    }
+    if (msg == NULL) {
+        return BSP_ERR_NULL;
+    }
+    if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) {
+        return BSP_ERR_TIMEOUT;
+    }
+    osMessageQueueId_t queue = BSP_CAN_FindQueue(can, can_id);
+    osMutexRelease(queue_mutex);
+    if (queue == NULL) {
+        return BSP_ERR_NO_DEV;
+    }
+    osStatus_t result = osMessageQueueGet(queue, msg, NULL, timeout);
+    return (result == osOK) ? BSP_OK : BSP_ERR;
+}
+
+int32_t BSP_CAN_GetQueueCount(BSP_CAN_t can, uint32_t can_id) {
+    if (!inited) {
+        return -1;
+    }
+    if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) {
+        return -1;
+    }
+    osMessageQueueId_t queue = BSP_CAN_FindQueue(can, can_id);
+    osMutexRelease(queue_mutex);
+    if (queue == NULL) {
+        return -1;
+    }
+    return (int32_t)osMessageQueueGetCount(queue);
+}
+
+int8_t BSP_CAN_FlushQueue(BSP_CAN_t can, uint32_t can_id) {
+    if (!inited) {
+        return BSP_ERR_INITED;
+    }
+    if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) {
+        return BSP_ERR_TIMEOUT;
+    }
+    osMessageQueueId_t queue = BSP_CAN_FindQueue(can, can_id);
+    osMutexRelease(queue_mutex);
+    if (queue == NULL) {
+        return BSP_ERR_NO_DEV;
+    }
+    BSP_CAN_Message_t temp_msg;
+    while (osMessageQueueGet(queue, &temp_msg, NULL, BSP_CAN_TIMEOUT_IMMEDIATE) == osOK) {
+        // 清空
+    }
+    return BSP_OK;
+}
+
+int8_t BSP_CAN_RegisterIdParser(BSP_CAN_IdParser_t parser) {
+    if (!inited) {
+        return BSP_ERR_INITED;
+    }
+    if (parser == NULL) {
+        return BSP_ERR_NULL;
+    }
+    
+    id_parser = parser;
+    return BSP_OK;
+}
+
+uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
+    if (id_parser != NULL) {
+        return id_parser(original_id, frame_type);
+    }
+    return BSP_CAN_DefaultIdParser(original_id, frame_type);
+}
+
+

assets/User_code/bsp/can.h

@@ -0,0 +1,259 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <can.h>
+#include "bsp/bsp.h"
+#include "bsp/mm.h"
+#include <stdint.h>
+#include <stdbool.h>
+#include <cmsis_os.h>
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Exported constants ------------------------------------------------------- */
+#define BSP_CAN_MAX_DLC                 8
+#define BSP_CAN_DEFAULT_QUEUE_SIZE      10
+#define BSP_CAN_TIMEOUT_IMMEDIATE       0
+#define BSP_CAN_TIMEOUT_FOREVER         osWaitForever
+#define BSP_CAN_TX_QUEUE_SIZE           32    /* 发送队列大小 */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef enum {
+  BSP_CAN_1,
+  BSP_CAN_2,
+  BSP_CAN_NUM,
+  BSP_CAN_ERR,
+} BSP_CAN_t;
+
+typedef enum {
+  HAL_CAN_TX_MAILBOX0_CPLT_CB,
+  HAL_CAN_TX_MAILBOX1_CPLT_CB,
+  HAL_CAN_TX_MAILBOX2_CPLT_CB,
+  HAL_CAN_TX_MAILBOX0_ABORT_CB,
+  HAL_CAN_TX_MAILBOX1_ABORT_CB,
+  HAL_CAN_TX_MAILBOX2_ABORT_CB,
+  HAL_CAN_RX_FIFO0_MSG_PENDING_CB,
+  HAL_CAN_RX_FIFO0_FULL_CB,
+  HAL_CAN_RX_FIFO1_MSG_PENDING_CB,
+  HAL_CAN_RX_FIFO1_FULL_CB,
+  HAL_CAN_SLEEP_CB,
+  HAL_CAN_WAKEUP_FROM_RX_MSG_CB,
+  HAL_CAN_ERROR_CB,
+  BSP_CAN_CB_NUM,
+} BSP_CAN_Callback_t;
+
+/* CAN消息格式枚举 - 用于发送和接收消息时指定格式 */
+typedef enum {
+  BSP_CAN_FORMAT_STD_DATA,    /* 标准数据帧 */
+  BSP_CAN_FORMAT_EXT_DATA,    /* 扩展数据帧 */
+  BSP_CAN_FORMAT_STD_REMOTE,  /* 标准远程帧 */
+  BSP_CAN_FORMAT_EXT_REMOTE,  /* 扩展远程帧 */
+} BSP_CAN_Format_t;
+
+/* CAN帧类型枚举 - 用于区分不同类型的CAN帧 */
+typedef enum {
+  BSP_CAN_FRAME_STD_DATA,     /* 标准数据帧 */
+  BSP_CAN_FRAME_EXT_DATA,     /* 扩展数据帧 */
+  BSP_CAN_FRAME_STD_REMOTE,   /* 标准远程帧 */
+  BSP_CAN_FRAME_EXT_REMOTE,   /* 扩展远程帧 */
+} BSP_CAN_FrameType_t;
+
+/* CAN消息结构体 - 支持不同类型帧 */
+typedef struct {
+    BSP_CAN_FrameType_t frame_type;     /* 帧类型 */
+    uint32_t original_id;               /* 原始ID（未解析） */
+    uint32_t parsed_id;                 /* 解析后的实际ID */
+    uint8_t dlc;                        /* 数据长度 */
+    uint8_t data[BSP_CAN_MAX_DLC];      /* 数据 */
+    uint32_t timestamp;                 /* 时间戳（可选） */
+} BSP_CAN_Message_t;
+
+/* 标准数据帧结构 */
+typedef struct {
+    uint32_t id;                        /* CAN ID */
+    uint8_t dlc;                        /* 数据长度 */
+    uint8_t data[BSP_CAN_MAX_DLC];      /* 数据 */
+} BSP_CAN_StdDataFrame_t;
+
+/* 扩展数据帧结构 */
+typedef struct {
+    uint32_t id;                        /* 扩展CAN ID */
+    uint8_t dlc;                        /* 数据长度 */
+    uint8_t data[BSP_CAN_MAX_DLC];      /* 数据 */
+} BSP_CAN_ExtDataFrame_t;
+
+/* 远程帧结构 */
+typedef struct {
+    uint32_t id;                        /* CAN ID */
+    uint8_t dlc;                        /* 请求的数据长度 */
+    bool is_extended;                   /* 是否为扩展帧 */
+} BSP_CAN_RemoteFrame_t;
+
+/* ID解析回调函数类型 */
+typedef uint32_t (*BSP_CAN_IdParser_t)(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
+
+/* CAN发送消息结构体 */
+typedef struct {
+    CAN_TxHeaderTypeDef header;         /* 发送头 */
+    uint8_t data[BSP_CAN_MAX_DLC];      /* 数据 */
+} BSP_CAN_TxMessage_t;
+
+/* 无锁环形队列结构体 */
+typedef struct {
+    BSP_CAN_TxMessage_t buffer[BSP_CAN_TX_QUEUE_SIZE];  /* 缓冲区 */
+    volatile uint32_t head;             /* 队列头 */
+    volatile uint32_t tail;             /* 队列尾 */
+} BSP_CAN_TxQueue_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 初始化 CAN 模块
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_Init(void);
+
+/**
+ * @brief 获取 CAN 句柄
+ * @param can CAN 枚举
+ * @return CAN_HandleTypeDef 指针，失败返回 NULL
+ */
+CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can);
+
+/**
+ * @brief 注册 CAN 回调函数
+ * @param can CAN 枚举
+ * @param type 回调类型
+ * @param callback 回调函数指针
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_RegisterCallback(BSP_CAN_t can, BSP_CAN_Callback_t type,
+                                void (*callback)(void));
+
+/**
+ * @brief 发送 CAN 消息
+ * @param can CAN 枚举
+ * @param format 消息格式
+ * @param id CAN ID
+ * @param data 数据指针
+ * @param dlc 数据长度
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format,
+                        uint32_t id, uint8_t *data, uint8_t dlc);
+
+/**
+ * @brief 发送标准数据帧
+ * @param can CAN 枚举
+ * @param frame 标准数据帧指针
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_TransmitStdDataFrame(BSP_CAN_t can, BSP_CAN_StdDataFrame_t *frame);
+
+/**
+ * @brief 发送扩展数据帧
+ * @param can CAN 枚举
+ * @param frame 扩展数据帧指针
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_TransmitExtDataFrame(BSP_CAN_t can, BSP_CAN_ExtDataFrame_t *frame);
+
+/**
+ * @brief 发送远程帧
+ * @param can CAN 枚举
+ * @param frame 远程帧指针
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_TransmitRemoteFrame(BSP_CAN_t can, BSP_CAN_RemoteFrame_t *frame);
+
+
+/**
+ * @brief 获取发送队列中待发送消息数量
+ * @param can CAN 枚举
+ * @return 队列中消息数量，-1表示错误
+ */
+int32_t BSP_CAN_GetTxQueueCount(BSP_CAN_t can);
+
+/**
+ * @brief 清空发送队列
+ * @param can CAN 枚举
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_FlushTxQueue(BSP_CAN_t can);
+
+/**
+ * @brief 注册 CAN ID 接收队列
+ * @param can CAN 枚举
+ * @param can_id 解析后的CAN ID
+ * @param queue_size 队列大小，0使用默认值
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size);
+
+
+
+/**
+ * @brief 获取 CAN 消息
+ * @param can CAN 枚举
+ * @param can_id 解析后的CAN ID
+ * @param msg 存储消息的结构体指针
+ * @param timeout 超时时间（毫秒），0为立即返回，osWaitForever为永久等待
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_GetMessage(BSP_CAN_t can, uint32_t can_id, BSP_CAN_Message_t *msg, uint32_t timeout);
+
+/**
+ * @brief 获取指定ID队列中的消息数量
+ * @param can CAN 枚举
+ * @param can_id 解析后的CAN ID
+ * @return 消息数量，-1表示队列不存在
+ */
+int32_t BSP_CAN_GetQueueCount(BSP_CAN_t can, uint32_t can_id);
+
+/**
+ * @brief 清空指定ID队列中的所有消息
+ * @param can CAN 枚举
+ * @param can_id 解析后的CAN ID
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_FlushQueue(BSP_CAN_t can, uint32_t can_id);
+
+/**
+ * @brief 注册ID解析器
+ * @param parser ID解析回调函数
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_RegisterIdParser(BSP_CAN_IdParser_t parser);
+
+
+/**
+ * @brief 解析CAN ID
+ * @param original_id 原始ID
+ * @param frame_type 帧类型
+ * @return 解析后的ID
+ */
+uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/bsp/describe.csv

@@ -0,0 +1,11 @@
+uart,请开启uart的dma和中断
+can,请开启can中断，使用函数前请确保can已经初始化。一定要开启can发送中断！！！
+gpio,会自动读取cubemx中配置为gpio的引脚，并自动区分输入输出和中断。
+spi,请开启spi的dma和中断
+i2c,要求开始spi中断
+mm,这是套了一层的动态内存分配
+time,获取时间戳函数，需要开启freerots
+dwt,需要开启dwt，获取时间
+i2c,请开启i2c的dma和中断
+pwm,用于选择那些勇于输出pwm
+

assets/User_code/bsp/dwt.c

@@ -0,0 +1,138 @@
+/**
+ ******************************************************************************
+ * @file	dwt.c
+ * @author  Wang Hongxi
+ * @version V1.1.0
+ * @date    2022/3/8
+ * @brief
+ ******************************************************************************
+ * @attention
+ *
+ ******************************************************************************
+ */
+#include "bsp/dwt.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+DWT_Time_t SysTime;
+static uint32_t CPU_FREQ_Hz, CPU_FREQ_Hz_ms, CPU_FREQ_Hz_us;
+static uint32_t CYCCNT_RountCount;
+static uint32_t CYCCNT_LAST;
+uint64_t CYCCNT64;
+static void DWT_CNT_Update(void);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+void DWT_Init(uint32_t CPU_Freq_mHz)
+{
+    /* 使能DWT外设 */
+    CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
+
+    /* DWT CYCCNT寄存器计数清0 */
+    DWT->CYCCNT = (uint32_t)0u;
+
+    /* 使能Cortex-M DWT CYCCNT寄存器 */
+    DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
+
+    CPU_FREQ_Hz = CPU_Freq_mHz * 1000000;
+    CPU_FREQ_Hz_ms = CPU_FREQ_Hz / 1000;
+    CPU_FREQ_Hz_us = CPU_FREQ_Hz / 1000000;
+    CYCCNT_RountCount = 0;
+}
+
+float DWT_GetDeltaT(uint32_t *cnt_last)
+{
+    volatile uint32_t cnt_now = DWT->CYCCNT;
+    float dt = ((uint32_t)(cnt_now - *cnt_last)) / ((float)(CPU_FREQ_Hz));
+    *cnt_last = cnt_now;
+
+    DWT_CNT_Update();
+
+    return dt;
+}
+
+double DWT_GetDeltaT64(uint32_t *cnt_last)
+{
+    volatile uint32_t cnt_now = DWT->CYCCNT;
+    double dt = ((uint32_t)(cnt_now - *cnt_last)) / ((double)(CPU_FREQ_Hz));
+    *cnt_last = cnt_now;
+
+    DWT_CNT_Update();
+
+    return dt;
+}
+
+void DWT_SysTimeUpdate(void)
+{
+    volatile uint32_t cnt_now = DWT->CYCCNT;
+    static uint64_t CNT_TEMP1, CNT_TEMP2, CNT_TEMP3;
+
+    DWT_CNT_Update();
+
+    CYCCNT64 = (uint64_t)CYCCNT_RountCount * (uint64_t)UINT32_MAX + (uint64_t)cnt_now;
+    CNT_TEMP1 = CYCCNT64 / CPU_FREQ_Hz;
+    CNT_TEMP2 = CYCCNT64 - CNT_TEMP1 * CPU_FREQ_Hz;
+    SysTime.s = CNT_TEMP1;
+    SysTime.ms = CNT_TEMP2 / CPU_FREQ_Hz_ms;
+    CNT_TEMP3 = CNT_TEMP2 - SysTime.ms * CPU_FREQ_Hz_ms;
+    SysTime.us = CNT_TEMP3 / CPU_FREQ_Hz_us;
+}
+
+float DWT_GetTimeline_s(void)
+{
+    DWT_SysTimeUpdate();
+
+    float DWT_Timelinef32 = SysTime.s + SysTime.ms * 0.001f + SysTime.us * 0.000001f;
+
+    return DWT_Timelinef32;
+}
+
+float DWT_GetTimeline_ms(void)
+{
+    DWT_SysTimeUpdate();
+
+    float DWT_Timelinef32 = SysTime.s * 1000 + SysTime.ms + SysTime.us * 0.001f;
+
+    return DWT_Timelinef32;
+}
+
+uint64_t DWT_GetTimeline_us(void)
+{
+    DWT_SysTimeUpdate();
+
+    uint64_t DWT_Timelinef32 = SysTime.s * 1000000 + SysTime.ms * 1000 + SysTime.us;
+
+    return DWT_Timelinef32;
+}
+
+static void DWT_CNT_Update(void)
+{
+    volatile uint32_t cnt_now = DWT->CYCCNT;
+
+    if (cnt_now < CYCCNT_LAST)
+        CYCCNT_RountCount++;
+
+    CYCCNT_LAST = cnt_now;
+}
+
+void DWT_Delay(float Delay)
+{
+    uint32_t tickstart = DWT->CYCCNT;
+    float wait = Delay;
+
+    while ((DWT->CYCCNT - tickstart) < wait * (float)CPU_FREQ_Hz)
+    {
+    }
+}

assets/User_code/bsp/dwt.h

@@ -0,0 +1,53 @@
+/**
+ ******************************************************************************
+ * @file	dwt.h
+ * @author  Wang Hongxi
+ * @version V1.1.0
+ * @date    2022/3/8
+ * @brief
+ ******************************************************************************
+ * @attention
+ *
+ ******************************************************************************
+ */
+#ifndef _DWT_H
+#define _DWT_H
+
+#include "main.h"
+#include "stdint.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+typedef struct
+{
+    uint32_t s;
+    uint16_t ms;
+    uint16_t us;
+} DWT_Time_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+void DWT_Init(uint32_t CPU_Freq_mHz);
+float DWT_GetDeltaT(uint32_t *cnt_last);
+double DWT_GetDeltaT64(uint32_t *cnt_last);
+float DWT_GetTimeline_s(void);
+float DWT_GetTimeline_ms(void);
+uint64_t DWT_GetTimeline_us(void);
+void DWT_Delay(float Delay);
+void DWT_SysTimeUpdate(void);
+
+extern DWT_Time_t SysTime;
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#endif /* DWT_H_ */

assets/User_code/bsp/gpio.c

@@ -0,0 +1,98 @@
+/* Includes ----------------------------------------------------------------- */
+#include "bsp/gpio.h"
+
+#include <gpio.h>
+#include <main.h>
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private define ----------------------------------------------------------- */
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+typedef struct {
+  uint16_t pin;
+  GPIO_TypeDef *gpio;
+} BSP_GPIO_MAP_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private variables -------------------------------------------------------- */
+static const BSP_GPIO_MAP_t GPIO_Map[BSP_GPIO_NUM] = {
+/* AUTO GENERATED BSP_GPIO_MAP */
+};
+
+static void (*GPIO_Callback[16])(void);
+
+/* Private function  -------------------------------------------------------- */
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
+  for (uint8_t i = 0; i < 16; i++) {
+    if (GPIO_Pin & (1 << i)) {
+      if (GPIO_Callback[i]) {
+        GPIO_Callback[i]();
+      }
+    }
+  }
+}
+
+/* Exported functions ------------------------------------------------------- */
+int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, void (*callback)(void)) {
+  if (callback == NULL) return BSP_ERR_NULL;
+  if (gpio >= BSP_GPIO_NUM) return BSP_ERR;
+
+  // 从GPIO映射中获取对应的pin值
+  uint16_t pin = GPIO_Map[gpio].pin;
+  
+  for (uint8_t i = 0; i < 16; i++) {
+    if (pin & (1 << i)) {
+      GPIO_Callback[i] = callback;
+      break;
+    }
+  }
+  return BSP_OK;
+}
+
+int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) {
+  switch (gpio) {
+/* AUTO GENERATED BSP_GPIO_ENABLE_IRQ */
+    default:
+      return BSP_ERR;
+  }
+  return BSP_OK;
+}
+
+int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) {
+  switch (gpio) {
+/* AUTO GENERATED BSP_GPIO_DISABLE_IRQ */
+    default:
+      return BSP_ERR;
+  }
+  return BSP_OK;
+}
+int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value){
+  if (gpio >= BSP_GPIO_NUM) return BSP_ERR;
+  HAL_GPIO_WritePin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin, value);
+  return BSP_OK;
+}
+
+int8_t BSP_GPIO_TogglePin(BSP_GPIO_t gpio){
+  if (gpio >= BSP_GPIO_NUM) return BSP_ERR;
+  HAL_GPIO_TogglePin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin);
+  return BSP_OK;
+}
+
+bool BSP_GPIO_ReadPin(BSP_GPIO_t gpio){
+  if (gpio >= BSP_GPIO_NUM) return false;
+  return HAL_GPIO_ReadPin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin) == GPIO_PIN_SET;
+}

assets/User_code/bsp/gpio.h

@@ -0,0 +1,47 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "bsp/bsp.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Exported types ----------------------------------------------------------- */
+typedef enum {
+/* AUTO GENERATED BSP_GPIO_ENUM */
+  BSP_GPIO_NUM,
+  BSP_GPIO_ERR,
+} BSP_GPIO_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, void (*callback)(void));
+
+int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio);
+int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio);
+
+int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value);
+int8_t BSP_GPIO_TogglePin(BSP_GPIO_t gpio);
+
+bool BSP_GPIO_ReadPin(BSP_GPIO_t gpio);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/bsp/i2c.c

@@ -0,0 +1,188 @@
+/* Includes ----------------------------------------------------------------- */
+#include "bsp\i2c.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private define ----------------------------------------------------------- */
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private variables -------------------------------------------------------- */
+static void (*I2C_Callback[BSP_I2C_NUM][BSP_I2C_CB_NUM])(void);
+
+/* Private function  -------------------------------------------------------- */
+static BSP_I2C_t I2C_Get(I2C_HandleTypeDef *hi2c) {
+/* AUTO GENERATED I2C_GET */
+  else
+    return BSP_I2C_ERR;
+}
+
+void HAL_I2C_MasterTxCpltCallback(I2C_HandleTypeDef *hi2c) {
+  BSP_I2C_t bsp_i2c = I2C_Get(hi2c);
+  if (bsp_i2c != BSP_I2C_ERR) {
+    if (I2C_Callback[bsp_i2c][HAL_I2C_MASTER_TX_CPLT_CB])
+      I2C_Callback[bsp_i2c][HAL_I2C_MASTER_TX_CPLT_CB]();
+  }
+}
+
+void HAL_I2C_MasterRxCpltCallback(I2C_HandleTypeDef *hi2c) {
+  BSP_I2C_t bsp_i2c = I2C_Get(hi2c);
+  if (bsp_i2c != BSP_I2C_ERR) {
+    if (I2C_Callback[bsp_i2c][HAL_I2C_MASTER_RX_CPLT_CB])
+      I2C_Callback[bsp_i2c][HAL_I2C_MASTER_RX_CPLT_CB]();
+  }
+}
+
+void HAL_I2C_SlaveTxCpltCallback(I2C_HandleTypeDef *hi2c) {
+  BSP_I2C_t bsp_i2c = I2C_Get(hi2c);
+  if (bsp_i2c != BSP_I2C_ERR) {
+    if (I2C_Callback[bsp_i2c][HAL_I2C_SLAVE_TX_CPLT_CB])
+      I2C_Callback[bsp_i2c][HAL_I2C_SLAVE_TX_CPLT_CB]();
+  }
+}
+
+void HAL_I2C_SlaveRxCpltCallback(I2C_HandleTypeDef *hi2c) {
+  BSP_I2C_t bsp_i2c = I2C_Get(hi2c);
+  if (bsp_i2c != BSP_I2C_ERR) {
+    if (I2C_Callback[bsp_i2c][HAL_I2C_SLAVE_RX_CPLT_CB])
+      I2C_Callback[bsp_i2c][HAL_I2C_SLAVE_RX_CPLT_CB]();
+  }
+}
+
+void HAL_I2C_ListenCpltCallback(I2C_HandleTypeDef *hi2c) {
+  BSP_I2C_t bsp_i2c = I2C_Get(hi2c);
+  if (bsp_i2c != BSP_I2C_ERR) {
+    if (I2C_Callback[bsp_i2c][HAL_I2C_LISTEN_CPLT_CB])
+      I2C_Callback[bsp_i2c][HAL_I2C_LISTEN_CPLT_CB]();
+  }
+}
+
+void HAL_I2C_MemTxCpltCallback(I2C_HandleTypeDef *hi2c) {
+  BSP_I2C_t bsp_i2c = I2C_Get(hi2c);
+  if (bsp_i2c != BSP_I2C_ERR) {
+    if (I2C_Callback[bsp_i2c][HAL_I2C_MEM_TX_CPLT_CB])
+      I2C_Callback[bsp_i2c][HAL_I2C_MEM_TX_CPLT_CB]();
+  }
+}
+
+void HAL_I2C_MemRxCpltCallback(I2C_HandleTypeDef *hi2c) {
+  BSP_I2C_t bsp_i2c = I2C_Get(hi2c);
+  if (bsp_i2c != BSP_I2C_ERR) {
+    if (I2C_Callback[bsp_i2c][HAL_I2C_MEM_RX_CPLT_CB])
+      I2C_Callback[bsp_i2c][HAL_I2C_MEM_RX_CPLT_CB]();
+  }
+}
+
+void HAL_I2C_ErrorCallback(I2C_HandleTypeDef *hi2c) {
+  BSP_I2C_t bsp_i2c = I2C_Get(hi2c);
+  if (bsp_i2c != BSP_I2C_ERR) {
+    if (I2C_Callback[bsp_i2c][HAL_I2C_ERROR_CB])
+      I2C_Callback[bsp_i2c][HAL_I2C_ERROR_CB]();
+  }
+}
+
+void HAL_I2C_AbortCpltCallback(I2C_HandleTypeDef *hi2c) {
+  BSP_I2C_t bsp_i2c = I2C_Get(hi2c);
+  if (bsp_i2c != BSP_I2C_ERR) {
+    if (I2C_Callback[bsp_i2c][HAL_I2C_ABORT_CPLT_CB])
+      I2C_Callback[bsp_i2c][HAL_I2C_ABORT_CPLT_CB]();
+  }
+}
+
+/* Exported functions ------------------------------------------------------- */
+I2C_HandleTypeDef *BSP_I2C_GetHandle(BSP_I2C_t i2c) {
+  switch (i2c) {
+/* AUTO GENERATED BSP_I2C_GET_HANDLE */
+    default:
+      return NULL;
+  }
+}
+
+int8_t BSP_I2C_RegisterCallback(BSP_I2C_t i2c, BSP_I2C_Callback_t type,
+                                void (*callback)(void)) {
+  if (callback == NULL) return BSP_ERR_NULL;
+  I2C_Callback[i2c][type] = callback;
+  return BSP_OK;
+}
+
+int8_t BSP_I2C_Transmit(BSP_I2C_t i2c, uint16_t devAddr, uint8_t *data,
+                        uint16_t size, bool dma) {
+  if (i2c >= BSP_I2C_NUM) return BSP_ERR;
+  I2C_HandleTypeDef *hi2c = BSP_I2C_GetHandle(i2c);
+  if (hi2c == NULL) return BSP_ERR;
+
+  if (dma) {
+    return HAL_I2C_Master_Transmit_DMA(hi2c, devAddr, data, size);
+  } else {
+    return HAL_I2C_Master_Transmit(hi2c, devAddr, data, size, 10);
+  }
+}
+
+int8_t BSP_I2C_Receive(BSP_I2C_t i2c, uint16_t devAddr, uint8_t *data,
+                       uint16_t size, bool dma) {
+  if (i2c >= BSP_I2C_NUM) return BSP_ERR;
+  I2C_HandleTypeDef *hi2c = BSP_I2C_GetHandle(i2c);
+  if (hi2c == NULL) return BSP_ERR;
+
+  if (dma) {
+    return HAL_I2C_Master_Receive_DMA(hi2c, devAddr, data, size);
+  } else {
+    return HAL_I2C_Master_Receive(hi2c, devAddr, data, size, 10);
+  }
+}
+
+uint8_t BSP_I2C_MemReadByte(BSP_I2C_t i2c, uint16_t devAddr, uint16_t memAddr) {
+  if (i2c >= BSP_I2C_NUM) return 0xFF;
+  I2C_HandleTypeDef *hi2c = BSP_I2C_GetHandle(i2c);
+  if (hi2c == NULL) return 0xFF;
+
+  uint8_t data;
+  HAL_I2C_Mem_Read(hi2c, devAddr, memAddr, I2C_MEMADD_SIZE_16BIT, &data, 1, HAL_MAX_DELAY);
+  return data;
+}
+
+int8_t BSP_I2C_MemWriteByte(BSP_I2C_t i2c, uint16_t devAddr, uint16_t memAddr,
+                            uint8_t data) {
+  if (i2c >= BSP_I2C_NUM) return BSP_ERR;
+  I2C_HandleTypeDef *hi2c = BSP_I2C_GetHandle(i2c);
+  if (hi2c == NULL) return BSP_ERR;
+
+  return HAL_I2C_Mem_Write(hi2c, devAddr, memAddr, I2C_MEMADD_SIZE_16BIT, &data, 1, HAL_MAX_DELAY);
+}
+
+int8_t BSP_I2C_MemRead(BSP_I2C_t i2c, uint16_t devAddr, uint16_t memAddr,
+                       uint8_t *data, uint16_t size, bool dma) {
+  if (i2c >= BSP_I2C_NUM || data == NULL || size == 0) return BSP_ERR;
+  I2C_HandleTypeDef *hi2c = BSP_I2C_GetHandle(i2c);
+  if (hi2c == NULL) return BSP_ERR;
+
+  if (dma) {
+    return HAL_I2C_Mem_Read_DMA(hi2c, devAddr, memAddr, I2C_MEMADD_SIZE_16BIT, data, size);
+  }
+  else {
+    return HAL_I2C_Mem_Read(hi2c, devAddr, memAddr, I2C_MEMADD_SIZE_16BIT, data, size, HAL_MAX_DELAY);
+  }
+}
+
+
+int8_t BSP_I2C_MemWrite(BSP_I2C_t i2c, uint16_t devAddr, uint16_t memAddr,
+                        uint8_t *data, uint16_t size, bool dma) {
+  if (i2c >= BSP_I2C_NUM || data == NULL || size == 0) return BSP_ERR;
+  I2C_HandleTypeDef *hi2c = BSP_I2C_GetHandle(i2c);
+  if (hi2c == NULL) return BSP_ERR;
+
+  if (dma) {
+    return HAL_I2C_Mem_Write_DMA(hi2c, devAddr, memAddr, I2C_MEMADD_SIZE_16BIT, data, size);
+  } else {
+    return HAL_I2C_Mem_Write(hi2c, devAddr, memAddr, I2C_MEMADD_SIZE_16BIT, data, size, HAL_MAX_DELAY);
+  }
+}

assets/User_code/bsp/i2c.h

@@ -0,0 +1,78 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <i2c.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "bsp/bsp.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Exported types ----------------------------------------------------------- */
+
+/* 要添加使用I2C的新设备，需要先在此添加对应的枚举值 */
+
+/* I2C实体枚举，与设备对应 */
+typedef enum {
+/* AUTO GENERATED BSP_I2C_NAME */
+  /* USER BSP_I2C BEGIN*/
+  /* USER_I2C_XXX */
+  /* USER BSP_I2C END */
+  BSP_I2C_NUM,
+  BSP_I2C_ERR,
+} BSP_I2C_t;
+
+/* I2C支持的中断回调函数类型*/
+typedef enum {
+  HAL_I2C_MASTER_TX_CPLT_CB,
+  HAL_I2C_MASTER_RX_CPLT_CB,
+  HAL_I2C_SLAVE_TX_CPLT_CB,
+  HAL_I2C_SLAVE_RX_CPLT_CB,
+  HAL_I2C_LISTEN_CPLT_CB,
+  HAL_I2C_MEM_TX_CPLT_CB,
+  HAL_I2C_MEM_RX_CPLT_CB,
+  HAL_I2C_ERROR_CB,
+  HAL_I2C_ABORT_CPLT_CB,
+  BSP_I2C_CB_NUM,
+} BSP_I2C_Callback_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+I2C_HandleTypeDef *BSP_I2C_GetHandle(BSP_I2C_t i2c);
+int8_t BSP_I2C_RegisterCallback(BSP_I2C_t i2c, BSP_I2C_Callback_t type,
+                                void (*callback)(void));
+
+int8_t BSP_I2C_Transmit(BSP_I2C_t i2c, uint16_t devAddr, uint8_t *data,
+                        uint16_t size, bool dma);
+int8_t BSP_I2C_Receive(BSP_I2C_t i2c, uint16_t devAddr, uint8_t *data,
+                       uint16_t size, bool dma);
+
+
+uint8_t BSP_I2C_MemReadByte(BSP_I2C_t i2c, uint16_t devAddr, uint16_t memAddr);
+int8_t BSP_I2C_MemWriteByte(BSP_I2C_t i2c, uint16_t devAddr, uint16_t memAddr,
+                            uint8_t data);
+
+int8_t BSP_I2C_MemRead(BSP_I2C_t i2c, uint16_t devAddr, uint16_t memAddr,
+                       uint8_t *data, uint16_t size, bool dma);
+int8_t BSP_I2C_MemWrite(BSP_I2C_t i2c, uint16_t devAddr, uint16_t memAddr,
+                        uint8_t *data, uint16_t size, bool dma);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/bsp/mm.c

@@ -0,0 +1,30 @@
+/* Includes ----------------------------------------------------------------- */
+#include "bsp/mm.h"
+
+#include "FreeRTOS.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Private variables -------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private function  -------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+inline void *BSP_Malloc(size_t size) { return pvPortMalloc(size); }
+
+inline void BSP_Free(void *pv) { vPortFree(pv); }
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */

assets/User_code/bsp/mm.h

@@ -0,0 +1,32 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stddef.h>
+#include <stdint.h>
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Exported types ----------------------------------------------------------- */
+/* Exported functions prototypes -------------------------------------------- */
+void *BSP_Malloc(size_t size);
+void BSP_Free(void *pv);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/bsp/pwm.c

@@ -0,0 +1,110 @@
+/* Includes ----------------------------------------------------------------- */
+#include "tim.h"
+#include "bsp/pwm.h"
+#include "bsp.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private define ----------------------------------------------------------- */
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+typedef struct {
+  TIM_HandleTypeDef *tim;
+  uint16_t channel;
+} BSP_PWM_Config_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private variables -------------------------------------------------------- */
+static const BSP_PWM_Config_t PWM_Map[BSP_PWM_NUM] = {
+/* AUTO GENERATED BSP_PWM_MAP */
+};
+
+/* Private function  -------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch) {
+  if (ch >= BSP_PWM_NUM) return BSP_ERR;
+  
+  HAL_TIM_PWM_Start(PWM_Map[ch].tim, PWM_Map[ch].channel);
+  return BSP_OK;
+}
+
+int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle) {
+  if (ch >= BSP_PWM_NUM) return BSP_ERR;
+  
+  if (duty_cycle > 1.0f) {
+    duty_cycle = 1.0f;
+  }
+  if (duty_cycle < 0.0f) {
+    duty_cycle = 0.0f;
+  }
+  // 获取ARR值（周期值）
+  uint32_t arr = __HAL_TIM_GET_AUTORELOAD(PWM_Map[ch].tim);
+  
+  // 计算比较值：CCR = duty_cycle * (ARR + 1)
+  uint32_t ccr = (uint32_t)(duty_cycle * (arr + 1));
+  
+  __HAL_TIM_SET_COMPARE(PWM_Map[ch].tim, PWM_Map[ch].channel, ccr);
+
+  return BSP_OK;
+}
+
+int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq) {
+  if (ch >= BSP_PWM_NUM) return BSP_ERR;
+
+  uint32_t timer_clock = HAL_RCC_GetPCLK1Freq(); // Get the timer clock frequency
+  uint32_t prescaler = PWM_Map[ch].tim->Init.Prescaler;
+  uint32_t period = (timer_clock / (prescaler + 1)) / freq - 1;
+  
+  if (period > UINT16_MAX) {
+    return BSP_ERR; // Frequency too low
+  }
+  __HAL_TIM_SET_AUTORELOAD(PWM_Map[ch].tim, period);
+  
+  return BSP_OK;
+}
+
+int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch) {
+  if (ch >= BSP_PWM_NUM) return BSP_ERR;
+  
+  HAL_TIM_PWM_Stop(PWM_Map[ch].tim, PWM_Map[ch].channel);
+  return BSP_OK;
+}
+
+uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch) {
+    if (ch >= BSP_PWM_NUM) return BSP_ERR;
+    return PWM_Map[ch].tim->Init.AutoReloadPreload;  
+}
+
+TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch) {
+  return PWM_Map[ch].tim;
+}
+
+
+uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch) {
+  if (ch >= BSP_PWM_NUM) return BSP_ERR;
+  return PWM_Map[ch].channel;
+}
+
+int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length) {
+  if (ch >= BSP_PWM_NUM) return BSP_ERR;
+  
+  HAL_TIM_PWM_Start_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel, pData, Length);
+  return BSP_OK;
+}
+
+int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch) {
+  if (ch >= BSP_PWM_NUM) return BSP_ERR;
+  
+  HAL_TIM_PWM_Stop_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel);
+  return BSP_OK;
+}

assets/User_code/bsp/pwm.h

@@ -0,0 +1,48 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdint.h>
+#include "tim.h"
+#include "bsp.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Exported types ----------------------------------------------------------- */
+/* PWM通道 */
+typedef enum {
+/* AUTO GENERATED BSP_PWM_ENUM */
+  BSP_PWM_NUM,
+  BSP_PWM_ERR,
+} BSP_PWM_Channel_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch);
+int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle);
+int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq);
+int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch);
+uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch);
+uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch);
+TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch);
+int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length);
+int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/bsp/spi.c

@@ -0,0 +1,181 @@
+/* Includes ----------------------------------------------------------------- */
+#include <spi.h>
+#include "bsp/spi.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private define ----------------------------------------------------------- */
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private variables -------------------------------------------------------- */
+static void (*SPI_Callback[BSP_SPI_NUM][BSP_SPI_CB_NUM])(void);
+
+/* Private function  -------------------------------------------------------- */
+static BSP_SPI_t SPI_Get(SPI_HandleTypeDef *hspi) {
+  if (hspi->Instance == SPI1)
+    return BSP_SPI_BMI088;
+  else
+    return BSP_SPI_ERR;
+}
+
+void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *hspi) {
+  BSP_SPI_t bsp_spi = SPI_Get(hspi);
+  if (bsp_spi != BSP_SPI_ERR) {
+    if (SPI_Callback[bsp_spi][BSP_SPI_TX_CPLT_CB]) {
+      SPI_Callback[bsp_spi][BSP_SPI_TX_CPLT_CB]();
+    }
+  }
+}
+
+void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi) {
+  BSP_SPI_t bsp_spi = SPI_Get(hspi);
+  if (bsp_spi != BSP_SPI_ERR) {
+    if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_CPLT_CB])
+      SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_CPLT_CB]();
+  }
+}
+
+void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) {
+  BSP_SPI_t bsp_spi = SPI_Get(hspi);
+  if (bsp_spi != BSP_SPI_ERR) {
+    if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_CPLT_CB])
+      SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_CPLT_CB]();
+  }
+}
+
+void HAL_SPI_TxHalfCpltCallback(SPI_HandleTypeDef *hspi) {
+  BSP_SPI_t bsp_spi = SPI_Get(hspi);
+  if (bsp_spi != BSP_SPI_ERR) {
+    if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_HALF_CPLT_CB])
+      SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_HALF_CPLT_CB]();
+  }
+}
+
+void HAL_SPI_RxHalfCpltCallback(SPI_HandleTypeDef *hspi) {
+  BSP_SPI_t bsp_spi = SPI_Get(hspi);
+  if (bsp_spi != BSP_SPI_ERR) {
+    if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_HALF_CPLT_CB])
+      SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_HALF_CPLT_CB]();
+  }
+}
+
+void HAL_SPI_TxRxHalfCpltCallback(SPI_HandleTypeDef *hspi) {
+  BSP_SPI_t bsp_spi = SPI_Get(hspi);
+  if (bsp_spi != BSP_SPI_ERR) {
+    if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_HALF_CPLT_CB])
+      SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_HALF_CPLT_CB]();
+  }
+}
+
+void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi) {
+  BSP_SPI_t bsp_spi = SPI_Get(hspi);
+  if (bsp_spi != BSP_SPI_ERR) {
+    if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_ERROR_CB])
+      SPI_Callback[SPI_Get(hspi)][BSP_SPI_ERROR_CB]();
+  }
+}
+
+void HAL_SPI_AbortCpltCallback(SPI_HandleTypeDef *hspi) {
+  BSP_SPI_t bsp_spi = SPI_Get(hspi);
+  if (bsp_spi != BSP_SPI_ERR) {
+    if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_ABORT_CPLT_CB])
+      SPI_Callback[SPI_Get(hspi)][BSP_SPI_ABORT_CPLT_CB]();
+  }
+}
+
+/* Exported functions ------------------------------------------------------- */
+SPI_HandleTypeDef *BSP_SPI_GetHandle(BSP_SPI_t spi) {
+  switch (spi) {
+    case BSP_SPI_BMI088:
+      return &hspi1;
+    default:
+      return NULL;
+  }
+}
+
+int8_t BSP_SPI_RegisterCallback(BSP_SPI_t spi, BSP_SPI_Callback_t type,
+                                void (*callback)(void)) {
+  if (callback == NULL) return BSP_ERR_NULL;
+  SPI_Callback[spi][type] = callback;
+  return BSP_OK;
+}
+
+int8_t BSP_SPI_Transmit(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma) {
+  if (spi >= BSP_SPI_NUM) return BSP_ERR;
+  SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi);
+  if (hspi == NULL) return BSP_ERR;
+
+  if (dma) {
+    return HAL_SPI_Transmit_DMA(hspi, data, size)!= HAL_OK;;
+  } else {
+    return HAL_SPI_Transmit(hspi, data, size, 20)!= HAL_OK;;
+  }
+}
+
+int8_t BSP_SPI_Receive(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma) {
+  if (spi >= BSP_SPI_NUM) return BSP_ERR;
+  SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi);
+  if (hspi == NULL) return BSP_ERR;
+
+  if (dma) {
+    return HAL_SPI_Receive_DMA(hspi, data, size)!= HAL_OK;;
+  } else {
+    return HAL_SPI_Receive(hspi, data, size, 20)!= HAL_OK;;
+  }
+}
+
+int8_t BSP_SPI_TransmitReceive(BSP_SPI_t spi, uint8_t *txData, uint8_t *rxData,
+                               uint16_t size, bool dma) {
+  if (spi >= BSP_SPI_NUM) return BSP_ERR;
+  SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi);
+  if (hspi == NULL) return BSP_ERR;
+  
+  if (dma) {
+    return HAL_SPI_TransmitReceive_DMA(hspi, txData, rxData, size)!= HAL_OK;;
+  } else {
+    return HAL_SPI_TransmitReceive(hspi, txData, rxData, size, 20)!= HAL_OK;;
+  }
+}
+
+uint8_t BSP_SPI_MemReadByte(BSP_SPI_t spi, uint8_t reg) {
+  if (spi >= BSP_SPI_NUM) return 0xFF;
+  uint8_t tmp[2] = {reg | 0x80, 0x00};
+  BSP_SPI_TransmitReceive(spi, tmp, tmp, 2u, true);
+  return tmp[1];
+}
+
+int8_t BSP_SPI_MemWriteByte(BSP_SPI_t spi, uint8_t reg, uint8_t data) {
+  if (spi >= BSP_SPI_NUM) return BSP_ERR;
+  uint8_t tmp[2] = {reg & 0x7f, data};
+  return BSP_SPI_Transmit(spi, tmp, 2u, true);
+}
+
+int8_t BSP_SPI_MemRead(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size) {
+  if (spi >= BSP_SPI_NUM) return BSP_ERR;
+  if (data == NULL || size == 0) return BSP_ERR_NULL;
+  reg = reg | 0x80;
+  BSP_SPI_Transmit(spi, &reg, 1u, true);
+  return BSP_SPI_Receive(spi, data, size, true);
+}
+
+int8_t BSP_SPI_MemWrite(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size) {
+  if (spi >= BSP_SPI_NUM) return BSP_ERR;
+  if (data == NULL || size == 0) return BSP_ERR_NULL;
+  reg = reg & 0x7f;
+  BSP_SPI_Transmit(spi, &reg, 1u, true);
+  return BSP_SPI_Transmit(spi, data, size, true);
+}
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */

assets/User_code/bsp/spi.h

@@ -0,0 +1,70 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <spi.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "bsp/bsp.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Exported types ----------------------------------------------------------- */
+
+/* 要添加使用SPI的新设备，需要先在此添加对应的枚举值 */
+
+/* SPI实体枚举，与设备对应 */
+typedef enum {
+/* AUTO GENERATED BSP_SPI_NAME */
+  BSP_SPI_NUM,
+  BSP_SPI_ERR,
+} BSP_SPI_t;
+
+/* SPI支持的中断回调函数类型，具体参考HAL中定义 */
+typedef enum {
+  BSP_SPI_TX_CPLT_CB,
+  BSP_SPI_RX_CPLT_CB,
+  BSP_SPI_TX_RX_CPLT_CB,
+  BSP_SPI_TX_HALF_CPLT_CB,
+  BSP_SPI_RX_HALF_CPLT_CB,
+  BSP_SPI_TX_RX_HALF_CPLT_CB,
+  BSP_SPI_ERROR_CB,
+  BSP_SPI_ABORT_CPLT_CB,
+  BSP_SPI_CB_NUM,
+} BSP_SPI_Callback_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+SPI_HandleTypeDef *BSP_SPI_GetHandle(BSP_SPI_t spi);
+int8_t BSP_SPI_RegisterCallback(BSP_SPI_t spi, BSP_SPI_Callback_t type,
+                                void (*callback)(void));
+
+
+int8_t BSP_SPI_Transmit(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma);
+int8_t BSP_SPI_Receive(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma);
+int8_t BSP_SPI_TransmitReceive(BSP_SPI_t spi, uint8_t *txData, uint8_t *rxData,
+                               uint16_t size, bool dma);
+
+uint8_t BSP_SPI_MemReadByte(BSP_SPI_t spi, uint8_t reg);
+int8_t BSP_SPI_MemWriteByte(BSP_SPI_t spi, uint8_t reg, uint8_t data);
+int8_t BSP_SPI_MemRead(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size);
+int8_t BSP_SPI_MemWrite(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/bsp/time.c

@@ -0,0 +1,81 @@
+/* Includes ----------------------------------------------------------------- */
+#include "bsp/time.h"
+#include "bsp.h"
+
+#include <cmsis_os2.h>
+#include "FreeRTOS.h"
+#include "main.h"
+#include "task.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+/* Private define ----------------------------------------------------------- */
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private variables -------------------------------------------------------- */
+/* Private function  -------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+uint32_t BSP_TIME_Get_ms() { return xTaskGetTickCount(); }
+
+uint64_t BSP_TIME_Get_us() {
+  uint32_t tick_freq = osKernelGetTickFreq();
+  uint32_t ticks_old = xTaskGetTickCount()*(1000/tick_freq);
+  uint32_t tick_value_old = SysTick->VAL;
+  uint32_t ticks_new = xTaskGetTickCount()*(1000/tick_freq);
+  uint32_t tick_value_new = SysTick->VAL;
+  if (ticks_old == ticks_new) {
+    return ticks_new * 1000 + 1000 - tick_value_old * 1000 / (SysTick->LOAD + 1);
+  } else {
+    return ticks_new * 1000 + 1000 - tick_value_new * 1000 / (SysTick->LOAD + 1);
+  }
+}
+
+uint64_t BSP_TIME_Get() __attribute__((alias("BSP_TIME_Get_us")));
+
+int8_t BSP_TIME_Delay_ms(uint32_t ms) {
+  uint32_t tick_period = 1000u / osKernelGetTickFreq();
+  uint32_t ticks = ms / tick_period;
+
+  switch (osKernelGetState()) {
+    case osKernelError:
+    case osKernelReserved:
+    case osKernelLocked:
+    case osKernelSuspended:
+      return BSP_ERR;
+
+    case osKernelRunning:
+      osDelay(ticks ? ticks : 1);
+      break;
+
+    case osKernelInactive:
+    case osKernelReady:
+      HAL_Delay(ms);
+      break;
+  }
+  return BSP_OK;
+}
+
+/*阻塞us延迟*/
+int8_t BSP_TIME_Delay_us(uint32_t us) {
+    uint64_t start = BSP_TIME_Get_us();
+    while (BSP_TIME_Get_us() - start < us) {
+        // 等待us时间
+    }
+    return BSP_OK;
+}
+
+int8_t BSP_TIME_Delay(uint32_t ms) __attribute__((alias("BSP_TIME_Delay_ms")));
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */

assets/User_code/bsp/time.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdint.h>
+
+#include "bsp/bsp.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Exported types ----------------------------------------------------------- */
+/* Exported functions prototypes -------------------------------------------- */
+uint32_t BSP_TIME_Get_ms();
+
+uint64_t BSP_TIME_Get_us();
+
+uint64_t BSP_TIME_Get();
+
+int8_t BSP_TIME_Delay_ms(uint32_t ms);
+
+/*微秒阻塞延时，一般别用*/
+int8_t BSP_TIME_Delay_us(uint32_t us);
+
+int8_t BSP_TIME_Delay(uint32_t ms);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/bsp/uart.c

@@ -0,0 +1,153 @@
+/* Includes ----------------------------------------------------------------- */
+#include <usart.h>
+
+#include "bsp/uart.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private define ----------------------------------------------------------- */
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private variables -------------------------------------------------------- */
+static void (*UART_Callback[BSP_UART_NUM][BSP_UART_CB_NUM])(void);
+
+/* Private function  -------------------------------------------------------- */
+static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
+/* AUTO GENERATED UART_GET */
+  else
+    return BSP_UART_ERR;
+}
+
+void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart) {
+  BSP_UART_t bsp_uart = UART_Get(huart);
+  if (bsp_uart != BSP_UART_ERR) {
+    if (UART_Callback[bsp_uart][BSP_UART_TX_CPLT_CB]) {
+      UART_Callback[bsp_uart][BSP_UART_TX_CPLT_CB]();
+    }
+  }
+}
+
+void HAL_UART_TxHalfCpltCallback(UART_HandleTypeDef *huart) {
+  BSP_UART_t bsp_uart = UART_Get(huart);
+  if (bsp_uart != BSP_UART_ERR) {
+    if (UART_Callback[bsp_uart][BSP_UART_TX_HALF_CPLT_CB]) {
+      UART_Callback[bsp_uart][BSP_UART_TX_HALF_CPLT_CB]();
+    }
+  }
+}
+
+void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
+  BSP_UART_t bsp_uart = UART_Get(huart);
+  if (bsp_uart != BSP_UART_ERR) {
+    if (UART_Callback[bsp_uart][BSP_UART_RX_CPLT_CB]) {
+      UART_Callback[bsp_uart][BSP_UART_RX_CPLT_CB]();
+    }
+  }
+}
+
+void HAL_UART_RxHalfCpltCallback(UART_HandleTypeDef *huart) {
+  BSP_UART_t bsp_uart = UART_Get(huart);
+  if (bsp_uart != BSP_UART_ERR) {
+    if (UART_Callback[bsp_uart][BSP_UART_RX_HALF_CPLT_CB]) {
+      UART_Callback[bsp_uart][BSP_UART_RX_HALF_CPLT_CB]();
+    }
+  }
+}
+
+void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) {
+  BSP_UART_t bsp_uart = UART_Get(huart);
+  if (bsp_uart != BSP_UART_ERR) {
+    if (UART_Callback[bsp_uart][BSP_UART_ERROR_CB]) {
+      UART_Callback[bsp_uart][BSP_UART_ERROR_CB]();
+    }
+  }
+}
+
+void HAL_UART_AbortCpltCallback(UART_HandleTypeDef *huart) {
+  BSP_UART_t bsp_uart = UART_Get(huart);
+  if (bsp_uart != BSP_UART_ERR) {
+    if (UART_Callback[bsp_uart][BSP_UART_ABORT_CPLT_CB]) {
+      UART_Callback[bsp_uart][BSP_UART_ABORT_CPLT_CB]();
+    }
+  }
+}
+
+void HAL_UART_AbortTransmitCpltCallback(UART_HandleTypeDef *huart) {
+  BSP_UART_t bsp_uart = UART_Get(huart);
+  if (bsp_uart != BSP_UART_ERR) {
+    if (UART_Callback[bsp_uart][BSP_UART_ABORT_TX_CPLT_CB]) {
+      UART_Callback[bsp_uart][BSP_UART_ABORT_TX_CPLT_CB]();
+    }
+  }
+}
+
+void HAL_UART_AbortReceiveCpltCallback(UART_HandleTypeDef *huart) {
+  BSP_UART_t bsp_uart = UART_Get(huart);
+  if (bsp_uart != BSP_UART_ERR) {
+    if (UART_Callback[bsp_uart][BSP_UART_ABORT_RX_CPLT_CB]) {
+      UART_Callback[bsp_uart][BSP_UART_ABORT_RX_CPLT_CB]();
+    }
+  }
+}
+
+/* Exported functions ------------------------------------------------------- */
+void BSP_UART_IRQHandler(UART_HandleTypeDef *huart) {
+  if (__HAL_UART_GET_FLAG(huart, UART_FLAG_IDLE)) {
+    __HAL_UART_CLEAR_IDLEFLAG(huart);
+    if (UART_Callback[UART_Get(huart)][BSP_UART_IDLE_LINE_CB]) {
+      UART_Callback[UART_Get(huart)][BSP_UART_IDLE_LINE_CB]();
+    }
+  }
+}
+
+UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
+  switch (uart) {
+/* AUTO GENERATED BSP_UART_GET_HANDLE */
+    default:
+      return NULL;
+  }
+}
+
+int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type,
+                                 void (*callback)(void)) {
+  if (callback == NULL) return BSP_ERR_NULL;
+  if (uart >= BSP_UART_NUM || type >= BSP_UART_CB_NUM) return BSP_ERR;
+  UART_Callback[uart][type] = callback;
+  return BSP_OK;
+}
+
+int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma) {
+  if (uart >= BSP_UART_NUM) return BSP_ERR;
+  if (data == NULL || size == 0) return BSP_ERR_NULL;
+
+  if (dma) {
+    return HAL_UART_Transmit_DMA(BSP_UART_GetHandle(uart), data, size);
+  } else {
+    return HAL_UART_Transmit_IT(BSP_UART_GetHandle(uart), data, size);
+  }
+}
+
+int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma) {
+  if (uart >= BSP_UART_NUM) return BSP_ERR;
+  if (data == NULL || size == 0) return BSP_ERR_NULL;
+
+  if (dma) {
+    return HAL_UART_Receive_DMA(BSP_UART_GetHandle(uart), data, size);
+  } else {
+    return HAL_UART_Receive_IT(BSP_UART_GetHandle(uart), data, size);
+  }
+}
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */

assets/User_code/bsp/uart.h

@@ -0,0 +1,68 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <usart.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "bsp/bsp.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Exported types ----------------------------------------------------------- */
+
+/* 要添加使用UART的新设备，需要先在此添加对应的枚举值 */
+
+/* UART实体枚举，与设备对应 */
+typedef enum {
+/* AUTO GENERATED BSP_UART_NAME */
+  BSP_UART_NUM,
+  BSP_UART_ERR,
+} BSP_UART_t;
+
+/* UART支持的中断回调函数类型，具体参考HAL中定义 */
+typedef enum {
+  BSP_UART_TX_HALF_CPLT_CB,
+  BSP_UART_TX_CPLT_CB,
+  BSP_UART_RX_HALF_CPLT_CB,
+  BSP_UART_RX_CPLT_CB,
+  BSP_UART_ERROR_CB,
+  BSP_UART_ABORT_CPLT_CB,
+  BSP_UART_ABORT_TX_CPLT_CB,
+  BSP_UART_ABORT_RX_CPLT_CB,
+
+  BSP_UART_IDLE_LINE_CB,
+  BSP_UART_CB_NUM,
+} BSP_UART_Callback_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart);
+
+void BSP_UART_IRQHandler(UART_HandleTypeDef *huart);
+
+int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type,
+                                 void (*callback)(void));
+
+int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma);
+int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/component/FreeRTOS_CLI.c

@@ -0,0 +1,352 @@
+/*
+ * FreeRTOS+CLI V1.0.4
+ * Copyright (C) 2017 Amazon.com, Inc. or its affiliates.  All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of
+ * this software and associated documentation files (the "Software"), to deal in
+ * the Software without restriction, including without limitation the rights to
+ * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
+ * the Software, and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
+ * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+ * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+ * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * http://www.FreeRTOS.org
+ * http://aws.amazon.com/freertos
+ *
+ * 1 tab == 4 spaces!
+ */
+
+/* Standard includes. */
+#include <string.h>
+#include <stdint.h>
+
+/* FreeRTOS includes. */
+#include "FreeRTOS.h"
+#include "task.h"
+
+/* Utils includes. */
+#include "FreeRTOS_CLI.h"
+
+/* If the application writer needs to place the buffer used by the CLI at a
+fixed address then set configAPPLICATION_PROVIDES_cOutputBuffer to 1 in
+FreeRTOSConfig.h, then declare an array with the following name and size in 
+one of the application files:
+	char cOutputBuffer[ configCOMMAND_INT_MAX_OUTPUT_SIZE ];
+*/
+#ifndef configAPPLICATION_PROVIDES_cOutputBuffer
+	#define configAPPLICATION_PROVIDES_cOutputBuffer 0
+#endif
+
+typedef struct xCOMMAND_INPUT_LIST
+{
+	const CLI_Command_Definition_t *pxCommandLineDefinition;
+	struct xCOMMAND_INPUT_LIST *pxNext;
+} CLI_Definition_List_Item_t;
+
+/*
+ * The callback function that is executed when "help" is entered.  This is the
+ * only default command that is always present.
+ */
+static BaseType_t prvHelpCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString );
+
+/*
+ * Return the number of parameters that follow the command name.
+ */
+static int8_t prvGetNumberOfParameters( const char *pcCommandString );
+
+/* The definition of the "help" command.  This command is always at the front
+of the list of registered commands. */
+static const CLI_Command_Definition_t xHelpCommand =
+{
+	"help",
+	"\r\nhelp:\r\n Lists all the registered commands\r\n\r\n",
+	prvHelpCommand,
+	0
+};
+
+/* The definition of the list of commands.  Commands that are registered are
+added to this list. */
+static CLI_Definition_List_Item_t xRegisteredCommands =
+{
+	&xHelpCommand,	/* The first command in the list is always the help command, defined in this file. */
+	NULL			/* The next pointer is initialised to NULL, as there are no other registered commands yet. */
+};
+
+/* A buffer into which command outputs can be written is declared here, rather
+than in the command console implementation, to allow multiple command consoles
+to share the same buffer.  For example, an application may allow access to the
+command interpreter by UART and by Ethernet.  Sharing a buffer is done purely
+to save RAM.  Note, however, that the command console itself is not re-entrant,
+so only one command interpreter interface can be used at any one time.  For that
+reason, no attempt at providing mutual exclusion to the cOutputBuffer array is
+attempted.
+
+configAPPLICATION_PROVIDES_cOutputBuffer is provided to allow the application
+writer to provide their own cOutputBuffer declaration in cases where the
+buffer needs to be placed at a fixed address (rather than by the linker). */
+
+#if( configAPPLICATION_PROVIDES_cOutputBuffer == 0 )
+	static char cOutputBuffer[ configCOMMAND_INT_MAX_OUTPUT_SIZE ];
+#else
+	extern char cOutputBuffer[ configCOMMAND_INT_MAX_OUTPUT_SIZE ];
+#endif
+
+
+/*---------------------------------------------------------- */
+
+BaseType_t FreeRTOS_CLIRegisterCommand( const CLI_Command_Definition_t * const pxCommandToRegister )
+{
+static CLI_Definition_List_Item_t *pxLastCommandInList = &xRegisteredCommands;
+CLI_Definition_List_Item_t *pxNewListItem;
+BaseType_t xReturn = pdFAIL;
+
+	/* Check the parameter is not NULL. */
+	configASSERT( pxCommandToRegister );
+
+	/* Create a new list item that will reference the command being registered. */
+	pxNewListItem = ( CLI_Definition_List_Item_t * ) pvPortMalloc( sizeof( CLI_Definition_List_Item_t ) );
+	configASSERT( pxNewListItem );
+
+	if( pxNewListItem != NULL )
+	{
+		taskENTER_CRITICAL();
+		{
+			/* Reference the command being registered from the newly created
+			list item. */
+			pxNewListItem->pxCommandLineDefinition = pxCommandToRegister;
+
+			/* The new list item will get added to the end of the list, so
+			pxNext has nowhere to point. */
+			pxNewListItem->pxNext = NULL;
+
+			/* Add the newly created list item to the end of the already existing
+			list. */
+			pxLastCommandInList->pxNext = pxNewListItem;
+
+			/* Set the end of list marker to the new list item. */
+			pxLastCommandInList = pxNewListItem;
+		}
+		taskEXIT_CRITICAL();
+
+		xReturn = pdPASS;
+	}
+
+	return xReturn;
+}
+/*---------------------------------------------------------- */
+
+BaseType_t FreeRTOS_CLIProcessCommand( const char * const pcCommandInput, char * pcWriteBuffer, size_t xWriteBufferLen  )
+{
+static const CLI_Definition_List_Item_t *pxCommand = NULL;
+BaseType_t xReturn = pdTRUE;
+const char *pcRegisteredCommandString;
+size_t xCommandStringLength;
+
+	/* Note:  This function is not re-entrant.  It must not be called from more
+	thank one task. */
+
+	if( pxCommand == NULL )
+	{
+		/* Search for the command string in the list of registered commands. */
+		for( pxCommand = &xRegisteredCommands; pxCommand != NULL; pxCommand = pxCommand->pxNext )
+		{
+			pcRegisteredCommandString = pxCommand->pxCommandLineDefinition->pcCommand;
+			xCommandStringLength = strlen( pcRegisteredCommandString );
+
+			/* To ensure the string lengths match exactly, so as not to pick up
+			a sub-string of a longer command, check the byte after the expected
+			end of the string is either the end of the string or a space before
+			a parameter. */
+			if( ( pcCommandInput[ xCommandStringLength ] == ' ' ) || ( pcCommandInput[ xCommandStringLength ] == 0x00 ) )
+			{
+				if( strncmp( pcCommandInput, pcRegisteredCommandString, xCommandStringLength ) == 0 )
+				{
+					/* The command has been found.  Check it has the expected
+					number of parameters.  If cExpectedNumberOfParameters is -1,
+					then there could be a variable number of parameters and no
+					check is made. */
+					if( pxCommand->pxCommandLineDefinition->cExpectedNumberOfParameters >= 0 )
+					{
+						if( prvGetNumberOfParameters( pcCommandInput ) != pxCommand->pxCommandLineDefinition->cExpectedNumberOfParameters )
+						{
+							xReturn = pdFALSE;
+						}
+					}
+
+					break;
+				}
+			}
+		}
+	}
+
+	if( ( pxCommand != NULL ) && ( xReturn == pdFALSE ) )
+	{
+		/* The command was found, but the number of parameters with the command
+		was incorrect. */
+		strncpy( pcWriteBuffer, "Incorrect command parameter(s).  Enter \"help\" to view a list of available commands.\r\n\r\n", xWriteBufferLen );
+		pxCommand = NULL;
+	}
+	else if( pxCommand != NULL )
+	{
+		/* Call the callback function that is registered to this command. */
+		xReturn = pxCommand->pxCommandLineDefinition->pxCommandInterpreter( pcWriteBuffer, xWriteBufferLen, pcCommandInput );
+
+		/* If xReturn is pdFALSE, then no further strings will be returned
+		after this one, and	pxCommand can be reset to NULL ready to search
+		for the next entered command. */
+		if( xReturn == pdFALSE )
+		{
+			pxCommand = NULL;
+		}
+	}
+	else
+	{
+		/* pxCommand was NULL, the command was not found. */
+		strncpy( pcWriteBuffer, "Command not recognised.  Enter 'help' to view a list of available commands.\r\n\r\n", xWriteBufferLen );
+		xReturn = pdFALSE;
+	}
+
+	return xReturn;
+}
+/*---------------------------------------------------------- */
+
+char *FreeRTOS_CLIGetOutputBuffer( void )
+{
+	return cOutputBuffer;
+}
+/*---------------------------------------------------------- */
+
+const char *FreeRTOS_CLIGetParameter( const char *pcCommandString, UBaseType_t uxWantedParameter, BaseType_t *pxParameterStringLength )
+{
+UBaseType_t uxParametersFound = 0;
+const char *pcReturn = NULL;
+
+	*pxParameterStringLength = 0;
+
+	while( uxParametersFound < uxWantedParameter )
+	{
+		/* Index the character pointer past the current word.  If this is the start
+		of the command string then the first word is the command itself. */
+		while( ( ( *pcCommandString ) != 0x00 ) && ( ( *pcCommandString ) != ' ' ) )
+		{
+			pcCommandString++;
+		}
+
+		/* Find the start of the next string. */
+		while( ( ( *pcCommandString ) != 0x00 ) && ( ( *pcCommandString ) == ' ' ) )
+		{
+			pcCommandString++;
+		}
+
+		/* Was a string found? */
+		if( *pcCommandString != 0x00 )
+		{
+			/* Is this the start of the required parameter? */
+			uxParametersFound++;
+
+			if( uxParametersFound == uxWantedParameter )
+			{
+				/* How long is the parameter? */
+				pcReturn = pcCommandString;
+				while( ( ( *pcCommandString ) != 0x00 ) && ( ( *pcCommandString ) != ' ' ) )
+				{
+					( *pxParameterStringLength )++;
+					pcCommandString++;
+				}
+
+				if( *pxParameterStringLength == 0 )
+				{
+					pcReturn = NULL;
+				}
+
+				break;
+			}
+		}
+		else
+		{
+			break;
+		}
+	}
+
+	return pcReturn;
+}
+/*---------------------------------------------------------- */
+
+static BaseType_t prvHelpCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString )
+{
+static const CLI_Definition_List_Item_t * pxCommand = NULL;
+BaseType_t xReturn;
+
+	( void ) pcCommandString;
+
+	if( pxCommand == NULL )
+	{
+		/* Reset the pxCommand pointer back to the start of the list. */
+		pxCommand = &xRegisteredCommands;
+	}
+
+	/* Return the next command help string, before moving the pointer on to
+	the next command in the list. */
+	strncpy( pcWriteBuffer, pxCommand->pxCommandLineDefinition->pcHelpString, xWriteBufferLen );
+	pxCommand = pxCommand->pxNext;
+
+	if( pxCommand == NULL )
+	{
+		/* There are no more commands in the list, so there will be no more
+		strings to return after this one and pdFALSE should be returned. */
+		xReturn = pdFALSE;
+	}
+	else
+	{
+		xReturn = pdTRUE;
+	}
+
+	return xReturn;
+}
+/*---------------------------------------------------------- */
+
+static int8_t prvGetNumberOfParameters( const char *pcCommandString )
+{
+int8_t cParameters = 0;
+BaseType_t xLastCharacterWasSpace = pdFALSE;
+
+	/* Count the number of space delimited words in pcCommandString. */
+	while( *pcCommandString != 0x00 )
+	{
+		if( ( *pcCommandString ) == ' ' )
+		{
+			if( xLastCharacterWasSpace != pdTRUE )
+			{
+				cParameters++;
+				xLastCharacterWasSpace = pdTRUE;
+			}
+		}
+		else
+		{
+			xLastCharacterWasSpace = pdFALSE;
+		}
+
+		pcCommandString++;
+	}
+
+	/* If the command string ended with spaces, then there will have been too
+	many parameters counted. */
+	if( xLastCharacterWasSpace == pdTRUE )
+	{
+		cParameters--;
+	}
+
+	/* The value returned is one less than the number of space delimited words,
+	as the first word should be the command itself. */
+	return cParameters;
+}
+

assets/User_code/component/FreeRTOS_CLI.h

@@ -0,0 +1,108 @@
+/*
+ * FreeRTOS+CLI V1.0.4
+ * Copyright (C) 2017 Amazon.com, Inc. or its affiliates.  All Rights Reserved.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy of
+ * this software and associated documentation files (the "Software"), to deal in
+ * the Software without restriction, including without limitation the rights to
+ * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
+ * the Software, and to permit persons to whom the Software is furnished to do so,
+ * subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
+ * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
+ * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
+ * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
+ * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+ *
+ * http://www.FreeRTOS.org
+ * http://aws.amazon.com/freertos
+ *
+ * 1 tab == 4 spaces!
+ */
+
+#ifndef COMMAND_INTERPRETER_H
+#define COMMAND_INTERPRETER_H
+
+/* This config should be defined in FreeRTOSConfig.h. But due to the limition of CubeMX I put it here. */
+#define configCOMMAND_INT_MAX_OUTPUT_SIZE  512
+
+/* The prototype to which callback functions used to process command line
+commands must comply.  pcWriteBuffer is a buffer into which the output from
+executing the command can be written, xWriteBufferLen is the length, in bytes of
+the pcWriteBuffer buffer, and pcCommandString is the entire string as input by
+the user (from which parameters can be extracted).*/
+typedef BaseType_t (*pdCOMMAND_LINE_CALLBACK)( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString );
+
+/* The structure that defines command line commands.  A command line command
+should be defined by declaring a const structure of this type. */
+typedef struct xCOMMAND_LINE_INPUT
+{
+	const char * const pcCommand;				/* The command that causes pxCommandInterpreter to be executed.  For example "help".  Must be all lower case. */
+	const char * const pcHelpString;			/* String that describes how to use the command.  Should start with the command itself, and end with "\r\n".  For example "help: Returns a list of all the commands\r\n". */
+	const pdCOMMAND_LINE_CALLBACK pxCommandInterpreter;	/* A pointer to the callback function that will return the output generated by the command. */
+	int8_t cExpectedNumberOfParameters;			/* Commands expect a fixed number of parameters, which may be zero. */
+} CLI_Command_Definition_t;
+
+/* For backward compatibility. */
+#define xCommandLineInput CLI_Command_Definition_t
+
+/*
+ * Register the command passed in using the pxCommandToRegister parameter.
+ * Registering a command adds the command to the list of commands that are
+ * handled by the command interpreter.  Once a command has been registered it
+ * can be executed from the command line.
+ */
+BaseType_t FreeRTOS_CLIRegisterCommand( const CLI_Command_Definition_t * const pxCommandToRegister );
+
+/*
+ * Runs the command interpreter for the command string "pcCommandInput".  Any
+ * output generated by running the command will be placed into pcWriteBuffer.
+ * xWriteBufferLen must indicate the size, in bytes, of the buffer pointed to
+ * by pcWriteBuffer.
+ *
+ * FreeRTOS_CLIProcessCommand should be called repeatedly until it returns pdFALSE.
+ *
+ * pcCmdIntProcessCommand is not reentrant.  It must not be called from more
+ * than one task - or at least - by more than one task at a time.
+ */
+BaseType_t FreeRTOS_CLIProcessCommand( const char * const pcCommandInput, char * pcWriteBuffer, size_t xWriteBufferLen  );
+
+/*---------------------------------------------------------- */
+
+/*
+ * A buffer into which command outputs can be written is declared in the
+ * main command interpreter, rather than in the command console implementation,
+ * to allow application that provide access to the command console via multiple
+ * interfaces to share a buffer, and therefore save RAM.  Note, however, that
+ * the command interpreter itself is not re-entrant, so only one command
+ * console interface can be used at any one time.  For that reason, no attempt
+ * is made to provide any mutual exclusion mechanism on the output buffer.
+ *
+ * FreeRTOS_CLIGetOutputBuffer() returns the address of the output buffer.
+ */
+char *FreeRTOS_CLIGetOutputBuffer( void );
+
+/*
+ * Return a pointer to the xParameterNumber'th word in pcCommandString.
+ */
+const char *FreeRTOS_CLIGetParameter( const char *pcCommandString, UBaseType_t uxWantedParameter, BaseType_t *pxParameterStringLength );
+
+#endif /* COMMAND_INTERPRETER_H */
+
+
+
+
+
+
+
+
+
+
+
+
+

assets/User_code/component/ahrs.c

@@ -0,0 +1,417 @@
+/*
+  开源的AHRS算法。
+  MadgwickAHRS
+*/
+
+#include "ahrs.h"
+
+#include <string.h>
+
+#include "user_math.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+#define BETA_IMU (0.033f)
+#define BETA_AHRS (0.041f)
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* 2 * proportional gain (Kp) */
+static float beta = BETA_IMU;
+
+/**
+ * @brief 不使用磁力计计算姿态
+ *
+ * @param ahrs 姿态解算主结构体
+ * @param accl 加速度计数据
+ * @param gyro 陀螺仪数据
+ * @return int8_t 0对应没有错误
+ */
+static int8_t AHRS_UpdateIMU(AHRS_t *ahrs, const AHRS_Accl_t *accl,
+                             const AHRS_Gyro_t *gyro) {
+  if (ahrs == NULL) return -1;
+  if (accl == NULL) return -1;
+  if (gyro == NULL) return -1;
+
+  beta = BETA_IMU;
+
+  float ax = accl->x;
+  float ay = accl->y;
+  float az = accl->z;
+
+  float gx = gyro->x;
+  float gy = gyro->y;
+  float gz = gyro->z;
+
+  float recip_norm;
+  float s0, s1, s2, s3;
+  float q_dot1, q_dot2, q_dot3, q_dot4;
+  float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2,
+      q3q3;
+
+  /* Rate of change of quaternion from gyroscope */
+  q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy -
+                   ahrs->quat.q3 * gz);
+  q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz -
+                   ahrs->quat.q3 * gy);
+  q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz +
+                   ahrs->quat.q3 * gx);
+  q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy -
+                   ahrs->quat.q2 * gx);
+
+  /* Compute feedback only if accelerometer measurement valid (avoids NaN in
+   * accelerometer normalisation) */
+  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+    /* Normalise accelerometer measurement */
+    recip_norm = InvSqrt(ax * ax + ay * ay + az * az);
+    ax *= recip_norm;
+    ay *= recip_norm;
+    az *= recip_norm;
+
+    /* Auxiliary variables to avoid repeated arithmetic */
+    _2q0 = 2.0f * ahrs->quat.q0;
+    _2q1 = 2.0f * ahrs->quat.q1;
+    _2q2 = 2.0f * ahrs->quat.q2;
+    _2q3 = 2.0f * ahrs->quat.q3;
+    _4q0 = 4.0f * ahrs->quat.q0;
+    _4q1 = 4.0f * ahrs->quat.q1;
+    _4q2 = 4.0f * ahrs->quat.q2;
+    _8q1 = 8.0f * ahrs->quat.q1;
+    _8q2 = 8.0f * ahrs->quat.q2;
+    q0q0 = ahrs->quat.q0 * ahrs->quat.q0;
+    q1q1 = ahrs->quat.q1 * ahrs->quat.q1;
+    q2q2 = ahrs->quat.q2 * ahrs->quat.q2;
+    q3q3 = ahrs->quat.q3 * ahrs->quat.q3;
+
+    /* Gradient decent algorithm corrective step */
+    s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
+    s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * ahrs->quat.q1 -
+         _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
+    s2 = 4.0f * q0q0 * ahrs->quat.q2 + _2q0 * ax + _4q2 * q3q3 -
+         _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
+    s3 = 4.0f * q1q1 * ahrs->quat.q3 - _2q1 * ax +
+         4.0f * q2q2 * ahrs->quat.q3 - _2q2 * ay;
+
+    /* normalise step magnitude */
+    recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
+
+    s0 *= recip_norm;
+    s1 *= recip_norm;
+    s2 *= recip_norm;
+    s3 *= recip_norm;
+
+    /* Apply feedback step */
+    q_dot1 -= beta * s0;
+    q_dot2 -= beta * s1;
+    q_dot3 -= beta * s2;
+    q_dot4 -= beta * s3;
+  }
+
+  /* Integrate rate of change of quaternion to yield quaternion */
+  ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq;
+  ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq;
+  ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq;
+  ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq;
+
+  /* Normalise quaternion */
+  recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 +
+                       ahrs->quat.q1 * ahrs->quat.q1 +
+                       ahrs->quat.q2 * ahrs->quat.q2 +
+                       ahrs->quat.q3 * ahrs->quat.q3);
+  ahrs->quat.q0 *= recip_norm;
+  ahrs->quat.q1 *= recip_norm;
+  ahrs->quat.q2 *= recip_norm;
+  ahrs->quat.q3 *= recip_norm;
+
+  return 0;
+}
+
+/**
+ * @brief 初始化姿态解算
+ *
+ * @param ahrs 姿态解算主结构体
+ * @param magn 磁力计数据
+ * @param sample_freq 采样频率
+ * @return int8_t 0对应没有错误
+ */
+int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq) {
+  if (ahrs == NULL) return -1;
+
+  ahrs->inv_sample_freq = 1.0f / sample_freq;
+
+  ahrs->quat.q0 = 1.0f;
+  ahrs->quat.q1 = 0.0f;
+  ahrs->quat.q2 = 0.0f;
+  ahrs->quat.q3 = 0.0f;
+
+  if (magn) {
+    float yaw = -atan2(magn->y, magn->x);
+
+    if ((magn->x == 0.0f) && (magn->y == 0.0f) && (magn->z == 0.0f)) {
+      ahrs->quat.q0 = 0.800884545f;
+      ahrs->quat.q1 = 0.00862364192f;
+      ahrs->quat.q2 = -0.00283267116f;
+      ahrs->quat.q3 = 0.598749936f;
+
+    } else if ((yaw < (M_PI / 2.0f)) || (yaw > 0.0f)) {
+      ahrs->quat.q0 = 0.997458339f;
+      ahrs->quat.q1 = 0.000336312107f;
+      ahrs->quat.q2 = -0.0057230792f;
+      ahrs->quat.q3 = 0.0740156546;
+
+    } else if ((yaw < M_PI) || (yaw > (M_PI / 2.0f))) {
+      ahrs->quat.q0 = 0.800884545f;
+      ahrs->quat.q1 = 0.00862364192f;
+      ahrs->quat.q2 = -0.00283267116f;
+      ahrs->quat.q3 = 0.598749936f;
+
+    } else if ((yaw < 90.0f) || (yaw > M_PI)) {
+      ahrs->quat.q0 = 0.800884545f;
+      ahrs->quat.q1 = 0.00862364192f;
+      ahrs->quat.q2 = -0.00283267116f;
+      ahrs->quat.q3 = 0.598749936f;
+
+    } else if ((yaw < 90.0f) || (yaw > 0.0f)) {
+      ahrs->quat.q0 = 0.800884545f;
+      ahrs->quat.q1 = 0.00862364192f;
+      ahrs->quat.q2 = -0.00283267116f;
+      ahrs->quat.q3 = 0.598749936f;
+    }
+  }
+  return 0;
+}
+
+/**
+ * @brief 姿态运算更新一次
+ * @note 输入数据必须是NED(North East Down) 参考坐标系
+ *
+ * @param ahrs 姿态解算主结构体
+ * @param accl 加速度计数据
+ * @param gyro 陀螺仪数据
+ * @param magn 磁力计数据
+ * @return int8_t 0对应没有错误
+ */
+int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl,
+                   const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn) {
+  if (ahrs == NULL) return -1;
+  if (accl == NULL) return -1;
+  if (gyro == NULL) return -1;
+
+  beta = BETA_AHRS;
+
+  float recip_norm;
+  float s0, s1, s2, s3;
+  float q_dot1, q_dot2, q_dot3, q_dot4;
+  float hx, hy;
+  float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1,
+      _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3,
+      q2q2, q2q3, q3q3;
+
+  if (magn == NULL) return AHRS_UpdateIMU(ahrs, accl, gyro);
+
+  float mx = magn->x;
+  float my = magn->y;
+  float mz = magn->z;
+
+  /* Use IMU algorithm if magnetometer measurement invalid (avoids NaN in */
+  /* magnetometer normalisation) */
+  if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
+    return AHRS_UpdateIMU(ahrs, accl, gyro);
+  }
+
+  float ax = accl->x;
+  float ay = accl->y;
+  float az = accl->z;
+
+  float gx = gyro->x;
+  float gy = gyro->y;
+  float gz = gyro->z;
+
+  /* Rate of change of quaternion from gyroscope */
+  q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy -
+                   ahrs->quat.q3 * gz);
+  q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz -
+                   ahrs->quat.q3 * gy);
+  q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz +
+                   ahrs->quat.q3 * gx);
+  q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy -
+                   ahrs->quat.q2 * gx);
+
+  /* Compute feedback only if accelerometer measurement valid (avoids NaN in
+   * accelerometer normalisation) */
+  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+    /* Normalise accelerometer measurement */
+    recip_norm = InvSqrt(ax * ax + ay * ay + az * az);
+    ax *= recip_norm;
+    ay *= recip_norm;
+    az *= recip_norm;
+
+    /* Normalise magnetometer measurement */
+    recip_norm = InvSqrt(mx * mx + my * my + mz * mz);
+    mx *= recip_norm;
+    my *= recip_norm;
+    mz *= recip_norm;
+
+    /* Auxiliary variables to avoid repeated arithmetic */
+    _2q0mx = 2.0f * ahrs->quat.q0 * mx;
+    _2q0my = 2.0f * ahrs->quat.q0 * my;
+    _2q0mz = 2.0f * ahrs->quat.q0 * mz;
+    _2q1mx = 2.0f * ahrs->quat.q1 * mx;
+    _2q0 = 2.0f * ahrs->quat.q0;
+    _2q1 = 2.0f * ahrs->quat.q1;
+    _2q2 = 2.0f * ahrs->quat.q2;
+    _2q3 = 2.0f * ahrs->quat.q3;
+    _2q0q2 = 2.0f * ahrs->quat.q0 * ahrs->quat.q2;
+    _2q2q3 = 2.0f * ahrs->quat.q2 * ahrs->quat.q3;
+    q0q0 = ahrs->quat.q0 * ahrs->quat.q0;
+    q0q1 = ahrs->quat.q0 * ahrs->quat.q1;
+    q0q2 = ahrs->quat.q0 * ahrs->quat.q2;
+    q0q3 = ahrs->quat.q0 * ahrs->quat.q3;
+    q1q1 = ahrs->quat.q1 * ahrs->quat.q1;
+    q1q2 = ahrs->quat.q1 * ahrs->quat.q2;
+    q1q3 = ahrs->quat.q1 * ahrs->quat.q3;
+    q2q2 = ahrs->quat.q2 * ahrs->quat.q2;
+    q2q3 = ahrs->quat.q2 * ahrs->quat.q3;
+    q3q3 = ahrs->quat.q3 * ahrs->quat.q3;
+
+    /* Reference direction of Earth's magnetic field */
+    hx = mx * q0q0 - _2q0my * ahrs->quat.q3 +
+         _2q0mz * ahrs->quat.q2 + mx * q1q1 +
+         _2q1 * my * ahrs->quat.q2 + _2q1 * mz * ahrs->quat.q3 -
+         mx * q2q2 - mx * q3q3;
+    hy = _2q0mx * ahrs->quat.q3 + my * q0q0 -
+         _2q0mz * ahrs->quat.q1 + _2q1mx * ahrs->quat.q2 -
+         my * q1q1 + my * q2q2 + _2q2 * mz * ahrs->quat.q3 - my * q3q3;
+    // _2bx = sqrtf(hx * hx + hy * hy);
+    // 改为invsqrt
+    _2bx = 1.f / InvSqrt(hx * hx + hy * hy);
+    _2bz = -_2q0mx * ahrs->quat.q2 + _2q0my * ahrs->quat.q1 +
+           mz * q0q0 + _2q1mx * ahrs->quat.q3 - mz * q1q1 +
+           _2q2 * my * ahrs->quat.q3 - mz * q2q2 + mz * q3q3;
+    _4bx = 2.0f * _2bx;
+    _4bz = 2.0f * _2bz;
+
+    /* Gradient decent algorithm corrective step */
+    s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) +
+         _2q1 * (2.0f * q0q1 + _2q2q3 - ay) -
+         _2bz * ahrs->quat.q2 *
+             (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
+         (-_2bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) *
+             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
+         _2bx * ahrs->quat.q2 *
+             (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+    s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) +
+         _2q0 * (2.0f * q0q1 + _2q2q3 - ay) -
+         4.0f * ahrs->quat.q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
+         _2bz * ahrs->quat.q3 *
+             (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
+         (_2bx * ahrs->quat.q2 + _2bz * ahrs->quat.q0) *
+             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
+         (_2bx * ahrs->quat.q3 - _4bz * ahrs->quat.q1) *
+             (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+    s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) +
+         _2q3 * (2.0f * q0q1 + _2q2q3 - ay) -
+         4.0f * ahrs->quat.q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
+         (-_4bx * ahrs->quat.q2 - _2bz * ahrs->quat.q0) *
+             (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
+         (_2bx * ahrs->quat.q1 + _2bz * ahrs->quat.q3) *
+             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
+         (_2bx * ahrs->quat.q0 - _4bz * ahrs->quat.q2) *
+             (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+    s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) +
+         _2q2 * (2.0f * q0q1 + _2q2q3 - ay) +
+         (-_4bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) *
+             (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
+         (-_2bx * ahrs->quat.q0 + _2bz * ahrs->quat.q2) *
+             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
+         _2bx * ahrs->quat.q1 *
+             (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+    /* normalise step magnitude */
+    recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
+    s0 *= recip_norm;
+    s1 *= recip_norm;
+    s2 *= recip_norm;
+    s3 *= recip_norm;
+
+    /* Apply feedback step */
+    q_dot1 -= beta * s0;
+    q_dot2 -= beta * s1;
+    q_dot3 -= beta * s2;
+    q_dot4 -= beta * s3;
+  }
+
+  /* Integrate rate of change of quaternion to yield quaternion */
+  ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq;
+  ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq;
+  ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq;
+  ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq;
+
+  /* Normalise quaternion */
+  recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 +
+                       ahrs->quat.q1 * ahrs->quat.q1 +
+                       ahrs->quat.q2 * ahrs->quat.q2 +
+                       ahrs->quat.q3 * ahrs->quat.q3);
+  ahrs->quat.q0 *= recip_norm;
+  ahrs->quat.q1 *= recip_norm;
+  ahrs->quat.q2 *= recip_norm;
+  ahrs->quat.q3 *= recip_norm;
+
+  return 0;
+}
+
+/**
+ * @brief 通过姿态解算主结构体中的四元数计算欧拉角
+ *
+ * @param eulr 欧拉角
+ * @param ahrs 姿态解算主结构体
+ * @return int8_t 0对应没有错误
+ */
+int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs) {
+  if (eulr == NULL) return -1;
+  if (ahrs == NULL) return -1;
+
+  const float sinr_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q1 +
+                                  ahrs->quat.q2 * ahrs->quat.q3);
+  const float cosr_cosp =
+      1.0f - 2.0f * (ahrs->quat.q1 * ahrs->quat.q1 +
+                     ahrs->quat.q2 * ahrs->quat.q2);
+  eulr->pit = atan2f(sinr_cosp, cosr_cosp);
+
+  const float sinp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q2 -
+                             ahrs->quat.q3 * ahrs->quat.q1);
+
+  if (fabsf(sinp) >= 1.0f)
+    eulr->rol = copysignf(M_PI / 2.0f, sinp);
+  else
+    eulr->rol = asinf(sinp);
+
+  const float siny_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q3 +
+                                  ahrs->quat.q1 * ahrs->quat.q2);
+  const float cosy_cosp =
+      1.0f - 2.0f * (ahrs->quat.q2 * ahrs->quat.q2 +
+                     ahrs->quat.q3 * ahrs->quat.q3);
+  eulr->yaw = atan2f(siny_cosp, cosy_cosp);
+
+#if 0
+  eulr->yaw *= M_RAD2DEG_MULT;
+  eulr->rol *= M_RAD2DEG_MULT;
+  eulr->pit *= M_RAD2DEG_MULT;
+#endif
+
+  return 0;
+}
+
+/**
+ * \brief 将对应数据置零
+ *
+ * \param eulr 被操作的数据
+ */
+void AHRS_ResetEulr(AHRS_Eulr_t *eulr) { memset(eulr, 0, sizeof(*eulr)); }
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */

assets/User_code/component/ahrs.h

@@ -0,0 +1,114 @@
+/*
+  开源的AHRS算法。
+  MadgwickAHRS
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "user_math.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* 欧拉角（Euler angle） */
+typedef struct {
+  float yaw; /* 偏航角（Yaw angle） */
+  float pit; /* 俯仰角（Pitch angle） */
+  float rol; /* 翻滚角（Roll angle） */
+} AHRS_Eulr_t;
+
+/* 加速度计 Accelerometer */
+typedef struct {
+  float x;
+  float y;
+  float z;
+} AHRS_Accl_t;
+
+/* 陀螺仪 Gyroscope */
+typedef struct {
+  float x;
+  float y;
+  float z;
+} AHRS_Gyro_t;
+
+/* 磁力计 Magnetometer */
+typedef struct {
+  float x;
+  float y;
+  float z;
+} AHRS_Magn_t;
+
+/* 四元数 */
+typedef struct {
+  float q0;
+  float q1;
+  float q2;
+  float q3;
+} AHRS_Quaternion_t;
+
+/* 姿态解算算法主结构体 */
+typedef struct {
+  /* 四元数 */
+  AHRS_Quaternion_t quat;
+
+  float inv_sample_freq; /* 采样频率的的倒数 */
+} AHRS_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/**
+ * @brief 初始化姿态解算
+ *
+ * @param ahrs 姿态解算主结构体
+ * @param magn 磁力计数据
+ * @param sample_freq 采样频率
+ * @return int8_t 0对应没有错误
+ */
+int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq);
+
+/**
+ * @brief 姿态运算更新一次
+ *
+ * @param ahrs 姿态解算主结构体
+ * @param accl 加速度计数据
+ * @param gyro 陀螺仪数据
+ * @param magn 磁力计数据
+ * @return int8_t 0对应没有错误
+ */
+int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl,
+                   const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn);
+
+/**
+ * @brief 通过姿态解算主结构体中的四元数计算欧拉角
+ *
+ * @param eulr 欧拉角
+ * @param ahrs 姿态解算主结构体
+ * @return int8_t 0对应没有错误
+ */
+int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs);
+
+/**
+ * \brief 将对应数据置零
+ *
+ * \param eulr 被操作的数据
+ */
+void AHRS_ResetEulr(AHRS_Eulr_t *eulr);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/component/capacity.c

@@ -0,0 +1,70 @@
+/*
+  剩余电量算法。
+  通过电压值计算剩余电量。
+*/
+
+#include "capacity.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/**
+ * @brief 通过电压计算电池剩余电量
+ *
+ * @param volt 电压值
+ * @return float 剩余电量比例
+ */
+float Capacity_GetBatteryRemain(float volt) {
+  float percentage;
+  float volt_2 = volt * volt;
+  float volt_3 = volt_2 * volt;
+
+  if (volt < 19.5f)
+    percentage = 0.0f;
+
+  else if (volt < 21.9f)
+    percentage = 0.005664f * volt_3 - 0.3386f * volt_2 + 6.765f * volt - 45.17f;
+
+  else if (volt < 25.5f)
+    percentage = 0.02269f * volt_3 - 1.654f * volt_2 + 40.34f * volt - 328.4f;
+
+  else
+    percentage = 1.0f;
+
+  if (percentage < 0.0f)
+    percentage = 0.0f;
+
+  else if (percentage > 1.0f)
+    percentage = 1.0f;
+
+  return percentage;
+}
+
+/**
+ * @brief
+ *
+ * @param vcap 电容电压
+ * @param vbat 电池电压
+ * @param v_cutoff 截止电压
+ * @return float 电容剩余电量比例
+ */
+float Capacity_GetCapacitorRemain(float vcap, float vbat, float v_cutoff) {
+  float percentage = (vcap - v_cutoff) / (vbat - v_cutoff);
+
+  if (percentage < 0.0f)
+    percentage = 0.0f;
+
+  else if (percentage > 1.0f)
+    percentage = 1.0f;
+
+  return percentage;
+}
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */

assets/User_code/component/capacity.h

@@ -0,0 +1,47 @@
+/*
+  剩余电量算法。
+
+  通过电压值计算剩余电量。
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "user_math.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/**
+ * @brief 通过电压计算电池剩余电量
+ *
+ * @param volt 电压值
+ * @return float 剩余电量比例
+ */
+float Capacity_GetBatteryRemain(float volt);
+
+/**
+ * @brief
+ *
+ * @param vcap 电容电压
+ * @param vbat 电池电压
+ * @param v_cutoff 截止电压
+ * @return float 电容剩余电量比例
+ */
+float Capacity_GetCapacitorRemain(float vcap, float vbat, float v_cutoff);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/component/crc16.c

@@ -0,0 +1,62 @@
+#include "crc16.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+static const uint16_t crc16_tab[256] = {
+    0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48,
+    0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108,
+    0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, 0x9cc9, 0x8d40, 0xbfdb,
+    0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399,
+    0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e,
+    0xfae7, 0xc87c, 0xd9f5, 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e,
+    0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd,
+    0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
+    0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285,
+    0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44,
+    0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, 0x6306, 0x728f, 0x4014,
+    0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5,
+    0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3,
+    0x242a, 0x16b1, 0x0738, 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862,
+    0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e,
+    0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
+    0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1,
+    0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483,
+    0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, 0x2942, 0x38cb, 0x0a50,
+    0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710,
+    0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7,
+    0x6e6e, 0x5cf5, 0x4d7c, 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1,
+    0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72,
+    0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
+    0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e,
+    0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf,
+    0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, 0xf78f, 0xe606, 0xd49d,
+    0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c,
+    0x3de3, 0x2c6a, 0x1ef1, 0x0f78};
+
+static inline uint16_t CRC16_Byte(uint16_t crc, const uint8_t data) {
+  return (crc >> 8) ^ crc16_tab[(crc ^ data) & 0xff];
+}
+
+uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc) {
+  while (len--) crc = CRC16_Byte(crc, *buf++);
+  return crc;
+}
+
+bool CRC16_Verify(const uint8_t *buf, size_t len) {
+  if (len < 2) return false;
+
+  uint16_t expected = CRC16_Calc(buf, len - sizeof(uint16_t), CRC16_INIT);
+  return expected ==
+         ((const uint16_t *)((const uint8_t *)buf +
+                             (len % 2)))[len / sizeof(uint16_t) - 1];
+}
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */

assets/User_code/component/crc16.h

@@ -0,0 +1,30 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+
+#include "user_math.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+#define CRC16_INIT 0XFFFF
+
+uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc);
+bool CRC16_Verify(const uint8_t *buf, size_t len);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/component/crc8.c

@@ -0,0 +1,52 @@
+#include "crc8.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+static const uint8_t crc8_tab[256] = {
+    0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20,
+    0xa3, 0xfd, 0x1f, 0x41, 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e,
+    0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, 0x23, 0x7d, 0x9f, 0xc1,
+    0x42, 0x1c, 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62,
+    0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e,
+    0x1d, 0x43, 0xa1, 0xff, 0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5,
+    0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07, 0xdb, 0x85, 0x67, 0x39,
+    0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a,
+    0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45,
+    0xc6, 0x98, 0x7a, 0x24, 0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b,
+    0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, 0x8c, 0xd2, 0x30, 0x6e,
+    0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd,
+    0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31,
+    0xb2, 0xec, 0x0e, 0x50, 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c,
+    0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee, 0x32, 0x6c, 0x8e, 0xd0,
+    0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73,
+    0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea,
+    0x69, 0x37, 0xd5, 0x8b, 0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4,
+    0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, 0xe9, 0xb7, 0x55, 0x0b,
+    0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8,
+    0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54,
+    0xd7, 0x89, 0x6b, 0x35,
+};
+
+uint8_t CRC8_Calc(const uint8_t *buf, size_t len, uint8_t crc) {
+  /* loop over the buffer data */
+  while (len-- > 0) crc = crc8_tab[(crc ^ *buf++) & 0xff];
+
+  return crc;
+}
+
+bool CRC8_Verify(const uint8_t *buf, size_t len) {
+  if (len < 2) return false;
+
+  uint8_t expected = CRC8_Calc(buf, len - sizeof(uint8_t), CRC8_INIT);
+  return expected == buf[len - sizeof(uint8_t)];
+}
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */

assets/User_code/component/crc8.h

@@ -0,0 +1,30 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stddef.h>
+#include <stdint.h>
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+#define CRC8_INIT 0xFF
+
+uint8_t CRC8_Calc(const uint8_t *buf, size_t len, uint8_t crc);
+bool CRC8_Verify(const uint8_t *buf, size_t len);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/component/dependencies.csv

@@ -0,0 +1,8 @@
+ahrs,component/user_math.h
+capacity,component/user_math.h
+cmd,component/ahrs
+error_detect,bsp/mm
+pid,component/filter
+filter,component/ahrs
+mixer,component/user_math.h
+ui,component/user_math.h

assets/User_code/component/describe.csv

@@ -0,0 +1,14 @@
+pid,好用的
+ahrs,开源的AHRS算法，MadgwickAHRS
+capacity,电池容量计算
+cmd,通用控制命令
+crc8,CRC8校验rm
+crc16,CRC16校验rm
+error_detect,错误检测
+filter,各类滤波器
+FreeRTOS_CLI,FreeRTOS命令行接口
+limiter,限幅器
+mixer,混控器
+ui,用户交互
+user_math,用户自定义数学函数
+pid,PID控制器

assets/User_code/component/error_detect.c

@@ -0,0 +1,67 @@
+/*
+  错误检测。
+*/
+
+#include "error_detect.h"
+
+#include <stddef.h>
+#include <string.h>
+
+#include "bsp/mm.h"
+
+static ErrorDetect_t ged;
+static bool inited = false;
+
+int8_t ErrorDetect_Init(void) {
+  if (inited) return -1;
+
+  memset(&ged, 0x00, sizeof(ged));
+
+  for (uint8_t i = 0; i < ERROR_DETECT_UNIT_NUM; i++) {
+    ged.error[i].enable = true;
+    ged.error[i].priority = i;
+    ged.error[i].patient_lost = 500;
+    ged.error[i].patient_work = 500;
+  }
+  return 0;
+}
+
+void ErrorDetect_Processing(uint32_t sys_time) {
+  for (uint8_t i = 0; i < ERROR_DETECT_UNIT_NUM; i++) {
+    if (!ged.error[i].enable) continue;
+
+    if (sys_time - ged.error[i].showup > ged.error[i].patient_lost) {
+      ged.error[i].is_lost = true;
+      ged.error[i].found_lost = sys_time;
+    } else if (sys_time - ged.error[i].showup > ged.error[i].patient_lost) {
+    } else {
+      ged.error[i].cycle_time = ged.error[i].showup - ged.error[i].showup_last;
+    }
+  }
+}
+
+bool ErrorDetect_ErrorExist(ErrorDetect_Unit_t unit) {
+  if (unit == ERROR_DETECT_UNIT_NO_DEV) {
+    for (uint8_t i = ERROR_DETECT_UNIT_NUM; i > 0; i--) {
+      if (ged.error[i].error_exist) return true;
+    }
+    return false;
+  } else {
+    return ged.error[unit].error_exist;
+  }
+}
+
+ErrorDetect_Unit_t ErrorDetect_GetErrorUnit(void) {
+  for (uint8_t i = ERROR_DETECT_UNIT_NUM; i > 0; i--) {
+    if (ged.error[i].error_exist) return i;
+  }
+  return ERROR_DETECT_UNIT_NO_DEV;
+}
+
+const ErrorDetect_Error_t *ErrorDetect_GetDetail(ErrorDetect_Unit_t unit) {
+  return &ged.error[unit];
+}
+
+void ErrorDetect_Update(ErrorDetect_Unit_t unit, uint32_t time_current) {
+  ged.error[unit].showup = time_current;
+}

assets/User_code/component/error_detect.h

@@ -0,0 +1,82 @@
+/*
+  错误检测。
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+typedef enum {
+  /* Low priority */
+  ERROR_DETECT_UNIT_NO_DEV = 0,
+  ERROR_DETECT_UNIT_REFEREE,
+  ERROR_DETECT_UNIT_CHASSIS_M1,
+  ERROR_DETECT_UNIT_CHASSIS_M2,
+  ERROR_DETECT_UNIT_CHASSIS_M3,
+  ERROR_DETECT_UNIT_CHASSIS_M4,
+  ERROR_DETECT_UNIT_TRIGGER,
+  ERROR_DETECT_UNIT_FEED,
+  ERROR_DETECT_UNIT_GIMBAL_YAW,
+  ERROR_DETECT_UNIT_GIMBAL_PIT,
+  ERROR_DETECT_UNIT_GYRO,
+  ERROR_DETECT_UNIT_ACCL,
+  ERROR_DETECT_UNIT_MAGN,
+  ERROR_DETECT_UNIT_DBUS,
+  ERROR_DETECT_UNIT_NUM,
+  /* High priority */
+} ErrorDetect_Unit_t;
+
+typedef struct {
+  bool enable;
+  uint8_t priority;
+  uint32_t patient_lost;
+  uint32_t patient_work;
+
+  uint32_t showup;
+  uint32_t showup_last;
+  uint32_t cycle_time;
+  uint32_t duration_lost;
+  uint32_t duration_work;
+  uint32_t found_lost;
+  bool error_exist;
+  bool is_lost;
+  uint8_t data_is_error;
+
+} ErrorDetect_Error_t;
+
+typedef struct {
+  ErrorDetect_Error_t error[ERROR_DETECT_UNIT_NUM];
+} ErrorDetect_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+int8_t ErrorDetect_Init(void);
+void ErrorDetect_Processing(uint32_t sys_time);
+bool ErrorDetect_ErrorExist(ErrorDetect_Unit_t unit);
+ErrorDetect_Unit_t ErrorDetect_GetErrorUnit(void);
+const ErrorDetect_Error_t *ErrorDetect_GetDetail(ErrorDetect_Unit_t unit);
+
+void ErrorDetect_Update(ErrorDetect_Unit_t unit, uint32_t time_current);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/component/filter.c

@@ -0,0 +1,185 @@
+/*
+  各类滤波器。
+*/
+
+#include "filter.h"
+
+#include "user_math.h"
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param cutoff_freq 截止频率
+ */
+void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
+                          float cutoff_freq) {
+  if (f == NULL) return;
+
+  f->cutoff_freq = cutoff_freq;
+
+  f->delay_element_1 = 0.0f;
+  f->delay_element_2 = 0.0f;
+
+  if (f->cutoff_freq <= 0.0f) {
+    /* no filtering */
+    f->b0 = 1.0f;
+    f->b1 = 0.0f;
+    f->b2 = 0.0f;
+
+    f->a1 = 0.0f;
+    f->a2 = 0.0f;
+
+    return;
+  }
+  const float fr = sample_freq / f->cutoff_freq;
+  const float ohm = tanf(M_PI / fr);
+  const float c = 1.0f + 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm;
+
+  f->b0 = ohm * ohm / c;
+  f->b1 = 2.0f * f->b0;
+  f->b2 = f->b0;
+
+  f->a1 = 2.0f * (ohm * ohm - 1.0f) / c;
+  f->a2 = (1.0f - 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm) / c;
+}
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  /* do the filtering */
+  float delay_element_0 =
+      sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
+
+  if (isinf(delay_element_0)) {
+    /* don't allow bad values to propagate via the filter */
+    delay_element_0 = sample;
+  }
+
+  const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
+                       f->delay_element_2 * f->b2;
+
+  f->delay_element_2 = f->delay_element_1;
+  f->delay_element_1 = delay_element_0;
+
+  /* return the value. Should be no need to check limits */
+  return output;
+}
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  const float dval = sample / (f->b0 + f->b1 + f->b2);
+
+  if (isfinite(dval)) {
+    f->delay_element_1 = dval;
+    f->delay_element_2 = dval;
+
+  } else {
+    f->delay_element_1 = sample;
+    f->delay_element_2 = sample;
+  }
+
+  return LowPassFilter2p_Apply(f, sample);
+}
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param notch_freq 中心频率
+ * @param bandwidth 带宽
+ */
+void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
+                      float bandwidth) {
+  if (f == NULL) return;
+
+  f->notch_freq = notch_freq;
+  f->bandwidth = bandwidth;
+
+  f->delay_element_1 = 0.0f;
+  f->delay_element_2 = 0.0f;
+
+  if (notch_freq <= 0.0f) {
+    /* no filtering */
+    f->b0 = 1.0f;
+    f->b1 = 0.0f;
+    f->b2 = 0.0f;
+
+    f->a1 = 0.0f;
+    f->a2 = 0.0f;
+
+    return;
+  }
+
+  const float alpha = tanf(M_PI * bandwidth / sample_freq);
+  const float beta = -cosf(M_2PI * notch_freq / sample_freq);
+  const float a0_inv = 1.0f / (alpha + 1.0f);
+
+  f->b0 = a0_inv;
+  f->b1 = 2.0f * beta * a0_inv;
+  f->b2 = a0_inv;
+
+  f->a1 = f->b1;
+  f->a2 = (1.0f - alpha) * a0_inv;
+}
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+inline float NotchFilter_Apply(NotchFilter_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  /* Direct Form II implementation */
+  const float delay_element_0 =
+      sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
+  const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
+                       f->delay_element_2 * f->b2;
+
+  f->delay_element_2 = f->delay_element_1;
+  f->delay_element_1 = delay_element_0;
+
+  return output;
+}
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float NotchFilter_Reset(NotchFilter_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  float dval = sample;
+
+  if (fabsf(f->b0 + f->b1 + f->b2) > FLT_EPSILON) {
+    dval = dval / (f->b0 + f->b1 + f->b2);
+  }
+
+  f->delay_element_1 = dval;
+  f->delay_element_2 = dval;
+
+  return NotchFilter_Apply(f, sample);
+}

assets/User_code/component/filter.h

@@ -0,0 +1,120 @@
+/*
+  各类滤波器。
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "user_math.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* 二阶低通滤波器 */
+typedef struct {
+  float cutoff_freq; /* 截止频率 */
+
+  float a1;
+  float a2;
+
+  float b0;
+  float b1;
+  float b2;
+
+  float delay_element_1;
+  float delay_element_2;
+
+} LowPassFilter2p_t;
+
+/* 带阻滤波器 */
+typedef struct {
+  float notch_freq; /* 阻止频率 */
+  float bandwidth;  /* 带宽 */
+
+  float a1;
+  float a2;
+
+  float b0;
+  float b1;
+  float b2;
+  float delay_element_1;
+  float delay_element_2;
+
+} NotchFilter_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param cutoff_freq 截止频率
+ */
+void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
+                          float cutoff_freq);
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample);
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample);
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param notch_freq 中心频率
+ * @param bandwidth 带宽
+ */
+void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
+                      float bandwidth);
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float NotchFilter_Apply(NotchFilter_t *f, float sample);
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float NotchFilter_Reset(NotchFilter_t *f, float sample);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/component/limiter.c

@@ -0,0 +1,107 @@
+/*
+  限制器
+*/
+
+#include "limiter.h"
+
+#include <math.h>
+#include <stddef.h>
+
+#define POWER_BUFF_THRESHOLD 20
+#define CHASSIS_POWER_CHECK_FREQ 10
+#define CHASSIS_POWER_FACTOR_PASS 0.9f
+#define CHASSIS_POWER_FACTOR_NO_PASS 1.5f
+
+#define CHASSIS_MOTOR_CIRCUMFERENCE 0.12f
+
+/**
+ * @brief 限制底盘功率不超过power_limit
+ *
+ * @param power_limit 最大功率
+ * @param motor_out 电机输出值
+ * @param speed 电机转速
+ * @param len 电机数量
+ * @return int8_t 0对应没有错误
+ */
+int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out,
+                                float *speed, uint32_t len) {
+  /* power_limit小于0时不进行限制 */
+  if (motor_out == NULL || speed == NULL || power_limit < 0) return -1;
+
+  float sum_motor_out = 0.0f;
+  for (uint32_t i = 0; i < len; i++) {
+    /* 总功率计算 P=F(由转矩电流表示)*V(由转速表示) */
+    sum_motor_out +=
+        fabsf(motor_out[i]) * fabsf(speed[i]) * CHASSIS_MOTOR_CIRCUMFERENCE;
+  }
+
+  /* 保持每个电机输出值缩小时比例不变 */
+  if (sum_motor_out > power_limit) {
+    for (uint32_t i = 0; i < len; i++) {
+      motor_out[i] *= power_limit / sum_motor_out;
+    }
+  }
+
+  return 0;
+}
+
+/**
+ * @brief 电容输入功率计算
+ *
+ * @param power_in 底盘当前功率
+ * @param power_limit 裁判系统功率限制值
+ * @param power_buffer 缓冲能量
+ * @return float 裁判系统输出最大值
+ */
+float PowerLimit_CapInput(float power_in, float power_limit,
+                          float power_buffer) {
+  float target_power = 0.0f;
+
+  /* 计算下一个检测周期的剩余缓冲能量 */
+  float heat_buff = power_buffer - (float)(power_in - power_limit) /
+                                       (float)CHASSIS_POWER_CHECK_FREQ;
+  if (heat_buff < POWER_BUFF_THRESHOLD) { /* 功率限制 */
+    target_power = power_limit * CHASSIS_POWER_FACTOR_PASS;
+  } else {
+    target_power = power_limit * CHASSIS_POWER_FACTOR_NO_PASS;
+  }
+
+  return target_power;
+}
+
+/**
+ * @brief 使用缓冲能量计算底盘最大功率
+ *
+ * @param power_limit 裁判系统功率限制值
+ * @param power_buffer 缓冲能量
+ * @return float 底盘输出最大值
+ */
+float PowerLimit_TargetPower(float power_limit, float power_buffer) {
+  float target_power = 0.0f;
+
+  /* 根据剩余缓冲能量计算输出功率 */
+  target_power = power_limit * (power_buffer - 10.0f) / 20.0f;
+  if (target_power < 0.0f) target_power = 0.0f;
+
+  return target_power;
+}
+
+/**
+ * @brief 射击频率控制
+ *
+ * @param heat 当前热量
+ * @param heat_limit 热量上限
+ * @param cooling_rate 冷却速率
+ * @param heat_increase 冷却增加
+ * @param shoot_freq 经过热量限制后的射击频率
+ * @return float 射击频率
+ */
+float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
+                          float heat_increase, bool is_big) {
+  float heat_percent = heat / heat_limit;
+  float stable_freq = cooling_rate / heat_increase;
+  if (is_big)
+    return stable_freq;
+  else
+    return (heat_percent > 0.7f) ? stable_freq : 3.0f * stable_freq;
+}

assets/User_code/component/limiter.h

@@ -0,0 +1,63 @@
+/*
+  限制器
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/**
+ * @brief 限制底盘功率不超过power_limit
+ *
+ * @param power_limit 最大功率
+ * @param motor_out 电机输出值
+ * @param speed 电机转速
+ * @param len 电机数量
+ * @return int8_t 0对应没有错误
+ */
+int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out,
+                                float *speed, uint32_t len);
+/**
+ * @brief 电容输入功率计算
+ *
+ * @param power_in 底盘当前功率
+ * @param power_limit 裁判系统功率限制值
+ * @param power_buffer 缓冲能量
+ * @return float 裁判系统输出最大值
+ */
+float PowerLimit_CapInput(float power_in, float power_limit,
+                          float power_buffer);
+/**
+ * @brief 使用缓冲能量计算底盘最大功率
+ *
+ * @param power_limit 裁判系统功率限制值
+ * @param power_buffer 缓冲能量
+ * @return float 底盘输出最大值
+ */
+float PowerLimit_TargetPower(float power_limit, float power_buffer);
+
+/**
+ * @brief 射击频率控制
+ *
+ * @param heat 当前热量
+ * @param heat_limit 热量上限
+ * @param cooling_rate 冷却速率
+ * @param heat_increase 冷却增加
+ * @param shoot_freq 经过热量限制后的射击频率
+ * @return float 射击频率
+ */
+float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
+                          float heat_increase, bool is_big);

assets/User_code/component/mixer.c

@@ -0,0 +1,94 @@
+/*
+  混合器
+*/
+
+#include "mixer.h"
+
+#include "math.h"
+
+/**
+ * @brief 初始化混合器
+ *
+ * @param mixer 混合器
+ * @param mode 混合器模式
+ * @return int8_t 0对应没有错误
+ */
+int8_t Mixer_Init(Mixer_t *mixer, Mixer_Mode_t mode) {
+  if (mixer == NULL) return -1;
+
+  mixer->mode = mode;
+  return 0;
+}
+
+/**
+ * @brief 计算输出
+ *
+ * @param mixer 混合器
+ * @param move_vec 运动向量
+ * @param out 输出数组
+ * @param len 输出数组长短
+ * @param scale 输出放大因子
+ * @return int8_t 0对应没有错误
+ */
+int8_t Mixer_Apply(Mixer_t *mixer, MoveVector_t *move_vec, float *out,
+                   int8_t len, float scale) {
+  if (mixer == NULL) return -1;
+
+  switch (mixer->mode) {
+    case MIXER_MECANUM:
+      if (len == 4) {
+        out[0] = move_vec->vx - move_vec->vy + move_vec->wz;
+        out[1] = move_vec->vx + move_vec->vy + move_vec->wz;
+        out[2] = -move_vec->vx + move_vec->vy + move_vec->wz;
+        out[3] = -move_vec->vx - move_vec->vy + move_vec->wz;
+      } else {
+        goto error;
+      }
+      break;
+
+    case MIXER_PARLFIX4:
+      if (len == 4) {
+        out[0] = -move_vec->vx;
+        out[1] = move_vec->vx;
+        out[2] = move_vec->vx;
+        out[3] = -move_vec->vx;
+      } else {
+        goto error;
+      }
+    case MIXER_PARLFIX2:
+      if (len == 2) {
+        out[0] = -move_vec->vx;
+        out[1] = move_vec->vx;
+      } else {
+        goto error;
+      }
+    case MIXER_SINGLE:
+      if (len == 1) {
+        out[0] = move_vec->vx;
+      } else {
+        goto error;
+      }
+    case MIXER_OMNICROSS:
+    case MIXER_OMNIPLUS:
+      goto error;
+  }
+
+  float abs_max = 0.f;
+  for (int8_t i = 0; i < len; i++) {
+    const float abs_val = fabsf(out[i]);
+    abs_max = (abs_val > abs_max) ? abs_val : abs_max;
+  }
+  if (abs_max > 1.f) {
+    for (int8_t i = 0; i < len; i++) {
+      out[i] /= abs_max;
+    }
+  }
+  for (int8_t i = 0; i < len; i++) {
+    out[i] *= scale;
+  }
+  return 0;
+
+error:
+  for (uint8_t i = 0; i < len; i++) out[i] = 0;
+  return -1;
+}

assets/User_code/component/mixer.h

@@ -0,0 +1,76 @@
+/*
+  混合器
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "user_math.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/** 四轮布局 */
+/* 前 */
+/* 2 1 */
+/* 3 4 */
+
+/* 两轮布局 */
+/* 前 */
+/* 2 1 */
+
+/* 混合器模式 */
+typedef enum {
+  MIXER_MECANUM,   /* 麦克纳姆轮 */
+  MIXER_PARLFIX4,  /* 平行四驱动轮 */
+  MIXER_PARLFIX2,  /* 平行对侧两驱动轮 */
+  MIXER_OMNICROSS, /* 叉形全向轮 */
+  MIXER_OMNIPLUS,  /* 十字全向轮 */
+  MIXER_SINGLE,    /* 单个摩擦轮 */
+} Mixer_Mode_t;
+
+typedef struct {
+  Mixer_Mode_t mode;
+} Mixer_t; /* 混合器主结构体 */
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/**
+ * @brief 初始化混合器
+ *
+ * @param mixer 混合器
+ * @param mode 混合器模式
+ * @return int8_t 0对应没有错误
+ */
+int8_t Mixer_Init(Mixer_t *mixer, Mixer_Mode_t mode);
+
+/**
+ * @brief 计算输出
+ *
+ * @param mixer 混合器
+ * @param move_vec 运动向量
+ * @param out 输出数组
+ * @param len 输出数组长短
+ * @param scale 输出放大因子
+ * @return int8_t 0对应没有错误
+ */
+int8_t Mixer_Apply(Mixer_t *mixer, MoveVector_t *move_vec, float *out,
+                   int8_t len, float scale);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/component/pid.c

@@ -0,0 +1,158 @@
+/*
+   Modified from
+   https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.cpp
+
+   参考资料：
+   https://github.com/PX4/Firmware/issues/12362
+   https://dev.px4.io/master/en/flight_stack/controller_diagrams.html
+   https://docs.px4.io/master/en/config_mc/pid_tuning_guide_multicopter.html#standard_form
+   https://www.controleng.com/articles/not-all-pid-controllers-are-the-same/
+   https://en.wikipedia.org/wiki/PID_controller
+   http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
+*/
+
+#include "pid.h"
+
+#define SIGMA 0.000001f
+
+/**
+ * @brief 初始化PID
+ *
+ * @param pid PID结构体
+ * @param mode PID模式
+ * @param sample_freq 采样频率
+ * @param param PID参数
+ * @return int8_t 0对应没有错误
+ */
+int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq,
+                const KPID_Params_t *param) {
+  if (pid == NULL) return -1;
+
+  if (!isfinite(param->p)) return -1;
+  if (!isfinite(param->i)) return -1;
+  if (!isfinite(param->d)) return -1;
+  if (!isfinite(param->i_limit)) return -1;
+  if (!isfinite(param->out_limit)) return -1;
+  pid->param = param;
+
+  float dt_min = 1.0f / sample_freq;
+  if (isfinite(dt_min))
+    pid->dt_min = dt_min;
+  else
+    return -1;
+
+  LowPassFilter2p_Init(&(pid->dfilter), sample_freq, pid->param->d_cutoff_freq);
+
+  pid->mode = mode;
+  PID_Reset(pid);
+  return 0;
+}
+
+/**
+ * @brief PID计算
+ *
+ * @param pid PID结构体
+ * @param sp 设定值
+ * @param fb 反馈值
+ * @param fb_dot 反馈值微分
+ * @param dt 间隔时间
+ * @return float 计算的输出
+ */
+float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt) {
+  if (!isfinite(sp) || !isfinite(fb) || !isfinite(fb_dot) || !isfinite(dt)) {
+    return pid->last.out;
+  }
+
+  /* 计算误差值 */
+  const float err = CircleError(sp, fb, pid->param->range);
+
+  /* 计算P项 */
+  const float k_err = err * pid->param->k;
+
+  /* 计算D项 */
+  const float k_fb = pid->param->k * fb;
+  const float filtered_k_fb = LowPassFilter2p_Apply(&(pid->dfilter), k_fb);
+
+  float d;
+  switch (pid->mode) {
+    case KPID_MODE_CALC_D:
+      /* 通过fb计算D，避免了由于sp变化导致err突变的问题 */
+      /* 当sp不变时，err的微分等于负的fb的微分 */
+      d = (filtered_k_fb - pid->last.k_fb) / fmaxf(dt, pid->dt_min);
+      break;
+
+    case KPID_MODE_SET_D:
+      d = fb_dot;
+      break;
+
+    case KPID_MODE_NO_D:
+      d = 0.0f;
+      break;
+  }
+  pid->last.err = err;
+  pid->last.k_fb = filtered_k_fb;
+
+  if (!isfinite(d)) d = 0.0f;
+
+  /* 计算PD输出 */
+  float output = (k_err * pid->param->p) - (d * pid->param->d);
+
+  /* 计算I项 */
+  const float i = pid->i + (k_err * dt);
+  const float i_out = i * pid->param->i;
+  
+  if (pid->param->i > SIGMA) {
+    /* 检查是否饱和 */
+    if (isfinite(i)) {
+      if ((fabsf(output + i_out) <= pid->param->out_limit) &&
+          (fabsf(i) <= pid->param->i_limit)) {
+        /* 未饱和，使用新积分 */
+        pid->i = i;
+      }
+    }
+  }
+  
+  /* 计算PID输出 */
+  output += i_out;
+ 
+  /* 限制输出 */
+  if (isfinite(output)) {
+    if (pid->param->out_limit > SIGMA) {
+      output = AbsClip(output, pid->param->out_limit);
+    }
+    pid->last.out = output;
+  }
+  return pid->last.out;
+}
+
+/**
+ * @brief 重置微分项
+ *
+ * @param pid PID结构体
+ * @return int8_t 0对应没有错误
+ */
+int8_t PID_ResetIntegral(KPID_t *pid) {
+  if (pid == NULL) return -1;
+
+  pid->i = 0.0f;
+
+  return 0;
+}
+
+/**
+ * @brief 重置PID
+ *
+ * @param pid PID结构体
+ * @return int8_t 0对应没有错误
+ */
+int8_t PID_Reset(KPID_t *pid) {
+  if (pid == NULL) return -1;
+
+  pid->i = 0.0f;
+  pid->last.err = 0.0f;
+  pid->last.k_fb = 0.0f;
+  pid->last.out = 0.0f;
+  LowPassFilter2p_Reset(&(pid->dfilter), 0.0f);
+
+  return 0;
+}

assets/User_code/component/pid.h

@@ -0,0 +1,107 @@
+/*
+  Modified from
+  https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.h
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+#include "filter.h"
+#include "user_math.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* PID模式 */
+typedef enum {
+  KPID_MODE_NO_D = 0, /* 不使用微分项，PI控制器 */
+  KPID_MODE_CALC_D, /* 根据反馈的值计算离散微分，忽略PID_Calc中的fb_dot */
+  KPID_MODE_SET_D /* 直接提供微分值，PID_Calc中的fb_dot将被使用，(Gyros) */
+} KPID_Mode_t;
+
+/* PID参数 */
+typedef struct {
+  float k;             /* 控制器增益，设置为1用于并行模式 */
+  float p;             /* 比例项增益，设置为1用于标准形式 */
+  float i;             /* 积分项增益 */
+  float d;             /* 微分项增益 */
+  float i_limit;       /* 积分项上限 */
+  float out_limit;     /* 输出绝对值限制 */
+  float d_cutoff_freq; /* D项低通截止频率 */
+  float range;         /* 计算循环误差时使用，大于0时启用 */
+} KPID_Params_t;
+
+/* PID主结构体 */
+typedef struct {
+  KPID_Mode_t mode;
+  const KPID_Params_t *param;
+
+  float dt_min; /* 最小PID_Calc调用间隔 */
+  float i;      /* 积分 */
+
+  struct {
+    float err;  /* 上次误差 */
+    float k_fb; /* 上次反馈值 */
+    float out;  /* 上次输出 */
+  } last;
+
+  LowPassFilter2p_t dfilter; /* D项低通滤波器 */
+} KPID_t;
+
+/**
+ * @brief 初始化PID
+ *
+ * @param pid PID结构体
+ * @param mode PID模式
+ * @param sample_freq 采样频率
+ * @param param PID参数
+ * @return int8_t 0对应没有错误
+ */
+int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq,
+                const KPID_Params_t *param);
+
+/**
+ * @brief PID计算
+ *
+ * @param pid PID结构体
+ * @param sp 设定值
+ * @param fb 反馈值
+ * @param fb_dot 反馈值微分
+ * @param dt 间隔时间
+ * @return float 计算的输出
+ */
+float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt);
+
+/**
+ * @brief 重置微分项
+ *
+ * @param pid PID结构体
+ * @return int8_t 0对应没有错误
+ */
+int8_t PID_ResetIntegral(KPID_t *pid);
+
+/**
+ * @brief 重置PID
+ *
+ * @param pid PID结构体
+ * @return int8_t 0对应没有错误
+ */
+int8_t PID_Reset(KPID_t *pid);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/component/ui.c

@@ -0,0 +1,301 @@
+/*
+  UI相关命令
+*/
+#include "component/ui.h"
+
+#include <stdio.h>
+
+/**
+ * @brief UI_绘制直线段
+ *
+ * @param grapic_line 结构体
+ * @param name 图形名首地址
+ * @param type_op 操作类型
+ * @param layer 图层数
+ * @param color 颜色
+ * @param width 线条宽度
+ * @param x_start 起点x坐标
+ * @param y_start 起点y坐标
+ * @param x_end 终点x坐标
+ * @param y_end 终点y坐标
+ * @return int8_t
+ */
+int8_t UI_DrawLine(UI_Ele_t *grapic_line, const char *name, uint8_t type_op,
+                   uint8_t layer, uint8_t color, uint16_t width,
+                   uint16_t x_start, uint16_t y_start, uint16_t x_end,
+                   uint16_t y_end) {
+  if (grapic_line == NULL) return -1;
+  snprintf((char *)grapic_line->name, 2, "%s", name);
+  grapic_line->layer = layer;
+  grapic_line->type_op = type_op;
+  grapic_line->type_ele = 0;
+  grapic_line->color = color;
+  grapic_line->width = width;
+  grapic_line->x_start = x_start;
+  grapic_line->y_start = y_start;
+  grapic_line->x_end = x_end;
+  grapic_line->y_end = y_end;
+  return 0;
+}
+
+/**
+ * @brief UI_绘制矩形
+ *
+ * @param grapic_rectangle 结构体
+ * @param name 图形名首地址
+ * @param type_op 操作类型
+ * @param layer 图层数
+ * @param color 颜色
+ * @param width 线条宽度
+ * @param x_start 起点x坐标
+ * @param y_start 起点y坐标
+ * @param x_end 对角顶点x坐标
+ * @param y_end 对角顶点y坐标
+ * @return int8_t
+ */
+int8_t UI_DrawRectangle(UI_Ele_t *grapic_rectangle, const char *name,
+                        uint8_t type_op, uint8_t layer, uint8_t color,
+                        uint16_t width, uint16_t x_start, uint16_t y_start,
+                        uint16_t x_end, uint16_t y_end) {
+  if (grapic_rectangle == NULL) return -1;
+  snprintf((char *)grapic_rectangle->name, 2, "%s", name);
+  grapic_rectangle->type_op = type_op;
+  grapic_rectangle->type_ele = 1;
+  grapic_rectangle->layer = layer;
+  grapic_rectangle->color = color;
+  grapic_rectangle->width = width;
+  grapic_rectangle->x_start = x_start;
+  grapic_rectangle->y_start = y_start;
+  grapic_rectangle->x_end = x_end;
+  grapic_rectangle->y_end = y_end;
+  return 0;
+}
+
+/**
+ * @brief UI_绘制正圆
+ *
+ * @param grapic_cycle 结构体
+ * @param name 图形名首地址
+ * @param type_op 操作类型
+ * @param layer 图层数
+ * @param color 颜色
+ * @param width 线条宽度
+ * @param x_center 圆心x坐标
+ * @param y_center 圆心y坐标
+ * @param radius 半径
+ * @return int8_t
+ */
+int8_t UI_DrawCycle(UI_Ele_t *grapic_cycle, const char *name, uint8_t type_op,
+                    uint8_t layer, uint8_t color, uint16_t width,
+                    uint16_t x_center, uint16_t y_center, uint16_t radius) {
+  if (grapic_cycle == NULL) return -1;
+  snprintf((char *)grapic_cycle->name, 2, "%s", name);
+  grapic_cycle->type_op = type_op;
+  grapic_cycle->layer = layer;
+  grapic_cycle->type_ele = 2;
+  grapic_cycle->color = color;
+  grapic_cycle->width = width;
+  grapic_cycle->x_start = x_center;
+  grapic_cycle->y_start = y_center;
+  grapic_cycle->radius = radius;
+  return 0;
+}
+
+/**
+ * @brief UI_绘制椭圆
+ *
+ * @param grapic_oval 结构体
+ * @param name 图形名首地址
+ * @param type_op 操作类型
+ * @param layer 图层数
+ * @param color 颜色
+ * @param width 线条宽度
+ * @param x_center 圆心x坐标
+ * @param y_center 圆心y坐标
+ * @param x_semiaxis x半轴长度
+ * @param y_semiaxis y半轴长度
+ * @return int8_t
+ */
+int8_t UI_DrawOval(UI_Ele_t *grapic_oval, const char *name, uint8_t type_op,
+                   uint8_t layer, uint8_t color, uint16_t width,
+                   uint16_t x_center, uint16_t y_center, uint16_t x_semiaxis,
+                   uint16_t y_semiaxis) {
+  if (grapic_oval == NULL) return -1;
+  snprintf((char *)grapic_oval->name, 2, "%s", name);
+  grapic_oval->type_op = type_op;
+  grapic_oval->type_ele = 3;
+  grapic_oval->layer = layer;
+  grapic_oval->color = color;
+  grapic_oval->width = width;
+  grapic_oval->x_start = x_center;
+  grapic_oval->y_start = y_center;
+  grapic_oval->x_end = x_semiaxis;
+  grapic_oval->y_end = y_semiaxis;
+  return 0;
+}
+
+/**
+ * @brief UI_绘制圆弧
+ *
+ * @param grapic_arc 结构体
+ * @param name 图形名首地址
+ * @param type_op 操作类型
+ * @param layer 图层数
+ * @param color 颜色
+ * @param angle_start 起始角度
+ * @param angle_end 终止角度
+ * @param width 线条宽度
+ * @param x_center 圆心x坐标
+ * @param y_center 圆心y坐标
+ * @param x_semiaxis x半轴长度
+ * @param y_semiaxis y半轴长度
+ * @return int8_t
+ */
+int8_t UI_DrawArc(UI_Ele_t *grapic_arc, const char *name, uint8_t type_op,
+                  uint8_t layer, uint8_t color, uint16_t angle_start,
+                  uint16_t angle_end, uint16_t width, uint16_t x_center,
+                  uint16_t y_center, uint16_t x_semiaxis, uint16_t y_semiaxis) {
+  if (grapic_arc == NULL) return -1;
+  snprintf((char *)grapic_arc->name, 2, "%s", name);
+  grapic_arc->type_op = type_op;
+  grapic_arc->type_ele = 4;
+  grapic_arc->layer = layer;
+  grapic_arc->color = color;
+  grapic_arc->angle_start = angle_start;
+  grapic_arc->angle_end = angle_end;
+  grapic_arc->width = width;
+  grapic_arc->x_start = x_center;
+  grapic_arc->y_start = y_center;
+  grapic_arc->x_end = x_semiaxis;
+  grapic_arc->y_end = y_semiaxis;
+  return 0;
+}
+
+/**
+ * @brief UI_绘制浮点数
+ *
+ * @param grapic_float 结构体
+ * @param name 图形名首地址
+ * @param type_op 操作类型
+ * @param layer 图层数
+ * @param color 颜色
+ * @param font_size 字体大小
+ * @param digits 小数点后有效位数
+ * @param width 线条宽度
+ * @param x_start 起点x坐标
+ * @param y_start 起点y坐标
+ * @param float_high 32位浮点数
+ * @param float_middle 32位浮点数
+ * @param float_low 32位浮点数
+ * @return int8_t
+ */
+int8_t UI_DrawFloating(UI_Ele_t *grapic_floating, const char *name,
+                       uint8_t type_op, uint8_t layer, uint8_t color,
+                       uint16_t font_size, uint16_t digits, uint16_t width,
+                       uint16_t x_start, uint16_t y_start, uint16_t float_high,
+                       uint16_t float_middle, uint16_t float_low) {
+  if (grapic_floating == NULL) return -1;
+  snprintf((char *)grapic_floating->name, 2, "%s", name);
+  grapic_floating->type_op = type_op;
+  grapic_floating->type_ele = 5;
+  grapic_floating->layer = layer;
+  grapic_floating->color = color;
+  grapic_floating->angle_start = font_size;
+  grapic_floating->angle_end = digits;
+  grapic_floating->width = width;
+  grapic_floating->x_start = x_start;
+  grapic_floating->y_start = y_start;
+  grapic_floating->radius = float_high;
+  grapic_floating->x_end = float_middle;
+  grapic_floating->y_end = float_low;
+  return 0;
+}
+
+/**
+ * @brief UI_绘制整型数
+ *
+ * @param grapic_integer 结构体
+ * @param name 图形名首地址
+ * @param type_op 操作类型
+ * @param layer 图层数
+ * @param color 颜色
+ * @param font_size 字体大小
+ * @param width 线条宽度
+ * @param x_start 起点x坐标
+ * @param y_start 起点y坐标
+ * @param int32_t_high 32位整型数
+ * @param int32_t_middle 32位整型数
+ * @param int32_t_low 32位整型数
+ * @return int8_t
+ */
+int8_t UI_DrawInteger(UI_Ele_t *grapic_integer, const char *name,
+                      uint8_t type_op, uint8_t layer, uint8_t color,
+                      uint16_t font_size, uint16_t width, uint16_t x_start,
+                      uint16_t y_start, uint16_t int32_t_high,
+                      uint16_t int32_t_middle, uint16_t int32_t_low) {
+  if (grapic_integer == NULL) return -1;
+  snprintf((char *)grapic_integer->name, 2, "%s", name);
+  grapic_integer->type_op = type_op;
+  grapic_integer->type_ele = 6;
+  grapic_integer->layer = layer;
+  grapic_integer->color = color;
+  grapic_integer->angle_start = font_size;
+  grapic_integer->width = width;
+  grapic_integer->x_start = x_start;
+  grapic_integer->y_start = y_start;
+  grapic_integer->radius = int32_t_high;
+  grapic_integer->x_end = int32_t_middle;
+  grapic_integer->y_end = int32_t_low;
+  return 0;
+}
+
+/**
+ * @brief UI_绘制字符
+ *
+ * @param grapic_character 结构体
+ * @param name 图形名首地址
+ * @param type_op 操作类型
+ * @param layer 图层数
+ * @param color 颜色
+ * @param font_size 字体大小
+ * @param length 字符长度
+ * @param width 线条宽度
+ * @param x_start 起点x坐标
+ * @param y_start 起点y坐标
+ * @param character 字符串首地址
+ * @return int8_t
+ */
+int8_t UI_DrawCharacter(UI_Drawcharacter_t *grapic_character, const char *name,
+                        uint8_t type_op, uint8_t layer, uint8_t color,
+                        uint16_t font_size, uint16_t length, uint16_t width,
+                        uint16_t x_start, uint16_t y_start,
+                        const char *character) {
+  if (grapic_character == NULL) return -1;
+  snprintf((char *)grapic_character->grapic.name, 2, "%s", name);
+  grapic_character->grapic.type_op = type_op;
+  grapic_character->grapic.type_ele = 7;
+  grapic_character->grapic.layer = layer;
+  grapic_character->grapic.color = color;
+  grapic_character->grapic.angle_start = font_size;
+  grapic_character->grapic.angle_end = length;
+  grapic_character->grapic.width = width;
+  grapic_character->grapic.x_start = x_start;
+  grapic_character->grapic.y_start = y_start;
+  snprintf((char *)grapic_character->character, 29, "%s", character);
+  return 0;
+}
+
+/**
+ * @brief UI_删除图层
+ *
+ * @param del 结构体
+ * @param opt 操作
+ * @param layer 图层
+ * @return int8_t
+ */
+int8_t UI_DelLayer(UI_Del_t *del, uint8_t opt, uint8_t layer) {
+  if (del == NULL) return -1;
+  del->del_operation = opt;
+  del->layer = layer;
+  return 0;
+}

assets/User_code/component/ui.h

@@ -0,0 +1,284 @@
+/*
+  UI相关命令
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+#include <string.h>
+
+#include "component/user_math.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+#define UI_DEL_OPERATION_NOTHING (0)
+#define UI_DEL_OPERATION_DEL (1)
+#define UI_DEL_OPERATION_DEL_ALL (2)
+
+#define UI_GRAPIC_OPERATION_NOTHING (0)
+#define UI_GRAPIC_OPERATION_ADD (1)
+#define UI_GRAPIC_OPERATION_REWRITE (2)
+#define UI_GRAPIC_OPERATION_DEL (3)
+
+#define UI_GRAPIC_LAYER_CONST (0)
+#define UI_GRAPIC_LAYER_AUTOAIM (1)
+#define UI_GRAPIC_LAYER_CHASSIS (2)
+#define UI_GRAPIC_LAYER_CAP (3)
+#define UI_GRAPIC_LAYER_GIMBAL (4)
+#define UI_GRAPIC_LAYER_SHOOT (5)
+#define UI_GRAPIC_LAYER_CMD (6)
+
+#define UI_DEFAULT_WIDTH (0x01)
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+#define UI_CHAR_DEFAULT_WIDTH (0x02)
+
+typedef enum {
+  RED_BLUE,
+  YELLOW,
+  GREEN,
+  ORANGE,
+  PURPLISH_RED,
+  PINK,
+  CYAN,
+  BLACK,
+  WHITE
+} UI_Color_t;
+
+typedef struct __packed {
+  uint8_t op;
+  uint8_t num_layer;
+} UI_InterStudent_UIDel_t;
+
+typedef struct __packed {
+  uint8_t name[3];
+  uint8_t type_op : 3;
+  uint8_t type_ele : 3;
+  uint8_t layer : 4;
+  uint8_t color : 4;
+  uint16_t angle_start : 9;
+  uint16_t angle_end : 9;
+  uint16_t width : 10;
+  uint16_t x_start : 11;
+  uint16_t y_start : 11;
+  uint16_t radius : 10;
+  uint16_t x_end : 11;
+  uint16_t y_end : 11;
+} UI_Ele_t;
+
+typedef struct __packed {
+  UI_Ele_t grapic;
+} UI_Drawgrapic_1_t;
+
+typedef struct __packed {
+  UI_Ele_t grapic[2];
+} UI_Drawgrapic_2_t;
+
+typedef struct __packed {
+  UI_Ele_t grapic[5];
+} UI_Drawgrapic_5_t;
+
+typedef struct __packed {
+  UI_Ele_t grapic[7];
+} UI_Drawgrapic_7_t;
+
+typedef struct __packed {
+  UI_Ele_t grapic;
+  uint8_t character[30];
+} UI_Drawcharacter_t;
+
+typedef struct __packed {
+  uint8_t del_operation;
+  uint8_t layer;
+} UI_Del_t;
+
+/**
+ * @brief UI_绘制直线段
+ *
+ * @param grapic_line 结构体
+ * @param name 图形名首地址
+ * @param type_op 操作类型
+ * @param layer 图层数
+ * @param color 颜色
+ * @param width 线条宽度
+ * @param x_start 起点x坐标
+ * @param y_start 起点y坐标
+ * @param x_end 终点x坐标
+ * @param y_end 终点y坐标
+ * @return int8_t
+ */
+int8_t UI_DrawLine(UI_Ele_t *grapic_line, const char *name, uint8_t type_op,
+                   uint8_t layer, uint8_t color, uint16_t width,
+                   uint16_t x_start, uint16_t y_start, uint16_t x_end,
+                   uint16_t y_end);
+
+/**
+ * @brief UI_绘制矩形
+ *
+ * @param grapic_rectangle 结构体
+ * @param name 图形名首地址
+ * @param type_op 操作类型
+ * @param layer 图层数
+ * @param color 颜色
+ * @param width 线条宽度
+ * @param x_start 起点x坐标
+ * @param y_start 起点y坐标
+ * @param x_end 对角顶点x坐标
+ * @param y_end 对角顶点y坐标
+ * @return int8_t
+ */
+int8_t UI_DrawRectangle(UI_Ele_t *grapic_rectangle, const char *name,
+                        uint8_t type_op, uint8_t layer, uint8_t color,
+                        uint16_t width, uint16_t x_start, uint16_t y_start,
+                        uint16_t x_end, uint16_t y_end);
+
+/**
+ * @brief UI_绘制正圆
+ *
+ * @param grapic_cycle 结构体
+ * @param name 图形名首地址
+ * @param type_op 操作类型
+ * @param layer 图层数
+ * @param color 颜色
+ * @param width 线条宽度
+ * @param x_center 圆心x坐标
+ * @param y_center 圆心y坐标
+ * @param radius 半径
+ * @return int8_t
+ */
+int8_t UI_DrawCycle(UI_Ele_t *grapic_cycle, const char *name, uint8_t type_op,
+                    uint8_t layer, uint8_t color, uint16_t width,
+                    uint16_t x_center, uint16_t y_center, uint16_t radius);
+
+/**
+ * @brief UI_绘制椭圆
+ *
+ * @param grapic_oval 结构体
+ * @param name 图形名首地址
+ * @param type_op 操作类型
+ * @param layer 图层数
+ * @param color 颜色
+ * @param width 线条宽度
+ * @param x_center 圆心x坐标
+ * @param y_center 圆心y坐标
+ * @param x_semiaxis x半轴长度
+ * @param y_semiaxis y半轴长度
+ * @return int8_t
+ */
+int8_t UI_DrawOval(UI_Ele_t *grapic_oval, const char *name, uint8_t type_op,
+                   uint8_t layer, uint8_t color, uint16_t width,
+                   uint16_t x_center, uint16_t y_center, uint16_t x_semiaxis,
+                   uint16_t y_semiaxis);
+
+/**
+ * @brief UI_绘制圆弧
+ *
+ * @param grapic_arc 结构体
+ * @param name 图形名首地址
+ * @param type_op 操作类型
+ * @param layer 图层数
+ * @param color 颜色
+ * @param angle_start 起始角度
+ * @param angle_end 终止角度
+ * @param width 线条宽度
+ * @param x_center 圆心x坐标
+ * @param y_center 圆心y坐标
+ * @param x_semiaxis x半轴长度
+ * @param y_semiaxis y半轴长度
+ * @return int8_t
+ */
+int8_t UI_DrawArc(UI_Ele_t *grapic_arc, const char *name, uint8_t type_op,
+                  uint8_t layer, uint8_t color, uint16_t angle_start,
+                  uint16_t angle_end, uint16_t width, uint16_t x_center,
+                  uint16_t y_center, uint16_t x_semiaxis, uint16_t y_semiaxis);
+
+/**
+ * @brief UI_绘制浮点数
+ *
+ * @param grapic_float 结构体
+ * @param name 图形名首地址
+ * @param type_op 操作类型
+ * @param layer 图层数
+ * @param color 颜色
+ * @param font_size 字体大小
+ * @param digits 小数点后有效位数
+ * @param width 线条宽度
+ * @param x_start 起点x坐标
+ * @param y_start 起点y坐标
+ * @param float_high 32位浮点数
+ * @param float_middle 32位浮点数
+ * @param float_low 32位浮点数
+ * @return int8_t
+ */
+int8_t UI_DrawFloating(UI_Ele_t *grapic_floating, const char *name,
+                       uint8_t type_op, uint8_t layer, uint8_t color,
+                       uint16_t font_size, uint16_t digits, uint16_t width,
+                       uint16_t x_start, uint16_t y_start, uint16_t float_high,
+                       uint16_t float_middle, uint16_t float_low);
+
+/**
+ * @brief UI_绘制整型数
+ *
+ * @param grapic_integer 结构体
+ * @param name 图形名首地址
+ * @param type_op 操作类型
+ * @param layer 图层数
+ * @param color 颜色
+ * @param font_size 字体大小
+ * @param width 线条宽度
+ * @param x_start 起点x坐标
+ * @param y_start 起点y坐标
+ * @param int32_t_high 32位整型数
+ * @param int32_t_middle 32位整型数
+ * @param int32_t_low 32位整型数
+ * @return int8_t
+ */
+int8_t UI_DrawInteger(UI_Ele_t *grapic_integer, const char *name,
+                      uint8_t type_op, uint8_t layer, uint8_t color,
+                      uint16_t font_size, uint16_t width, uint16_t x_start,
+                      uint16_t y_start, uint16_t int32_t_high,
+                      uint16_t int32_t_middle, uint16_t int32_t_low);
+
+/**
+ * @brief UI_绘制字符
+ *
+ * @param grapic_character 结构体
+ * @param name 图形名首地址
+ * @param type_op 操作类型
+ * @param layer 图层数
+ * @param color 颜色
+ * @param font_size 字体大小
+ * @param length 字符长度
+ * @param width 线条宽度
+ * @param x_start 起点x坐标
+ * @param y_start 起点y坐标
+ * @param character 字符串首地址
+ * @return int8_t
+ */
+int8_t UI_DrawCharacter(UI_Drawcharacter_t *grapic_character, const char *name,
+                        uint8_t type_op, uint8_t layer, uint8_t color,
+                        uint16_t font_size, uint16_t length, uint16_t width,
+                        uint16_t x_start, uint16_t y_start,
+                        const char *character);
+
+/**
+ * @brief UI_删除图层
+ *
+ * @param del 结构体
+ * @param opt 操作
+ * @param layer 图层
+ * @return int8_t
+ */
+int8_t UI_DelLayer(UI_Del_t *del, uint8_t opt, uint8_t layer);
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/component/user_math.c

@@ -0,0 +1,134 @@
+/*
+  自定义的数学运算。
+*/
+
+#include "user_math.h"
+#include <string.h>
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+inline float InvSqrt(float x) {
+//#if 0
+  /* Fast inverse square-root */
+  /* See: http://en.wikipedia.org/wiki/Fast_inverse_square_root */
+	float halfx = 0.5f * x;
+	float y = x;
+	long i = *(long*)&y;
+	i = 0x5f3759df - (i>>1);
+	y = *(float*)&i;
+	y = y * (1.5f - (halfx * y * y));
+	y = y * (1.5f - (halfx * y * y));
+	return y;	
+//#else
+//  return 1.0f / sqrtf(x);
+//#endif
+}
+
+inline float AbsClip(float in, float limit) {
+  return (in < -limit) ? -limit : ((in > limit) ? limit : in);
+}
+
+float fAbs(float in){
+  return (in > 0) ? in : -in;
+}
+
+inline void Clip(float *origin, float min, float max) {
+  if (*origin > max) *origin = max;
+  if (*origin < min) *origin = min;
+}
+
+inline float Sign(float in) { return (in > 0) ? 1.0f : 0.0f; }
+
+/**
+ * \brief 将运动向量置零
+ *
+ * \param mv 被操作的值
+ */
+inline void ResetMoveVector(MoveVector_t *mv) { memset(mv, 0, sizeof(*mv)); }
+
+/**
+ * \brief 计算循环值的误差，适用于设定值与反馈值均在（x,y）范围内循环的情况，range应设定为y-x
+ * 例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b;
+ * \param sp 设定值
+ * \param fb 反馈值
+ * \param range 被操作的值变化范围，正数时起效
+ * \return 函数运行结果
+ */
+inline float CircleError(float sp, float fb, float range) {
+  float error = sp - fb;
+  if (range > 0.0f) {
+    float half_range = range / 2.0f;
+
+    if (error > half_range)
+      error -= range;
+    else if (error < -half_range)
+      error += range;
+  }
+  return error;
+}
+
+/**
+ * \brief 循环加法，适用于被操作的值在（0,range）范围内循环的情况
+ * \param origin 被操作的值
+ * \param delta 变化量
+ * \param range 被操作的值变化范围，正数时起效
+ */
+inline void CircleAdd(float *origin, float delta, float range) {
+  float out = *origin + delta;
+  if (range > 0.0f) {
+    if (out >= range)
+      out -= range;
+    else if (out < 0.0f)
+      out += range;
+  }
+  *origin = out;
+}
+
+/**
+ * @brief 循环值取反
+ *
+ * @param origin 被操作的值
+ */
+inline void CircleReverse(float *origin) { *origin = -(*origin) + M_2PI; }
+
+/**
+ * @brief 根据目标弹丸速度计算摩擦轮转速
+ *
+ * @param bullet_speed 弹丸速度
+ * @param fric_radius 摩擦轮半径
+ * @param is17mm 是否为17mm
+ * @return 摩擦轮转速
+ */
+inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) {
+  if (bullet_speed == 0.0f) return 0.f;
+  if (is17mm) {
+    if (bullet_speed == 15.0f) return 4670.f;
+    if (bullet_speed == 18.0f) return 5200.f;
+    if (bullet_speed == 30.0f) return 7350.f;
+  } else {
+    if (bullet_speed == 10.0f) return 4450.f;
+    if (bullet_speed == 16.0f) return 5800.f;
+  }
+
+  /* 不为裁判系统设定值时,计算转速 */
+  return 60.0f * (float)bullet_speed / (M_2PI * fric_radius);
+}
+
+// /**
+//  * @brief 断言失败处理
+//  *
+//  * @param file 文件名
+//  * @param line 行号
+//  */
+// void VerifyFailed(const char *file, uint32_t line) {
+//   UNUSED(file);
+//   UNUSED(line);
+//   while (1) {
+//     __NOP();
+//   }
+// }
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */

assets/User_code/component/user_math.h

@@ -0,0 +1,179 @@
+/*
+  自定义的数学运算。
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <float.h>
+#include <math.h>
+#include <stdbool.h>
+#include <stdint.h>
+#include <stddef.h>
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+#define M_DEG2RAD_MULT (0.01745329251f)
+#define M_RAD2DEG_MULT (57.2957795131f)
+
+#ifndef M_PI_2
+#define M_PI_2 1.57079632679f
+#endif
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846f
+#endif
+
+#ifndef M_2PI
+#define M_2PI 6.28318530717958647692f
+#endif
+
+#ifndef __packed
+  #define __packed __attribute__((__packed__))
+#endif /* __packed */
+
+#define max(a, b)           \
+  ({                        \
+    __typeof__(a) _a = (a); \
+    __typeof__(b) _b = (b); \
+    _a > _b ? _a : _b;      \
+  })
+
+#define min(a, b)           \
+  ({                        \
+    __typeof__(a) _a = (a); \
+    __typeof__(b) _b = (b); \
+    _a < _b ? _a : _b;      \
+  })
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+
+
+/* 移动向量 */
+typedef struct {
+  float vx; /* 前后平移 */
+  float vy; /* 左右平移 */
+  float wz; /* 转动 */
+} MoveVector_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+float InvSqrt(float x);
+
+float AbsClip(float in, float limit);
+
+float fAbs(float in);
+
+void Clip(float *origin, float min, float max);
+
+float Sign(float in);
+
+/**
+ * \brief 将运动向量置零
+ *
+ * \param mv 被操作的值
+ */
+void ResetMoveVector(MoveVector_t *mv);
+
+/**
+ * \brief 计算循环值的误差，适用于设定值与反馈值均在（x,y）范围内循环的情况，range应设定为y-x
+ * 例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b;
+ * \param sp 设定值
+ * \param fb 反馈值
+ * \param range 被操作的值变化范围，正数时起效
+ * \return 函数运行结果
+ */
+float CircleError(float sp, float fb, float range);
+
+/**
+ * \brief 循环加法，适用于被操作的值在（0,range）范围内循环的情况
+ * \param origin 被操作的值
+ * \param delta 变化量
+ * \param range 被操作的值变化范围，正数时起效
+ */
+void CircleAdd(float *origin, float delta, float range);
+
+/**
+ * @brief 循环值取反
+ *
+ * @param origin 被操作的值
+ */
+void CircleReverse(float *origin);
+
+/**
+ * @brief 根据目标弹丸速度计算摩擦轮转速
+ *
+ * @param bullet_speed 弹丸速度
+ * @param fric_radius 摩擦轮半径
+ * @param is17mm 是否为17mm
+ * @return 摩擦轮转速
+ */
+float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm);
+
+#ifdef __cplusplus
+}
+#endif
+
+#ifdef DEBUG
+
+/**
+ * @brief 如果表达式的值为假则运行处理函数
+ *
+ */
+#define ASSERT(expr)                    \
+  do {                                  \
+    if (!(expr)) {                      \
+      VerifyFailed(__FILE__, __LINE__); \
+    }                                   \
+  } while (0)
+#else
+
+/**
+ * @brief 未定DEBUG，表达式不会运行，断言被忽略
+ *
+ */
+#define ASSERT(expr) ((void)(0))
+#endif
+
+#ifdef DEBUG
+
+/**
+ * @brief 如果表达式的值为假则运行处理函数
+ *
+ */
+#define VERIFY(expr)                    \
+  do {                                  \
+    if (!(expr)) {                      \
+      VerifyFailed(__FILE__, __LINE__); \
+    }                                   \
+  } while (0)
+#else
+
+/**
+ * @brief 表达式会运行，忽略表达式结果
+ *
+ */
+#define VERIFY(expr) ((void)(expr))
+#endif
+
+// /**
+//  * @brief 断言失败处理
+//  *
+//  * @param file 文件名
+//  * @param line 行号
+//  */
+// void VerifyFailed(const char *file, uint32_t line);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */

assets/User_code/config.csv

@@ -0,0 +1,4 @@
+bsp,can,dwt,gpio,i2c,mm,spi,uart,pwm,time
+component,ahrs,capacity,crc8,crc16,error_detect,filter,FreeRTOS_CLI,limiter,mixer,pid,ui,user_math
+device,dr16,bmi088,ist8310,motor,motor_rm,motor_dm,motor_vesc,motor_lk,motor_lz,motor_odrive,dm_imu,rc_can,servo,buzzer,led,ws2812,vofa,ops9,ai
+module,config,

assets/User_code/device/ai.c

@@ -0,0 +1,142 @@
+/*
+AI
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "ai.h"
+
+#include <stdbool.h>
+#include <string.h>
+
+#include "bsp/time.h"
+#include "bsp/uart.h"
+#include "component/ahrs.h"
+#include "component/crc16.h"
+#include "component/crc8.h"
+#include "component/user_math.h"
+#include "component/filter.h"
+
+
+/* Private define ----------------------------------------------------------- */
+#define AI_LEN_RX_BUFF (sizeof(AI_DownPackage_t))
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+
+static uint8_t rxbuf[AI_LEN_RX_BUFF];
+
+static bool inited = false;
+
+static osThreadId_t thread_alert;
+
+static uint32_t drop_message = 0;
+
+// uint16_t crc16;
+
+/* Private function  -------------------------------------------------------- */
+
+static void Ai_RxCpltCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
+}
+
+static void Ai_IdleLineCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
+}
+
+/* Exported functions ------------------------------------------------------- */
+int8_t AI_Init(AI_t *ai) {
+  UNUSED(ai);
+  if (inited) return DEVICE_ERR_INITED;
+  thread_alert = osThreadGetId();
+
+  BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB,
+                            Ai_RxCpltCallback);
+  BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_IDLE_LINE_CB,
+                            Ai_IdleLineCallback);
+  
+  inited = true;
+  return 0;
+}
+
+int8_t AI_Restart(AI_t *ai) {
+  UNUSED(ai);
+  __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_AI));
+  __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_AI));
+  return DEVICE_OK;
+}
+
+int8_t AI_StartReceiving(AI_t *ai) {
+  UNUSED(ai);
+  // if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf,
+  //                          AI_LEN_RX_BUFF) == HAL_OK)
+  if (BSP_UART_Receive(BSP_UART_AI, rxbuf,
+                           AI_LEN_RX_BUFF, true) == HAL_OK)
+    return DEVICE_OK;
+  return DEVICE_ERR;
+}
+
+bool AI_WaitDmaCplt(void) {
+  return (osThreadFlagsWait(SIGNAL_AI_RAW_REDY, osFlagsWaitAll,0) ==
+          SIGNAL_AI_RAW_REDY);
+}
+
+int8_t AI_ParseHost(AI_t *ai) {
+  // crc16 = CRC16_Calc((const uint8_t *)&(rxbuf), sizeof(ai->from_host) - 2, CRC16_INIT);
+  if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->from_host)))
+    goto error;
+  ai->header.online = true;
+  ai->header.last_online_time = BSP_TIME_Get();
+  memcpy(&(ai->from_host), rxbuf, sizeof(ai->from_host));
+  memset(rxbuf, 0, AI_LEN_RX_BUFF);
+  return DEVICE_OK;
+
+error:
+  drop_message++;
+  return DEVICE_ERR;
+}
+
+int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *data){
+  if (ai == NULL || data == NULL) return DEVICE_ERR_NULL;
+  ai->to_host.mcu.id = AI_ID_MCU;
+  ai->to_host.mcu.package.quat=*data;
+  ai->to_host.mcu.package.notice = ai->status;
+  ai->to_host.mcu.crc16 = CRC16_Calc((const uint8_t *)&(ai->to_host.mcu), sizeof(AI_UpPackageMCU_t) - 2, CRC16_INIT);
+  return DEVICE_OK;
+}
+
+int8_t AI_PackRef(AI_t *ai, const AI_UpPackageReferee_t *data) {
+  if (ai == NULL || data == NULL) return DEVICE_ERR_NULL;
+  ai->to_host.ref = *data;
+  return DEVICE_OK;
+}
+
+int8_t AI_HandleOffline(AI_t *ai) {
+  if (ai == NULL) return DEVICE_ERR_NULL;
+  if (BSP_TIME_Get() - ai->header.last_online_time >
+      100000) {
+    ai->header.online = false;
+  }
+  return DEVICE_OK;
+}
+
+int8_t AI_StartSend(AI_t *ai, bool ref_online){
+  if (ai == NULL) return DEVICE_ERR_NULL;
+  
+  if (ref_online) {
+    // 发送裁判系统数据和MCU数据
+    if (BSP_UART_Transmit(BSP_UART_AI, (uint8_t *)&(ai->to_host),
+                          sizeof(ai->to_host.ref) + sizeof(ai->to_host.mcu), true) == HAL_OK)
+      return DEVICE_OK;
+    else
+      return DEVICE_ERR;
+  } else {
+    // 只发送MCU数据
+    if (BSP_UART_Transmit(BSP_UART_AI, (uint8_t *)&(ai->to_host.mcu),
+                          sizeof(ai->to_host.mcu), true) == HAL_OK)
+      return DEVICE_OK;
+    else
+      return DEVICE_ERR;
+  }
+}
+

assets/User_code/device/ai.h

@@ -0,0 +1,131 @@
+/*
+  AI
+*/
+
+#pragma once
+
+#include <sys/cdefs.h>
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "component/ahrs.h"
+#include "component/filter.h"
+#include "component/user_math.h"
+#include "device/device.h"
+#include <cmsis_os2.h>
+#include <stdbool.h>
+#include <stdint.h>
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+#define AI_ID_MCU (0xC4)
+#define AI_ID_REF (0xA8)
+#define AI_ID_AI (0xA1)
+/* Exported types ----------------------------------------------------------- */
+
+typedef enum {
+  AI_ARMOR_HERO = 0, /*英雄机器人*/
+  AI_ARMOR_INFANTRY, /*步兵机器人*/
+  AI_ARMOR_SENTRY,   /*哨兵机器人*/
+  AI_ARMOR_ENGINEER, /*工程机器人*/
+  AI_ARMOR_OUTPOST,  /*前哨占*/
+  AI_ARMOR_BASE,     /*基地*/
+  AI_ARMOR_NORMAL,   /*由AI自动选择*/
+} AI_ArmorsType_t;
+
+typedef enum {
+  AI_STATUS_OFF = 0, /* 关闭 */
+  AI_STATUS_AUTOAIM, /* 自瞄 */
+  AI_STATUS_AUTOPICK, /* 自动取矿 */
+  AI_STATUS_AUTOPUT,  /* 自动兑矿 */
+  AI_STATUS_AUTOHITBUFF,  /* 自动打符 */
+  AI_STATUS_AUTONAV,
+} AI_Status_t;
+
+typedef enum { 
+  AI_NOTICE_NONE = 0,
+  AI_NOTICE_SEARCH,
+  AI_NOTICE_FIRE,
+}AI_Notice_t;
+
+/* 电控 -> 视觉 MCU数据结构体*/
+typedef struct __packed {
+  AHRS_Quaternion_t quat; /* 四元数 */
+  // struct {
+  //   AI_ArmorsType_t armor_type;
+  //   AI_Status_t status;
+  // }notice; /* 控制命令 */
+  uint8_t notice;
+} AI_Protucol_UpDataMCU_t;
+
+/* 电控 -> 视觉 裁判系统数据结构体*/
+typedef struct __packed {
+  /* USER REFEREE BEGIN */
+  uint16_t team; /* 本身队伍 */
+  uint16_t time; /* 比赛开始时间 */
+  /* USER REFEREE END */
+} AI_Protocol_UpDataReferee_t;
+
+/* 视觉 -> 电控 数据包结构体*/
+typedef struct __packed {
+  AHRS_Eulr_t eulr;      /* 欧拉角 */
+  MoveVector_t move_vec; /* 运动向量 */
+  uint8_t notice;        /* 控制命令 */
+} AI_Protocol_DownData_t;
+
+/* 电控 -> 视觉 裁判系统数据包 */
+typedef struct __packed {
+  uint8_t id;                          /* 包ID */
+  AI_Protocol_UpDataReferee_t package; /* 数据包 */
+  uint16_t crc16;                      /* CRC16校验 */
+} AI_UpPackageReferee_t;
+
+/* 电控 -> 视觉 MUC数据包 */
+typedef struct __packed {
+  uint8_t id;
+  AI_Protucol_UpDataMCU_t package;
+  uint16_t crc16;
+} AI_UpPackageMCU_t;
+
+/* 视觉 -> 电控 数据包 */
+typedef struct __packed {
+  uint8_t id;                     /* 包ID */
+  AI_Protocol_DownData_t package; /* 数据包 */
+  uint16_t crc16;                 /* CRC16校验 */
+} AI_DownPackage_t;
+
+typedef struct __packed {
+  DEVICE_Header_t header; /* 设备通用头部 */
+  AI_DownPackage_t from_host;
+  AI_Status_t status;
+  struct {
+    AI_UpPackageReferee_t ref;
+    AI_UpPackageMCU_t mcu;
+  } to_host;
+} AI_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+int8_t AI_Init(AI_t *ai);
+
+int8_t AI_Restart(AI_t *ai);
+
+int8_t AI_StartReceiving(AI_t *ai);
+
+bool AI_WaitDmaCplt(void);
+
+int8_t AI_ParseHost(AI_t *ai);
+
+int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat);
+
+int8_t AI_PackRef(AI_t *ai, const AI_UpPackageReferee_t *data);
+
+int8_t AI_HandleOffline(AI_t *ai);
+
+int8_t AI_StartSend(AI_t *ai, bool ref_online);
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/bmi088.c

@@ -0,0 +1,381 @@
+/*
+     BMI088 陀螺仪+加速度计传感器。
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "bmi088.h"
+
+#include <cmsis_os2.h>
+#include <gpio.h>
+#include <stdbool.h>
+#include <string.h>
+
+#include "bsp/time.h"
+#include "bsp/gpio.h"
+#include "bsp/spi.h"
+#include "component/user_math.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private define ----------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+#define BMI088_REG_ACCL_CHIP_ID (0x00)
+#define BMI088_REG_ACCL_ERR (0x02)
+#define BMI088_REG_ACCL_STATUS (0x03)
+#define BMI088_REG_ACCL_X_LSB (0x12)
+#define BMI088_REG_ACCL_X_MSB (0x13)
+#define BMI088_REG_ACCL_Y_LSB (0x14)
+#define BMI088_REG_ACCL_Y_MSB (0x15)
+#define BMI088_REG_ACCL_Z_LSB (0x16)
+#define BMI088_REG_ACCL_Z_MSB (0x17)
+#define BMI088_REG_ACCL_SENSORTIME_0 (0x18)
+#define BMI088_REG_ACCL_SENSORTIME_1 (0x19)
+#define BMI088_REG_ACCL_SENSORTIME_2 (0x1A)
+#define BMI088_REG_ACCL_INT_STAT_1 (0x1D)
+#define BMI088_REG_ACCL_TEMP_MSB (0x22)
+#define BMI088_REG_ACCL_TEMP_LSB (0x23)
+#define BMI088_REG_ACCL_CONF (0x40)
+#define BMI088_REG_ACCL_RANGE (0x41)
+#define BMI088_REG_ACCL_INT1_IO_CONF (0x53)
+#define BMI088_REG_ACCL_INT2_IO_CONF (0x54)
+#define BMI088_REG_ACCL_INT1_INT2_MAP_DATA (0x58)
+#define BMI088_REG_ACCL_SELF_TEST (0x6D)
+#define BMI088_REG_ACCL_PWR_CONF (0x7C)
+#define BMI088_REG_ACCL_PWR_CTRL (0x7D)
+#define BMI088_REG_ACCL_SOFTRESET (0x7E)
+
+#define BMI088_REG_GYRO_CHIP_ID (0x00)
+#define BMI088_REG_GYRO_X_LSB (0x02)
+#define BMI088_REG_GYRO_X_MSB (0x03)
+#define BMI088_REG_GYRO_Y_LSB (0x04)
+#define BMI088_REG_GYRO_Y_MSB (0x05)
+#define BMI088_REG_GYRO_Z_LSB (0x06)
+#define BMI088_REG_GYRO_Z_MSB (0x07)
+#define BMI088_REG_GYRO_INT_STAT_1 (0x0A)
+#define BMI088_REG_GYRO_RANGE (0x0F)
+#define BMI088_REG_GYRO_BANDWIDTH (0x10)
+#define BMI088_REG_GYRO_LPM1 (0x11)
+#define BMI088_REG_GYRO_SOFTRESET (0x14)
+#define BMI088_REG_GYRO_INT_CTRL (0x15)
+#define BMI088_REG_GYRO_INT3_INT4_IO_CONF (0x16)
+#define BMI088_REG_GYRO_INT3_INT4_IO_MAP (0x18)
+#define BMI088_REG_GYRO_SELF_TEST (0x3C)
+
+#define BMI088_CHIP_ID_ACCL (0x1E)
+#define BMI088_CHIP_ID_GYRO (0x0F)
+
+#define BMI088_LEN_RX_BUFF (19)
+/* Private macro ------------------------------------------------------------ */
+#define BMI088_ACCL_NSS_SET() \
+  BSP_GPIO_WritePin(BSP_GPIO_ACCL_CS, GPIO_PIN_SET)
+#define BMI088_ACCL_NSS_RESET() \
+  BSP_GPIO_WritePin(BSP_GPIO_ACCL_CS, GPIO_PIN_RESET)
+
+#define BMI088_GYRO_NSS_SET() \
+  BSP_GPIO_WritePin(BSP_GPIO_GYRO_CS, GPIO_PIN_SET)
+#define BMI088_GYRO_NSS_RESET() \
+  BSP_GPIO_WritePin(BSP_GPIO_GYRO_CS, GPIO_PIN_RESET)
+
+/* Private typedef ---------------------------------------------------------- */
+typedef enum {
+  BMI_ACCL,
+  BMI_GYRO,
+} BMI_Device_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private variables -------------------------------------------------------- */
+static uint8_t buffer[2];
+static uint8_t bmi088_rxbuf[BMI088_LEN_RX_BUFF];
+
+static osThreadId_t thread_alert;
+static bool inited = false;
+
+/* Private function  -------------------------------------------------------- */
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+static void BMI_WriteSingle(BMI_Device_t dv, uint8_t reg, uint8_t data) {
+  buffer[0] = (reg & 0x7f);
+  buffer[1] = data;
+
+  BSP_TIME_Delay(1);
+  switch (dv) {
+    case BMI_ACCL:
+      BMI088_ACCL_NSS_RESET();
+      break;
+
+    case BMI_GYRO:
+      BMI088_GYRO_NSS_RESET();
+      break;
+  }
+
+  BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 2u, false);
+
+  switch (dv) {
+    case BMI_ACCL:
+      BMI088_ACCL_NSS_SET();
+      break;
+
+    case BMI_GYRO:
+      BMI088_GYRO_NSS_SET();
+      break;
+  }
+}
+
+static uint8_t BMI_ReadSingle(BMI_Device_t dv, uint8_t reg) {
+  BSP_TIME_Delay(1);
+  switch (dv) {
+    case BMI_ACCL:
+      BMI088_ACCL_NSS_RESET();
+      break;
+
+    case BMI_GYRO:
+      BMI088_GYRO_NSS_RESET();
+      break;
+  }
+  buffer[0] = (uint8_t)(reg | 0x80);
+  BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 1u, false);
+  BSP_SPI_Receive(BSP_SPI_BMI088, buffer, 2u, false);
+
+  switch (dv) {
+    case BMI_ACCL:
+      BMI088_ACCL_NSS_SET();
+      return buffer[1];
+
+    case BMI_GYRO:
+      BMI088_GYRO_NSS_SET();
+      return buffer[0];
+  }
+}
+
+static void BMI_Read(BMI_Device_t dv, uint8_t reg, uint8_t *data, uint8_t len) {
+  if (data == NULL) return;
+
+  switch (dv) {
+    case BMI_ACCL:
+      BMI088_ACCL_NSS_RESET();
+      break;
+
+    case BMI_GYRO:
+      BMI088_GYRO_NSS_RESET();
+      break;
+  }
+  buffer[0] = (uint8_t)(reg | 0x80);
+  BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 1u, false); 
+  BSP_SPI_Receive(BSP_SPI_BMI088, data, len, true);
+}
+
+static void BMI088_RxCpltCallback(void) {
+  if (BSP_GPIO_ReadPin(BSP_GPIO_ACCL_CS) == GPIO_PIN_RESET) {
+    BMI088_ACCL_NSS_SET();
+    osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_RAW_REDY);
+  }
+  if (BSP_GPIO_ReadPin(BSP_GPIO_GYRO_CS) == GPIO_PIN_RESET) {
+    BMI088_GYRO_NSS_SET();
+    osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_RAW_REDY);
+  }
+}
+
+static void BMI088_AcclIntCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_NEW_DATA);
+}
+
+static void BMI088_GyroIntCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_NEW_DATA);
+}
+
+/* Exported functions ------------------------------------------------------- */
+int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali) {
+  if (bmi088 == NULL) return DEVICE_ERR_NULL;
+  if (cali == NULL) return DEVICE_ERR_NULL;
+  if (inited) return DEVICE_ERR_INITED;
+  if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
+
+  bmi088->cali = cali;
+
+  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_SOFTRESET, 0xB6);
+  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_SOFTRESET, 0xB6);
+  BSP_TIME_Delay(30);
+
+  /* Switch accl to SPI mode. */
+  BMI_ReadSingle(BMI_ACCL, BMI088_CHIP_ID_ACCL);
+
+  if (BMI_ReadSingle(BMI_ACCL, BMI088_REG_ACCL_CHIP_ID) != BMI088_CHIP_ID_ACCL)
+    return DEVICE_ERR_NO_DEV;
+
+  if (BMI_ReadSingle(BMI_GYRO, BMI088_REG_GYRO_CHIP_ID) != BMI088_CHIP_ID_GYRO)
+    return DEVICE_ERR_NO_DEV;
+
+  BSP_GPIO_DisableIRQ(BSP_GPIO_ACCL_INT);
+  BSP_GPIO_DisableIRQ(BSP_GPIO_GYRO_INT);
+
+  BSP_SPI_RegisterCallback(BSP_SPI_BMI088, BSP_SPI_RX_CPLT_CB,
+                           BMI088_RxCpltCallback);
+  BSP_GPIO_RegisterCallback(BSP_GPIO_ACCL_INT, BMI088_AcclIntCallback);
+  BSP_GPIO_RegisterCallback(BSP_GPIO_GYRO_INT, BMI088_GyroIntCallback);
+
+  /* Accl init. */
+  /* Filter setting: Normal. */
+  /* ODR: 0xAB: 800Hz. 0xAA: 400Hz. 0xA9: 200Hz. 0xA8: 100Hz. 0xA6: 25Hz. */
+  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_CONF, 0xAA);
+
+  /* 0x00: +-3G. 0x01: +-6G. 0x02: +-12G. 0x03: +-24G. */
+  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_RANGE, 0x01);
+
+  /* INT1 as output. Push-pull. Active low. Output. */
+  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_IO_CONF, 0x08);
+
+  /* Map data ready interrupt to INT1. */
+  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_INT2_MAP_DATA, 0x04);
+
+  /* Turn on accl. Now we can read data. */
+  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_PWR_CTRL, 0x04);
+  BSP_TIME_Delay(50);
+
+  /* Gyro init. */
+  /* 0x00: +-2000. 0x01: +-1000. 0x02: +-500. 0x03: +-250. 0x04: +-125. */
+  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_RANGE, 0x01);
+
+  /* Filter bw: 47Hz. */
+  /* ODR: 0x02: 1000Hz. 0x03: 400Hz. 0x06: 200Hz. 0x07: 100Hz. */
+  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_BANDWIDTH, 0x03);
+
+  /* INT3 and INT4 as output. Push-pull. Active low. */
+  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_CONF, 0x00);
+
+  /* Map data ready interrupt to INT3. */
+  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_MAP, 0x01);
+
+  /* Enable new data interrupt. */
+  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT_CTRL, 0x80);
+
+  BSP_TIME_Delay(10);
+
+  inited = true;
+
+  BSP_GPIO_EnableIRQ(BSP_GPIO_ACCL_INT);
+  BSP_GPIO_EnableIRQ(BSP_GPIO_GYRO_INT);
+  return DEVICE_OK;
+}
+
+bool BMI088_GyroStable(AHRS_Gyro_t *gyro) {
+  return ((gyro->x < 0.03f) && (gyro->y < 0.03f) && (gyro->z < 0.03f));
+}
+
+uint32_t BMI088_WaitNew() {
+  return osThreadFlagsWait(
+      SIGNAL_BMI088_ACCL_NEW_DATA | SIGNAL_BMI088_GYRO_NEW_DATA, osFlagsWaitAll,
+      osWaitForever);
+}
+
+int8_t BMI088_AcclStartDmaRecv() {
+  BMI_Read(BMI_ACCL, BMI088_REG_ACCL_X_LSB, bmi088_rxbuf, BMI088_LEN_RX_BUFF);
+  return DEVICE_OK;
+}
+
+uint32_t BMI088_AcclWaitDmaCplt() {
+  return osThreadFlagsWait(SIGNAL_BMI088_ACCL_RAW_REDY, osFlagsWaitAll,
+                           osWaitForever);
+}
+
+int8_t BMI088_GyroStartDmaRecv() {
+  BMI_Read(BMI_GYRO, BMI088_REG_GYRO_X_LSB, bmi088_rxbuf + 7, 6u);
+  return DEVICE_OK;
+}
+
+uint32_t BMI088_GyroWaitDmaCplt() {
+  return osThreadFlagsWait(SIGNAL_BMI088_GYRO_RAW_REDY, osFlagsWaitAll,
+                           osWaitForever);
+}
+
+int8_t BMI088_ParseAccl(BMI088_t *bmi088) {
+  if (bmi088 == NULL) return DEVICE_ERR_NULL;
+
+#if 1
+  int16_t raw_x, raw_y, raw_z;
+  memcpy(&raw_x, bmi088_rxbuf + 1, sizeof(raw_x));
+  memcpy(&raw_y, bmi088_rxbuf + 3, sizeof(raw_y));
+  memcpy(&raw_z, bmi088_rxbuf + 5, sizeof(raw_z));
+
+  bmi088->accl.x = (float)raw_x;
+  bmi088->accl.y = (float)raw_y;
+  bmi088->accl.z = (float)raw_z;
+
+#else
+  const int16_t *praw_x = (int16_t *)(bmi088_rxbuf + 1);
+  const int16_t *praw_y = (int16_t *)(bmi088_rxbuf + 3);
+  const int16_t *praw_z = (int16_t *)(bmi088_rxbuf + 5);
+
+  bmi088->accl.x = (float)*praw_x;
+  bmi088->accl.y = (float)*praw_y;
+  bmi088->accl.z = (float)*praw_z;
+
+#endif
+
+  /* 3G: 10920. 6G: 5460. 12G: 2730. 24G: 1365. */
+  bmi088->accl.x /= 5460.0f;
+  bmi088->accl.y /= 5460.0f;
+  bmi088->accl.z /= 5460.0f;
+
+  int16_t raw_temp =
+      (uint16_t)((bmi088_rxbuf[17] << 3) | (bmi088_rxbuf[18] >> 5));
+
+  if (raw_temp > 1023) raw_temp -= 2048;
+
+  bmi088->temp = (float)raw_temp * 0.125f + 23.0f;
+
+  return DEVICE_OK;
+}
+
+int8_t BMI088_ParseGyro(BMI088_t *bmi088) {
+  if (bmi088 == NULL) return DEVICE_ERR_NULL;
+
+#if 1
+  /* Gyroscope imu_raw -> degrees/sec -> radians/sec */
+  int16_t raw_x, raw_y, raw_z;
+  memcpy(&raw_x, bmi088_rxbuf + 7, sizeof(raw_x));
+  memcpy(&raw_y, bmi088_rxbuf + 9, sizeof(raw_y));
+  memcpy(&raw_z, bmi088_rxbuf + 11, sizeof(raw_z));
+
+  bmi088->gyro.x = (float)raw_x;
+  bmi088->gyro.y = (float)raw_y;
+  bmi088->gyro.z = (float)raw_z;
+
+#else
+  /* Gyroscope imu_raw -> degrees/sec -> radians/sec */
+  const int16_t *raw_x = (int16_t *)(bmi088_rxbuf + 7);
+  const int16_t *raw_y = (int16_t *)(bmi088_rxbuf + 9);
+  const int16_t *raw_z = (int16_t *)(bmi088_rxbuf + 11);
+
+  bmi088->gyro.x = (float)*raw_x;
+  bmi088->gyro.y = (float)*raw_y;
+  bmi088->gyro.z = (float)*raw_z;
+#endif
+
+  /* FS125: 262.144. FS250: 131.072. FS500: 65.536. FS1000: 32.768.
+   * FS2000: 16.384.*/
+  bmi088->gyro.x /= 32.768f;
+  bmi088->gyro.y /= 32.768f;
+  bmi088->gyro.z /= 32.768f;
+
+  bmi088->gyro.x *= M_DEG2RAD_MULT;
+  bmi088->gyro.y *= M_DEG2RAD_MULT;
+  bmi088->gyro.z *= M_DEG2RAD_MULT;
+
+  bmi088->gyro.x -= bmi088->cali->gyro_offset.x;
+  bmi088->gyro.y -= bmi088->cali->gyro_offset.y;
+  bmi088->gyro.z -= bmi088->cali->gyro_offset.z;
+
+  return DEVICE_ERR_NULL;
+}
+
+float BMI088_GetUpdateFreq(BMI088_t *bmi088) {
+  (void)bmi088;
+  return 400.0f;
+}

assets/User_code/device/bmi088.h

@@ -0,0 +1,81 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "component/ahrs.h"
+#include "device/device.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef struct {
+  struct {
+    float x;
+    float y;
+    float z;
+  } gyro_offset; /* 陀螺仪偏置 */
+} BMI088_Cali_t; /* BMI088校准数据 */
+
+typedef struct {
+  DEVICE_Header_t header;
+  AHRS_Accl_t accl;
+  AHRS_Gyro_t gyro;
+
+  float temp; /* 温度 */
+
+  const BMI088_Cali_t *cali;
+} BMI088_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Exported functions prototypes -------------------------------------------- */
+int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali);
+int8_t BMI088_Restart(void);
+
+bool BMI088_GyroStable(AHRS_Gyro_t *gyro);
+
+/* Sensor use right-handed coordinate system. */
+/*
+                x < R(logo)
+                        y
+                UP is z
+        All implementation should follow this rule.
+ */
+uint32_t BMI088_WaitNew();
+
+/*
+  BMI088的Accl和Gyro共用同一个DMA通道，所以一次只能读一个传感器。
+  即BMI088_AcclStartDmaRecv() 和 BMI088_AcclWaitDmaCplt() 中间不能
+  出现 BMI088_GyroStartDmaRecv()。
+*/
+int8_t BMI088_AcclStartDmaRecv();
+uint32_t BMI088_AcclWaitDmaCplt();
+int8_t BMI088_GyroStartDmaRecv();
+uint32_t BMI088_GyroWaitDmaCplt();
+int8_t BMI088_ParseAccl(BMI088_t *bmi088);
+int8_t BMI088_ParseGyro(BMI088_t *bmi088);
+float BMI088_GetUpdateFreq(BMI088_t *bmi088);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/buzzer.c

@@ -0,0 +1,171 @@
+#include "device/buzzer.h"
+#include "bsp/time.h"
+#include <math.h>
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+#define MUSIC_DEFAULT_VOLUME 0.5f
+#define MUSIC_A4_FREQ 440.0f  // A4音符频率
+
+/* USER MUSIC MENU BEGIN */
+// RM音乐
+const Tone_t RM[] = {
+    {NOTE_B, 5, 200},
+    {NOTE_G, 4, 200},
+    {NOTE_B, 5, 400},
+    {NOTE_G, 4, 200},
+    {NOTE_B, 5, 400},
+    {NOTE_G, 4, 200},
+    {NOTE_D, 5, 400},
+    {NOTE_G, 4, 200},
+    {NOTE_C, 5, 200},
+    {NOTE_C, 5, 200},
+    {NOTE_G, 4, 200},
+    {NOTE_B, 5, 200},
+    {NOTE_C, 5, 200}
+};
+
+// Nokia 经典铃声音符
+const Tone_t NOKIA[] = {
+    {NOTE_E, 5, 125}, {NOTE_D, 5, 125}, {NOTE_FS, 4, 250}, {NOTE_GS, 4, 250},
+    {NOTE_CS, 5, 125}, {NOTE_B, 4, 125}, {NOTE_D, 4, 250}, {NOTE_E, 4, 250},
+    {NOTE_B, 4, 125}, {NOTE_A, 4, 125}, {NOTE_CS, 4, 250}, {NOTE_E, 4, 250},
+    {NOTE_A, 4, 500}
+};
+/* USER MUSIC MENU END */
+
+static void BUZZER_Update(BUZZER_t *buzzer){
+    buzzer->header.online = true;
+    buzzer->header.last_online_time = BSP_TIME_Get_ms();
+}
+
+// 根据音符和八度计算频率的辅助函数
+static float BUZZER_CalcFreq(NOTE_t note, uint8_t octave) {
+    if (note == NOTE_REST) {
+        return 0.0f; // 休止符返回0频率
+    }
+    
+    // 将音符和八度转换为MIDI音符编号
+    int midi_num = (int)note + (int)((octave + 1) * 12);
+    
+    // 使用A4 (440Hz) 作为参考，计算频率
+    // 公式: freq = 440 * 2^((midi_num - 69)/12)
+    float freq = 440.0f * powf(2.0f, ((float)midi_num - 69.0f) / 12.0f);
+    
+    return freq;
+}
+
+// 播放单个音符
+static int8_t BUZZER_PlayTone(BUZZER_t *buzzer, NOTE_t note, uint8_t octave, uint16_t duration_ms) {
+    if (buzzer == NULL || !buzzer->header.online) 
+        return DEVICE_ERR;
+    
+    float freq = BUZZER_CalcFreq(note, octave);
+    
+    if (freq > 0.0f) {
+        // 播放音符
+        if (BUZZER_Set(buzzer, freq, MUSIC_DEFAULT_VOLUME) != DEVICE_OK) 
+            return DEVICE_ERR;
+        
+        if (BUZZER_Start(buzzer) != DEVICE_OK) 
+            return DEVICE_ERR;
+    } else {
+        // 休止符，停止播放
+        BUZZER_Stop(buzzer);
+    }
+    
+    // 等待指定时间
+    BSP_TIME_Delay_ms(duration_ms);
+    
+    // 停止当前音符，为下一个音符做准备
+    BUZZER_Stop(buzzer);
+    BSP_TIME_Delay_ms(20); // 短暂间隔
+    
+    return DEVICE_OK;
+}
+
+int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel) {
+    if (buzzer == NULL) return DEVICE_ERR;
+    
+    buzzer->channel = channel;
+    buzzer->header.online = true;
+    
+    BUZZER_Stop(buzzer);
+    
+    return DEVICE_OK ;
+}
+
+int8_t BUZZER_Start(BUZZER_t *buzzer) {
+    if (buzzer == NULL || !buzzer->header.online) 
+        return DEVICE_ERR;
+    BUZZER_Update(buzzer);
+    return (BSP_PWM_Start(buzzer->channel) == BSP_OK) ? 
+           DEVICE_OK  : DEVICE_ERR;
+}
+
+int8_t BUZZER_Stop(BUZZER_t *buzzer) {
+    if (buzzer == NULL || !buzzer->header.online) 
+        return DEVICE_ERR;
+    BUZZER_Update(buzzer);
+    return (BSP_PWM_Stop(buzzer->channel) == BSP_OK) ? 
+           DEVICE_OK  : DEVICE_ERR;
+}
+
+int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle) {
+    if (buzzer == NULL || !buzzer->header.online) 
+        return DEVICE_ERR;
+    
+    int result = DEVICE_OK ;
+    BUZZER_Update(buzzer);
+    if (BSP_PWM_SetFreq(buzzer->channel, freq) != BSP_OK) 
+        result = DEVICE_ERR;
+    
+    if (BSP_PWM_SetComp(buzzer->channel, duty_cycle) != BSP_OK) 
+        result = DEVICE_ERR;
+    
+    return result;
+}
+
+int8_t BUZZER_PlayMusic(BUZZER_t *buzzer, MUSIC_t music) {
+    if (buzzer == NULL || !buzzer->header.online) 
+        return DEVICE_ERR;
+    
+    const Tone_t *melody = NULL;
+    size_t melody_length = 0;
+    
+    // 根据音乐类型选择对应的音符数组
+    switch (music) {
+        case MUSIC_RM:
+            melody = RM;
+            melody_length = sizeof(RM) / sizeof(Tone_t);
+            break;
+        case MUSIC_NOKIA:
+            melody = NOKIA;
+            melody_length = sizeof(NOKIA) / sizeof(Tone_t);
+            break;
+        default:
+            return DEVICE_ERR;
+    }
+    
+    // 播放整首音乐
+    for (size_t i = 0; i < melody_length; i++) {
+        if (BUZZER_PlayTone(buzzer, melody[i].note, melody[i].octave, melody[i].duration_ms) != DEVICE_OK) {
+            BUZZER_Stop(buzzer); // 出错时停止播放
+            return DEVICE_ERR;
+        }
+    }
+    
+    // 音乐播放完成后停止
+    BUZZER_Stop(buzzer);
+    return DEVICE_OK;
+}
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */

assets/User_code/device/buzzer.h

@@ -0,0 +1,138 @@
+/**
+ * @file buzzer.h
+ * @brief 蜂鸣器设备驱动头文件
+ * @details 提供蜂鸣器音频播放功能，支持单音符播放和预设音乐播放
+ * @author Generated by STM32CubeMX
+ * @date 2025年10月23日
+ */
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "bsp/pwm.h"    // PWM底层硬件抽象层
+#include "device.h"     // 设备通用头文件
+#include <stddef.h>     // 标准定义
+#include <stdint.h>     // 标准整型定义
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Exported constants ------------------------------------------------------- */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Exported types ----------------------------------------------------------- */
+
+/**
+ * @brief 音符枚举类型
+ * @details 定义标准十二平均律音符，用于音乐播放
+ */
+typedef enum {
+  NOTE_C = 0,       ///< Do音符
+  NOTE_CS = 1,      ///< Do#音符 (升Do)
+  NOTE_D = 2,       ///< Re音符
+  NOTE_DS = 3,      ///< Re#音符 (升Re)
+  NOTE_E = 4,       ///< Mi音符
+  NOTE_F = 5,       ///< Fa音符
+  NOTE_FS = 6,      ///< Fa#音符 (升Fa)
+  NOTE_G = 7,       ///< Sol音符
+  NOTE_GS = 8,      ///< Sol#音符 (升Sol)
+  NOTE_A = 9,       ///< La音符
+  NOTE_AS = 10,     ///< La#音符 (升La)
+  NOTE_B = 11,      ///< Si音符
+  NOTE_REST = 255   ///< 休止符 (无声音)
+} NOTE_t;
+
+/**
+ * @brief 音调结构体
+ * @details 定义一个完整的音调信息，包括音符、八度和持续时间
+ */
+typedef struct {
+  NOTE_t note;          ///< 音符名称 (使用NOTE_t枚举)
+  uint8_t octave;       ///< 八度 (0-8，通常使用3-7)
+  uint16_t duration_ms; ///< 持续时间，单位毫秒
+} Tone_t;
+
+/**
+ * @brief 预设音乐枚举类型
+ * @details 定义可播放的预设音乐类型
+ */
+typedef enum {
+  /* USER MUSIC MENU BEGIN */
+  MUSIC_RM,     ///< RM战队音乐
+  MUSIC_NOKIA,  ///< 诺基亚经典铃声
+  /* USER MUSIC MENU END */
+} MUSIC_t;
+
+/**
+ * @brief 蜂鸣器设备结构体
+ * @details 蜂鸣器设备的完整描述，包含设备头信息和PWM通道
+ */
+typedef struct {
+  DEVICE_Header_t header;     ///< 设备通用头信息 (在线状态、时间戳等)
+  BSP_PWM_Channel_t channel;  ///< PWM输出通道
+} BUZZER_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 初始化蜂鸣器设备
+ * @param buzzer 蜂鸣器设备结构体指针
+ * @param channel PWM输出通道
+ * @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败
+ * @note 初始化后蜂鸣器处于停止状态
+ */
+int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel);
+
+/**
+ * @brief 启动蜂鸣器播放
+ * @param buzzer 蜂鸣器设备结构体指针
+ * @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败
+ * @note 需要先调用BUZZER_Set设置频率和占空比
+ */
+int8_t BUZZER_Start(BUZZER_t *buzzer);
+
+/**
+ * @brief 停止蜂鸣器播放
+ * @param buzzer 蜂鸣器设备结构体指针
+ * @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败
+ */
+int8_t BUZZER_Stop(BUZZER_t *buzzer);
+
+/**
+ * @brief 设置蜂鸣器频率和占空比
+ * @param buzzer 蜂鸣器设备结构体指针
+ * @param freq 频率 (Hz)，通常范围20Hz-20kHz
+ * @param duty_cycle 占空比 (0.0-1.0)，影响音量大小
+ * @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败
+ * @note 设置后需要调用BUZZER_Start才能听到声音
+ */
+int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle);
+
+/**
+ * @brief 播放预设音乐
+ * @param buzzer 蜂鸣器设备结构体指针
+ * @param music 音乐类型 (使用MUSIC_t枚举)
+ * @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败
+ * @note 这是一个阻塞函数，会播放完整首音乐后返回
+ */
+int8_t BUZZER_PlayMusic(BUZZER_t *buzzer, MUSIC_t music);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/config.yaml

@@ -0,0 +1,227 @@
+devices:
+  dr16:
+    name: "DR16"
+    description: "大疆遥控器接收机"
+    dependencies:
+      bsp: ["uart"]
+      component: ["user_math"]
+    bsp_requirements:
+      - type: "uart"
+        var_name: "BSP_UART_DR16"  # 需要替换的变量名
+        description: "用于接收遥控器数据"
+    thread_signals:
+      - name: "SIGNAL_DR16_RAW_REDY"
+    files:
+      header: "dr16.h"
+      source: "dr16.c"
+  
+  ops9:
+    name: "OPS9"
+    description: "ACTION OPS9 码盘"
+    dependencies:
+      bsp: ["uart"]
+      component: ["user_math"]
+    bsp_requirements:
+      - type: "uart"
+        var_name: "BSP_UART_OPS9"  # 需要替换的变量名
+        description: "用于接收码盘"
+    thread_signals:
+      - name: "SIGNAL_OPS9_RAW_REDY"
+    files:
+      header: "ops9.h"
+      source: "ops9.c"
+
+  bmi088:
+    name: "BMI088"
+    description: "BMI088 陀螺仪+加速度计传感器"
+    dependencies:
+      bsp: ["spi", "gpio"]
+      component: ["user_math"]
+    bsp_requirements:
+      - type: "spi"
+        var_name: "BSP_SPI_BMI088"
+        description: "用于与 BMI088 通信的 SPI 总线"
+      - type: "gpio"
+        var_name: "BSP_GPIO_ACCL_CS"
+        description: "加速度计片选输出引脚"
+        gpio_type: "output"
+      - type: "gpio"
+        var_name: "BSP_GPIO_GYRO_CS"
+        description: "陀螺仪片选输出引脚"
+        gpio_type: "output"
+      - type: "gpio"
+        var_name: "BSP_GPIO_ACCL_INT"
+        description: "加速度计中断输入引脚"
+        gpio_type: "EXTI"
+      - type: "gpio"
+        var_name: "BSP_GPIO_GYRO_INT"
+        description: "陀螺仪中断输入引脚"
+        gpio_type: "EXTI"
+    thread_signals:
+      - name: "SIGNAL_BMI088_ACCL_RAW_REDY"
+      - name: "SIGNAL_BMI088_GYRO_RAW_REDY"
+      - name: "SIGNAL_BMI088_ACCL_NEW_DATA"
+      - name: "SIGNAL_BMI088_GYRO_NEW_DATA"
+    files:
+      header: "bmi088.h"
+      source: "bmi088.c"
+
+  ist8310:
+    name: "IST8310"
+    description: "IST8310 地磁传感器"
+    dependencies:
+      bsp: ["i2c", "gpio"]
+      component: []
+    bsp_requirements:
+      - type: "i2c"
+        var_name: "BSP_I2C_COMP"
+        description: "用于与 IST8310 通信的 I2C 总线"
+      - type: "gpio"
+        var_name: "CMPS_RST_Pin"
+        description: "IST8310 复位引脚"
+        gpio_type: "output"
+      - type: "gpio"
+        var_name: "CMPS_INT_Pin"
+        description: "IST8310 数据中断引脚"
+        gpio_type: "EXTI"
+    thread_signals:
+      - name: "SIGNAL_IST8310_MAGN_RAW_REDY"
+      - name: "SIGNAL_IST8310_MAGN_NEW_DATA"
+    files:
+      header: "ist8310.h"
+      source: "ist8310.c"
+
+  motor_vesc:
+    name: "VESC 电调"
+    description: "VESC 电调驱动"
+    dependencies:
+      bsp: ["can", "time", "mm"]
+      component: ["user_math"]
+    files:
+      header: "motor_vesc.h"
+      source: "motor_vesc.c"
+  
+  motor_odrive:
+    name: "ODrive 电机"
+    description: "ODrive 电机驱动"
+    dependencies:
+      bsp: ["can", "time", "mm"]
+      component: ["user_math"]
+    files:
+      header: "motor_odrive.h"
+      source: "motor_odrive.c"
+
+  motor_rm:
+    name: "RM 电机" 
+    description: "RM 电机驱动"
+    dependencies:
+      bsp: ["can", "time", "mm"]
+      component: ["user_math"]
+    files:
+      header: "motor_rm.h"
+      source: "motor_rm.c"
+
+  motor:
+    name: "通用电机"
+    description: "通用电机驱动"
+    dependencies:
+      bsp: []
+      component: []
+    bsp_requirements: []
+    thread_signals: []
+    files:
+      header: "motor.h"
+      source: "motor.c"
+
+  ws2812:
+    name: "WS2812 LED 灯"
+    description: "WS2812 RGB LED 灯驱动"
+    dependencies:
+      bsp: ["pwm", "time"]
+      component: []
+    thread_signals: []
+    files:
+      header: "ws2812.h"
+      source: "ws2812.c"
+
+  buzzer:
+    name: "蜂鸣器"
+    description: "蜂鸣器驱动"
+    dependencies:
+      bsp: ["pwm"]
+      component: []
+    bsp_requirements:
+      - type: "pwm"
+        var_name: "BSP_PWM_BUZZER"
+        description: "用于蜂鸣器的PWM通道"
+    thread_signals: []
+    files:
+      header: "buzzer.h"
+      source: "buzzer.c"
+
+  dm_imu:
+    name: "DM IMU"
+    description: "DM IMU 传感器"
+    dependencies:
+      bsp: ["can", "time"]
+      component: ["user_math"]
+    files:
+      header: "dm_imu.h"
+      source: "dm_imu.c"
+
+  led:
+    name: "LED 灯"
+    description: "LED 灯驱动"
+    dependencies:
+      bsp: ["gpio", "pwm"]
+      component: []
+    thread_signals: []
+    files:
+      header: "led.h"
+      source: "led.c"
+
+  motor_lk:
+    name: "LK 电机"
+    description: "LK 电机驱动"
+    dependencies:
+      bsp: ["can", "time", "mm"]
+      component: ["user_math"]
+    files:
+      header: "motor_lk.h"
+      source: "motor_lk.c"
+
+  motor_lz:
+    name: "LZ 电机"
+    description: "LZ 电机驱动"
+    dependencies:
+      bsp: ["can", "time", "mm"]
+      component: ["user_math"]
+    files:
+      header: "motor_lz.h"
+      source: "motor_lz.c"
+
+  servo:
+    name: "舵机"
+    description: "舵机驱动"
+    dependencies:
+      bsp: ["pwm"]
+      component: []
+    thread_signals: []
+    files:
+      header: "servo.h"
+      source: "servo.c"
+
+  vofa:
+    name: "VOFA"
+    description: "VOFA 数据传输协议"
+    dependencies:
+      bsp: ["uart"]
+      component: []
+    bsp_requirements:
+      - type: "uart"
+        var_name: "BSP_UART_VOFA"  # 需要替换的变量名
+        description: "用于VOFA数据传输"
+    thread_signals: []
+    files:
+      header: "vofa.h"
+      source: "vofa.c"

assets/User_code/device/device.h

@@ -0,0 +1,47 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+#define DEVICE_OK (0)
+#define DEVICE_ERR (-1)
+#define DEVICE_ERR_NULL (-2)
+#define DEVICE_ERR_INITED (-3)
+#define DEVICE_ERR_NO_DEV (-4)
+
+/* AUTO GENERATED SIGNALS BEGIN */
+
+/* AUTO GENERATED SIGNALS END */
+
+/* USER SIGNALS BEGIN */
+
+/* USER SIGNALS END */
+/*设备层通用Header*/
+typedef struct {
+    bool online;
+    uint64_t last_online_time;
+} DEVICE_Header_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/dm_imu.c

@@ -0,0 +1,271 @@
+/*
+    DM_IMU数据获取（CAN）
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "dm_imu.h"
+
+#include "bsp/can.h"
+#include "bsp/time.h"
+#include "component/user_math.h"
+#include <string.h>
+
+/* Private define ----------------------------------------------------------- */
+#define DM_IMU_OFFLINE_TIMEOUT      1000    // 设备离线判定时间1000ms
+
+#define ACCEL_CAN_MAX (58.8f)
+#define ACCEL_CAN_MIN	(-58.8f)
+#define GYRO_CAN_MAX	(34.88f)
+#define GYRO_CAN_MIN	(-34.88f)
+#define PITCH_CAN_MAX	(90.0f)
+#define PITCH_CAN_MIN	(-90.0f)
+#define ROLL_CAN_MAX	(180.0f)
+#define ROLL_CAN_MIN	(-180.0f)
+#define YAW_CAN_MAX		(180.0f)
+#define YAW_CAN_MIN 	(-180.0f)
+#define TEMP_MIN			(0.0f)
+#define TEMP_MAX			(60.0f)
+#define Quaternion_MIN	(-1.0f)
+#define Quaternion_MAX	(1.0f)
+
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+/* Private function --------------------------------------------------------- */
+
+static uint8_t count = 0; // 计数器，用于判断设备是否离线
+/**
+ * @brief: 无符号整数转换为浮点数函数
+ */
+static float uint_to_float(int x_int, float x_min, float x_max, int bits)
+{
+    float span = x_max - x_min;
+    float offset = x_min;
+    return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
+}
+
+/**
+ * @brief 解析加速度计数据
+ */
+static int8_t DM_IMU_ParseAccelData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
+    if (imu == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR;
+    }
+    int8_t temp = data[1];
+    uint16_t acc_x_raw = (data[3] << 8) | data[2];
+    uint16_t acc_y_raw = (data[5] << 8) | data[4];
+    uint16_t acc_z_raw = (data[7] << 8) | data[6];
+    imu->data.temp = (float)temp;
+    imu->data.accl.x = uint_to_float(acc_x_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
+    imu->data.accl.y = uint_to_float(acc_y_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
+    imu->data.accl.z = uint_to_float(acc_z_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16);
+    return DEVICE_OK;
+}
+/**
+ * @brief 解析陀螺仪数据
+ */
+static int8_t DM_IMU_ParseGyroData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
+    if (imu == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR;
+    }
+    uint16_t gyro_x_raw = (data[3] << 8) | data[2];
+    uint16_t gyro_y_raw = (data[5] << 8) | data[4];
+    uint16_t gyro_z_raw = (data[7] << 8) | data[6];
+    imu->data.gyro.x = uint_to_float(gyro_x_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
+    imu->data.gyro.y = uint_to_float(gyro_y_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
+    imu->data.gyro.z = uint_to_float(gyro_z_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16);
+    return DEVICE_OK;
+}
+/**
+ * @brief 解析欧拉角数据
+ */
+static int8_t DM_IMU_ParseEulerData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
+    if (imu == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR;
+    }
+    uint16_t pit_raw = (data[3] << 8) | data[2];
+    uint16_t yaw_raw = (data[5] << 8) | data[4];
+    uint16_t rol_raw = (data[7] << 8) | data[6];
+    imu->data.euler.pit = uint_to_float(pit_raw, PITCH_CAN_MIN, PITCH_CAN_MAX, 16) * M_DEG2RAD_MULT;
+    imu->data.euler.yaw = uint_to_float(yaw_raw, YAW_CAN_MIN, YAW_CAN_MAX, 16) * M_DEG2RAD_MULT;
+    imu->data.euler.rol = uint_to_float(rol_raw, ROLL_CAN_MIN, ROLL_CAN_MAX, 16) * M_DEG2RAD_MULT;
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 解析四元数数据
+ */
+static int8_t DM_IMU_ParseQuaternionData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
+    if (imu == NULL || data == NULL || len < 8) {
+        return DEVICE_ERR;
+    }
+    int w = (data[1] << 6) | ((data[2] & 0xF8) >> 2);
+    int x = ((data[2] & 0x03) << 12) | (data[3] << 4) | ((data[4] & 0xF0) >> 4);
+    int y = ((data[4] & 0x0F) << 10) | (data[5] << 2) | ((data[6] & 0xC0) >> 6);
+    int z = ((data[6] & 0x3F) << 8) | data[7];
+    imu->data.quat.q0 = uint_to_float(w, Quaternion_MIN, Quaternion_MAX, 14);
+    imu->data.quat.q1 = uint_to_float(x, Quaternion_MIN, Quaternion_MAX, 14);
+    imu->data.quat.q2 = uint_to_float(y, Quaternion_MIN, Quaternion_MAX, 14);
+    imu->data.quat.q3 = uint_to_float(z, Quaternion_MIN, Quaternion_MAX, 14);
+    return DEVICE_OK;
+}
+
+
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * @brief 初始化DM IMU设备
+ */
+int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param) {
+    if (imu == NULL || param == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    // 初始化设备头部
+    imu->header.online = false;
+    imu->header.last_online_time = 0;
+    
+    // 配置参数
+    imu->param.can = param->can;
+    imu->param.can_id = param->can_id;
+    imu->param.device_id = param->device_id;
+    imu->param.master_id = param->master_id;
+    
+    // 清零数据
+    memset(&imu->data, 0, sizeof(DM_IMU_Data_t));
+    
+    // 注册CAN接收队列，用于接收回复报文
+    int8_t result = BSP_CAN_RegisterId(imu->param.can, imu->param.master_id, 10);
+    if (result != BSP_OK) {
+        return DEVICE_ERR;
+    }
+
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 请求IMU数据
+ */
+int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) {
+    if (imu == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    // 构造发送数据：id_L, id_H(DM_IMU_ID), RID, 0xcc
+    uint8_t tx_data[4] = {
+        imu->param.device_id & 0xFF,        // id_L
+        (imu->param.device_id >> 8) & 0xFF, // id_H
+        (uint8_t)rid,                       // RID
+        0xCC                                // 固定值
+    };
+    
+    // 发送标准数据帧
+    BSP_CAN_StdDataFrame_t frame = {
+        .id = imu->param.can_id,
+        .dlc = 4,
+    };
+    memcpy(frame.data, tx_data, 4);
+    int8_t result = BSP_CAN_TransmitStdDataFrame(imu->param.can, &frame);
+    return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR;
+}
+
+/**
+ * @brief 更新IMU数据（从CAN中获取所有数据并解析）
+ */
+int8_t DM_IMU_Update(DM_IMU_t *imu) {
+    if (imu == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    BSP_CAN_Message_t msg;
+    int8_t result;
+    bool data_received = false;
+    
+    // 持续接收所有可用消息
+    while ((result = BSP_CAN_GetMessage(imu->param.can, imu->param.master_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE)) == BSP_OK) {
+        // 验证回复数据格式（至少检查数据长度）
+        if (msg.dlc < 3) {
+            continue; // 跳过无效消息
+        }
+        
+        // 根据数据位的第0位确定反馈报文类型
+        uint8_t rid = msg.data[0] & 0x0F; // 取第0位的低4位作为RID
+        
+        // 根据RID类型解析数据
+        int8_t parse_result = DEVICE_ERR;
+        switch (rid) {
+            case 0x01: // RID_ACCL
+                parse_result = DM_IMU_ParseAccelData(imu, msg.data, msg.dlc);
+                break;
+            case 0x02: // RID_GYRO
+                parse_result = DM_IMU_ParseGyroData(imu, msg.data, msg.dlc);
+                break;
+            case 0x03: // RID_EULER
+                parse_result = DM_IMU_ParseEulerData(imu, msg.data, msg.dlc);
+                break;
+            case 0x04: // RID_QUATERNION
+                parse_result = DM_IMU_ParseQuaternionData(imu, msg.data, msg.dlc);
+                break;
+            default:
+                continue; // 跳过未知类型的消息
+        }
+        
+        // 如果解析成功，标记为收到数据
+        if (parse_result == DEVICE_OK) {
+            data_received = true;
+        }
+    }
+    
+    // 如果收到任何有效数据，更新设备状态
+    if (data_received) {
+        imu->header.online = true;
+        imu->header.last_online_time = BSP_TIME_Get_ms();
+        return DEVICE_OK;
+    }
+    
+    return DEVICE_ERR;
+}
+
+/** 
+ * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz）
+ */ 
+int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu){
+    if (imu == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    switch (count) {
+        case 0:
+            DM_IMU_Request(imu, RID_ACCL);
+            break;
+        case 1:
+            DM_IMU_Request(imu, RID_GYRO);
+            break;
+        case 2:
+            DM_IMU_Request(imu, RID_EULER);
+            break;
+        case 3:
+            DM_IMU_Request(imu, RID_QUATERNION);
+            DM_IMU_Update(imu); // 更新所有数据
+            break;
+    }
+    count++;
+    if (count >= 4) {
+        count = 0; // 重置计数器
+        return DEVICE_OK;
+    }
+    return DEVICE_ERR;
+}
+
+/**
+ * @brief 检查设备是否在线
+ */
+bool DM_IMU_IsOnline(DM_IMU_t *imu) {
+    if (imu == NULL) {
+        return false;
+    }
+    
+    uint32_t current_time = BSP_TIME_Get_ms();
+    return imu->header.online && 
+           (current_time - imu->header.last_online_time < DM_IMU_OFFLINE_TIMEOUT);
+}

assets/User_code/device/dm_imu.h

@@ -0,0 +1,90 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/device.h"
+#include "component/ahrs.h"
+#include "bsp/can.h"
+/* Exported constants ------------------------------------------------------- */
+
+#define DM_IMU_CAN_ID_DEFAULT 0x6FF
+#define DM_IMU_ID_DEFAULT 0x42
+#define DM_IMU_MST_ID_DEFAULT 0x43
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+typedef struct {
+  BSP_CAN_t can;       // CAN总线句柄
+  uint16_t can_id;     // CAN通信ID
+  uint8_t device_id;   // 设备ID
+  uint8_t master_id;   // 主机ID
+} DM_IMU_Param_t;
+typedef enum {
+  RID_ACCL = 0x01, // 加速度计
+  RID_GYRO = 0x02, // 陀螺仪
+  RID_EULER = 0x03, // 欧拉角
+  RID_QUATERNION = 0x04, // 四元数
+} DM_IMU_RID_t;
+
+typedef struct {
+  AHRS_Accl_t accl; // 加速度计
+  AHRS_Gyro_t gyro; // 陀螺仪
+  AHRS_Eulr_t euler; // 欧拉角
+  AHRS_Quaternion_t quat; // 四元数
+  float temp; // 温度
+} DM_IMU_Data_t;
+
+typedef struct {
+  DEVICE_Header_t header;
+  DM_IMU_Param_t param; // IMU参数配置
+  DM_IMU_Data_t data; // IMU数据
+} DM_IMU_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 初始化DM IMU设备
+ * @param imu DM IMU设备结构体指针
+ * @param param IMU参数配置指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param);
+
+/**
+ * @brief 请求IMU数据
+ * @param imu DM IMU设备结构体指针
+ * @param rid 请求的数据类型
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid);
+
+
+/**
+ * @brief 更新IMU数据（从CAN中获取所有数据并解析）
+ * @param imu DM IMU设备结构体指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t DM_IMU_Update(DM_IMU_t *imu);
+
+/**
+ * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz，运行4次才有完整数据）
+ * @param imu DM IMU设备结构体指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu);
+
+/**
+ * @brief 检查设备是否在线
+ * @param imu DM IMU设备结构体指针
+ * @return true 在线，false 离线
+ */
+bool DM_IMU_IsOnline(DM_IMU_t *imu);
+
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/dr16.c

@@ -0,0 +1,169 @@
+/*
+    DR16接收机
+    Example：
+
+  DR16_Init(&dr16);
+  
+  while (1) {
+    DR16_StartDmaRecv(&dr16);
+    if (DR16_WaitDmaCplt(20)) {
+      DR16_ParseData(&dr16);
+    } else {
+      DR16_Offline(&dr16);
+    }
+}
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "dr16.h"
+#include "bsp/uart.h"
+#include "bsp/time.h"
+#include "device.h"
+
+#include <string.h>
+#include <stdbool.h>
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+/* Private define ----------------------------------------------------------- */
+#define DR16_CH_VALUE_MIN (364u)
+#define DR16_CH_VALUE_MID (1024u)
+#define DR16_CH_VALUE_MAX (1684u)
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+
+static osThreadId_t thread_alert;
+static bool inited = false;
+
+/* Private function  -------------------------------------------------------- */
+static void DR16_RxCpltCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY);
+}
+
+static bool DR16_DataCorrupted(const DR16_t *dr16) {
+  if (dr16 == NULL) return DEVICE_ERR_NULL;
+
+  if ((dr16->raw_data.ch_r_x < DR16_CH_VALUE_MIN) ||
+      (dr16->raw_data.ch_r_x > DR16_CH_VALUE_MAX))
+    return DEVICE_ERR;
+
+  if ((dr16->raw_data.ch_r_y < DR16_CH_VALUE_MIN) ||
+      (dr16->raw_data.ch_r_y > DR16_CH_VALUE_MAX))
+    return DEVICE_ERR;
+
+  if ((dr16->raw_data.ch_l_x < DR16_CH_VALUE_MIN) ||
+      (dr16->raw_data.ch_l_x > DR16_CH_VALUE_MAX))
+    return DEVICE_ERR;
+
+  if ((dr16->raw_data.ch_l_y < DR16_CH_VALUE_MIN) ||
+      (dr16->raw_data.ch_l_y > DR16_CH_VALUE_MAX))
+    return DEVICE_ERR;
+
+  if (dr16->raw_data.sw_l == 0) return DEVICE_ERR;
+
+  if (dr16->raw_data.sw_r == 0) return DEVICE_ERR;
+
+  return DEVICE_OK;
+}
+
+/* Exported functions ------------------------------------------------------- */
+int8_t DR16_Init(DR16_t *dr16) {
+  if (dr16 == NULL) return DEVICE_ERR_NULL;
+  if (inited) return DEVICE_ERR_INITED;
+  if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
+
+  BSP_UART_RegisterCallback(BSP_UART_DR16, BSP_UART_RX_CPLT_CB,
+                            DR16_RxCpltCallback);
+
+  inited = true;
+  return DEVICE_OK;
+}
+
+int8_t DR16_Restart(void) {
+  __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_DR16));
+  __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_DR16));
+  return DEVICE_OK;
+}
+
+int8_t DR16_StartDmaRecv(DR16_t *dr16) {
+  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_DR16),
+                           (uint8_t *)&(dr16->raw_data),
+                           sizeof(dr16->raw_data)) == HAL_OK)
+    return DEVICE_OK;
+  return DEVICE_ERR;
+}
+
+bool DR16_WaitDmaCplt(uint32_t timeout) {
+  return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) ==
+          SIGNAL_DR16_RAW_REDY);
+}
+
+int8_t DR16_ParseData(DR16_t *dr16){
+  if (dr16 == NULL) return DEVICE_ERR_NULL;
+
+  if (DR16_DataCorrupted(dr16)) {
+    return DEVICE_ERR;
+  }
+
+  dr16->header.online = true;
+  dr16->header.last_online_time = BSP_TIME_Get_us();
+  
+  memset(&(dr16->data), 0, sizeof(dr16->data));
+
+  float full_range = (float)(DR16_CH_VALUE_MAX - DR16_CH_VALUE_MIN);
+
+  // 解析摇杆数据
+  dr16->data.ch_r_x = 2.0f * ((float)dr16->raw_data.ch_r_x - DR16_CH_VALUE_MID) / full_range;
+  dr16->data.ch_r_y = 2.0f * ((float)dr16->raw_data.ch_r_y - DR16_CH_VALUE_MID) / full_range;
+  dr16->data.ch_l_x = 2.0f * ((float)dr16->raw_data.ch_l_x - DR16_CH_VALUE_MID) / full_range;
+  dr16->data.ch_l_y = 2.0f * ((float)dr16->raw_data.ch_l_y - DR16_CH_VALUE_MID) / full_range;
+
+  // 解析拨杆位置
+  dr16->data.sw_l = (DR16_SwitchPos_t)dr16->raw_data.sw_l;
+  dr16->data.sw_r = (DR16_SwitchPos_t)dr16->raw_data.sw_r;
+
+  // 解析鼠标数据
+  dr16->data.mouse.x = dr16->raw_data.x;
+  dr16->data.mouse.y = dr16->raw_data.y;
+  dr16->data.mouse.z = dr16->raw_data.z;
+
+  dr16->data.mouse.l_click = dr16->raw_data.press_l;
+  dr16->data.mouse.r_click = dr16->raw_data.press_r;
+
+  // 解析键盘按键 - 使用union简化代码
+  uint16_t key_value = dr16->raw_data.key;
+  
+  // 解析键盘位映射（W-B键，位0-15）
+  for (int i = DR16_KEY_W; i <= DR16_KEY_B; i++) {
+    dr16->data.keyboard.key[i] = (key_value & (1 << i)) != 0;
+  }
+
+  // 解析鼠标点击
+  dr16->data.keyboard.key[DR16_L_CLICK] = dr16->data.mouse.l_click;
+  dr16->data.keyboard.key[DR16_R_CLICK] = dr16->data.mouse.r_click;
+
+  // 解析第五通道
+  dr16->data.ch_res = 2.0f * ((float)dr16->raw_data.res - DR16_CH_VALUE_MID) / full_range;
+  
+  return DEVICE_OK;
+}
+
+int8_t DR16_Offline(DR16_t *dr16){
+  if (dr16 == NULL) return DEVICE_ERR_NULL;
+
+  dr16->header.online = false;
+  memset(&(dr16->data), 0, sizeof(dr16->data));
+
+  return DEVICE_OK;
+}
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */

assets/User_code/device/dr16.h

@@ -0,0 +1,117 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <cmsis_os2.h>
+
+#include "component/user_math.h"
+#include "device.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef struct __packed {
+  uint16_t ch_r_x : 11;
+  uint16_t ch_r_y : 11;
+  uint16_t ch_l_x : 11;
+  uint16_t ch_l_y : 11;
+  uint8_t sw_r : 2;
+  uint8_t sw_l : 2;
+  int16_t x;
+  int16_t y;
+  int16_t z;
+  uint8_t press_l;
+  uint8_t press_r;
+  uint16_t key;
+  uint16_t res;
+} DR16_RawData_t;
+
+typedef enum {
+  DR16_SW_ERR = 0,
+  DR16_SW_UP = 1,
+  DR16_SW_MID = 3,
+  DR16_SW_DOWN = 2,
+} DR16_SwitchPos_t;
+
+/* 键盘按键值 */
+typedef enum {
+  DR16_KEY_W = 0,
+  DR16_KEY_S,
+  DR16_KEY_A,
+  DR16_KEY_D,
+  DR16_KEY_SHIFT,
+  DR16_KEY_CTRL,
+  DR16_KEY_Q,
+  DR16_KEY_E,
+  DR16_KEY_R,
+  DR16_KEY_F,
+  DR16_KEY_G,
+  DR16_KEY_Z,
+  DR16_KEY_X,
+  DR16_KEY_C,
+  DR16_KEY_V,
+  DR16_KEY_B,
+  DR16_L_CLICK,
+  DR16_R_CLICK,
+  DR16_KEY_NUM,
+} DR16_Key_t;
+
+typedef struct {
+  float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
+  float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
+  float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
+  float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
+
+  float ch_res; /* 第五通道值 */
+
+   DR16_SwitchPos_t sw_r; /* 右侧拨杆位置 */
+   DR16_SwitchPos_t sw_l; /* 左侧拨杆位置 */
+
+  struct {
+    int16_t x;
+    int16_t y;
+    int16_t z;
+    bool l_click; /* 左键 */
+    bool r_click; /* 右键 */
+  } mouse;        /* 鼠标值 */
+
+  union {
+    bool key[DR16_KEY_NUM]; /* 键盘按键值 */
+    uint16_t value;        /* 键盘按键值的位映射 */
+  } keyboard;
+
+  uint16_t res; /* 保留，未启用 */
+} DR16_Data_t;
+
+typedef struct {
+  DEVICE_Header_t header;
+  DR16_RawData_t raw_data;
+  DR16_Data_t data;
+} DR16_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+int8_t DR16_Init(DR16_t *dr16);
+int8_t DR16_Restart(void);
+int8_t DR16_StartDmaRecv(DR16_t *dr16);
+bool DR16_WaitDmaCplt(uint32_t timeout);
+int8_t DR16_ParseData(DR16_t *dr16);
+int8_t DR16_Offline(DR16_t *dr16);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/ist8310.c

@@ -0,0 +1,178 @@
+/*
+    IST8310 地磁传感器。
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "ist8310.h"
+
+#include <gpio.h>
+#include <stdbool.h>
+#include <string.h>
+
+#include "bsp/time.h"
+#include "bsp/gpio.h"
+#include "bsp/i2c.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private define ----------------------------------------------------------- */
+#define IST8310_WAI (0x00)
+#define IST8310_STAT1 (0x02)
+#define IST8310_DATAXL (0x03)
+#define IST8310_STAT2 (0x09)
+#define IST8310_CNTL1 (0x0A)
+#define IST8310_CNTL2 (0x0B)
+#define IST8310_STR (0x0C)
+#define IST8310_TEMPL (0x1C)
+#define IST8310_TEMPH (0x1D)
+#define IST8310_AVGCNTL (0x41)
+#define IST8310_PDCNTL (0x42)
+
+#define IST8310_CHIP_ID (0x10)
+
+#define IST8310_IIC_ADDRESS (0x0E << 1)
+
+#define IST8310_LEN_RX_BUFF (6)
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Private macro ------------------------------------------------------------ */
+#define IST8310_SET() \
+  BSP_GPIO_WritePin(CMPS_RST_Pin, GPIO_PIN_SET)
+#define IST8310_RESET() \
+  BSP_GPIO_WritePin(CMPS_RST_Pin, GPIO_PIN_RESET)
+
+/* Private typedef ---------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private variables -------------------------------------------------------- */
+uint8_t ist8310_rxbuf[IST8310_LEN_RX_BUFF];
+
+static osThreadId_t thread_alert;
+static bool inited = false;
+
+/* Private function  -------------------------------------------------------- */
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+static void IST8310_WriteSingle(uint8_t reg, uint8_t data) {
+  BSP_I2C_MemWriteByte(BSP_I2C_COMP, IST8310_IIC_ADDRESS, reg, data);
+}
+
+static uint8_t IST8310_ReadSingle(uint8_t reg) {
+  return BSP_I2C_MemReadByte(BSP_I2C_COMP, IST8310_IIC_ADDRESS, reg);
+}
+
+
+static void IST8310_Read(uint8_t reg, uint8_t *data, uint8_t len) {
+  if (data == NULL) return;
+  BSP_I2C_MemRead(BSP_I2C_COMP, IST8310_IIC_ADDRESS, reg, data, len, true);
+}
+
+static void IST8310_MemRxCpltCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_IST8310_MAGN_RAW_REDY);
+}
+
+static void IST8310_IntCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_IST8310_MAGN_NEW_DATA);
+}
+
+/* Exported functions ------------------------------------------------------- */
+int8_t IST8310_Init(IST8310_t *ist8310, const IST8310_Cali_t *cali) {
+  if (ist8310 == NULL) return DEVICE_ERR_NULL;
+  if (cali == NULL) return DEVICE_ERR_NULL;
+  if (inited) return DEVICE_ERR_INITED;
+  if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
+
+  ist8310->cali = cali;
+
+  IST8310_RESET();
+  BSP_TIME_Delay(50);
+  IST8310_SET();
+  BSP_TIME_Delay(50);
+
+  if (IST8310_ReadSingle(IST8310_WAI) != IST8310_CHIP_ID)
+    return DEVICE_ERR_NO_DEV;
+
+  BSP_GPIO_DisableIRQ(CMPS_INT_Pin);
+
+  BSP_I2C_RegisterCallback(BSP_I2C_COMP, HAL_I2C_MEM_RX_CPLT_CB,
+                           IST8310_MemRxCpltCallback);
+  BSP_GPIO_RegisterCallback(CMPS_INT_Pin, IST8310_IntCallback);
+
+  /* Init. */
+  /* 0x00: Stand-By mode. 0x01: Single measurement mode. */
+
+  /* 0x08: Data ready function enable. DRDY signal active low*/
+  IST8310_WriteSingle(IST8310_CNTL2, 0x08);
+
+  IST8310_WriteSingle(IST8310_AVGCNTL, 0x09);
+  IST8310_WriteSingle(IST8310_PDCNTL, 0xC0);
+  IST8310_WriteSingle(IST8310_CNTL1, 0x0B);
+  BSP_TIME_Delay(10);
+
+  inited = true;
+
+  BSP_GPIO_EnableIRQ(CMPS_INT_Pin);
+  return DEVICE_OK;
+}
+
+bool IST8310_WaitNew(uint32_t timeout) {
+  return (osThreadFlagsWait(SIGNAL_IST8310_MAGN_NEW_DATA, osFlagsWaitAll,
+                            timeout) == SIGNAL_IST8310_MAGN_NEW_DATA);
+}
+
+int8_t IST8310_StartDmaRecv() {
+  IST8310_Read(IST8310_DATAXL, ist8310_rxbuf, IST8310_LEN_RX_BUFF);
+  return DEVICE_OK;
+}
+
+uint32_t IST8310_WaitDmaCplt() {
+  return osThreadFlagsWait(SIGNAL_IST8310_MAGN_RAW_REDY, osFlagsWaitAll,
+                           osWaitForever);
+}
+
+int8_t IST8310_Parse(IST8310_t *ist8310) {
+  if (ist8310 == NULL) return DEVICE_ERR_NULL;
+
+#if 1
+  /* Magn -> T */
+  int16_t raw_x, raw_y, raw_z;
+  memcpy(&raw_x, ist8310_rxbuf + 0, sizeof(raw_x));
+  memcpy(&raw_y, ist8310_rxbuf + 2, sizeof(raw_y));
+  memcpy(&raw_z, ist8310_rxbuf + 4, sizeof(raw_z));
+
+  ist8310->magn.x = (float)raw_x;
+  ist8310->magn.y = (float)raw_y;
+  ist8310->magn.z = (float)-raw_z;
+
+#else
+  const int16_t *raw_x = (int16_t *)(ist8310_rxbuf + 0);
+  const int16_t *raw_y = (int16_t *)(ist8310_rxbuf + 2);
+  const int16_t *raw_z = (int16_t *)(ist8310_rxbuf + 4);
+
+  ist8310->magn.x = (float)*raw_x;
+  ist8310->magn.y = (float)*raw_y;
+  ist8310->magn.z = -(float)*raw_z;
+#endif
+
+  ist8310->magn.x *= 3.0f / 20.0f;
+  ist8310->magn.y *= 3.0f / 20.0f;
+  ist8310->magn.z *= 3.0f / 20.0f;
+
+  ist8310->magn.x = (ist8310->magn.x - ist8310->cali->magn_offset.x) *
+                    ist8310->cali->magn_scale.x;
+  ist8310->magn.y = (ist8310->magn.y - ist8310->cali->magn_offset.y) *
+                    ist8310->cali->magn_scale.y;
+  ist8310->magn.z = (ist8310->magn.z - ist8310->cali->magn_offset.y) *
+                    ist8310->cali->magn_scale.z;
+
+  return DEVICE_OK;
+}

assets/User_code/device/ist8310.h

@@ -0,0 +1,49 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <cmsis_os2.h>
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "component/ahrs.h"
+#include "device/device.h"
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef struct {
+  struct {
+    float x;
+    float y;
+    float z;
+  } magn_offset; /* 磁力计偏置 */
+
+  struct {
+    float x;
+    float y;
+    float z;
+  } magn_scale;   /* 磁力计缩放 */
+} IST8310_Cali_t; /* IST8310校准数据 */
+
+typedef struct {
+  DEVICE_Header_t header;
+  AHRS_Magn_t magn;
+  const IST8310_Cali_t *cali;
+} IST8310_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+int8_t IST8310_Init(IST8310_t *ist8310, const IST8310_Cali_t *cali);
+int8_t IST8310_Restart(void);
+
+bool IST8310_WaitNew(uint32_t timeout);
+int8_t IST8310_StartDmaRecv();
+uint32_t IST8310_WaitDmaCplt();
+int8_t IST8310_Parse(IST8310_t *ist8310);
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/led.c

@@ -0,0 +1,37 @@
+/*
+   led控制
+*/
+/*Includes   -----------------------------------------*/
+#include "device/led.h"
+#include "device.h"
+
+
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+#ifdef LED_PWM
+int8_t DEVICE_LED_PWM_Set(BSP_PWM_Channel_t channel, float duty_cycle)
+{
+    if (duty_cycle < 0.0f || duty_cycle > 1.0f) {
+        return DEVICE_ERR_NULL; // 错误：占空比超出范围
+    }
+    uint16_t pulse = (uint16_t)(duty_cycle * (float)UINT16_MAX);
+    BSP_PWM_Start(channel);
+    BSP_PWM_SetComp(channel, pulse);
+    return DEVICE_OK;
+}
+#endif
+
+#ifdef LED_GPIO
+int8_t DEVICE_LED_GPIO_Set(BSP_GPIO_t gpio, bool value)
+{
+	if (value) {
+		BSP_GPIO_WritePin(gpio, true);
+	} else {
+		BSP_GPIO_WritePin(gpio, false);
+	}
+	return DEVICE_OK;
+}
+#endif
+
+

assets/User_code/device/led.h

@@ -0,0 +1,63 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+/* USER DEFIN BEGIN */
+
+/* USER DEFIN END */
+#include <stdint.h>
+#ifdef LED_GPIO
+#include "bsp/gpio.h"
+#endif
+#ifdef LED_PWM
+#include "bsp/pwm.h"
+#endif
+#include "bsp/bsp.h"
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+
+typedef struct {
+#ifdef LED_GPIO
+BSP_GPIO_t gpio;
+#endif
+#ifdef LED_PWM
+BSP_PWM_Channel_t channel;	
+#endif
+} DEVICE_LED_t;
+
+
+ extern DEVICE_LED_t LED_Map;
+/* Exported functions prototypes -------------------------------------------- */
+#ifdef LED_PWM
+int8_t DEVICE_LED_PWM_Set(BSP_PWM_Channel_t channel, float duty_cycle)
+{
+    if (duty_cycle < 0.0f || duty_cycle > 1.0f) {
+        return DEVICE_ERR_NULL; // 错误：占空比超出范围
+    }
+    uint16_t pulse = (uint16_t)(duty_cycle * (float)UINT16_MAX);
+    BSP_PWM_Start(channel);
+    BSP_PWM_SetComp(channel, pulse);
+    return DEVICE_OK;
+}
+#endif
+
+#ifdef LED_GPIO
+int8_t DEVICE_LED_GPIO_Set(BSP_GPIO_t gpio, bool value)
+{
+	if (value) {
+		BSP_GPIO_WritePin(gpio, true);
+	} else {
+		BSP_GPIO_WritePin(gpio, false);
+	}
+	return DEVICE_OK;
+}
+#endif
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/motor.c

@@ -0,0 +1,52 @@
+/*
+    电机通用函数
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "motor.h"
+
+#include <string.h>
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+
+/* Private define ----------------------------------------------------------- */
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private variables -------------------------------------------------------- */
+
+/* Private function  -------------------------------------------------------- */
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+/* Exported functions ------------------------------------------------------- */
+float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor) {
+  if (motor == NULL) return DEVICE_ERR_NULL;
+  return motor->feedback.rotor_abs_angle;
+}
+
+float MOTOR_GetRotorSpeed(const MOTOR_t *motor) {
+  if (motor == NULL) return DEVICE_ERR_NULL;
+  return motor->feedback.rotor_speed;
+}
+
+float MOTOR_GetTorqueCurrent(const MOTOR_t *motor) {
+  if (motor == NULL) return DEVICE_ERR_NULL;
+  return motor->feedback.torque_current;
+}
+
+float MOTOR_GetTemp(const MOTOR_t *motor) {
+  if (motor == NULL) return DEVICE_ERR_NULL;
+  return motor->feedback.temp;
+}

assets/User_code/device/motor.h

@@ -0,0 +1,68 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/device.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef struct {
+    float rotor_abs_angle; /* 转子绝对角度 */
+    float rotor_speed; /* 实际转子转速 */
+    float torque_current; /* 转矩电流 */
+    float temp; /* 温度 */
+} MOTOR_Feedback_t;
+
+/**
+ * @brief mit电机输出参数结构体
+ */
+typedef struct {
+    float torque;      /* 目标力矩 */
+    float velocity;    /* 目标速度 */
+    float angle;       /* 目标位置 */
+    float kp;         /* 位置环增益 */
+    float kd;         /* 速度环增益 */
+} MOTOR_MIT_Output_t;
+
+/**
+ * @brief 转矩电流控制模式参数结构体
+ */
+typedef struct {
+    float current;    /* 目标电流 */
+} MOTOR_Current_Output_t;
+
+typedef struct {
+    DEVICE_Header_t header;
+    bool reverse; /* 是否反装 true表示反装 */
+    MOTOR_Feedback_t feedback;
+} MOTOR_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Exported functions prototypes -------------------------------------------- */
+float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor);
+float MOTOR_GetRotorSpeed(const MOTOR_t *motor);
+float MOTOR_GetTorqueCurrent(const MOTOR_t *motor);
+float MOTOR_GetTemp(const MOTOR_t *motor);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/motor_dm.c

@@ -0,0 +1,496 @@
+#define MOTOR_DM_FLOAT_TO_INT_SIGNED(x, x_min, x_max, bits) \
+    ((int32_t)roundf(((x) / ((x_max) - (x_min))) * (1 << (bits)) + (1 << ((bits) - 1))))
+
+#define MOTOR_DM_INT_TO_FLOAT_SIGNED(x_int, x_min, x_max, bits) \
+    (((float)((int32_t)(x_int) - (1 << ((bits) - 1))) * ((x_max) - (x_min)) / (float)(1 << (bits))))
+/* Includes ----------------------------------------------------------------- */
+#include "device/motor_dm.h"
+#include "bsp/mm.h"
+#include "bsp/time.h"
+#include "component/user_math.h"
+#include "string.h"
+#include "math.h"
+
+/* Private constants -------------------------------------------------------- */
+/* DM电机数据映射范围 */
+#define DM_P_MIN   (-12.56637f)
+#define DM_P_MAX   (12.56637f)
+#define DM_V_MIN   (-30.0f)
+#define DM_V_MAX   (30.0f)
+#define DM_T_MIN   (-12.0f)
+#define DM_T_MAX   (12.0f)
+#define DM_KP_MIN  (0.0f)
+#define DM_KP_MAX  (500.0f)
+#define DM_KD_MIN  (0.0f)
+#define DM_KD_MAX  (5.0f)
+
+/* CAN ID偏移量 */
+#define DM_CAN_ID_OFFSET_POS_VEL  0x100
+#define DM_CAN_ID_OFFSET_VEL      0x200
+
+/* Private macro ------------------------------------------------------------ */
+#define FLOAT_TO_UINT(x, x_min, x_max, bits) \
+    (uint32_t)((x - x_min) * ((1 << bits) - 1) / (x_max - x_min))
+
+#define UINT_TO_FLOAT(x_int, x_min, x_max, bits) \
+    ((float)(x_int) * (x_max - x_min) / ((1 << bits) - 1) + x_min)
+
+
+/* Private variables -------------------------------------------------------- */
+static MOTOR_DM_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
+
+static int float_to_uint(float x_float, float x_min, float x_max, int bits)
+{
+	/* Converts a float to an unsigned int, given range and number of bits */
+	float span = x_max - x_min;
+	float offset = x_min;
+	return (int) ((x_float-offset)*((float)((1<<bits)-1))/span);
+}
+
+static float uint_to_float(int x_int, float x_min, float x_max, int bits)
+{
+	/* converts unsigned int to float, given range and number of bits */
+	float span = x_max - x_min;
+	float offset = x_min;
+	return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
+}
+/* Private function prototypes ---------------------------------------------- */
+static int8_t MOTOR_DM_ParseFeedbackFrame(MOTOR_DM_t *motor, const uint8_t *data);
+static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output);
+static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel);
+static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel);
+static MOTOR_DM_CANManager_t* MOTOR_DM_GetCANManager(BSP_CAN_t can);
+
+/* Private functions -------------------------------------------------------- */
+
+/**
+ * @brief 解析DM电机反馈帧
+ * @param motor 电机实例
+ * @param data CAN数据
+ * @return 解析结果
+ */
+static int8_t MOTOR_DM_ParseFeedbackFrame(MOTOR_DM_t *motor, const uint8_t *data) {
+    if (motor == NULL || data == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    uint16_t p_int=(data[1]<<8)|data[2];
+    motor->motor.feedback.rotor_abs_angle = uint_to_float(p_int, DM_P_MIN, DM_P_MAX, 16); // (-12.5,12.5)
+    uint16_t v_int=(data[3]<<4)|(data[4]>>4);
+    motor->motor.feedback.rotor_speed = uint_to_float(v_int, DM_V_MIN, DM_V_MAX, 12); // (-30.0,30.0)
+    uint16_t t_int=((data[4]&0xF)<<8)|data[5];
+    motor->motor.feedback.torque_current = uint_to_float(t_int, DM_T_MIN, DM_T_MAX, 12);  // (-12.0,12.0)
+    motor->motor.feedback.temp = (float)(data[6]);
+
+    while (motor->motor.feedback.rotor_abs_angle < 0) {
+        motor->motor.feedback.rotor_abs_angle += M_2PI;
+    }
+    while (motor->motor.feedback.rotor_abs_angle >= M_2PI) {
+        motor->motor.feedback.rotor_abs_angle -= M_2PI;
+    }
+
+    if (motor->param.reverse) {
+        motor->motor.feedback.rotor_abs_angle = M_2PI - motor->motor.feedback.rotor_abs_angle;
+        motor->motor.feedback.rotor_speed = -motor->motor.feedback.rotor_speed;
+        motor->motor.feedback.torque_current = -motor->motor.feedback.torque_current;
+    }
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 发送MIT模式控制命令
+ * @param motor 电机实例
+ * @param output MIT控制参数
+ * @return 发送结果
+ */
+static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output) {
+    if (motor == NULL || output == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+	uint8_t data[8];
+	uint16_t pos_tmp,vel_tmp,kp_tmp,kd_tmp,tor_tmp;
+	uint16_t id = motor->param.can_id;
+
+	pos_tmp = float_to_uint(output->angle, DM_P_MIN , DM_P_MAX, 16);
+	vel_tmp = float_to_uint(output->velocity, DM_V_MIN , DM_V_MAX, 12);
+	kp_tmp  = float_to_uint(output->kp,   DM_KP_MIN, DM_KP_MAX, 12);
+	kd_tmp  = float_to_uint(output->kd,   DM_KD_MIN, DM_KD_MAX, 12);
+	tor_tmp = float_to_uint(output->torque, DM_T_MIN , DM_T_MAX, 12);
+
+    /* 打包数据 */
+	data[0] = (pos_tmp >> 8);
+	data[1] = pos_tmp;
+	data[2] = (vel_tmp >> 4);
+	data[3] = ((vel_tmp&0xF)<<4)|(kp_tmp>>8);
+	data[4] = kp_tmp;
+	data[5] = (kd_tmp >> 4);
+	data[6] = ((kd_tmp&0xF)<<4)|(tor_tmp>>8);
+	data[7] = tor_tmp;
+    
+    /* 发送CAN消息 */
+    BSP_CAN_StdDataFrame_t frame;
+    frame.id = motor->param.can_id;
+    frame.dlc = 8;
+    memcpy(frame.data, data, 8);
+    
+
+    return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+/**
+ * @brief 发送位置速度模式控制命令
+ * @param motor 电机实例
+ * @param pos 目标位置
+ * @param vel 目标速度
+ * @return 发送结果
+ */
+static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel) {
+    if (motor == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    uint8_t data[8] = {0};
+
+    
+    /* 直接发送浮点数数据 */
+    memcpy(&data[0], &pos, 4);  // 位置，低位在前
+    memcpy(&data[4], &vel, 4);  // 速度，低位在前
+    
+    /* 发送CAN消息，ID为原ID+0x100 */
+    uint32_t can_id = DM_CAN_ID_OFFSET_POS_VEL + motor->param.can_id;
+    BSP_CAN_StdDataFrame_t frame;
+    frame.id = can_id;
+    frame.dlc = 8;
+    memcpy(frame.data, data, 8);
+    
+    return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+/**
+ * @brief 发送速度模式控制命令
+ * @param motor 电机实例
+ * @param vel 目标速度
+ * @return 发送结果
+ */
+static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel) {
+    if (motor == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    uint8_t data[4] = {0};
+    
+    /* 直接发送浮点数数据 */
+    memcpy(&data[0], &vel, 4);  // 速度，低位在前
+    
+    /* 发送CAN消息，ID为原ID+0x200 */
+    uint32_t can_id = DM_CAN_ID_OFFSET_VEL + motor->param.can_id;
+    BSP_CAN_StdDataFrame_t frame;
+    frame.id = can_id;
+    frame.dlc = 4;
+    memcpy(frame.data, data, 4);
+    
+    return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+/**
+ * @brief 获取指定CAN总线的管理器
+ * @param can CAN总线
+ * @return CAN管理器指针
+ */
+static MOTOR_DM_CANManager_t* MOTOR_DM_GetCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) {
+        return NULL;
+    }
+    
+    return can_managers[can];
+}
+
+/**
+ * @brief 创建CAN管理器
+ * @param can CAN总线
+ * @return 创建结果
+ */
+static int8_t MOTOR_DM_CreateCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return DEVICE_ERR;
+    if (can_managers[can] != NULL) return DEVICE_OK;
+    
+    can_managers[can] = (MOTOR_DM_CANManager_t*)BSP_Malloc(sizeof(MOTOR_DM_CANManager_t));
+    if (can_managers[can] == NULL) return DEVICE_ERR;
+    
+    memset(can_managers[can], 0, sizeof(MOTOR_DM_CANManager_t));
+    can_managers[can]->can = can;
+    return DEVICE_OK;
+}
+
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * @brief 注册一个DM电机
+ * @param param 电机参数
+ * @return 注册结果
+ */
+int8_t MOTOR_DM_Register(MOTOR_DM_Param_t *param) {
+    if (param == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    /* 创建CAN管理器 */
+    if (MOTOR_DM_CreateCANManager(param->can) != DEVICE_OK) {
+        return DEVICE_ERR;
+    }
+    
+    /* 获取CAN管理器 */
+    MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
+    if (manager == NULL) {
+        return DEVICE_ERR;
+    }
+    
+    /* 检查是否已注册 */
+    for (int i = 0; i < manager->motor_count; i++) {
+        if (manager->motors[i] && manager->motors[i]->param.master_id == param->master_id) {
+            return DEVICE_ERR_INITED;
+        }
+    }
+    
+    /* 检查是否已达到最大数量 */
+    if (manager->motor_count >= MOTOR_DM_MAX_MOTORS) {
+        return DEVICE_ERR;
+    }
+    
+    /* 分配内存 */
+    MOTOR_DM_t *motor = (MOTOR_DM_t *)BSP_Malloc(sizeof(MOTOR_DM_t));
+    if (motor == NULL) {
+        return DEVICE_ERR;
+    }
+    
+    /* 初始化电机 */
+    memset(motor, 0, sizeof(MOTOR_DM_t));
+    memcpy(&motor->param, param, sizeof(MOTOR_DM_Param_t));
+    motor->motor.header.online = false;
+    motor->motor.reverse = param->reverse;
+    
+    /* 注册CAN接收ID - DM电机使用Master ID接收反馈 */
+    uint16_t feedback_id = param->master_id;
+    if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) {
+        BSP_Free(motor);
+        return DEVICE_ERR;
+    }
+    
+    /* 添加到管理器 */
+    manager->motors[manager->motor_count] = motor;
+    manager->motor_count++;
+    
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 更新指定电机数据
+ * @param param 电机参数
+ * @return 更新结果
+ */
+int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param) {
+    if (param == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
+    if (manager == NULL) {
+        return DEVICE_ERR_NO_DEV;
+    }
+    
+    /* 查找电机 */
+    MOTOR_DM_t *motor = NULL;
+    for (int i = 0; i < manager->motor_count; i++) {
+        if (manager->motors[i] && manager->motors[i]->param.master_id == param->master_id) {
+            motor = manager->motors[i];
+            break;
+        }
+    }
+    
+    if (motor == NULL) {
+        return DEVICE_ERR_NO_DEV;
+    }
+    
+    /* 主动接收CAN消息 */
+    uint16_t feedback_id = param->master_id;
+    BSP_CAN_Message_t rx_msg;
+    if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
+        uint64_t now_time = BSP_TIME_Get();
+        if (now_time - motor->motor.header.last_online_time > 100000) {  // 100ms超时，单位微秒
+            motor->motor.header.online = false;
+        }
+        return DEVICE_ERR;
+    }
+    
+    motor->motor.header.online = true;
+    motor->motor.header.last_online_time = BSP_TIME_Get();
+    MOTOR_DM_ParseFeedbackFrame(motor, rx_msg.data);
+    
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 更新所有电机数据
+ * @return 更新结果
+ */
+int8_t MOTOR_DM_UpdateAll(void) {
+    int8_t ret = DEVICE_OK;
+    for (int can = 0; can < BSP_CAN_NUM; can++) {
+        MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager((BSP_CAN_t)can);
+        if (manager == NULL) continue;
+        
+        for (int i = 0; i < manager->motor_count; i++) {
+            MOTOR_DM_t *motor = manager->motors[i];
+            if (motor != NULL) {
+                if (MOTOR_DM_Update(&motor->param) != DEVICE_OK) {
+                    ret = DEVICE_ERR;
+                }
+            }
+        }
+    }
+    return ret;
+}
+
+/**
+ * @brief MIT模式控制
+ * @param param 电机参数
+ * @param output MIT控制参数
+ * @return 控制结果
+ */
+int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output) {
+    if (param == NULL || output == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
+    if (motor == NULL) {
+        return DEVICE_ERR_NO_DEV;
+    }
+    
+    return MOTOR_DM_SendMITCmd(motor, output);
+}
+
+/**
+ * @brief 位置速度模式控制
+ * @param param 电机参数
+ * @param target_pos 目标位置
+ * @param target_vel 目标速度
+ * @return 控制结果
+ */
+int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel) {
+    if (param == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
+    if (motor == NULL) {
+        return DEVICE_ERR_NO_DEV;
+    }
+    
+    return MOTOR_DM_SendPosVelCmd(motor, target_pos, target_vel);
+}
+
+/**
+ * @brief 速度模式控制
+ * @param param 电机参数
+ * @param target_vel 目标速度
+ * @return 控制结果
+ */
+int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel) {
+    if (param == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
+    if (motor == NULL) {
+        return DEVICE_ERR_NO_DEV;
+    }
+    
+    return MOTOR_DM_SendVelCmd(motor, target_vel);
+}
+
+/**
+ * @brief 获取指定电机的实例指针
+ * @param param 电机参数
+ * @return 电机实例指针
+ */
+MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param) {
+    if (param == NULL) {
+        return NULL;
+    }
+    
+    MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
+    if (manager == NULL) {
+        return NULL;
+    }
+    
+    /* 查找对应的电机 */
+    for (int i = 0; i < manager->motor_count; i++) {
+        MOTOR_DM_t *motor = manager->motors[i];
+        if (motor && motor->param.can == param->can && 
+            motor->param.master_id == param->master_id) {
+            return motor;
+        }
+    }
+    
+    return NULL;
+}
+
+
+int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param){
+    if (param == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
+    if (motor == NULL) {
+        return DEVICE_ERR_NO_DEV;
+    }
+
+    BSP_CAN_StdDataFrame_t frame;
+    frame.id = motor->param.can_id;
+    frame.dlc = 8;
+    frame.data[0] = 0XFF;
+    frame.data[1] = 0xFF;
+    frame.data[2] = 0xFF;
+    frame.data[3] = 0xFF;
+    frame.data[4] = 0xFF;
+    frame.data[5] = 0xFF;
+    frame.data[6] = 0xFF;
+    frame.data[7] = 0xFC;
+
+    return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+/**
+ * @brief 使电机松弛（设置输出为0）
+ * @param param 电机参数
+ * @return 操作结果
+ */
+int8_t MOTOR_DM_Relax(MOTOR_DM_Param_t *param) {
+    if (param == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    MOTOR_MIT_Output_t output = {0};
+    return MOTOR_DM_MITCtrl(param, &output);
+}
+
+/**
+ * @brief 使电机离线（设置在线状态为false）
+ * @param param 电机参数
+ * @return 操作结果
+ */
+int8_t MOTOR_DM_Offine(MOTOR_DM_Param_t *param) {
+    if (param == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    
+    MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
+    if (motor == NULL) {
+        return DEVICE_ERR_NO_DEV;
+    }
+    
+    motor->motor.header.online = false;
+    return DEVICE_OK;
+}

assets/User_code/device/motor_dm.h

@@ -0,0 +1,98 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/device.h"
+#include "device/motor.h"
+#include "bsp/can.h"
+
+/* Exported constants ------------------------------------------------------- */
+#define MOTOR_DM_MAX_MOTORS 32
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef enum {
+    MOTOR_DM_J4310,
+} MOTOR_DM_Module_t;
+
+/*每个电机需要的参数*/
+typedef struct {
+    BSP_CAN_t can;
+    uint16_t master_id;    /* 主站ID，用于发送控制命令 */
+    uint16_t can_id;       /* 反馈ID，用于接收电机反馈 */
+    MOTOR_DM_Module_t module;
+    bool reverse;
+} MOTOR_DM_Param_t;
+
+/*电机实例*/
+typedef struct{
+    MOTOR_DM_Param_t param;
+    MOTOR_t motor;
+} MOTOR_DM_t;
+
+/*CAN管理器，管理一个CAN总线上所有的电机*/
+typedef struct {
+    BSP_CAN_t can;
+    MOTOR_DM_t *motors[MOTOR_DM_MAX_MOTORS];
+    uint8_t motor_count;
+} MOTOR_DM_CANManager_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 注册一个LK电机
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_DM_Register(MOTOR_DM_Param_t *param);
+
+/**
+ * @brief 更新指定电机数据
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param);
+
+/**
+ * @brief 更新所有电机数据
+ * @return 
+ */
+int8_t MOTOR_DM_UpdateAll(void);
+
+int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output);
+
+int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel);
+
+int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel);
+
+/**
+ * @brief 获取指定电机的实例指针
+ * @param param 电机参数
+ * @return 
+ */
+MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param);
+
+int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param);
+
+/**
+ * @brief 使电机松弛（设置输出为0）
+ * @param param 
+ * @return 
+ */
+int8_t MOTOR_DM_Relax(MOTOR_DM_Param_t *param);
+
+/**
+ * @brief 使电机离线（设置在线状态为false）
+ * @param param 
+ * @return 
+ */
+int8_t MOTOR_DM_Offine(MOTOR_DM_Param_t *param);
+
+
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/motor_lk.c

@@ -0,0 +1,329 @@
+/*
+  LK电机驱动
+*/
+/* Includes ----------------------------------------------------------------- */
+#include "motor_lk.h"
+#include <stdbool.h>
+#include <string.h>
+#include "bsp/can.h"
+#include "bsp/mm.h"
+#include "bsp/time.h"
+#include "component/user_math.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private define ----------------------------------------------------------- */
+#define LK_CTRL_ID_BASE        (0x140)
+#define LK_FB_ID_BASE          (0x240)
+
+// LK电机命令字节定义
+#define LK_CMD_FEEDBACK        (0x9C)    // 反馈命令字节
+#define LK_CMD_MOTOR_OFF       (0x80)    // 电机关闭命令
+#define LK_CMD_MOTOR_ON        (0x88)    // 电机运行命令
+#define LK_CMD_TORQUE_CTRL     (0xA1)    // 转矩闭环控制命令
+
+// LK电机参数定义
+#define LK_CURR_LSB_MF         (33.0f / 4096.0f)   // MF电机转矩电流分辨率 A/LSB
+#define LK_CURR_LSB_MG         (66.0f / 4096.0f)   // MG电机转矩电流分辨率 A/LSB
+#define LK_POWER_RANGE_MS      (1000)              // MS电机功率范围 ±1000
+#define LK_TORQUE_RANGE        (2048)              // 转矩控制值范围 ±2048
+#define LK_TORQUE_CURRENT_MF   (16.5f)             // MF电机最大转矩电流 A
+#define LK_TORQUE_CURRENT_MG   (33.0f)             // MG电机最大转矩电流 A
+
+#define MOTOR_TX_BUF_SIZE      (8)
+#define MOTOR_RX_BUF_SIZE      (8)
+
+// 编码器分辨率定义
+#define LK_ENC_14BIT_MAX       (16383)   // 14位编码器最大值
+#define LK_ENC_15BIT_MAX       (32767)   // 15位编码器最大值  
+#define LK_ENC_16BIT_MAX       (65535)   // 16位编码器最大值
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private variables -------------------------------------------------------- */
+static MOTOR_LK_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
+
+/* Private functions -------------------------------------------------------- */
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+static float MOTOR_LK_GetCurrentLSB(MOTOR_LK_Module_t module) {
+    switch (module) {
+        case MOTOR_LK_MF9025:
+        case MOTOR_LK_MF9035:
+            return LK_CURR_LSB_MF;
+        default:
+            return LK_CURR_LSB_MG;  // 默认使用MG的分辨率
+    }
+}
+
+static uint16_t MOTOR_LK_GetEncoderMax(MOTOR_LK_Module_t module) {
+    // 根据电机型号返回编码器最大值，这里假设都使用16位编码器
+    // 实际使用时需要根据具体电机型号配置
+    return LK_ENC_16BIT_MAX;
+}
+
+static MOTOR_LK_CANManager_t* MOTOR_LK_GetCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return NULL;
+    return can_managers[can];
+}
+
+static int8_t MOTOR_LK_CreateCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return DEVICE_ERR;
+    if (can_managers[can] != NULL) return DEVICE_OK;
+    
+    can_managers[can] = (MOTOR_LK_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LK_CANManager_t));
+    if (can_managers[can] == NULL) return DEVICE_ERR;
+    
+    memset(can_managers[can], 0, sizeof(MOTOR_LK_CANManager_t));
+    can_managers[can]->can = can;
+    return DEVICE_OK;
+}
+
+static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) {
+
+    // 检查命令字节是否为反馈命令
+    if (msg->data[0] != LK_CMD_FEEDBACK) {
+        // 如果不是标准反馈命令，可能是其他格式的数据
+        // 临时跳过命令字节检查，直接解析数据
+        // return;
+    }
+    
+    // 解析温度 (DATA[1])
+    motor->motor.feedback.temp = (int8_t)msg->data[1];
+
+    // 解析转矩电流值或功率值 (DATA[2], DATA[3])
+    int16_t raw_current_or_power = (int16_t)((msg->data[3] << 8) | msg->data[2]);
+
+    // 根据电机类型解析电流或功率
+    switch (motor->param.module) {
+        case MOTOR_LK_MF9025:
+        case MOTOR_LK_MF9035:
+            motor->motor.feedback.torque_current = raw_current_or_power * MOTOR_LK_GetCurrentLSB(motor->param.module);
+            break;
+        default:
+            motor->motor.feedback.torque_current = (float)raw_current_or_power;
+            break;
+    }
+
+    // 解析转速 (DATA[4], DATA[5]) - 单位：1dps/LSB
+    int16_t raw_speed = (int16_t)((msg->data[5] << 8) | msg->data[4]);
+    motor->motor.feedback.rotor_speed = motor->param.reverse ? -raw_speed : raw_speed;
+
+    // 解析编码器值 (DATA[6], DATA[7])
+    uint16_t raw_encoder = (uint16_t)((msg->data[7] << 8) | msg->data[6]);
+    uint16_t encoder_max = MOTOR_LK_GetEncoderMax(motor->param.module);
+
+    // 将编码器值转换为弧度 (0 ~ 2π)
+    float angle = (float)raw_encoder / (float)encoder_max * M_2PI;
+    motor->motor.feedback.rotor_abs_angle = motor->param.reverse ? (M_2PI - angle) : angle;
+}
+
+/* Exported functions ------------------------------------------------------- */
+
+int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    if (MOTOR_LK_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
+    MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR;
+    
+    // 检查是否已注册
+    for (int i = 0; i < manager->motor_count; i++) {
+        if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
+            return DEVICE_ERR_INITED;
+        }
+    }
+    
+    // 检查数量
+    if (manager->motor_count >= MOTOR_LK_MAX_MOTORS) return DEVICE_ERR;
+    
+    // 创建新电机实例
+    MOTOR_LK_t *new_motor = (MOTOR_LK_t*)BSP_Malloc(sizeof(MOTOR_LK_t));
+    if (new_motor == NULL) return DEVICE_ERR;
+    
+    memcpy(&new_motor->param, param, sizeof(MOTOR_LK_Param_t));
+    memset(&new_motor->motor, 0, sizeof(MOTOR_t));
+    new_motor->motor.reverse = param->reverse;
+    
+    // 对于某些LK电机，反馈数据可能通过命令ID发送
+    // 根据实际测试，使用命令ID接收反馈数据
+    uint16_t feedback_id = param->id;  // 使用命令ID作为反馈ID
+    
+    // 注册CAN接收ID
+    if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) {
+        BSP_Free(new_motor);
+        return DEVICE_ERR;
+    }
+    
+    manager->motors[manager->motor_count] = new_motor;
+    manager->motor_count++;
+    return DEVICE_OK;
+}
+
+int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR_NO_DEV;
+    
+    for (int i = 0; i < manager->motor_count; i++) {
+        MOTOR_LK_t *motor = manager->motors[i];
+        if (motor && motor->param.id == param->id) {
+            // 对于某些LK电机，反馈数据通过命令ID发送
+            uint16_t feedback_id = param->id;
+            
+            BSP_CAN_Message_t rx_msg;
+            if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
+                uint64_t now_time = BSP_TIME_Get();
+                if (now_time - motor->motor.header.last_online_time > 1000) {
+                    motor->motor.header.online = false;
+                    return DEVICE_ERR_NO_DEV;
+                }
+                return DEVICE_ERR;
+            }
+            
+            motor->motor.header.online = true;
+            motor->motor.header.last_online_time = BSP_TIME_Get();
+            MOTOR_LK_Decode(motor, &rx_msg);
+            return DEVICE_OK;
+        }
+    }
+    return DEVICE_ERR_NO_DEV;
+}
+
+int8_t MOTOR_LK_UpdateAll(void) {
+    int8_t ret = DEVICE_OK;
+    for (int can = 0; can < BSP_CAN_NUM; can++) {
+        MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager((BSP_CAN_t)can);
+        if (manager == NULL) continue;
+        
+        for (int i = 0; i < manager->motor_count; i++) {
+            MOTOR_LK_t *motor = manager->motors[i];
+            if (motor != NULL) {
+                if (MOTOR_LK_Update(&motor->param) != DEVICE_OK) {
+                    ret = DEVICE_ERR;
+                }
+            }
+        }
+    }
+    return ret;
+}
+
+int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+
+    MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR_NO_DEV;
+
+    // 限制输出值范围
+    if (value > 1.0f) value = 1.0f;
+    if (value < -1.0f) value = -1.0f;
+
+    MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param);
+    if (motor == NULL) return DEVICE_ERR_NO_DEV;
+
+    // 根据反转参数调整输出
+    float output = param->reverse ? -value : value;
+
+    // 转矩闭环控制命令 - 将输出值转换为转矩控制值
+    int16_t torque_control = (int16_t)(output * (float)LK_TORQUE_RANGE);
+
+    // 构建CAN帧
+    BSP_CAN_StdDataFrame_t tx_frame;
+    tx_frame.id = param->id;
+    tx_frame.dlc = MOTOR_TX_BUF_SIZE;
+
+    tx_frame.data[0] = LK_CMD_TORQUE_CTRL;
+    tx_frame.data[1] = 0x00;
+    tx_frame.data[2] = 0x00;
+    tx_frame.data[3] = 0x00;
+    tx_frame.data[4] = (uint8_t)(torque_control & 0xFF);
+    tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF);
+    tx_frame.data[6] = 0x00;
+    tx_frame.data[7] = 0x00;
+    return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param) {
+    // 对于LK电机，每次设置输出时就直接发送控制命令
+    // 这个函数可以用于发送其他控制命令，如电机开启/关闭
+    return DEVICE_OK;
+}
+
+int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    BSP_CAN_StdDataFrame_t tx_frame;
+    tx_frame.id = param->id;
+    tx_frame.dlc = MOTOR_TX_BUF_SIZE;
+    
+    // 电机运行命令
+    tx_frame.data[0] = LK_CMD_MOTOR_ON;     // 命令字节
+    tx_frame.data[1] = 0x00;
+    tx_frame.data[2] = 0x00;
+    tx_frame.data[3] = 0x00;
+    tx_frame.data[4] = 0x00;
+    tx_frame.data[5] = 0x00;
+    tx_frame.data[6] = 0x00;
+    tx_frame.data[7] = 0x00;
+    return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    BSP_CAN_StdDataFrame_t tx_frame;
+    tx_frame.id = param->id;
+    tx_frame.dlc = MOTOR_TX_BUF_SIZE;
+    
+    // 电机关闭命令
+    tx_frame.data[0] = LK_CMD_MOTOR_OFF;    // 命令字节
+    tx_frame.data[1] = 0x00;
+    tx_frame.data[2] = 0x00;
+    tx_frame.data[3] = 0x00;
+    tx_frame.data[4] = 0x00;
+    tx_frame.data[5] = 0x00;
+    tx_frame.data[6] = 0x00;
+    tx_frame.data[7] = 0x00;
+    return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param) {
+    if (param == NULL) return NULL;
+    
+    MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
+    if (manager == NULL) return NULL;
+    
+    for (int i = 0; i < manager->motor_count; i++) {
+        MOTOR_LK_t *motor = manager->motors[i];
+        if (motor && motor->param.id == param->id) {
+            return motor;
+        }
+    }
+    return NULL;
+}
+
+int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param) {
+    return MOTOR_LK_SetOutput(param, 0.0f);
+}
+
+int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param) {
+    MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param);
+    if (motor) {
+        motor->motor.header.online = false;
+        return DEVICE_OK;
+    }
+    return DEVICE_ERR_NO_DEV;
+}

assets/User_code/device/motor_lk.h

@@ -0,0 +1,119 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/device.h"
+#include "device/motor.h"
+#include "bsp/can.h"
+
+/* Exported constants ------------------------------------------------------- */
+#define MOTOR_LK_MAX_MOTORS 32
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef enum {
+    MOTOR_LK_MF9025,
+    MOTOR_LK_MF9035,
+} MOTOR_LK_Module_t;
+
+
+/*每个电机需要的参数*/
+typedef struct {
+    BSP_CAN_t can;
+    uint16_t id;
+    MOTOR_LK_Module_t module;
+    bool reverse;
+} MOTOR_LK_Param_t;
+
+/*电机实例*/
+typedef struct{
+    MOTOR_LK_Param_t param;
+    MOTOR_t motor;
+} MOTOR_LK_t;
+
+/*CAN管理器，管理一个CAN总线上所有的电机*/
+typedef struct {
+    BSP_CAN_t can;
+    MOTOR_LK_t *motors[MOTOR_LK_MAX_MOTORS];
+    uint8_t motor_count;
+} MOTOR_LK_CANManager_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 注册一个LK电机
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 更新指定电机数据
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 设置一个电机的输出
+ * @param param 电机参数
+ * @param value 输出值，范围[-1.0, 1.0]
+ * @return 
+ */
+int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value);
+
+/**
+ * @brief 发送控制命令到电机，注意一个CAN可以控制多个电机，所以只需要发送一次即可
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 发送电机开启命令
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 发送电机关闭命令
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 获取指定电机的实例指针
+ * @param param 电机参数
+ * @return 
+ */
+MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 使电机松弛（设置输出为0）
+ * @param param 
+ * @return 
+ */
+int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 使电机离线（设置在线状态为false）
+ * @param param 
+ * @return 
+ */
+int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 
+ * @param  
+ * @return 
+ */
+int8_t MOTOR_LK_UpdateAll(void);
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/motor_lz.c

@@ -0,0 +1,440 @@
+/*
+  灵足电机驱动
+  
+  灵足电机通信协议：
+  - CAN 2.0通信接口，波特率1Mbps
+  - 采用扩展帧格式(29位ID)
+  - ID格式：Bit28~24(通信类型) + Bit23~8(数据区2) + Bit7~0(目标地址)
+*/
+/* Includes ----------------------------------------------------------------- */
+#include "motor_lz.h"
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include "bsp/can.h"
+#include "bsp/mm.h"
+#include "bsp/time.h"
+#include "component/user_math.h"
+
+/* Private define ----------------------------------------------------------- */
+// 灵足电机协议参数
+#define LZ_ANGLE_RANGE_RAD          (12.57f)        /* 角度范围 ±12.57 rad */
+#define LZ_VELOCITY_RANGE_RAD_S     (20.0f)         /* 角速度范围 ±20 rad/s */
+#define LZ_TORQUE_RANGE_NM          (60.0f)         /* 力矩范围 ±60 Nm */
+#define LZ_KP_MAX                   (5000.0f)       /* Kp最大值 */
+#define LZ_KD_MAX                   (100.0f)        /* Kd最大值 */
+
+#define LZ_RAW_VALUE_MAX            (65535)         /* 16位原始值最大值 */
+#define LZ_TEMP_SCALE               (10.0f)         /* 温度缩放因子 */
+
+#define LZ_MAX_RECOVER_DIFF_RAD    (0.28f)
+#define MOTOR_TX_BUF_SIZE           (8)
+#define MOTOR_RX_BUF_SIZE           (8)
+
+/* Private macro ------------------------------------------------------------ */
+
+MOTOR_LZ_MotionParam_t lz_relax_param = {
+    .target_angle = 0.0f,
+    .target_velocity = 0.0f,
+    .kp = 0.0f,
+    .kd = 0.0f,
+    .torque = 0.0f,
+};
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+static MOTOR_LZ_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
+
+/* Private function prototypes ---------------------------------------------- */
+static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can);
+static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can);
+static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg);
+static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id);
+static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value);
+static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value);
+static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc);
+static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
+
+/* Private functions -------------------------------------------------------- */
+
+/**
+ * @brief 获取CAN管理器
+ */
+static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return NULL;
+    return can_managers[can];
+}
+
+/**
+ * @brief 创建CAN管理器
+ */
+static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return DEVICE_ERR;
+    if (can_managers[can] != NULL) return DEVICE_OK;
+    
+    can_managers[can] = (MOTOR_LZ_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LZ_CANManager_t));
+    if (can_managers[can] == NULL) return DEVICE_ERR;
+    
+    memset(can_managers[can], 0, sizeof(MOTOR_LZ_CANManager_t));
+    can_managers[can]->can = can;
+    return DEVICE_OK;
+}
+
+/**
+ * @brief 构建扩展ID
+ */
+static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id) {
+    uint32_t ext_id = 0;
+    ext_id |= ((uint32_t)cmd_type & 0x1F) << 24;      // Bit28~24: 通信类型
+    ext_id |= ((uint32_t)data2 & 0xFFFF) << 8;        // Bit23~8: 数据区2
+    ext_id |= ((uint32_t)target_id & 0xFF);           // Bit7~0: 目标地址
+    return ext_id;
+}
+
+/**
+ * @brief 浮点值转换为原始值（对称范围：-max_value ~ +max_value）
+ */
+static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value) {
+    // 限制范围
+    if (value > max_value) value = max_value;
+    if (value < -max_value) value = -max_value;
+    
+    // 转换为0~65535范围，对应-max_value~max_value
+    return (uint16_t)((value + max_value) / (2.0f * max_value) * (float)LZ_RAW_VALUE_MAX);
+}
+
+/**
+ * @brief 浮点值转换为原始值（单向范围：0 ~ +max_value）
+ */
+static uint16_t MOTOR_LZ_FloatToRawPositive(float value, float max_value) {
+    // 限制范围
+    if (value > max_value) value = max_value;
+    if (value < 0.0f) value = 0.0f;
+    
+    // 转换为0~65535范围，对应0~max_value
+    return (uint16_t)(value / max_value * (float)LZ_RAW_VALUE_MAX);
+}
+
+/**
+ * @brief 原始值转换为浮点值
+ */
+static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value) {
+    // 将0~65535范围转换为-max_value~max_value
+    return ((float)raw_value / (float)LZ_RAW_VALUE_MAX) * (2.0f * max_value) - max_value;
+}
+
+/**
+ * @brief 发送扩展帧
+ */
+static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc) {
+    BSP_CAN_ExtDataFrame_t tx_frame;
+    tx_frame.id = ext_id;
+    tx_frame.dlc = dlc;
+    if (data != NULL) {
+        memcpy(tx_frame.data, data, dlc);
+    } else {
+        memset(tx_frame.data, 0, dlc);
+    }
+    return BSP_CAN_TransmitExtDataFrame(can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+/**
+ * @brief 灵足电机ID解析器
+ * @param original_id 原始CAN ID（29位扩展帧）
+ * @param frame_type 帧类型
+ * @return 解析后的ID（用于队列匹配）
+ * 
+ * 灵足电机扩展ID格式：
+ * Bit28~24: 通信类型 (0x1=运控控制, 0x2=反馈数据, 0x3=使能, 0x4=停止, 0x6=设零位)
+ * Bit23~8:  数据区2 (根据通信类型而定)
+ * Bit7~0:   目标地址 (目标电机CAN ID)
+ */
+static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
+    // 只处理扩展数据帧
+    if (frame_type != BSP_CAN_FRAME_EXT_DATA) {
+        return original_id; // 非扩展帧直接返回原始ID
+    }
+    
+    // 解析扩展ID各个字段
+    uint8_t cmd_type = (original_id >> 24) & 0x1F;    // Bit28~24: 通信类型
+    uint16_t data2 = (original_id >> 8) & 0xFFFF;     // Bit23~8: 数据区2
+    uint8_t host_id = (uint8_t)(original_id & 0xFF);  // Bit7~0: 主机CAN ID
+    
+    // 对于反馈数据帧，我们使用特殊的解析规则
+    if (cmd_type == MOTOR_LZ_CMD_FEEDBACK) {
+        // 反馈数据的data2字段包含：
+        // Bit8~15: 当前电机CAN ID
+        // Bit16~21: 故障信息
+        // Bit22~23: 模式状态
+        uint8_t motor_can_id = data2 & 0xFF;          // bit8~15: 当前电机CAN ID
+        
+        // 返回格式化的ID，便于匹配
+        // 格式：0x02HHMMTT (02=反馈命令, HH=主机ID, MM=电机ID, TT=主机ID)
+        return (0x02000000) | (host_id << 16) | (motor_can_id << 8) | host_id;
+    }
+    
+    // 对于其他命令类型，直接返回原始ID
+    return original_id;
+}
+
+/**
+ * @brief 解码灵足电机反馈数据
+ */
+static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg) {
+    if (motor == NULL || msg == NULL) return;
+    uint8_t cmd_type = (msg->original_id >> 24) & 0x1F;
+    if (cmd_type != MOTOR_LZ_CMD_FEEDBACK) return;
+    uint16_t id_data2 = (msg->original_id >> 8) & 0xFFFF;
+    uint8_t motor_can_id = id_data2 & 0xFF;
+    uint8_t fault_info = (id_data2 >> 8) & 0x3F;
+    uint8_t mode_state = (id_data2 >> 14) & 0x03;
+    motor->lz_feedback.motor_can_id = motor_can_id;
+    motor->lz_feedback.fault.under_voltage = (fault_info & 0x01) != 0;
+    motor->lz_feedback.fault.driver_fault = (fault_info & 0x02) != 0;
+    motor->lz_feedback.fault.over_temp = (fault_info & 0x04) != 0;
+    motor->lz_feedback.fault.encoder_fault = (fault_info & 0x08) != 0;
+    motor->lz_feedback.fault.stall_overload = (fault_info & 0x10) != 0;
+    motor->lz_feedback.fault.uncalibrated = (fault_info & 0x20) != 0;
+    motor->lz_feedback.state = (MOTOR_LZ_State_t)mode_state;
+
+    // 反馈解码并自动反向
+    uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]);
+    float angle = MOTOR_LZ_RawToFloat(raw_angle, LZ_ANGLE_RANGE_RAD);
+    uint16_t raw_velocity = (uint16_t)((msg->data[2] << 8) | msg->data[3]);
+    float velocity = MOTOR_LZ_RawToFloat(raw_velocity, LZ_VELOCITY_RANGE_RAD_S);
+    uint16_t raw_torque = (uint16_t)((msg->data[4] << 8) | msg->data[5]);
+    float torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM);
+
+    while (angle <0){
+        angle += M_2PI;
+    }
+    while (angle > M_2PI){
+        angle -= M_2PI;
+    }
+    // 自动反向
+    if (motor->param.reverse) {
+        angle = M_2PI - angle;
+        velocity = -velocity;
+        torque = -torque;
+    }
+
+    motor->lz_feedback.current_angle = angle;
+    motor->lz_feedback.current_velocity = velocity;
+    motor->lz_feedback.current_torque = torque;
+
+    uint16_t raw_temp = (uint16_t)((msg->data[6] << 8) | msg->data[7]);
+    motor->lz_feedback.temperature = (float)raw_temp / LZ_TEMP_SCALE;
+
+    motor->motor.feedback.rotor_abs_angle = angle;
+    motor->motor.feedback.rotor_speed = velocity; 
+    motor->motor.feedback.torque_current = torque;
+    motor->motor.feedback.temp = (int8_t)motor->lz_feedback.temperature;
+    motor->motor.header.online = true;
+    motor->motor.header.last_online_time = BSP_TIME_Get();
+}
+
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * @brief 初始化灵足电机驱动系统
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Init(void) {
+    // 注册灵足电机专用的ID解析器
+    return BSP_CAN_RegisterIdParser(MOTOR_LZ_IdParser) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+
+int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    if (MOTOR_LZ_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
+    MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR;
+    
+    // 检查是否已注册
+    for (int i = 0; i < manager->motor_count; i++) {
+        if (manager->motors[i] && manager->motors[i]->param.motor_id == param->motor_id) {
+            return DEVICE_ERR; // 已注册
+        }
+    }
+    
+    // 检查数量
+    if (manager->motor_count >= MOTOR_LZ_MAX_MOTORS) return DEVICE_ERR;
+    
+    // 创建新电机实例
+    MOTOR_LZ_t *new_motor = (MOTOR_LZ_t*)BSP_Malloc(sizeof(MOTOR_LZ_t));
+    if (new_motor == NULL) return DEVICE_ERR;
+    
+    memcpy(&new_motor->param, param, sizeof(MOTOR_LZ_Param_t));
+    memset(&new_motor->motor, 0, sizeof(MOTOR_t));
+    memset(&new_motor->lz_feedback, 0, sizeof(MOTOR_LZ_Feedback_t));
+    memset(&new_motor->motion_param, 0, sizeof(MOTOR_LZ_MotionParam_t));
+    
+    new_motor->motor.reverse = param->reverse;
+    
+    // 注册CAN接收ID - 使用解析后的反馈数据ID
+    // 构建反馈数据的原始扩展ID
+    // 反馈数据：data2包含电机ID(bit8~15)，target_id是主机ID
+    uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id);
+    // 通过ID解析器得到解析后的ID
+    uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA);
+    
+    if (BSP_CAN_RegisterId(param->can, parsed_feedback_id, 3) != BSP_OK) {
+        BSP_Free(new_motor);
+        return DEVICE_ERR;
+    }
+    
+    manager->motors[manager->motor_count] = new_motor;
+    manager->motor_count++;
+    return DEVICE_OK;
+}
+
+int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR_NO_DEV;
+    
+    for (int i = 0; i < manager->motor_count; i++) {
+        MOTOR_LZ_t *motor = manager->motors[i];
+        if (motor && motor->param.motor_id == param->motor_id) {
+            // 获取反馈数据 - 使用解析后的ID
+            uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id);
+            uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA);
+            BSP_CAN_Message_t msg;
+            
+            while (BSP_CAN_GetMessage(param->can, parsed_feedback_id, &msg, 0) == BSP_OK) {
+                MOTOR_LZ_Decode(motor, &msg);
+            }
+            return DEVICE_OK;
+        }
+    }
+    return DEVICE_ERR_NO_DEV;
+}
+
+int8_t MOTOR_LZ_UpdateAll(void) {
+    int8_t ret = DEVICE_OK;
+    for (int can = 0; can < BSP_CAN_NUM; can++) {
+        MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager((BSP_CAN_t)can);
+        if (manager == NULL) continue;
+        
+        for (int i = 0; i < manager->motor_count; i++) {
+            MOTOR_LZ_t *motor = manager->motors[i];
+            if (motor) {
+                if (MOTOR_LZ_Update(&motor->param) != DEVICE_OK) {
+                    ret = DEVICE_ERR;
+                }
+            }
+        }
+    }
+    return ret;
+}
+
+int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param) {
+    if (param == NULL || motion_param == NULL) return DEVICE_ERR_NULL;
+    MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
+    if (motor == NULL) return DEVICE_ERR_NO_DEV;
+
+    // 自动反向控制
+    MOTOR_LZ_MotionParam_t send_param = *motion_param;
+    if (param->reverse) {
+        send_param.target_angle = -send_param.target_angle;
+        send_param.target_velocity = -send_param.target_velocity;
+        send_param.torque = -send_param.torque;
+    }
+
+    memcpy(&motor->motion_param, motion_param, sizeof(MOTOR_LZ_MotionParam_t));
+
+    uint16_t raw_torque = MOTOR_LZ_FloatToRaw(send_param.torque, LZ_TORQUE_RANGE_NM);
+    uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_MOTION, raw_torque, param->motor_id);
+    uint8_t data[8];
+    uint16_t raw_angle = MOTOR_LZ_FloatToRaw(send_param.target_angle, LZ_ANGLE_RANGE_RAD);
+    data[0] = (raw_angle >> 8) & 0xFF;
+    data[1] = raw_angle & 0xFF;
+    uint16_t raw_velocity = MOTOR_LZ_FloatToRaw(send_param.target_velocity, LZ_VELOCITY_RANGE_RAD_S);
+    data[2] = (raw_velocity >> 8) & 0xFF;
+    data[3] = raw_velocity & 0xFF;
+    uint16_t raw_kp = MOTOR_LZ_FloatToRawPositive(send_param.kp, LZ_KP_MAX);
+    data[4] = (raw_kp >> 8) & 0xFF;
+    data[5] = raw_kp & 0xFF;
+    uint16_t raw_kd = MOTOR_LZ_FloatToRawPositive(send_param.kd, LZ_KD_MAX);
+    data[6] = (raw_kd >> 8) & 0xFF;
+    data[7] = raw_kd & 0xFF;
+    return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
+}
+
+
+int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    // 构建扩展ID - 使能命令
+    uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_ENABLE, param->host_id, param->motor_id);
+    
+    // 数据区清零
+    uint8_t data[8] = {0};
+    
+    return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
+}
+
+int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    // 构建扩展ID - 停止命令
+    uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_DISABLE, param->host_id, param->motor_id);
+    
+    // 数据区
+    uint8_t data[8] = {0};
+    if (clear_fault) {
+        data[0] = 1; // Byte[0]=1时清故障
+    }
+    
+    return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
+}
+
+int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    // 构建扩展ID - 设置零位命令
+    uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_SET_ZERO, param->host_id, param->motor_id);
+    
+    // 数据区 - Byte[0]=1
+    uint8_t data[8] = {1, 0, 0, 0, 0, 0, 0, 0};
+    
+    return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
+}
+
+MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param) {
+    if (param == NULL) return NULL;
+    
+    MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
+    if (manager == NULL) return NULL;
+    
+    for (int i = 0; i < manager->motor_count; i++) {
+        MOTOR_LZ_t *motor = manager->motors[i];
+        if (motor && motor->param.motor_id == param->motor_id) {
+            return motor;
+        }
+    }
+    return NULL;
+}
+
+int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param) {
+    return MOTOR_LZ_MotionControl(param, &lz_relax_param);
+}
+
+int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param) {
+    MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
+    if (motor) {
+        motor->motor.header.online = false;
+        return DEVICE_OK;
+    }
+    return DEVICE_ERR_NO_DEV;
+}
+
+static MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param) {
+    MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
+    if (motor && motor->motor.header.online) {
+        return &motor->lz_feedback;
+    }
+    return NULL;
+}

assets/User_code/device/motor_lz.h

@@ -0,0 +1,212 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/device.h"
+#include "device/motor.h"
+#include "bsp/can.h"
+
+/* Exported constants ------------------------------------------------------- */
+#define MOTOR_LZ_MAX_MOTORS 32
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef enum {
+    MOTOR_LZ_RSO0,
+    MOTOR_LZ_RSO1,
+    MOTOR_LZ_RSO2,
+    MOTOR_LZ_RSO3,
+    MOTOR_LZ_RSO4,
+    MOTOR_LZ_RSO5,
+    MOTOR_LZ_RSO6,
+} MOTOR_LZ_Module_t;
+
+/* 灵足电机控制模式 */
+typedef enum {
+    MOTOR_LZ_MODE_MOTION = 0x1,     /* 运控模式 */
+    MOTOR_LZ_MODE_CURRENT = 0x2,    /* 电流模式 */
+    MOTOR_LZ_MODE_VELOCITY = 0x3,   /* 速度模式 */
+    MOTOR_LZ_MODE_POSITION = 0x4,   /* 位置模式 */
+} MOTOR_LZ_ControlMode_t;
+
+/* 灵足电机通信类型 */
+typedef enum {
+    MOTOR_LZ_CMD_MOTION = 0x1,      /* 运控模式控制 */
+    MOTOR_LZ_CMD_FEEDBACK = 0x2,    /* 电机反馈数据 */
+    MOTOR_LZ_CMD_ENABLE = 0x3,      /* 电机使能运行 */
+    MOTOR_LZ_CMD_DISABLE = 0x4,     /* 电机停止运行 */
+    MOTOR_LZ_CMD_SET_ZERO = 0x6,    /* 设置电机机械零位 */
+} MOTOR_LZ_CmdType_t;
+
+/* 灵足电机运行状态 */
+typedef enum {
+    MOTOR_LZ_STATE_RESET = 0,       /* Reset模式[复位] */
+    MOTOR_LZ_STATE_CALI = 1,        /* Cali模式[标定] */
+    MOTOR_LZ_STATE_MOTOR = 2,       /* Motor模式[运行] */
+} MOTOR_LZ_State_t;
+
+/* 灵足电机故障信息 */
+typedef struct {
+    bool uncalibrated;      /* bit21: 未标定 */
+    bool stall_overload;    /* bit20: 堵转过载故障 */
+    bool encoder_fault;     /* bit19: 磁编码故障 */
+    bool over_temp;         /* bit18: 过温 */
+    bool driver_fault;      /* bit17: 驱动故障 */
+    bool under_voltage;     /* bit16: 欠压故障 */
+} MOTOR_LZ_Fault_t;
+
+/* 灵足电机运控参数 */
+typedef struct {
+    float target_angle;     /* 目标角度 (-12.57f~12.57f rad) */
+    float target_velocity;  /* 目标角速度 (-20~20 rad/s) */
+    float kp;               /* 位置增益 (0.0~5000.0) */
+    float kd;               /* 微分增益 (0.0~100.0) */
+    float torque;           /* 力矩 (-60~60 Nm) */
+} MOTOR_LZ_MotionParam_t;
+
+/*每个电机需要的参数*/
+typedef struct {
+    BSP_CAN_t can;              /* CAN总线 */
+    uint8_t motor_id;           /* 电机CAN ID */
+    uint8_t host_id;            /* 主机CAN ID */
+    MOTOR_LZ_Module_t module;   /* 电机型号 */
+    bool reverse;               /* 是否反向 */
+    MOTOR_LZ_ControlMode_t mode; /* 控制模式 */
+} MOTOR_LZ_Param_t;
+
+/*电机反馈信息扩展*/
+typedef struct {
+    float current_angle;        /* 当前角度 (-12.57f~12.57f rad) */
+    float current_velocity;     /* 当前角速度 (-20~20 rad/s) */
+    float current_torque;       /* 当前力矩 (-60~60 Nm) */
+    float temperature;          /* 当前温度 (摄氏度) */
+    MOTOR_LZ_State_t state;     /* 运行状态 */
+    MOTOR_LZ_Fault_t fault;     /* 故障信息 */
+    uint8_t motor_can_id;       /* 当前电机CAN ID */
+} MOTOR_LZ_Feedback_t;
+
+/*电机实例*/
+typedef struct {
+    MOTOR_LZ_Param_t param;
+    MOTOR_t motor;
+    MOTOR_LZ_Feedback_t lz_feedback;    /* 灵足电机特有反馈信息 */
+    MOTOR_LZ_MotionParam_t motion_param; /* 运控模式参数 */
+} MOTOR_LZ_t;
+
+/*CAN管理器，管理一个CAN总线上所有的电机*/
+typedef struct {
+    BSP_CAN_t can;
+    MOTOR_LZ_t *motors[MOTOR_LZ_MAX_MOTORS];
+    uint8_t motor_count;
+} MOTOR_LZ_CANManager_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 初始化灵足电机驱动系统
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Init(void);
+
+/**
+ * @brief 注册一个灵足电机
+ * @param param 电机参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param);
+
+/**
+ * @brief 更新指定电机数据
+ * @param param 电机参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param);
+
+/**
+ * @brief 更新所有电机数据
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_UpdateAll(void);
+
+/**
+ * @brief 运控模式控制电机
+ * @param param 电机参数
+ * @param motion_param 运控参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param);
+
+/**
+ * @brief 电流(力矩)模式控制电机
+ * @param param 电机参数
+ * @param torque 目标力矩 (-60~60 Nm)
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_TorqueControl(MOTOR_LZ_Param_t *param, float torque);
+
+/**
+ * @brief 位置模式控制电机
+ * @param param 电机参数
+ * @param target_angle 目标角度 (-12.57~12.57 rad)
+ * @param max_velocity 最大速度 (0~20 rad/s)
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_PositionControl(MOTOR_LZ_Param_t *param, float target_angle, float max_velocity);
+
+/**
+ * @brief 速度模式控制电机
+ * @param param 电机参数
+ * @param target_velocity 目标速度 (-20~20 rad/s)
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_VelocityControl(MOTOR_LZ_Param_t *param, float target_velocity);
+
+/**
+ * @brief 电机使能运行
+ * @param param 电机参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param);
+
+/**
+ * @brief 电机停止运行
+ * @param param 电机参数
+ * @param clear_fault 是否清除故障
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault);
+
+/**
+ * @brief 设置电机机械零位
+ * @param param 电机参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param);
+
+/**
+ * @brief 获取指定电机的实例指针
+ * @param param 电机参数
+ * @return 电机实例指针，失败返回NULL
+ */
+MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param);
+
+/**
+ * @brief 使电机松弛（发送停止命令）
+ * @param param 电机参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param);
+
+/**
+ * @brief 使电机离线（设置在线状态为false）
+ * @param param 电机参数
+ * @return 设备状态码
+ */
+int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param);
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/motor_odrive.c

@@ -0,0 +1,327 @@
+/*
+  Odrive电机驱动
+*/
+/* Includes ----------------------------------------------------------------- */
+#include "motor_odrive.h"
+#include <stdbool.h>
+#include <string.h>
+#include "bsp/can.h"
+#include "bsp/mm.h"
+#include "bsp/time.h"
+#include "component/user_math.h"
+
+/* Private define ----------------------------------------------------------- */
+
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+static ODrive_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
+
+
+// 获取指定CAN总线的电机管理器指针
+static ODrive_CANManager_t* MOTOR_GetCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return NULL;
+    return can_managers[can];
+}
+
+// 为指定CAN总线创建电机管理器
+static int8_t MOTOR_CreateCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return DEVICE_ERR;
+    if (can_managers[can] != NULL) return DEVICE_OK;
+    can_managers[can] = (ODrive_CANManager_t*)BSP_Malloc(sizeof(ODrive_CANManager_t));
+    if (can_managers[can] == NULL) return DEVICE_ERR;
+    memset(can_managers[can], 0, sizeof(ODrive_CANManager_t));
+    can_managers[can]->can = can;
+    return DEVICE_OK;
+}
+
+// 解析CAN报文，更新电机反馈信息
+static void Motor_Decode(ODrive_t *motor, BSP_CAN_Message_t *msg)
+{
+    uint8_t axis_id = (msg->original_id >> 5) & 0x3F;   // 提取电机号（0~63）
+    uint8_t cmd_id  = msg->original_id & 0x1F;          // 提取命令 ID（低 5 位）
+
+    motor->param.id = axis_id; // 保存电机 ID
+
+    // 解析帧类型（数据帧或远程帧）
+    if (msg->frame_type == BSP_CAN_FRAME_STD_DATA) {
+        // 数据帧处理
+        switch (cmd_id)
+        {
+        case ODRIVE_HEARTBEAT_MESSAGE: // 0x001 ODrive心跳消息
+            // motor->motor.feedback.axis_error      = (msg->data[0] | msg->data[1]<<8 | msg->data[2]<<16 | msg->data[3]<<24);
+            // motor->motor.feedback.axis_state      = msg->data[4];
+            // motor->motor.feedback.controller_status = msg->data[5];
+            break;
+
+        case ENCODER_ESTIMATES: // 0x009
+            {
+                uint32_t raw_pos = (msg->data[0] | msg->data[1]<<8 | msg->data[2]<<16 | msg->data[3]<<24);
+                uint32_t raw_vel = (msg->data[4] | msg->data[5]<<8 | msg->data[6]<<16 | msg->data[7]<<24);
+                memcpy(&motor->motor.feedback.rotor_abs_angle, &raw_pos, sizeof(float));
+                memcpy(&motor->motor.feedback.rotor_speed, &raw_vel, sizeof(float));
+            }
+            break;
+
+        case GET_ENCODER_COUNT: // 0x014
+            // motor->motor.feedback.encoder_shadow = (msg->data[0] | msg->data[1]<<8 | msg->data[2]<<16 | msg->data[3]<<24);
+            // motor->motor.feedback.encoder_cpr    = (msg->data[4] | msg->data[5]<<8 | msg->data[6]<<16 | msg->data[7]<<24);
+            break;
+
+        case GET_BUS_VOLTAGE_CURRENT: // 0x017
+            {
+                uint32_t raw_vbus, raw_ibus;
+                raw_vbus = (msg->data[0] | msg->data[1]<<8 | msg->data[2]<<16 | msg->data[3]<<24);
+                raw_ibus = (msg->data[4] | msg->data[5]<<8 | msg->data[6]<<16 | msg->data[7]<<24);
+                // memcpy(&motor->motor.feedback.bus_voltage, &raw_vbus, sizeof(float));
+                memcpy(&motor->motor.feedback.torque_current, &raw_ibus, sizeof(float));
+            }
+            break;
+
+        case GET_IQ: // 0x018
+            {
+                uint32_t raw_iq_set, raw_iq_meas;
+                raw_iq_set  = (msg->data[0] | msg->data[1]<<8 | msg->data[2]<<16 | msg->data[3]<<24);
+                raw_iq_meas = (msg->data[4] | msg->data[5]<<8 | msg->data[6]<<16 | msg->data[7]<<24);
+                // memcpy(&motor->motor.feedback.iq_setpoint, &raw_iq_set, sizeof(float));
+                // memcpy(&motor->motor.feedback.iq_measured, &raw_iq_meas, sizeof(float));
+            }
+            break;
+
+        default:
+            
+            break;
+        }
+    }
+}
+
+
+/* Exported functions ------------------------------------------------------- */
+
+// 注册一个新的电机实例到管理器
+int8_t ODrive_Register(ODrive_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    if (MOTOR_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
+    ODrive_CANManager_t *manager = MOTOR_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR;
+    // 检查是否已注册
+    for (int i = 0; i < manager->motor_count; i++) {
+        if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
+            return DEVICE_ERR_INITED;
+        }
+    }
+    // 检查数量
+    if (manager->motor_count >= ODRIVE_MAX_MOTORS) return DEVICE_ERR;
+    // 创建新电机实例
+    ODrive_t *new_motor = (ODrive_t*)BSP_Malloc(sizeof(ODrive_t));
+    if (new_motor == NULL) return DEVICE_ERR;
+    memcpy(&new_motor->param, param, sizeof(ODrive_Param_t));
+    memset(&new_motor->motor, 0, sizeof(MOTOR_t));
+    new_motor->motor.reverse = param->reverse;
+    // 注册CAN接收ID
+    if (BSP_CAN_RegisterId(param->can, param->id, 3) != BSP_OK) {
+        BSP_Free(new_motor);
+        return DEVICE_ERR;
+    }
+    manager->motors[manager->motor_count] = new_motor;
+    manager->motor_count++;
+    return DEVICE_OK;
+}
+
+// 更新指定电机的反馈数据
+int8_t ODrive_Update(ODrive_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    ODrive_CANManager_t *manager = MOTOR_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR_NO_DEV;
+    for (int i = 0; i < manager->motor_count; i++) {
+        ODrive_t *motor = manager->motors[i];
+        if (motor && motor->param.id == param->id) {
+            BSP_CAN_Message_t rx_msg;
+            if (BSP_CAN_GetMessage(param->can, param->id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
+                uint64_t now_time = BSP_TIME_Get();
+                if (now_time - motor->motor.header.last_online_time > 1000) {
+                    motor->motor.header.online = false;
+                    return DEVICE_ERR_NO_DEV;
+                }
+                return DEVICE_ERR;
+            }
+            motor->motor.header.online = true;
+            motor->motor.header.last_online_time = BSP_TIME_Get();
+            Motor_Decode(motor, &rx_msg);
+            return DEVICE_OK;
+        }
+    }
+    return DEVICE_ERR_NO_DEV;
+}
+
+// 更新所有CAN总线下所有电机的反馈数据
+int8_t ODrive_UpdateAll(void) {
+    int8_t ret = DEVICE_OK;
+    for (int can = 0; can < BSP_CAN_NUM; can++) {
+        ODrive_CANManager_t *manager = MOTOR_GetCANManager((BSP_CAN_t)can);
+        if (manager == NULL) continue;
+        for (int i = 0; i < manager->motor_count; i++) {
+            ODrive_t *motor = manager->motors[i];
+            if (motor != NULL) {
+                if (ODrive_Update(&motor->param) != DEVICE_OK) {
+                    ret = DEVICE_ERR;
+                }
+            }
+        }
+    }
+    return ret;
+}
+
+// 获取指定参数对应的电机实例指针
+ODrive_t* ODrive_GetMotor(ODrive_Param_t *param) {
+    if (param == NULL) return NULL;
+    ODrive_CANManager_t *manager = MOTOR_GetCANManager(param->can);
+    if (manager == NULL) return NULL;
+    for (int i = 0; i < manager->motor_count; i++) {
+        ODrive_t *motor = manager->motors[i];
+        if (motor && motor->param.id == param->id) {
+            return motor;
+        }
+    }
+    return NULL;
+}
+
+// 设置指定电机的输出值
+int8_t ODrive_SetOutput(ODrive_Param_t *param, float value) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+
+    // 如果电机反转标志为 true，则反向值
+    if (param->reverse) {
+        value = -value;
+    }
+
+    BSP_CAN_StdDataFrame_t tx_frame;
+    uint16_t command_id;
+    uint8_t *pVal = (uint8_t *)&value;
+
+    // 选择命令 ID 和数据打包方式
+    switch (param->mode) {
+        case POSITION_CONTROL: {
+            command_id = SET_INPUT_POS;
+            float pos = value;
+            int16_t vel_ff = 0;     // 可扩展为参数传入  0就行
+            int16_t torque_ff = 0;  // 可扩展为参数传入  0就行
+            uint8_t *pPos = (uint8_t *)&pos;
+            uint8_t *pVel = (uint8_t *)&vel_ff;
+            uint8_t *pTor = (uint8_t *)&torque_ff;
+            memcpy(&tx_frame.data[0], pPos, 4);
+            memcpy(&tx_frame.data[4], pVel, 2);
+            memcpy(&tx_frame.data[6], pTor, 2);
+            tx_frame.dlc = 8;
+            break;
+        }
+        case VELOCITY_CONTROL: {
+            command_id = SET_INPUT_VEL;
+            float vel = value;
+            float torque_ff = 0.0f; // 可扩展为参数传入
+            uint8_t *pVel = (uint8_t *)&vel;
+            uint8_t *pTor = (uint8_t *)&torque_ff;
+            memcpy(&tx_frame.data[0], pVel, 4);
+            memcpy(&tx_frame.data[4], pTor, 4);
+            tx_frame.dlc = 8;
+            break;
+        }
+        case TORQUE_CONTROL: {
+            command_id = SET_INPUT_TORQUE;
+            memcpy(&tx_frame.data[0], pVal, 4);
+            tx_frame.dlc = 4;
+            break;
+        }
+        case VOLTAGE_CONTROL:
+        default:
+            return DEVICE_ERR; // 暂不支持电压模式
+    }
+
+    // 组装 CAN ID（标准帧）
+    tx_frame.id = (param->id << 5) | command_id;
+
+    // 标准数据帧
+    return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+// 设置加速度和减速度
+int8_t ODrive_SetAccel(ODrive_Param_t *param, float accel, float decel) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+
+    BSP_CAN_StdDataFrame_t tx_frame;
+    uint16_t command_id = SET_TRAJ_ACCEL_LIMITS;
+
+    uint8_t *pAccel = (uint8_t *)&accel;
+    uint8_t *pDecel = (uint8_t *)&decel;
+    memcpy(&tx_frame.data[0], pAccel, 4);
+    memcpy(&tx_frame.data[4], pDecel, 4);
+    tx_frame.dlc = 8;
+
+    tx_frame.id = (param->id << 5) | command_id;
+    return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+// 获取位置和速度反馈
+int8_t ODrive_RequestEncoderEstimates(ODrive_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+
+    BSP_CAN_StdDataFrame_t tx_frame;
+    uint16_t command_id = ENCODER_ESTIMATES;  // 请求编码器估计值命令
+    uint8_t zero_data[8] = {0};               // 发送全 0 数据（ODrive 协议要求）
+
+    memcpy(tx_frame.data, zero_data, 8);
+    tx_frame.dlc = 8;
+    tx_frame.id = (param->id << 5) | command_id;
+
+    return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+// 设置轴请求状态（一般用来重启 ODrive 的某个轴）
+// ODrive_SetAxisRequestedState(odrive_axis[0], CLOSED_LOOP_CONTROL);
+int8_t ODrive_SetAxisRequestedState(ODrive_Param_t *param, Axis_State state) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+
+    BSP_CAN_StdDataFrame_t tx_frame;
+    uint16_t command_id = SET_AXIS_REQUESTED_STATE;
+
+    // 将 state 转为 4 字节
+    memcpy(tx_frame.data, &state, 4);
+    memset(&tx_frame.data[4], 0, 4);  
+    tx_frame.dlc = 4;
+
+    // 组装 CAN ID
+    tx_frame.id = (param->id << 5) | command_id;
+
+    return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+// 清除错误
+int8_t ODrive_ClearErrors(ODrive_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+
+    BSP_CAN_StdDataFrame_t tx_frame;
+    uint16_t command_id = CLEAR_ERRORS;
+
+    memset(tx_frame.data, 0, 8);  
+    tx_frame.dlc = 0;
+
+    tx_frame.id = (param->id << 5) | command_id;
+
+    return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+// 重启 ODrive
+int8_t ODrive_Reboot(ODrive_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+
+    BSP_CAN_StdDataFrame_t tx_frame;
+    uint16_t command_id = REBOOT_ODRIVE;
+
+    memset(tx_frame.data, 0, 8); 
+    tx_frame.dlc = 0;
+
+    tx_frame.id = (param->id << 5) | command_id;
+
+    return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}

assets/User_code/device/motor_odrive.h

@@ -0,0 +1,162 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/device.h"
+#include "device/motor.h"
+#include "bsp/can.h"
+
+/* Private define ----------------------------------------------------------- */
+
+//ODrive型号根据实际情况调整
+#define ODRIVE_MAX_MOTORS 2
+
+//COMMAND ID
+#define ODRIVE_HEARTBEAT_MESSAGE		0x001 // ODrive心跳消息
+#define SET_AXIS_NODE_ID				0x006 // 设置电机节点ID
+#define GET_ENCODER_ESTIMATES			0x008 // 获取编码器估计值
+#define GET_ENCODER_COUNT				0x00A // 获取编码器计数
+#define SET_AXIS_REQUESTED_STATE 		0x007 // 设置电机请求状态
+#define ENCODER_ESTIMATES				0x009 // 编码器估计值
+#define GET_ENCODER_COUNT				0x00A // 获取编码器计数
+#define SET_CONTROLLER_MODES			0x00B // 设置控制器模式
+#define SET_INPUT_POS					0x00C // 设置输入位置
+#define SET_INPUT_VEL					0x00D // 设置输入速度
+#define SET_INPUT_TORQUE				0x00E // 设置输入转矩
+#define SET_LIMITS						0x00F // 设置限制
+#define GET_IQ							0x014 // 获取电流
+#define REBOOT_ODRIVE					0x016 // 重启ODrive
+#define GET_BUS_VOLTAGE_CURRENT			0x017 // 获取总线电压和电流
+#define CLEAR_ERRORS					0x018 // 清除错误
+#define SET_POSITION_GAIN				0x01A // 设置位置增益
+#define SET_VEL_GAINS					0x01B // 设置速度增益
+#define SET_TRAJ_ACCEL_LIMITS      0x012 // 设置轨迹加速度限制
+/* Exported constants ------------------------------------------------------- */
+
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+//Axis States
+typedef enum {
+    UNDEFINED = 0x0,
+    IDLE = 0x1,
+    STARTUP_SEQUENCE = 0x2,
+    FULL_CALIBRATION_SEQUENCE = 0x3,
+    MOTOR_CALIBRATION = 0x4,
+    ENCODER_INDEX_SEARCH = 0x6,
+    ENCODER_OFFSET_CALIBRATION = 0x7,
+    CLOSED_LOOP_CONTROL = 0x8,
+    LOCKIN_SPIN = 0x9,
+    ENCODER_DIR_FIND = 0xA,
+    HOMING = 0xB,
+    ENCODER_HALL_POLARITY_CALIBRATION = 0xC,
+    ENCODER_HALL_PHASE_CALIBRATION = 0xD
+} Axis_State;
+
+//Control Modes
+typedef enum{
+	VOLTAGE_CONTROL = 0x0,
+	TORQUE_CONTROL = 0x1,
+	VELOCITY_CONTROL = 0x2,
+	POSITION_CONTROL = 0x3
+} Control_Mode;
+
+
+/*每个电机需要的参数*/
+typedef struct {
+    BSP_CAN_t can;
+    uint16_t id;
+    uint16_t mode;
+    bool reverse;
+} ODrive_Param_t;
+
+/*电机实例*/
+typedef struct ODrive_t {
+    ODrive_Param_t param;
+    MOTOR_t motor;
+} ODrive_t;
+
+/*CAN管理器，管理一个CAN总线上所有的电机*/
+typedef struct {
+    BSP_CAN_t can;
+    ODrive_t *motors[ODRIVE_MAX_MOTORS];
+    uint8_t motor_count;
+} ODrive_CANManager_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 注册一个odrive电机
+ * @param param 电机参数
+ * @return 
+ */
+int8_t ODrive_Register(ODrive_Param_t *param);
+
+/**
+ * @brief 更新指定电机数据
+ * @param param 电机参数
+ * @return 
+ */
+
+int8_t ODrive_Update(ODrive_Param_t *param);
+
+/** * @brief 更新所有ODrive电机状态
+ * @return 
+ */
+int8_t ODrive_UpdateAll(void);
+
+/**
+ * @brief 设置一个电机的输出
+ * @param param 电机参数
+ * @param value 输出值
+ * @return 
+ */
+int8_t ODrive_SetOutput(ODrive_Param_t *param, float value);
+
+/** * @brief 设置电机加速度和减速度限制
+ * @param param 电机参数
+ * @param accel 加速度  
+ * @param decel 减速度
+ * @return 
+ */
+int8_t ODrive_SetAccel(ODrive_Param_t *param, float accel, float decel);
+
+/**
+ * @brief 获取指定电机的实例指针
+ * @param param 电机参数
+ * @return 
+ */
+ODrive_t* ODrive_GetMotor(ODrive_Param_t *param);
+
+/** * @brief 获取指定电机的编码器估计值
+ * @param param 电机参数
+ * @return 
+ */
+int8_t ODrive_RequestEncoderEstimates(ODrive_Param_t *param); 
+
+
+/** * @brief 设置轴请求状态（一般用来重启 ODrive 的某个轴）
+ * @param param 电机参数
+ * @return 
+ */
+int8_t ODrive_SetAxisRequestedState(ODrive_Param_t *param, Axis_State state);
+
+/** * @brief 清除错误
+ * @param param 电机参数
+ * @return 
+ */
+int8_t ODrive_ClearErrors(ODrive_Param_t *param); 
+
+/**  * @brief 重启 ODrive
+ * @param param 电机参数
+ * @return 
+ */
+int8_t ODrive_Reboot(ODrive_Param_t *param);
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/motor_rm.c

@@ -0,0 +1,321 @@
+/*
+  RM电机驱动
+*/
+/* Includes ----------------------------------------------------------------- */
+#include "motor_rm.h"
+#include <stdbool.h>
+#include <string.h>
+#include "bsp/can.h"
+#include "bsp/mm.h"
+#include "bsp/time.h"
+#include "component/user_math.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private define ----------------------------------------------------------- */
+#define GM6020_FB_ID_BASE        (0x205)
+#define GM6020_CTRL_ID_BASE      (0x1ff)
+#define GM6020_CTRL_ID_EXTAND    (0x2ff)
+
+#define M3508_M2006_FB_ID_BASE   (0x201)
+#define M3508_M2006_CTRL_ID_BASE (0x200)
+#define M3508_M2006_CTRL_ID_EXTAND (0x1ff)
+#define M3508_M2006_ID_SETTING_ID (0x700)
+
+#define GM6020_MAX_ABS_LSB       (30000)
+#define M3508_MAX_ABS_LSB        (16384)
+#define M2006_MAX_ABS_LSB        (10000)
+
+#define MOTOR_TX_BUF_SIZE        (8)
+#define MOTOR_RX_BUF_SIZE        (8)
+
+#define MOTOR_ENC_RES            (8192)   /* 电机编码器分辨率 */
+#define MOTOR_CUR_RES            (16384)  /* 电机转矩电流分辨率 */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private variables -------------------------------------------------------- */
+static MOTOR_RM_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
+
+/* Private function  -------------------------------------------------------- */
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module) {
+    switch (module) {
+        case MOTOR_M2006:
+        case MOTOR_M3508:
+            if (can_id >= M3508_M2006_FB_ID_BASE && can_id < M3508_M2006_FB_ID_BASE + 7) {
+                return can_id - M3508_M2006_FB_ID_BASE;
+            }
+            break;
+        case MOTOR_GM6020:
+            if (can_id >= GM6020_FB_ID_BASE && can_id < GM6020_FB_ID_BASE + 6) {
+                return can_id - GM6020_FB_ID_BASE + 4;
+            }
+            break;
+        default:
+            break;
+    }
+    return DEVICE_ERR;
+}
+
+static float MOTOR_RM_GetRatio(MOTOR_RM_Module_t module) {
+    switch (module) {
+        case MOTOR_M2006: return 36.0f;
+        case MOTOR_M3508: return 3591.0f / 187.0f;
+        case MOTOR_GM6020: return 1.0f;
+        default: return 1.0f;
+    }
+}
+
+static int16_t MOTOR_RM_GetLSB(MOTOR_RM_Module_t module) {
+    switch (module) {
+        case MOTOR_M2006: return M2006_MAX_ABS_LSB;
+        case MOTOR_M3508: return M3508_MAX_ABS_LSB;
+        case MOTOR_GM6020: return GM6020_MAX_ABS_LSB;
+        default: return DEVICE_ERR;
+    }
+}
+
+static MOTOR_RM_CANManager_t* MOTOR_RM_GetCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return NULL;
+    return can_managers[can];
+}
+
+static int8_t MOTOR_RM_CreateCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return DEVICE_ERR;
+    if (can_managers[can] != NULL) return DEVICE_OK;
+    can_managers[can] = (MOTOR_RM_CANManager_t*)BSP_Malloc(sizeof(MOTOR_RM_CANManager_t));
+    if (can_managers[can] == NULL) return DEVICE_ERR;
+    memset(can_managers[can], 0, sizeof(MOTOR_RM_CANManager_t));
+    can_managers[can]->can = can;
+    return DEVICE_OK;
+}
+
+static void Motor_RM_Decode(MOTOR_RM_t *motor, BSP_CAN_Message_t *msg) {
+    uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]);
+    int16_t raw_speed = (int16_t)((msg->data[2] << 8) | msg->data[3]);
+    int16_t raw_current = (int16_t)((msg->data[4] << 8) | msg->data[5]);
+    int16_t lsb = MOTOR_RM_GetLSB(motor->param.module);
+    float ratio = MOTOR_RM_GetRatio(motor->param.module);
+
+    float rotor_angle = raw_angle / (float)MOTOR_ENC_RES * M_2PI;
+    float rotor_speed = raw_speed;
+    float torque_current = raw_current * lsb / (float)MOTOR_CUR_RES;
+
+    if (motor->param.gear) {
+        // 多圈累加
+        int32_t delta = (int32_t)raw_angle - (int32_t)motor->last_raw_angle;
+        if (delta > (MOTOR_ENC_RES / 2)) {
+            motor->gearbox_round_count--;
+        } else if (delta < -(MOTOR_ENC_RES / 2)) {
+            motor->gearbox_round_count++;
+        }
+        motor->last_raw_angle = raw_angle;
+        float single_turn = rotor_angle;
+        motor->gearbox_total_angle = (motor->gearbox_round_count * M_2PI + single_turn) / ratio;
+        // 输出轴多圈绝对值
+        motor->feedback.rotor_abs_angle = motor->gearbox_total_angle;
+        motor->feedback.rotor_speed = rotor_speed / ratio;
+        motor->feedback.torque_current = torque_current * ratio;
+    } else {
+        // 非gear模式，直接用转子单圈
+        motor->gearbox_round_count = 0;
+        motor->last_raw_angle = raw_angle;
+        motor->gearbox_total_angle = 0.0f;
+        motor->feedback.rotor_abs_angle = rotor_angle;
+        motor->feedback.rotor_speed = rotor_speed;
+        motor->feedback.torque_current = torque_current;
+    }
+        while (motor->feedback.rotor_abs_angle < 0) {
+            motor->feedback.rotor_abs_angle += M_2PI;
+        }
+        while (motor->feedback.rotor_abs_angle >= M_2PI) {
+            motor->feedback.rotor_abs_angle -= M_2PI;
+        }
+    if (motor->motor.reverse) {
+        motor->feedback.rotor_abs_angle = M_2PI - motor->feedback.rotor_abs_angle;
+        motor->feedback.rotor_speed = -motor->feedback.rotor_speed;
+        motor->feedback.torque_current = -motor->feedback.torque_current;
+    }
+    motor->feedback.temp = msg->data[6];
+}
+
+/* Exported functions ------------------------------------------------------- */
+
+int8_t MOTOR_RM_Register(MOTOR_RM_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    if (MOTOR_RM_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
+    MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR;
+    // 检查是否已注册
+    for (int i = 0; i < manager->motor_count; i++) {
+        if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
+            return DEVICE_ERR_INITED;
+        }
+    }
+    // 检查数量
+    if (manager->motor_count >= MOTOR_RM_MAX_MOTORS) return DEVICE_ERR;
+    // 创建新电机实例
+    MOTOR_RM_t *new_motor = (MOTOR_RM_t*)BSP_Malloc(sizeof(MOTOR_RM_t));
+    if (new_motor == NULL) return DEVICE_ERR;
+    memcpy(&new_motor->param, param, sizeof(MOTOR_RM_Param_t));
+    memset(&new_motor->motor, 0, sizeof(MOTOR_t));
+    new_motor->motor.reverse = param->reverse;
+    // 注册CAN接收ID
+    if (BSP_CAN_RegisterId(param->can, param->id, 3) != BSP_OK) {
+        BSP_Free(new_motor);
+        return DEVICE_ERR;
+    }
+    manager->motors[manager->motor_count] = new_motor;
+    manager->motor_count++;
+    return DEVICE_OK;
+}
+
+int8_t MOTOR_RM_Update(MOTOR_RM_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR_NO_DEV;
+    for (int i = 0; i < manager->motor_count; i++) {
+        MOTOR_RM_t *motor = manager->motors[i];
+        if (motor && motor->param.id == param->id) {
+            BSP_CAN_Message_t rx_msg;
+            if (BSP_CAN_GetMessage(param->can, param->id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
+                uint64_t now_time = BSP_TIME_Get();
+                if (now_time - motor->motor.header.last_online_time > 1000) {
+                    motor->motor.header.online = false;
+                    return DEVICE_ERR_NO_DEV;
+                }
+                return DEVICE_ERR;
+            }
+            motor->motor.header.online = true;
+            motor->motor.header.last_online_time = BSP_TIME_Get();
+            Motor_RM_Decode(motor, &rx_msg);
+            motor->motor.feedback = motor->feedback;
+            return DEVICE_OK;
+        }
+    }
+    return DEVICE_ERR_NO_DEV;
+}
+
+int8_t MOTOR_RM_UpdateAll(void) {
+    int8_t ret = DEVICE_OK;
+    for (int can = 0; can < BSP_CAN_NUM; can++) {
+        MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager((BSP_CAN_t)can);
+        if (manager == NULL) continue;
+        for (int i = 0; i < manager->motor_count; i++) {
+            MOTOR_RM_t *motor = manager->motors[i];
+            if (motor != NULL) {
+                if (MOTOR_RM_Update(&motor->param) != DEVICE_OK) {
+                    ret = DEVICE_ERR;
+                }
+            }
+        }
+    }
+    return ret;
+}
+
+int8_t MOTOR_RM_SetOutput(MOTOR_RM_Param_t *param, float value) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR_NO_DEV;
+    if (value > 1.0f) value = 1.0f;
+    if (value < -1.0f) value = -1.0f;
+    if (param->reverse){
+        value = -value;
+    }
+    MOTOR_RM_t *motor = MOTOR_RM_GetMotor(param);
+    if (motor == NULL) return DEVICE_ERR_NO_DEV;
+    int8_t logical_index = MOTOR_RM_GetLogicalIndex(param->id, param->module);
+    if (logical_index < 0) return DEVICE_ERR;
+    MOTOR_RM_MsgOutput_t *output_msg = &manager->output_msg;
+    int16_t output_value = (int16_t)(value * (float)MOTOR_RM_GetLSB(param->module));
+    output_msg->output[logical_index] = output_value;
+    return DEVICE_OK;
+}
+
+int8_t MOTOR_RM_Ctrl(MOTOR_RM_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR_NO_DEV;
+    MOTOR_RM_MsgOutput_t *output_msg = &manager->output_msg;
+    BSP_CAN_StdDataFrame_t tx_frame;
+    uint16_t id = param->id;
+    switch (id) {
+        case M3508_M2006_FB_ID_BASE:
+        case M3508_M2006_FB_ID_BASE+1:
+        case M3508_M2006_FB_ID_BASE+2:
+        case M3508_M2006_FB_ID_BASE+3:
+            tx_frame.id = M3508_M2006_CTRL_ID_BASE;
+            tx_frame.dlc = MOTOR_TX_BUF_SIZE;
+            for (int i = 0; i < 4; i++) {
+                tx_frame.data[i*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF);
+                tx_frame.data[i*2+1] = (uint8_t)(output_msg->output[i] & 0xFF);
+            }
+            break;
+        case M3508_M2006_FB_ID_BASE+4:
+        case M3508_M2006_FB_ID_BASE+5:
+        case M3508_M2006_FB_ID_BASE+6:
+        case M3508_M2006_FB_ID_BASE+7:
+            tx_frame.id = M3508_M2006_CTRL_ID_EXTAND;
+            tx_frame.dlc = MOTOR_TX_BUF_SIZE;
+            for (int i = 4; i < 8; i++) {
+                tx_frame.data[(i-4)*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF);
+                tx_frame.data[(i-4)*2+1] = (uint8_t)(output_msg->output[i] & 0xFF);
+            }
+            break;
+        case GM6020_FB_ID_BASE+4:
+        case GM6020_FB_ID_BASE+5:
+        case GM6020_FB_ID_BASE+6:
+            tx_frame.id = GM6020_CTRL_ID_EXTAND;
+            tx_frame.dlc = MOTOR_TX_BUF_SIZE;
+            for (int i = 8; i < 11; i++) {
+                tx_frame.data[(i-8)*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF);
+                tx_frame.data[(i-8)*2+1] = (uint8_t)(output_msg->output[i] & 0xFF);
+            }
+            tx_frame.data[6] = 0;
+            tx_frame.data[7] = 0;
+            break;
+        default:
+            return DEVICE_ERR;
+    }
+    return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+MOTOR_RM_t* MOTOR_RM_GetMotor(MOTOR_RM_Param_t *param) {
+    if (param == NULL) return NULL;
+    MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
+    if (manager == NULL) return NULL;
+    for (int i = 0; i < manager->motor_count; i++) {
+        MOTOR_RM_t *motor = manager->motors[i];
+        if (motor && motor->param.id == param->id) {
+            return motor;
+        }
+    }
+    return NULL;
+}
+
+int8_t MOTOR_RM_Relax(MOTOR_RM_Param_t *param) {
+    return MOTOR_RM_SetOutput(param, 0.0f);
+}
+
+int8_t MOTOR_RM_Offine(MOTOR_RM_Param_t *param) {
+    MOTOR_RM_t *motor = MOTOR_RM_GetMotor(param);
+    if (motor) {
+        motor->motor.header.online = false;
+        return DEVICE_OK;
+    }
+    return DEVICE_ERR_NO_DEV;
+}

assets/User_code/device/motor_rm.h

@@ -0,0 +1,132 @@
+#pragma once
+
+#include "motor.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/device.h"
+#include "device/motor.h"
+#include "bsp/can.h"
+
+/* Exported constants ------------------------------------------------------- */
+#define MOTOR_RM_MAX_MOTORS 11
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef enum {
+    MOTOR_M2006,
+    MOTOR_M3508,
+    MOTOR_GM6020,
+} MOTOR_RM_Module_t;
+
+/*一个can最多控制11个电机*/
+typedef union {
+  int16_t output[MOTOR_RM_MAX_MOTORS];
+  struct {
+    int16_t m3508_m2006_id201;
+    int16_t m3508_m2006_id202;
+    int16_t m3508_m2006_id203;
+    int16_t m3508_m2006_id204;
+    int16_t m3508_m2006_gm6020_id205;
+    int16_t m3508_m2006_gm6020_id206;
+    int16_t m3508_m2006_gm6020_id207;
+    int16_t m3508_m2006_gm6020_id208;
+    int16_t gm6020_id209;
+    int16_t gm6020_id20A;
+    int16_t gm6020_id20B;
+  } named;
+} MOTOR_RM_MsgOutput_t;
+
+/*每个电机需要的参数*/
+typedef struct {
+    BSP_CAN_t can;
+    uint16_t id;
+    MOTOR_RM_Module_t module;
+    bool reverse;
+    bool gear;
+} MOTOR_RM_Param_t;
+
+typedef MOTOR_Feedback_t MOTOR_RM_Feedback_t;
+
+typedef struct {
+    MOTOR_RM_Param_t param;
+    MOTOR_RM_Feedback_t feedback;
+    MOTOR_t motor;
+    // 多圈相关变量，仅gear模式下有效
+    uint16_t last_raw_angle;
+    int32_t gearbox_round_count;
+    float gearbox_total_angle;
+} MOTOR_RM_t;
+
+/*CAN管理器，管理一个CAN总线上所有的电机*/
+typedef struct {
+    BSP_CAN_t can;
+    MOTOR_RM_MsgOutput_t output_msg;
+    MOTOR_RM_t *motors[MOTOR_RM_MAX_MOTORS];
+    uint8_t motor_count;
+} MOTOR_RM_CANManager_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 注册一个RM电机
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_RM_Register(MOTOR_RM_Param_t *param);
+
+/**
+ * @brief 更新指定电机数据
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_RM_Update(MOTOR_RM_Param_t *param);
+
+/**
+ * @brief 设置一个电机的输出
+ * @param param 电机参数
+ * @param value 输出值，范围[-1.0, 1.0]
+ * @return 
+ */
+int8_t MOTOR_RM_SetOutput(MOTOR_RM_Param_t *param, float value);
+
+/**
+ * @brief 发送控制命令到电机，注意一个CAN可以控制多个电机，所以只需要发送一次即可
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_RM_Ctrl(MOTOR_RM_Param_t *param);
+
+/**
+ * @brief 获取指定电机的实例指针
+ * @param param 电机参数
+ * @return 
+ */
+MOTOR_RM_t* MOTOR_RM_GetMotor(MOTOR_RM_Param_t *param);
+
+/**
+ * @brief 使电机松弛（设置输出为0）
+ * @param param 
+ * @return 
+ */
+int8_t MOTOR_RM_Relax(MOTOR_RM_Param_t *param);
+
+/**
+ * @brief 使电机离线（设置在线状态为false）
+ * @param param 
+ * @return 
+ */
+int8_t MOTOR_RM_Offine(MOTOR_RM_Param_t *param);
+
+/**
+ * @brief 
+ * @param  
+ * @return 
+ */
+int8_t MOTOR_RM_UpdateAll(void);
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/motor_vesc.c

@@ -0,0 +1,249 @@
+/*
+  VESC电机驱动
+*/
+/* Includes ----------------------------------------------------------------- */
+#include "motor_vesc.h"
+#include <stdbool.h>
+#include <string.h>
+#include "bsp/can.h"
+#include "bsp/mm.h"
+#include "bsp/time.h"
+#include "component/user_math.h"
+
+/* Private define ----------------------------------------------------------- */
+
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+
+/**************************************
+ * 限幅函数
+ **************************************/
+void assert_param_duty(float *duty){
+    // 如果 duty 是 -1.0 ~ 1.0，则最大值用 wtrcfg_VESC_COMMAND_DUTY_MAX / 100
+    float max_duty = wtrcfg_VESC_COMMAND_DUTY_MAX / 100.0f;
+    if (fabsf(*duty) > max_duty) {
+        *duty = (*duty > 0) ? max_duty : -max_duty;
+    }
+}
+
+void assert_param_current(float *current){
+	if( fabsf(*current) > wtrcfg_VESC_COMMAND_CURRENT_MAX )
+		*current = *current > 0 ? wtrcfg_VESC_COMMAND_CURRENT_MAX : - wtrcfg_VESC_COMMAND_CURRENT_MAX ;
+}
+void assert_param_rpm(float *rpm){
+	if( fabsf(*rpm) > wtrcfg_VESC_COMMAND_ERPM_MAX )
+		*rpm = *rpm > 0 ? wtrcfg_VESC_COMMAND_ERPM_MAX : - wtrcfg_VESC_COMMAND_ERPM_MAX ;
+}
+void assert_param_pos(float *pos){
+	if( fabsf(*pos) > wtrcfg_VESC_COMMAND_POS_MAX )
+		*pos = *pos > 0 ? wtrcfg_VESC_COMMAND_POS_MAX : - wtrcfg_VESC_COMMAND_POS_MAX ;
+}
+
+static VESC_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
+
+
+// 获取指定CAN总线的电机管理器指针
+static VESC_CANManager_t* MOTOR_GetCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return NULL;
+    return can_managers[can];
+}
+
+// 为指定CAN总线创建电机管理器
+static int8_t MOTOR_CreateCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return DEVICE_ERR;
+    if (can_managers[can] != NULL) return DEVICE_OK;
+    can_managers[can] = (VESC_CANManager_t*)BSP_Malloc(sizeof(VESC_CANManager_t));
+    if (can_managers[can] == NULL) return DEVICE_ERR;
+    memset(can_managers[can], 0, sizeof(VESC_CANManager_t));
+    can_managers[can]->can = can;
+    return DEVICE_OK;
+}
+
+// 解析CAN报文，更新电机反馈信息
+static void Motor_VESC_Decode(VESC_t *motor, BSP_CAN_Message_t *msg)
+{
+    if (motor == NULL || msg == NULL) return;
+    motor->motor.feedback.rotor_speed = 
+        ((int32_t)msg->data[0] << 24) |
+        ((int32_t)msg->data[1] << 16) |
+        ((int32_t)msg->data[2] << 8)  |
+        ((int32_t)msg->data[3]);
+
+    // torque_current: 低 2 字节 (data[4], data[5])
+    int16_t raw_current = (int16_t)((msg->data[5] << 8) | msg->data[4]);
+    motor->motor.feedback.torque_current = raw_current / 1000.0f;  // 从 0.1A -> A
+
+    // duty_cycle: 低 2 字节 (data[6], data[7])
+    int16_t raw_duty = (int16_t)((msg->data[7] << 8) | msg->data[6]);
+    //motor->motor.feedback.duty_cycle = raw_duty / 1000.0f;  // 从千分之一 -> (-1.0 ~ 1.0)
+
+}
+
+
+/* Exported functions ------------------------------------------------------- */
+
+// 注册一个新的电机实例到管理器
+int8_t VESC_Register(VESC_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    if (MOTOR_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
+    VESC_CANManager_t *manager = MOTOR_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR;
+    // 检查是否已注册
+    for (int i = 0; i < manager->motor_count; i++) {
+        if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
+            return DEVICE_ERR_INITED;
+        }
+    }
+    // 检查数量
+    if (manager->motor_count >= VESC_MAX_MOTORS) return DEVICE_ERR;
+    // 创建新电机实例
+    VESC_t *new_motor = (VESC_t*)BSP_Malloc(sizeof(VESC_t));
+    if (new_motor == NULL) return DEVICE_ERR;
+    memcpy(&new_motor->param, param, sizeof(VESC_Param_t));
+    memset(&new_motor->motor, 0, sizeof(MOTOR_t));
+    new_motor->motor.reverse = param->reverse;
+    // 注册CAN接收ID
+    if (BSP_CAN_RegisterId(param->can, param->id, 3) != BSP_OK) {
+        BSP_Free(new_motor);
+        return DEVICE_ERR;
+    }
+    manager->motors[manager->motor_count] = new_motor;
+    manager->motor_count++;
+    return DEVICE_OK;
+}
+
+// 更新指定电机的反馈数据（扩展帧方式）
+int8_t VESC_Update(VESC_Param_t *param)
+{
+    if (param == NULL) return DEVICE_ERR_NULL;
+    VESC_CANManager_t *manager = MOTOR_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR_NO_DEV;
+    VESC_t *motor = NULL;
+    for (int i = 0; i < manager->motor_count; i++) {
+        if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
+            motor = manager->motors[i];
+            break;
+        }
+    }
+    if (motor == NULL) return DEVICE_ERR_NO_DEV;
+    // 根据电机 ID 获取对应扩展帧 ID
+    uint32_t ext_id = 0;
+    switch (param->id) {
+        case VESC_1: ext_id = CAN_VESC5065_M1_MSG1; break;
+        case VESC_2: ext_id = CAN_VESC5065_M2_MSG1; break;
+        case VESC_4: ext_id = CAN_VESC5065_M3_MSG1; break;
+        default: return DEVICE_ERR_NO_DEV;
+    }
+    BSP_CAN_Message_t rx_msg;
+    if (BSP_CAN_GetMessage(param->can, ext_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
+        uint64_t now_time = BSP_TIME_Get();
+        if (now_time - motor->motor.header.last_online_time > 1000) {
+            motor->motor.header.online = false;
+            return DEVICE_ERR_NO_DEV;
+        }
+        return DEVICE_ERR;
+    }
+    motor->motor.header.online = true;
+    motor->motor.header.last_online_time = BSP_TIME_Get();
+    Motor_VESC_Decode(motor, &rx_msg);
+    return DEVICE_OK;
+}
+
+// 更新所有CAN总线下所有电机的反馈数据
+int8_t VESC_UpdateAll(void) {
+    int8_t ret = DEVICE_OK;
+    for (int can = 0; can < BSP_CAN_NUM; can++) {
+        VESC_CANManager_t *manager = MOTOR_GetCANManager((BSP_CAN_t)can);
+        if (manager == NULL) continue;
+        for (int i = 0; i < manager->motor_count; i++) {
+            VESC_t *motor = manager->motors[i];
+            if (motor != NULL) {
+                if (VESC_Update(&motor->param) != DEVICE_OK) {
+                    ret = DEVICE_ERR;
+                }
+            }
+        }
+    }
+    return ret;
+}
+
+// 获取指定参数对应的电机实例指针
+VESC_t* VESC_GetMotor(VESC_Param_t *param) {
+    if (param == NULL) return NULL;
+    VESC_CANManager_t *manager = MOTOR_GetCANManager(param->can);
+    if (manager == NULL) return NULL;
+    for (int i = 0; i < manager->motor_count; i++) {
+        VESC_t *motor = manager->motors[i];
+        if (motor && motor->param.id == param->id) {
+            return motor;
+        }
+    }
+    return NULL;
+}
+
+// 设置指定电机的输出值
+int8_t VESC_SetOutput(VESC_Param_t *param, float value)
+{
+    if (param == NULL) return DEVICE_ERR_NULL;
+    BSP_CAN_StdDataFrame_t tx_frame;
+    uint16_t command_id;
+
+    if (param->reverse) {
+        value = -value;
+    }
+
+    switch (param->mode)
+    {
+        case DUTY_CONTROL: {
+            assert_param_duty(&value); // 调用你现有的限幅函数
+            command_id = CAN_PACKET_SET_DUTY;
+            int32_t duty_val = (int32_t)(value * 1e5f); // duty 放大 1e5
+            memcpy(&tx_frame.data[0], &duty_val, 4);
+            tx_frame.dlc = 4;
+            break;
+        }
+        case RPM_CONTROL: {
+            assert_param_rpm(&value); 
+            command_id = CAN_PACKET_SET_RPM;
+            int32_t rpm_val = (int32_t)value;
+            memcpy(&tx_frame.data[0], &rpm_val, 4);
+            tx_frame.dlc = 4;
+            break;
+        }
+        case CURRENT_CONTROL: {
+            assert_param_current(&value); 
+            command_id = CAN_PACKET_SET_CURRENT;
+            int32_t cur_val = (int32_t)(value * 1e3f); // A -> mA （0-50A）
+            memcpy(&tx_frame.data[0], &cur_val, 4);
+            tx_frame.dlc = 4;
+            break;
+        }
+        case POSITION_CONTROL: {
+            assert_param_pos(&value); 
+            command_id = CAN_PACKET_SET_POS;
+            memcpy(&tx_frame.data[0], &value, 4); 
+            tx_frame.dlc = 4;
+            break;
+        }
+        default:
+            return DEVICE_ERR;
+    }
+    tx_frame.id = (param->id << 5) | command_id;
+    return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+  }
+
+int8_t VESC_Relax(VESC_Param_t *param) {
+    return VESC_SetOutput(param, 0.0f);
+}
+
+
+int8_t VESC_Offine(VESC_Param_t *param) {
+    VESC_t *motor = VESC_GetMotor(param);
+    if (motor) {
+        motor->motor.header.online = false;
+        return DEVICE_OK;
+    }
+    return DEVICE_ERR_NO_DEV;
+}

assets/User_code/device/motor_vesc.h

@@ -0,0 +1,146 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/device.h"
+#include "device/motor.h"
+#include "bsp/can.h"
+
+/* Private define ----------------------------------------------------------- */
+#define wtrcfg_VESC_COMMAND_DUTY_MAX 100
+#define wtrcfg_VESC_COMMAND_CURRENT_MAX 10
+#define wtrcfg_VESC_COMMAND_POS_MAX 360
+#define wtrcfg_VESC_COMMAND_ERPM_MAX 35000
+#define wtrcfg_VESC_UART_TIMEOUT 0xff
+
+// VESC数量根据实际情况调整
+#define VESC_MAX_MOTORS 4
+
+/* Exported constants ------------------------------------------------------- */
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+typedef enum
+{
+	VESC_1 = 1,
+	VESC_2 = 2,
+	VESC_3 = 3,
+	VESC_4 = 4,
+	CAN_VESC5065_M1_MSG1 = 0x901, // vesc的数据回传使用了扩展id，[0:7]为驱动器id，[8:15]为帧类型
+	CAN_VESC5065_M2_MSG1 = 0x902,
+	CAN_VESC5065_M3_MSG1 = 0x903,
+	CAN_VESC5065_M4_MSG1 = 0x904, 
+}VESC_ID;
+
+typedef enum
+{
+	CAN_PACKET_SET_DUTY = 0,
+	CAN_PACKET_SET_CURRENT = 1,
+	CAN_PACKET_SET_CURRENT_BRAKE = 2,
+	CAN_PACKET_SET_RPM = 3,
+	CAN_PACKET_SET_POS = 4,
+	CAN_PACKET_FILL_RX_BUFFER = 5,
+	CAN_PACKET_FILL_RX_BUFFER_LONG = 6,
+	CAN_PACKET_PROCESS_RX_BUFFER = 7,
+	CAN_PACKET_PROCESS_SHORT_BUFFER = 8,
+	CAN_PACKET_STATUS = 9,
+	CAN_PACKET_SET_CURRENT_REL = 10,
+	CAN_PACKET_SET_CURRENT_BRAKE_REL = 11,
+	CAN_PACKET_SET_CURRENT_HANDBRAKE = 12,
+	CAN_PACKET_SET_CURRENT_HANDBRAKE_REL = 13
+} CAN_PACKET_ID;
+
+// Control Modes
+typedef enum
+{
+	DUTY_CONTROL = 0x0,
+	RPM_CONTROL = 0x1,
+	CURRENT_CONTROL = 0x2,
+	POSITION_CONTROL = 0x3
+} Control_Mode;
+
+/*每个电机需要的参数*/
+typedef struct
+{
+	BSP_CAN_t can;
+	uint16_t id;
+	uint16_t mode;
+	bool reverse;
+} VESC_Param_t;
+
+/*电机实例*/
+typedef struct ODrive_t
+{
+	VESC_Param_t param;
+	MOTOR_t motor;
+} VESC_t;
+
+/*CAN管理器，管理一个CAN总线上所有的电机*/
+typedef struct
+{
+	BSP_CAN_t can;
+	VESC_t *motors[VESC_MAX_MOTORS];
+	uint8_t motor_count;
+} VESC_CANManager_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 注册一个vesc电机
+ * @param param 电机参数
+ * @return
+ */
+int8_t VESC_Register(VESC_Param_t *param);
+
+/**
+ * @brief 更新指定电机数据
+ * @param param 电机参数
+ * @return
+ */
+int8_t VESC_Update(VESC_Param_t *param);
+
+/**
+ * @brief 更新所有电机数据
+ * @return
+ */
+int8_t VESC_UpdateAll(void);
+
+/**
+ * @brief 设置一个电机的输出
+ * @param param 电机参数
+ * @param value 输出值
+ * @return
+ */
+
+int8_t VESC_SetOutput(VESC_Param_t *param, float value);
+
+/**
+ * @brief 获取指定电机的实例指针
+ * @param param 电机参数
+ * @return
+ */
+
+VESC_t* VESC_GetMotor(VESC_Param_t *param);
+
+/**
+ * @brief 使电机松弛（设置输出为0）
+ * @param param
+ * @return
+ */
+int8_t VESC_Relax(VESC_Param_t *param);
+/**
+ * @brief 使电机离线（设置在线状态为false）
+ * @param param
+ * @return
+ */
+int8_t VESC_Offine(VESC_Param_t *param);
+
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/ops9.c

@@ -0,0 +1,58 @@
+/*
+    ACTION全场定位码盘ops9
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/ops9.h"
+
+#include <string.h>
+
+#include "bsp/uart.h"
+#include "bsp/time.h"
+/* Private define ----------------------------------------------------------- */
+
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+static osThreadId_t thread_alert;
+static bool inited = false;
+
+/* Private function  -------------------------------------------------------- */
+static void OPS9_RxCpltCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_OPS9_RAW_REDY);
+
+}
+
+/* Exported functions ------------------------------------------------------- */
+int8_t OPS9_init(OPS9_t *ops9) {
+  if (ops9 == NULL) return DEVICE_ERR_NULL;
+  if (inited) return DEVICE_ERR_INITED;
+  if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
+
+  BSP_UART_RegisterCallback(BSP_UART_OPS9, BSP_UART_RX_CPLT_CB,
+                            OPS9_RxCpltCallback);
+
+  inited = true;
+  return DEVICE_OK;
+}
+
+int8_t OPS9_Restart(void) {
+  __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_OPS9));
+  __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_OPS9));
+  return DEVICE_OK;
+}
+
+int8_t OPS9_StartDmaRecv(OPS9_t *ops9) {
+  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_OPS9),
+                           (uint8_t *)&(ops9->data),
+                           sizeof(ops9->data)) == HAL_OK)
+    return DEVICE_OK;
+  return DEVICE_ERR;
+}
+
+bool OPS9_WaitDmaCplt(uint32_t timeout) {
+  return (osThreadFlagsWait(SIGNAL_OPS9_RAW_REDY, osFlagsWaitAll, timeout) ==
+          SIGNAL_OPS9_RAW_REDY);
+}
+

assets/User_code/device/ops9.h

@@ -0,0 +1,49 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <cmsis_os2.h>
+#include <stdint.h>
+
+#include "component/user_math.h"
+#include "device/device.h"
+/* Exported constants ------------------------------------------------------- */
+
+#define OPS9_HEADER 0x0D0A
+#define OPS9_TAIL   0x0A0D
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+// 数据包结构体
+typedef struct __packed {
+    uint16_t header; // 2字节
+    float yaw;                // 4字节
+    float pitch;              // 4字节
+    float roll;               // 4字节
+    float x;                  // 4字节
+    float y;                  // 4字节
+    float angular_velocity;   // 4字节
+    uint16_t tail;     // 2字节
+} OPS9_Data_t; // 共28字节
+
+typedef struct {
+    DEVICE_Header_t header; // 设备头
+    OPS9_Data_t data;          // 存储接收到的数据
+} OPS9_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+int8_t OPS9_init(OPS9_t *ops9);
+int8_t OPS9_Restart(void);
+int8_t OPS9_StartDmaRecv(OPS9_t *ops9);
+bool OPS9_WaitDmaCplt(uint32_t timeout);
+
+
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/rc_can.c

@@ -0,0 +1,328 @@
+/* Includes ----------------------------------------------------------------- */
+#include "device/rc_can.h"
+#include "bsp/time.h"
+#include "device/device.h"
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private constants -------------------------------------------------------- */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Private macro ------------------------------------------------------------ */
+/* Private types ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+
+/* USER VARIABLE BEGIN */
+
+/* USER VARIABLE END */
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+/* Private function prototypes ---------------------------------------------- */
+static int8_t RC_CAN_ValidateParams(const RC_CAN_Param_t *param);
+static int8_t RC_CAN_RegisterIds(RC_CAN_t *rc_can);
+
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * @brief 初始化RC CAN发送模块
+ * @param rc_can RC_CAN结构体指针
+ * @param param 初始化参数
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t RC_CAN_Init(RC_CAN_t *rc_can, RC_CAN_Param_t *param) {
+  if (rc_can == NULL || param == NULL) {
+    return DEVICE_ERR_NULL;
+  }
+
+  // 参数验证
+  if (RC_CAN_ValidateParams(param) != DEVICE_OK) {
+    return DEVICE_ERR;
+  }
+
+  rc_can->param = *param;
+
+  // 初始化header
+  rc_can->header.online = false;
+  rc_can->header.last_online_time = 0;
+
+  // 手动初始化数据结构
+  rc_can->data.joy.ch_l_x = 0.0f;
+  rc_can->data.joy.ch_l_y = 0.0f;
+  rc_can->data.joy.ch_r_x = 0.0f;
+  rc_can->data.joy.ch_r_y = 0.0f;
+  rc_can->data.sw.sw_l = RC_CAN_SW_ERR;
+  rc_can->data.sw.sw_r = RC_CAN_SW_ERR;
+  rc_can->data.sw.ch_res = 0.0f;
+  rc_can->data.mouse.x = 0.0f;
+  rc_can->data.mouse.y = 0.0f;
+  rc_can->data.mouse.z = 0.0f;
+  rc_can->data.mouse.mouse_l = false;
+  rc_can->data.mouse.mouse_r = false;
+  rc_can->data.keyboard.key_value = 0;
+  for (int i = 0; i < 6; i++) {
+    rc_can->data.keyboard.keys[i] = RC_CAN_KEY_NONE;
+  }
+
+  // 注册CAN ID队列（从机模式需要接收数据）
+  if (rc_can->param.mode == RC_CAN_MODE_SLAVE) {
+    return RC_CAN_RegisterIds(rc_can);
+  }
+
+  return DEVICE_OK;
+}
+
+/**
+ * @brief 更新并发送数据到CAN总线
+ * @param rc_can RC_CAN结构体指针
+ * @param data_type 数据类型
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t RC_CAN_SendData(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type) {
+  if (rc_can == NULL) {
+    return DEVICE_ERR_NULL;
+  }
+  if (rc_can->param.mode != RC_CAN_MODE_MASTER) {
+    return DEVICE_ERR;
+  }
+  BSP_CAN_StdDataFrame_t frame;
+  frame.dlc = 8;
+  // 边界裁剪宏
+  #define RC_CAN_CLAMP(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
+  switch (data_type) {
+  case RC_CAN_DATA_JOYSTICK: {
+    frame.id = rc_can->param.joy_id;
+    float l_x = RC_CAN_CLAMP(rc_can->data.joy.ch_l_x, -0.999969f, 0.999969f);
+    float l_y = RC_CAN_CLAMP(rc_can->data.joy.ch_l_y, -0.999969f, 0.999969f);
+    float r_x = RC_CAN_CLAMP(rc_can->data.joy.ch_r_x, -0.999969f, 0.999969f);
+    float r_y = RC_CAN_CLAMP(rc_can->data.joy.ch_r_y, -0.999969f, 0.999969f);
+    int16_t l_x_i = (int16_t)(l_x * 32768.0f);
+    int16_t l_y_i = (int16_t)(l_y * 32768.0f);
+    int16_t r_x_i = (int16_t)(r_x * 32768.0f);
+    int16_t r_y_i = (int16_t)(r_y * 32768.0f);
+    frame.data[0] = (uint8_t)(l_x_i & 0xFF);
+    frame.data[1] = (uint8_t)((l_x_i >> 8) & 0xFF);
+    frame.data[2] = (uint8_t)(l_y_i & 0xFF);
+    frame.data[3] = (uint8_t)((l_y_i >> 8) & 0xFF);
+    frame.data[4] = (uint8_t)(r_x_i & 0xFF);
+    frame.data[5] = (uint8_t)((r_x_i >> 8) & 0xFF);
+    frame.data[6] = (uint8_t)(r_y_i & 0xFF);
+    frame.data[7] = (uint8_t)((r_y_i >> 8) & 0xFF);
+    break;
+  }
+  case RC_CAN_DATA_SWITCH: {
+    frame.id = rc_can->param.sw_id;
+    frame.data[0] = (uint8_t)(rc_can->data.sw.sw_l);
+    frame.data[1] = (uint8_t)(rc_can->data.sw.sw_r);
+    float ch_res = RC_CAN_CLAMP(rc_can->data.sw.ch_res, -0.999969f, 0.999969f);
+    int16_t ch_res_i = (int16_t)(ch_res * 32768.0f);
+    frame.data[2] = (uint8_t)(ch_res_i & 0xFF);
+    frame.data[3] = (uint8_t)((ch_res_i >> 8) & 0xFF);
+    frame.data[4] = 0; // 保留字节
+    frame.data[5] = 0; // 保留字节
+    frame.data[6] = 0; // 保留字节
+    frame.data[7] = 0; // 保留字节
+    break;
+  }
+  case RC_CAN_DATA_MOUSE: {
+    frame.id = rc_can->param.mouse_id;
+    // 鼠标x/y/z一般为增量，若有极限也可加clamp
+    int16_t x = (int16_t)(rc_can->data.mouse.x);
+    int16_t y = (int16_t)(rc_can->data.mouse.y);
+    int16_t z = (int16_t)(rc_can->data.mouse.z);
+    frame.data[0] = (uint8_t)(x & 0xFF);
+    frame.data[1] = (uint8_t)((x >> 8) & 0xFF);
+    frame.data[2] = (uint8_t)(y & 0xFF);
+    frame.data[3] = (uint8_t)((y >> 8) & 0xFF);
+    frame.data[4] = (uint8_t)(z & 0xFF);
+    frame.data[5] = (uint8_t)((z >> 8) & 0xFF);
+    frame.data[6] = (uint8_t)(rc_can->data.mouse.mouse_l ? 1 : 0);
+    frame.data[7] = (uint8_t)(rc_can->data.mouse.mouse_r ? 1 : 0);
+    break;
+  }
+  case RC_CAN_DATA_KEYBOARD: {
+    frame.id = rc_can->param.keyboard_id;
+    frame.data[0] = (uint8_t)(rc_can->data.keyboard.key_value & 0xFF);
+    frame.data[1] = (uint8_t)((rc_can->data.keyboard.key_value >> 8) & 0xFF);
+    for (int i = 0; i < 6; i++) {
+      frame.data[2 + i] = (i < 6) ? (uint8_t)(rc_can->data.keyboard.keys[i]) : 0;
+    }
+    break;
+  }
+  default:
+    return DEVICE_ERR;
+  }
+  #undef RC_CAN_CLAMP
+  if (BSP_CAN_Transmit(rc_can->param.can, BSP_CAN_FORMAT_STD_DATA, frame.id,
+                       frame.data, frame.dlc) != BSP_OK) {
+    return DEVICE_ERR;
+  }
+  return DEVICE_OK;
+}
+
+/**
+ * @brief 接收并更新CAN数据
+ * @param rc_can RC_CAN结构体指针
+ * @param data_type 数据类型
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t RC_CAN_Update(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type) {
+  if (rc_can == NULL) {
+    return DEVICE_ERR_NULL;
+  }
+
+  // 只有从机模式才能接收数据
+  if (rc_can->param.mode != RC_CAN_MODE_SLAVE) {
+    return DEVICE_ERR;
+  }
+  BSP_CAN_Message_t msg;
+  
+  switch (data_type) {
+  case RC_CAN_DATA_JOYSTICK:
+    if (BSP_CAN_GetMessage(rc_can->param.can, rc_can->param.joy_id, &msg,
+                           BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
+      return DEVICE_ERR;
+    }
+    // 解包数据
+    int16_t ch_l_x = (int16_t)((msg.data[1] << 8) | msg.data[0]);
+    int16_t ch_l_y = (int16_t)((msg.data[3] << 8) | msg.data[2]);
+    int16_t ch_r_x = (int16_t)((msg.data[5] << 8) | msg.data[4]);
+    int16_t ch_r_y = (int16_t)((msg.data[7] << 8) | msg.data[6]);
+
+    // 转换为浮点数（范围：-1.0到1.0）
+    rc_can->data.joy.ch_l_x = (float)ch_l_x / 32768.0f;
+    rc_can->data.joy.ch_l_y = (float)ch_l_y / 32768.0f;
+    rc_can->data.joy.ch_r_x = (float)ch_r_x / 32768.0f;
+    rc_can->data.joy.ch_r_y = (float)ch_r_y / 32768.0f;
+    break;
+  case RC_CAN_DATA_SWITCH:
+    if (BSP_CAN_GetMessage(rc_can->param.can, rc_can->param.sw_id, &msg,
+                           BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
+      return DEVICE_ERR;
+    }
+    // 解包数据
+    rc_can->data.sw.sw_l = (RC_CAN_SW_t)msg.data[0];
+    rc_can->data.sw.sw_r = (RC_CAN_SW_t)msg.data[1];
+
+    int16_t ch_res = (int16_t)((msg.data[3] << 8) | msg.data[2]);
+    rc_can->data.sw.ch_res = (float)ch_res / 32768.0f;
+    break;
+  case RC_CAN_DATA_MOUSE:
+    if (BSP_CAN_GetMessage(rc_can->param.can, rc_can->param.mouse_id, &msg,
+                           BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
+      return DEVICE_ERR;
+    }
+    // 解包数据
+    int16_t x = (int16_t)((msg.data[1] << 8) | msg.data[0]);
+    int16_t y = (int16_t)((msg.data[3] << 8) | msg.data[2]);
+    int16_t z = (int16_t)((msg.data[5] << 8) | msg.data[4]);
+    rc_can->data.mouse.x = (float)x;
+    rc_can->data.mouse.y = (float)y;
+    rc_can->data.mouse.z = (float)z;
+    rc_can->data.mouse.mouse_l = (msg.data[6] & 0x01) ? true : false;
+    rc_can->data.mouse.mouse_r = (msg.data[7] & 0x01) ? true : false;
+    break;
+  case RC_CAN_DATA_KEYBOARD:
+    if (BSP_CAN_GetMessage(rc_can->param.can, rc_can->param.keyboard_id, &msg,
+                           BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
+      return DEVICE_ERR;
+    }
+    if (msg.dlc < 2) {
+      return DEVICE_ERR;
+    }
+    // 解包数据
+    rc_can->data.keyboard.key_value =
+        (uint16_t)((msg.data[1] << 8) | msg.data[0]);
+    for (int i = 0; i < 6 && (i + 2) < msg.dlc; i++) {
+      rc_can->data.keyboard.keys[i] = (RC_CAN_Key_t)(msg.data[2 + i]);
+    }
+    // 清空未使用的按键位置
+    for (int i = (msg.dlc > 2 ? msg.dlc - 2 : 0); i < 6; i++) {
+      rc_can->data.keyboard.keys[i] = RC_CAN_KEY_NONE;
+    }
+    break;
+  default:
+    return DEVICE_ERR;
+  }
+
+  // 更新header状态
+  rc_can->header.online = true;
+  rc_can->header.last_online_time = BSP_TIME_Get_us();
+
+  return DEVICE_OK;
+}
+
+/* Private functions -------------------------------------------------------- */
+
+/**
+ * @brief 验证RC_CAN参数
+ * @param param 参数指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+static int8_t RC_CAN_ValidateParams(const RC_CAN_Param_t *param) {
+  if (param == NULL) {
+    return DEVICE_ERR_NULL;
+  }
+
+  // 检查CAN总线有效性
+  if (param->can >= BSP_CAN_NUM) {
+    return DEVICE_ERR;
+  }
+
+  // 检查工作模式有效性
+  if (param->mode != RC_CAN_MODE_MASTER && param->mode != RC_CAN_MODE_SLAVE) {
+    return DEVICE_ERR;
+  }
+
+  // 检查CAN ID是否重复
+  if (param->joy_id == param->sw_id || param->joy_id == param->mouse_id ||
+      param->joy_id == param->keyboard_id || param->sw_id == param->mouse_id ||
+      param->sw_id == param->keyboard_id ||
+      param->mouse_id == param->keyboard_id) {
+    return DEVICE_ERR;
+  }
+
+  return DEVICE_OK;
+}
+
+/**
+ * @brief 注册CAN ID
+ * @param rc_can RC_CAN结构体指针
+ * @return DEVICE_OK 成功，其他值失败
+ */
+static int8_t RC_CAN_RegisterIds(RC_CAN_t *rc_can) {
+  if (BSP_CAN_RegisterId(rc_can->param.can, rc_can->param.joy_id, 0) !=
+      BSP_OK) {
+    return DEVICE_ERR;
+  }
+  if (BSP_CAN_RegisterId(rc_can->param.can, rc_can->param.sw_id, 0) != BSP_OK) {
+    return DEVICE_ERR;
+  }
+  if (BSP_CAN_RegisterId(rc_can->param.can, rc_can->param.mouse_id, 0) !=
+      BSP_OK) {
+    return DEVICE_ERR;
+  }
+  if (BSP_CAN_RegisterId(rc_can->param.can, rc_can->param.keyboard_id, 0) !=
+      BSP_OK) {
+    return DEVICE_ERR;
+  }
+
+  return DEVICE_OK;
+}
+
+int8_t RC_CAN_OFFLINE(RC_CAN_t *rc_can){
+    if (rc_can == NULL) {
+        return DEVICE_ERR_NULL;
+    }
+    rc_can->header.online = false;
+    return DEVICE_OK;
+}
+/* USER CODE BEGIN */
+
+/* USER CODE END */

assets/User_code/device/rc_can.h

@@ -0,0 +1,157 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "bsp/can.h"
+#include "device/device.h"
+#include <stdint.h>
+#include <stdbool.h>
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Exported constants ------------------------------------------------------- */
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef enum {
+  RC_CAN_SW_ERR = 0,
+  RC_CAN_SW_UP = 1,
+  RC_CAN_SW_MID = 3,
+  RC_CAN_SW_DOWN = 2,
+} RC_CAN_SW_t;
+
+typedef enum {
+    RC_CAN_MODE_MASTER = 0, // 主机模式
+    RC_CAN_MODE_SLAVE = 1, // 从机模式
+} RC_CAN_Mode_t;
+
+typedef enum {
+    RC_CAN_DATA_JOYSTICK = 0,
+    RC_CAN_DATA_SWITCH,
+    RC_CAN_DATA_MOUSE,
+    RC_CAN_DATA_KEYBOARD
+} RC_CAN_DataType_t;
+
+typedef enum {
+  RC_CAN_KEY_NONE = 0xFF,  // 无按键
+  RC_CAN_KEY_W = 0,
+  RC_CAN_KEY_S,
+  RC_CAN_KEY_A,
+  RC_CAN_KEY_D,
+  RC_CAN_KEY_SHIFT,
+  RC_CAN_KEY_CTRL,
+  RC_CAN_KEY_Q,
+  RC_CAN_KEY_E,
+  RC_CAN_KEY_R,
+  RC_CAN_KEY_F,
+  RC_CAN_KEY_G,
+  RC_CAN_KEY_Z,
+  RC_CAN_KEY_X,
+  RC_CAN_KEY_C,
+  RC_CAN_KEY_V,
+  RC_CAN_KEY_B,
+  RC_CAN_KEY_NUM,
+} RC_CAN_Key_t;
+
+// 遥杆数据包
+typedef struct {
+    float ch_l_x;
+    float ch_l_y;
+    float ch_r_x;
+    float ch_r_y;
+} RC_CAN_JoyData_t;
+
+// 拨杆数据包
+typedef struct {
+    RC_CAN_SW_t sw_l;     // 左拨杆状态
+    RC_CAN_SW_t sw_r;     // 右拨杆状态 
+    float ch_res;   // 第五通道
+} RC_CAN_SwitchData_t;
+
+// 鼠标数据包
+typedef struct {
+    float x;  // 鼠标X轴移动
+    float y;  // 鼠标Y轴移动
+    float z;  // 鼠标Z轴(滚轮)
+    bool mouse_l;  // 鼠标左键
+    bool mouse_r;  // 鼠标右键
+} RC_CAN_MouseData_t;
+
+// 键盘数据包
+typedef struct {
+    uint16_t key_value; // 键盘按键位映射
+    RC_CAN_Key_t keys[16];
+} RC_CAN_KeyboardData_t;
+
+
+typedef struct {
+    RC_CAN_JoyData_t joy;
+    RC_CAN_SwitchData_t sw;
+    RC_CAN_MouseData_t mouse;
+    RC_CAN_KeyboardData_t keyboard;
+} RC_CAN_Data_t;
+
+// RC_CAN 参数结构
+typedef struct {
+    BSP_CAN_t can;         // 使用的CAN总线
+    RC_CAN_Mode_t mode;    // 工作模式
+    uint16_t joy_id;      // 遥杆CAN ID
+    uint16_t sw_id;   // 拨杆CAN ID
+    uint16_t mouse_id; // 鼠标CAN ID
+    uint16_t keyboard_id; // 键盘CAN ID
+} RC_CAN_Param_t;
+
+// RC_CAN 主结构
+typedef struct {
+    DEVICE_Header_t header;
+    RC_CAN_Param_t param;
+    RC_CAN_Data_t data;
+} RC_CAN_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 初始化RC CAN发送模块
+ * @param rc_can RC_CAN结构体指针
+ * @param param 初始化参数
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t RC_CAN_Init(RC_CAN_t *rc_can, RC_CAN_Param_t *param);
+
+/**
+ * @brief 更新并发送数据到CAN总线
+ * @param rc_can RC_CAN结构体指针
+ * @param data_type 数据类型
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t RC_CAN_SendData(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type);
+
+/**
+ * @brief 接收并更新CAN数据
+ * @param rc_can RC_CAN结构体指针
+ * @param data_type 数据类型
+ * @return DEVICE_OK 成功，其他值失败
+ */
+int8_t RC_CAN_Update(RC_CAN_t *rc_can , RC_CAN_DataType_t data_type);
+
+int8_t RC_CAN_OFFLINE(RC_CAN_t *rc_can);
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/servo.c

@@ -0,0 +1,47 @@
+/*
+	pwm<EFBFBD><EFBFBD><EFBFBD>ƶ<EFBFBD><EFBFBD>
+*/
+
+/*Includes   -----------------------------------------*/ 
+
+#include "bsp/pwm.h"
+#include "servo.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+#define SERVO_MIN_DUTY  0.025f   
+#define SERVO_MAX_DUTY  0.125f   
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */   
+
+/**
+ * @brief  
+ * @param  
+ * @retval BSP_OK / BSP_ERR
+ */
+
+int8_t SERVO_Init(SERVO_t *servo) {
+    if (servo == NULL) return BSP_ERR;
+    return BSP_PWM_Start(servo->pwm_ch);
+}
+
+int8_t SERVO_SetAngle(SERVO_t *servo, float angle) {
+    if (servo == NULL) return BSP_ERR;
+    
+	  /*<2A><><EFBFBD>ƽǶȷ<C7B6>Χ*/
+    if (angle < 0.0f)   angle = 0.0f;
+    if (angle > 180.0f) angle = 180.0f;
+    /*<2A>Ƕ<EFBFBD>ӳ<EFBFBD>䵽ռ<E4B5BD>ձ<EFBFBD>*/
+    float duty = servo->min_duty + (angle / 180.0f) * (servo->max_duty - servo->min_duty);
+
+    return BSP_PWM_Set(servo->pwm_ch, duty);
+}
+
+int8_t SERVO_Stop(SERVO_t *servo) {
+    if (servo == NULL) return BSP_ERR;
+    return BSP_PWM_Stop(servo->pwm_ch);
+}

assets/User_code/device/servo.h

@@ -0,0 +1,68 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <cmsis_os2.h>
+	
+#include "bsp/pwm.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+	
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+/**
+ * @brief 
+ */
+typedef struct {
+   BSP_PWM_Channel_t pwm_ch;  
+   float min_duty;         
+   float max_duty;      
+} SERVO_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/**
+ * @brief  
+ * @param  servo 
+ * @retval BSP_OK / BSP_ERR
+ */
+
+int8_t SERVO_Init(SERVO_t *servo);
+
+/**
+ * @brief 
+ * @param  servo
+ * @param  angle 
+ * @retval BSP_OK / BSP_ERR
+ */
+int8_t SERVO_SetAngle(SERVO_t *servo, float angle);
+
+/**
+ * @brief 
+ * @param  servo 
+ * @retval BSP_OK / BSP_ERR
+ */
+ 
+int8_t SERVO_Stop(SERVO_t *servo);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+	
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/vofa.c

@@ -0,0 +1,106 @@
+/* Includes ----------------------------------------------------------------- */
+#include <stdio.h>
+#include <string.h>
+#include "device/vofa.h"
+#include "bsp/uart.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private define ----------------------------------------------------------- */
+
+#define MAX_CHANNEL  64u   		// 根据实际最大通道数调整
+
+#define JUSTFLOAT_TAIL 0x7F800000
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private variables -------------------------------------------------------- */
+static uint8_t vofa_tx_buf[sizeof(float) * MAX_CHANNEL + sizeof(uint32_t)];
+static VOFA_Protocol_t current_protocol = VOFA_PROTOCOL_FIREWATER;  // 默认协议
+
+/* Private function  -------------------------------------------------------- */
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+/************************  RawData  *************************/
+void VOFA_RawData_Send(const char* data, bool dma) {
+    BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t*)data, strlen(data), dma);
+}
+
+/************************ FireWater *************************/
+void VOFA_FireWater_Send(float *channels, uint8_t channel_count, bool dma)
+{
+    if (channel_count == 0 || channel_count > MAX_CHANNEL)
+        return;
+
+    char *buf = (char *)vofa_tx_buf;
+    size_t len = 0;
+
+    for (uint8_t i = 0; i < channel_count; ++i) {
+        len += snprintf(buf + len,
+                        sizeof(vofa_tx_buf) - len,
+                        "%s%.2f",
+                        (i ? "," : ""),
+                        channels[i]);
+    }
+    snprintf(buf + len, sizeof(vofa_tx_buf) - len, "\n");
+
+    BSP_UART_Transmit(BSP_UART_VOFA, vofa_tx_buf, strlen(buf), dma);
+}
+
+/************************ JustFloat *************************/
+void VOFA_JustFloat_Send(float *channels, uint8_t channel_count, bool dma)
+{
+    if (channel_count == 0 || channel_count > MAX_CHANNEL)
+        return;
+    memcpy(vofa_tx_buf, channels, channel_count * sizeof(float));
+
+    uint32_t tail = JUSTFLOAT_TAIL;                     // 0x7F800000
+    memcpy(vofa_tx_buf + channel_count * sizeof(float), &tail, sizeof(tail));
+
+    BSP_UART_Transmit(BSP_UART_VOFA, vofa_tx_buf, channel_count * sizeof(float) + sizeof(tail), dma);
+}
+
+/* Exported functions ------------------------------------------------------- */
+int8_t VOFA_init(VOFA_Protocol_t protocol) {
+    current_protocol = protocol;
+    return DEVICE_OK;
+}
+
+int8_t VOFA_Send(float* channels, uint8_t channel_count, bool dma) {
+    switch (current_protocol) {
+        case VOFA_PROTOCOL_RAWDATA:
+            {
+                char data[256];
+                if (channel_count >= 1) {
+                    sprintf(data, "Channel1: %.2f", channels[0]);
+                    if (channel_count >= 2) {
+                        sprintf(data + strlen(data), ", Channel2: %.2f", channels[1]);
+                    }
+                    strcat(data, "\n");
+                    VOFA_RawData_Send(data, dma);
+                }
+            }
+            break;
+        case VOFA_PROTOCOL_FIREWATER:
+            VOFA_FireWater_Send(channels, channel_count, dma);
+            break;
+        case VOFA_PROTOCOL_JUSTFLOAT:
+            VOFA_JustFloat_Send(channels, channel_count, dma);
+            break;
+        default:
+            return DEVICE_ERR;
+    }
+    return DEVICE_OK;
+}

assets/User_code/device/vofa.h

@@ -0,0 +1,39 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "bsp/uart.h"
+#include "device/device.h"
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+/* Exported functions prototypes -------------------------------------------- */
+
+typedef enum {
+    VOFA_PROTOCOL_RAWDATA,
+    VOFA_PROTOCOL_FIREWATER,
+    VOFA_PROTOCOL_JUSTFLOAT,
+} VOFA_Protocol_t;
+
+/**
+ * @brief 初始化VOFA设备
+ * @param protocol 设置通信协议
+ * @return 
+ */
+int8_t VOFA_init(VOFA_Protocol_t protocol);
+
+/**
+ * @brief 发送数据到VOFA
+ * @param channels 要发送的通道数据
+ * @param channel_count 通道数量
+ * @param dma 是否使用DMA发送
+ * @return 
+ */
+int8_t VOFA_Send(float* channels, uint8_t channel_count, bool dma);
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/device/ws2812.c

@@ -0,0 +1,114 @@
+/* Includes ----------------------------------------------------------------- */
+#include "ws2812.h"
+#include "device.h"
+
+#include "bsp/pwm.h"
+#include <stdlib.h>
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private define ----------------------------------------------------------- */
+#define DEVICE_WS2812_T1H            (uint16_t)(BSP_PWM_GetAutoReloadPreload(BSP_PWM_WS2812) * 0.56)   // High-level width of logic-1 pulse
+#define DEVICE_WS2812_T0H            (BSP_PWM_GetAutoReloadPreload(BSP_PWM_WS2812) * 0.29)             // High-level width of logic-0 pulse
+#define DEVICE_WS2812_WS_REST        40   // Number of reset pulses (low level) after data stream
+#define DEVICE_WS2812_DATA_LEN       24   // WS2812 data length: 24 bits (GRB) per LED
+#define DEVICE_WS2812_RST_NUM        50   // Extra reset pulses reserved at the end of the buffer
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private variables -------------------------------------------------------- */
+static uint16_t DEVICE_WS2812_LED_NUM; 	       // Total number of LEDs
+static uint16_t *DEVICE_WS2812_RGB_Buff = NULL;// PWM duty buffer for DMA
+/* Private function  -------------------------------------------------------- */
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+/* Exported functions ------------------------------------------------------- */
+/**
+ * Set color of a single WS2812 LED
+ * @param num LED index (1-based)
+ * @param R   Red value   (0-255)
+ * @param G   Green value (0-255)
+ * @param B   Blue value  (0-255)
+ * @return    DEVICE_OK on success, DEVICE_ERR if num is invalid
+ */
+uint8_t DEVICE_WS2812_Set(uint16_t num, uint8_t R, uint8_t G, uint8_t B)
+{
+  if(num<1 || num>DEVICE_WS2812_LED_NUM) return DEVICE_ERR;
+  uint32_t indexx = (num-1) * DEVICE_WS2812_DATA_LEN;
+  
+  /* WS2812 uses GRB order, MSB first */
+  for (uint8_t i = 0; i < 8; i++) {
+    // G
+    DEVICE_WS2812_RGB_Buff[indexx + i]      = (G & (0x80 >> i)) ? DEVICE_WS2812_T1H : DEVICE_WS2812_T0H;
+    // R
+    DEVICE_WS2812_RGB_Buff[indexx + i + 8]  = (R & (0x80 >> i)) ? DEVICE_WS2812_T1H : DEVICE_WS2812_T0H;
+    // B
+    DEVICE_WS2812_RGB_Buff[indexx + i + 16] = (B & (0x80 >> i)) ? DEVICE_WS2812_T1H : DEVICE_WS2812_T0H;
+  }
+  return DEVICE_OK;
+}
+
+/**
+ * Initialize WS2812 driver
+ * @param ledNum Number of LEDs in the strip
+ * @return DEVICE_OK on success, DEVICE_ERR if memory allocation or PWM setup fails
+ */
+uint8_t DEVICE_WS2812_Init(uint16_t ledNum)
+{
+	DEVICE_WS2812_LED_NUM = ledNum;
+
+    if (DEVICE_WS2812_RGB_Buff != NULL)
+    {
+        free(DEVICE_WS2812_RGB_Buff);
+        DEVICE_WS2812_RGB_Buff = NULL;
+    }
+
+    /* Allocate new buffer: 24 PWM samples per LED + reset pulses */
+    size_t bufLen = ledNum * DEVICE_WS2812_DATA_LEN + DEVICE_WS2812_RST_NUM;
+    DEVICE_WS2812_RGB_Buff = (uint16_t *)malloc(bufLen * sizeof(uint16_t));
+    if (DEVICE_WS2812_RGB_Buff == NULL)
+        return DEVICE_ERR;              
+
+    /* Initialize all LEDs to dim green */
+    for (int i = 1; i <= ledNum; i++)
+        DEVICE_WS2812_Set(i, 0, 20, 0);
+	
+	/* Configure PWM frequency to 800 kHz and start DMA */
+    if (BSP_PWM_SetFreq(BSP_PWM_WS2812, 800000) == DEVICE_OK)
+        BSP_PWM_Start_DMA(
+            BSP_PWM_WS2812,
+            (uint32_t *)DEVICE_WS2812_RGB_Buff,
+            bufLen);
+    else
+        return DEVICE_ERR;
+
+    return DEVICE_OK;
+}
+
+/**
+ * De-initialize WS2812 driver
+ * Frees the DMA buffer and stops PWM
+ */
+void DEVICE_WS2812_DeInit()
+{
+	for (int i = 1; i <= DEVICE_WS2812_LED_NUM; i++)
+        DEVICE_WS2812_Set(i, 0, 0, 0);
+	if (DEVICE_WS2812_RGB_Buff != NULL)
+    {
+        free(DEVICE_WS2812_RGB_Buff);
+        DEVICE_WS2812_RGB_Buff = NULL;
+    }
+	BSP_PWM_Stop_DMA(BSP_PWM_WS2812);
+}

assets/User_code/device/ws2812.h

@@ -0,0 +1,19 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdint.h>
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+/* Exported functions prototypes -------------------------------------------- */
+uint8_t DEVICE_WS2812_Init(uint16_t led_num);
+uint8_t DEVICE_WS2812_Set(uint16_t num, uint8_t R, uint8_t G, uint8_t B);
+void DEVICE_WS2812_DeInit();
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/module/config.c

@@ -0,0 +1,31 @@
+/*
+ * 配置相关
+ */
+
+/* Includes ----------------------------------------------------------------- */
+#include "module/config.h"
+
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+
+/* Exported variables ------------------------------------------------------- */
+
+// 机器人参数配置
+Config_RobotParam_t robot_config = {
+
+
+};
+
+/* Private function prototypes ---------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * @brief 获取机器人配置参数
+ * @return 机器人配置参数指针
+ */
+Config_RobotParam_t* Config_GetRobotParam(void) {
+    return &robot_config;
+}

assets/User_code/module/config.h

@@ -0,0 +1,27 @@
+/*
+ * 配置相关
+ */
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+typedef struct {
+    
+} Config_RobotParam_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 获取机器人配置参数
+ * @return 机器人配置参数指针
+ */
+Config_RobotParam_t* Config_GetRobotParam(void);
+
+#ifdef __cplusplus
+}
+#endif

assets/User_code/module/gimbal/2_axis_gimbal/2_axis_gimbal.c

@@ -0,0 +1,259 @@
+/*
+ * 云台模组
+ */
+
+/* Includes ----------------------------------------------------------------- */
+#include "bsp/can.h"
+#include "bsp/time.h"
+#include "component/filter.h"
+#include "component/pid.h"
+#include "device/motor_dm.h"
+#include "device/motor_rm.h"
+#include "gimbal.h"
+#include <math.h>
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* Private function  -------------------------------------------------------- */
+
+/**
+ * \brief 设置云台模式
+ *
+ * \param c 包含云台数据的结构体
+ * \param mode 要设置的模式
+ *
+ * \return 函数运行结果
+ */
+static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
+  if (g == NULL)
+    return -1;
+  if (mode == g->mode)
+    return GIMBAL_OK;
+
+  PID_Reset(&g->pid.yaw_angle);
+  PID_Reset(&g->pid.yaw_omega);
+  PID_Reset(&g->pid.pit_angle);
+  PID_Reset(&g->pid.pit_omega);
+  LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f);
+  LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f);
+
+  MOTOR_DM_Enable(&(g->param->yaw_motor));
+
+  AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
+  // if (g->mode == GIMBAL_MODE_RELAX) {
+  //   if (mode == GIMBAL_MODE_ABSOLUTE) {
+  //     g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
+  //   } else if (mode == GIMBAL_MODE_RELATIVE) {
+  //     g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
+  //   }
+  // }
+  g->setpoint.eulr.pit = g->feedback.imu.eulr.rol;
+  g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
+
+  g->mode = mode;
+  return 0;
+}
+
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * \brief 初始化云台
+ *
+ * \param g 包含云台数据的结构体
+ * \param param 包含云台参数的结构体指针
+ * \param target_freq 任务预期的运行频率
+ *
+ * \return 函数运行结果
+ */
+int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param,
+                   float target_freq) {
+  if (g == NULL)
+    return -1;
+
+  g->param = param;
+  g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 */
+
+  /* 初始化云台电机控制PID和LPF */
+  PID_Init(&(g->pid.yaw_angle), KPID_MODE_NO_D, target_freq,
+           &(g->param->pid.yaw_angle));
+  PID_Init(&(g->pid.yaw_omega), KPID_MODE_CALC_D, target_freq,
+           &(g->param->pid.yaw_omega));
+  PID_Init(&(g->pid.pit_angle), KPID_MODE_NO_D, target_freq,
+           &(g->param->pid.pit_angle));
+  PID_Init(&(g->pid.pit_omega), KPID_MODE_CALC_D, target_freq,
+           &(g->param->pid.pit_omega));
+
+  LowPassFilter2p_Init(&g->filter_out.yaw, target_freq,
+                       g->param->low_pass_cutoff_freq.out);
+  LowPassFilter2p_Init(&g->filter_out.pit, target_freq,
+                       g->param->low_pass_cutoff_freq.out);
+  g->limit.yaw.max = g->param->mech_zero.yaw + g->param->travel.yaw;
+  g->limit.yaw.min = g->param->mech_zero.yaw;
+  g->limit.pit.max = g->param->mech_zero.pit + g->param->travel.pit;
+  g->limit.pit.min = g->param->mech_zero.pit;
+  BSP_CAN_Init();
+
+  MOTOR_RM_Register(&(g->param->pit_motor));
+  MOTOR_DM_Register(&(g->param->yaw_motor));
+
+  MOTOR_DM_Enable(&(g->param->yaw_motor));
+  return 0;
+}
+
+/**
+ * \brief 通过CAN设备更新云台反馈信息
+ *
+ * \param gimbal 云台
+ * \param can CAN设备
+ *
+ * \return 函数运行结果
+ */
+int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) {
+  if (gimbal == NULL)
+    return -1;
+
+  /* 更新RM电机反馈数据（pitch轴） */
+  MOTOR_RM_Update(&(gimbal->param->pit_motor));
+  MOTOR_RM_t *rm_motor = MOTOR_RM_GetMotor(&(gimbal->param->pit_motor));
+  if (rm_motor != NULL) {
+    gimbal->feedback.motor.pit = rm_motor->feedback;
+  }
+
+  /* 更新DM电机反馈数据（yaw轴） */
+  MOTOR_DM_Update(&(gimbal->param->yaw_motor));
+  MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->yaw_motor));
+  if (dm_motor != NULL) {
+    gimbal->feedback.motor.yaw = dm_motor->motor.feedback;
+  }
+
+  return 0;
+}
+
+int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu) {
+
+  if (gimbal == NULL) {
+    return -1;
+  }
+  gimbal->feedback.imu.gyro = imu->gyro;
+  gimbal->feedback.imu.eulr = imu->eulr;
+}
+
+/**
+ * \brief 运行云台控制逻辑
+ *
+ * \param g 包含云台数据的结构体
+ * \param g_cmd 云台控制指令
+ *
+ * \return 函数运行结果
+ */
+int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
+  if (g == NULL || g_cmd == NULL) {
+    return -1;
+  }
+
+  g->dt = (BSP_TIME_Get_us() - g->lask_wakeup) / 1000000.0f;
+  g->lask_wakeup = BSP_TIME_Get_us();
+
+  Gimbal_SetMode(g, g_cmd->mode);
+
+  /* 处理yaw控制命令，软件限位 - 使用电机绝对角度 */
+  float delta_yaw = g_cmd->delta_yaw * g->dt * 1.5f;
+  if (g->param->travel.yaw > 0) {
+    /* 计算当前电机角度与IMU角度的偏差 */
+    float motor_imu_offset =
+        g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw;
+    /* 处理跨越±π的情况 */
+    if (motor_imu_offset > M_PI)
+      motor_imu_offset -= M_2PI;
+    if (motor_imu_offset < -M_PI)
+      motor_imu_offset += M_2PI;
+
+    /* 计算到限位边界的距离 */
+    const float delta_max = CircleError(
+        g->limit.yaw.max, (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw),
+        M_2PI);
+    const float delta_min = CircleError(
+        g->limit.yaw.min, (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw),
+        M_2PI);
+
+    /* 限制控制命令 */
+    if (delta_yaw > delta_max)
+      delta_yaw = delta_max;
+    if (delta_yaw < delta_min)
+      delta_yaw = delta_min;
+  }
+  CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw, M_2PI);
+
+  /* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */
+  float delta_pit = g_cmd->delta_pit * g->dt;
+  if (g->param->travel.pit > 0) {
+    /* 计算当前电机角度与IMU角度的偏差 */
+    float motor_imu_offset =
+        g->feedback.motor.pit.rotor_abs_angle - g->feedback.imu.eulr.rol;
+    /* 处理跨越±π的情况 */
+    if (motor_imu_offset > M_PI)
+      motor_imu_offset -= M_2PI;
+    if (motor_imu_offset < -M_PI)
+      motor_imu_offset += M_2PI;
+
+    /* 计算到限位边界的距离 */
+    const float delta_max = CircleError(
+        g->limit.pit.max, (g->setpoint.eulr.pit + motor_imu_offset + delta_pit),
+        M_2PI);
+    const float delta_min = CircleError(
+        g->limit.pit.min, (g->setpoint.eulr.pit + motor_imu_offset + delta_pit),
+        M_2PI);
+
+    /* 限制控制命令 */
+    if (delta_pit > delta_max)
+      delta_pit = delta_max;
+    if (delta_pit < delta_min)
+      delta_pit = delta_min;
+  }
+  CircleAdd(&(g->setpoint.eulr.pit), delta_pit, M_2PI);
+
+  /* 控制相关逻辑 */
+  float yaw_omega_set_point, pit_omega_set_point;
+  switch (g->mode) {
+  case GIMBAL_MODE_RELAX:
+    g->out.yaw = 0.0f;
+    g->out.pit = 0.0f;
+    break;
+
+  case GIMBAL_MODE_ABSOLUTE:
+    yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
+                                   g->feedback.imu.eulr.yaw, 0.0f, g->dt);
+    g->out.yaw = PID_Calc(&(g->pid.pit_omega), yaw_omega_set_point,
+                          g->feedback.imu.gyro.z, 0.f, g->dt);
+
+    pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
+                                   g->feedback.imu.eulr.rol, 0.0f, g->dt);
+    g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
+                          g->feedback.imu.gyro.y, 0.f, g->dt);
+
+    break;
+
+    /* 输出滤波 */
+    g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw);
+    g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit);
+
+    return 0;
+  }
+}
+
+/**
+ * \brief 云台输出
+ *
+ * \param s 包含云台数据的结构体
+ * \param out CAN设备云台输出结构体
+ */
+void Gimbal_Output(Gimbal_t *g) {
+  MOTOR_RM_SetOutput(&g->param->pit_motor, g->out.pit);
+  MOTOR_MIT_Output_t output = {0};
+  output.torque = g->out.yaw * 5.0f; // 乘以减速比
+  output.kd = 0.3f;
+  MOTOR_RM_Ctrl(&g->param->pit_motor);
+  MOTOR_DM_MITCtrl(&g->param->yaw_motor, &output);
+}