添加启动页

.gitignore

@@ -28,3 +28,7 @@ Examples/
 !*.axf
 !*.bin
 !*.hex
+
+build/
+dist/
+*.spec

MRobot.py

@@ -1,5 +1,6 @@
 import tkinter as tk
 from tkinter import ttk
+from PIL import Image, ImageTk
 import sys
 import os
 import threading
@@ -13,7 +14,11 @@ import xml.etree.ElementTree as ET
 # 配置常量
 REPO_DIR = "MRobot_repo"
 REPO_URL = "http://gitea.qutrobot.top/robofish/MRobot.git"
-CURRENT_DIR = os.path.dirname(os.path.abspath(__file__))
+if getattr(sys, 'frozen', False):  # 检查是否为打包后的环境
+    CURRENT_DIR = os.path.dirname(sys.executable)  # 使用可执行文件所在目录
+else:
+    CURRENT_DIR = os.path.dirname(os.path.abspath(__file__))  # 使用脚本所在目录
+
 MDK_ARM_DIR = os.path.join(CURRENT_DIR, "MDK-ARM")
 USER_DIR = os.path.join(CURRENT_DIR, "User")
 
@@ -53,122 +58,6 @@ class MRobotApp:
             print(f"删除仓库目录时出错: {e}")
 
 
-    def auto_configure_environment(self):
-        """自动配置环境"""
-        try:
-            self.add_groups_and_files()
-            self.add_include_path(r"..\User")
-            print("环境配置完成！")
-        except Exception as e:
-            print(f"自动配置环境时出错: {e}")
-
-
-    def log(self, message):
-        """统一日志输出"""
-        print(message)
-
-
-    def find_uvprojx_file(self):
-        """查找 MDK-ARM 文件夹中的 .uvprojx 文件"""
-        if not os.path.exists(MDK_ARM_DIR):
-            self.log(f"未找到 MDK-ARM 文件夹：{MDK_ARM_DIR}")
-            return None
-
-        uvprojx_files = [f for f in os.listdir(MDK_ARM_DIR) if f.endswith(".uvprojx")]
-        if not uvprojx_files:
-            self.log(f"在 {MDK_ARM_DIR} 中未找到任何 .uvprojx 文件！")
-            return None
-
-        project_file = os.path.join(MDK_ARM_DIR, uvprojx_files[0])
-        self.log(f"找到项目文件：{project_file}")
-        return project_file
-
-
-    def add_groups_and_files(self):
-        """添加 User 文件夹中的组和文件到 Keil 项目"""
-        project_file = self.find_uvprojx_file()
-        if not project_file:
-            return
-
-        tree = ET.parse(project_file)
-        root = tree.getroot()
-        groups_node = root.find(".//Groups")
-        if groups_node is None:
-            self.log("未找到 Groups 节点！")
-            return
-
-        existing_groups = {group.find("GroupName").text for group in groups_node.findall("Group")}
-        existing_files = {
-            file.text
-            for group in groups_node.findall("Group")
-            for file in group.findall(".//FileName")
-        }
-
-        for folder_name in os.listdir(USER_DIR):
-            folder_path = os.path.join(USER_DIR, folder_name)
-            if not os.path.isdir(folder_path):
-                continue
-
-            group_name = f"User/{folder_name}"
-            if group_name in existing_groups:
-                self.log(f"组 {group_name} 已存在，跳过...")
-                continue
-
-            group_node = ET.SubElement(groups_node, "Group")
-            ET.SubElement(group_node, "GroupName").text = group_name
-            files_node = ET.SubElement(group_node, "Files")
-
-            for file_name in os.listdir(folder_path):
-                file_path = os.path.join(folder_path, file_name)
-                if not os.path.isfile(file_path) or file_name in existing_files:
-                    self.log(f"文件 {file_name} 已存在或无效，跳过...")
-                    continue
-
-                file_node = ET.SubElement(files_node, "File")
-                ET.SubElement(file_node, "FileName").text = file_name
-                ET.SubElement(file_node, "FileType").text = "1"
-                relative_path = os.path.relpath(file_path, os.path.dirname(project_file)).replace("\\", "/")
-                ET.SubElement(file_node, "FilePath").text = relative_path
-
-        tree.write(project_file, encoding="utf-8", xml_declaration=True)
-        self.log("Keil 项目文件已更新！")
-
-    def add_include_path(self, new_path):
-        """添加新的 IncludePath 到 Keil 项目"""
-        project_file = self.find_uvprojx_file()
-        if not project_file:
-            return
-
-        tree = ET.parse(project_file)
-        root = tree.getroot()
-        include_path_nodes = root.findall(".//IncludePath")
-        if not include_path_nodes:
-            self.log("未找到任何 IncludePath 节点，无法添加路径。")
-            return
-
-        updated = False
-        for index, include_path_node in enumerate(include_path_nodes):
-            if index == 0:
-                self.log("跳过第一组 IncludePath 节点，不进行修改。")
-                continue
-
-            include_paths = include_path_node.text.split(";") if include_path_node.text else []
-            if new_path in include_paths:
-                self.log(f"路径 '{new_path}' 已存在于第 {index + 1} 组 IncludePath 节点中，无需重复添加。")
-                continue
-
-            include_paths.append(new_path)
-            include_path_node.text = ";".join(include_paths)
-            updated = True
-            self.log(f"路径 '{new_path}' 已成功添加到第 {index + 1} 组 IncludePath 节点中。")
-
-        if updated:
-            tree.write(project_file, encoding="utf-8", xml_declaration=True)
-            self.log(f"项目文件已更新：{project_file}")
-        else:
-            self.log("未对项目文件进行任何修改。")
-
-
     # 复制文件
     def copy_file_from_repo(self, src_path, dest_path):
         try:
@@ -295,87 +184,123 @@ class MRobotApp:
 
 
 
+    # 显示主窗口
+    # ...existing code...
+    # ...existing code...
+    
     # 显示主窗口
     def show_main_window(self):
         root = tk.Tk()
         root.title("MRobot 自动生成脚本")
-        root.geometry("800x600")  # 调整窗口大小以适应布局
+        root.geometry("1000x650")  # 调整窗口大小以适应布局
-
+    
         # 在窗口关闭时调用 on_closing 方法
         root.protocol("WM_DELETE_WINDOW", lambda: self.on_closing(root))
-
+    
         # 初始化 BooleanVar
         self.add_gitignore_var = tk.BooleanVar(value=False)
         self.auto_configure_var = tk.BooleanVar(value=False)  # 新增复选框变量
-
+    
         # 创建主框架
         main_frame = ttk.Frame(root)
         main_frame.pack(fill="both", expand=True)
-
+    
         # 添加标题
         title_label = ttk.Label(main_frame, text="MRobot 自动生成脚本", font=("Arial", 16, "bold"))
         title_label.pack(pady=10)
-
+    
         # 添加 FreeRTOS 状态标签
         freertos_status_label = ttk.Label(main_frame, text="FreeRTOS 状态: 检测中...", font=("Arial", 12))
         freertos_status_label.pack(pady=10)
         self.update_freertos_status(freertos_status_label)
-
+    
-        # 模块文件选择和任务管理框架
+        # 模块文件选择和任务管理框架（添加滚动功能）
         module_task_frame = ttk.Frame(main_frame)
         module_task_frame.pack(fill="both", expand=True, padx=10, pady=10)
-
+    
+        # 创建 Canvas 和 Scrollbar
+        canvas = tk.Canvas(module_task_frame)
+        scrollbar = ttk.Scrollbar(module_task_frame, orient="vertical", command=canvas.yview)
+        scrollable_frame = ttk.Frame(canvas)
+    
+        # 配置滚动区域
+        scrollable_frame.bind(
+            "<Configure>",
+            lambda e: canvas.configure(scrollregion=canvas.bbox("all"))
+        )
+        canvas.create_window((0, 0), window=scrollable_frame, anchor="nw")
+        canvas.configure(yscrollcommand=scrollbar.set)
+    
+        # 绑定鼠标滚轮事件
+        def on_mouse_wheel(event):
+            canvas.yview_scroll(-1 * int(event.delta / 120), "units")
+    
+        canvas.bind_all("<MouseWheel>", on_mouse_wheel)
+    
+        # 布局 Canvas 和 Scrollbar
+        canvas.pack(side="left", fill="both", expand=True)
+        scrollbar.pack(side="right", fill="y")
+    
+        # 左右布局：模块文件选择框和任务管理框
+        left_frame = ttk.Frame(scrollable_frame)
+        left_frame.pack(side="left", fill="both", expand=True, padx=5, pady=5)
+    
+        right_frame = ttk.Frame(scrollable_frame)
+        right_frame.pack(side="right", fill="both", expand=True, padx=5, pady=5)
+    
         # 模块文件选择框
-        header_files_frame = ttk.LabelFrame(module_task_frame, text="模块文件选择", padding=(10, 10))
+        header_files_frame = ttk.LabelFrame(left_frame, text="模块文件选择", padding=(10, 10))
-        header_files_frame.pack(side="left", fill="both", expand=True, padx=5)
+        header_files_frame.pack(fill="both", expand=True, padx=5)
         self.header_files_frame = header_files_frame
         self.update_header_files()
-
+    
+        # 任务管理框
         if self.ioc_data and self.check_freertos_enabled(self.ioc_data):
-            task_frame = ttk.LabelFrame(module_task_frame, text="任务管理", padding=(10, 10))
+            task_frame = ttk.LabelFrame(right_frame, text="任务管理", padding=(10, 10))
-            task_frame.pack(side="left", fill="both", expand=True, padx=5)
+            task_frame.pack(fill="both", expand=True, padx=5)
             self.task_frame = task_frame
             self.update_task_ui()
-
+    
         # 添加消息框和生成按钮在同一行
         bottom_frame = ttk.Frame(main_frame)
         bottom_frame.pack(fill="x", pady=10, side="bottom")
-
+    
         # 消息框
         self.message_box = tk.Text(bottom_frame, wrap="word", state="disabled", height=5, width=60)
         self.message_box.pack(side="left", fill="x", expand=True, padx=5, pady=5)
-
+    
         # 生成按钮和复选框选项
         button_frame = ttk.Frame(bottom_frame)
         button_frame.pack(side="right", padx=10)
-
+    
         # 添加复选框容器（横向排列复选框）
         checkbox_frame = ttk.Frame(button_frame)
         checkbox_frame.pack(side="top", pady=5)
-
+    
         # 添加 .gitignore 复选框（左侧）
         ttk.Checkbutton(checkbox_frame, text=".gitignore", variable=self.add_gitignore_var).pack(side="left", padx=5)
-
+    
         # 添加自动配置环境复选框（右侧）
         ttk.Checkbutton(checkbox_frame, text="自动环境", variable=self.auto_configure_var).pack(side="left", padx=5)
-
+    
         # 添加生成按钮（竖向排列在复选框下方）
         generate_button = ttk.Button(button_frame, text="一键生成MRobot代码", command=self.generate_action)
         generate_button.pack(side="top", pady=10)
         generate_button.config(width=25)  # 设置按钮宽度
-
+    
         # 重定向输出到消息框
         self.redirect_output()
-
+    
         # 打印欢迎信息
         print("欢迎使用 MRobot 自动生成脚本！")
         print("请根据需要选择模块文件和任务。")
         print("点击“一键生成MRobot代码”按钮开始生成。")
-
+    
         # 启动 Tkinter 主事件循环
         root.mainloop()
-
+    
-
+    # ...existing code...
+    # ...existing code...
 
     def redirect_output(self):
         """
@@ -774,10 +699,10 @@ class MRobotApp:
                 # 生成 task.c 文件
                 self.generate_task_files()
             
-            # 自动配置环境
+            # # 自动配置环境
-            if self.auto_configure_var.get():
+            # if self.auto_configure_var.get():
                 
-                self.auto_configure_environment()
+            #     self.auto_configure_environment()
 
 
         threading.Thread(target=task).start()

MRobot_repo

@@ -0,0 +1 @@
+Subproject commit c4731883f241d4748d45ed168520a345a042326e

README.md

@@ -3,7 +3,7 @@
 更加高效快捷的 STM32 开发架构，诞生于 Robocon 和 Robomaster，但绝不仅限于此。
 
 <div align="center">
-  <img src="./image/MRobot.jpeg" height="100" alt="MRobot Logo">
+  <img src="./img/MRobot.png" height="100" alt="MRobot Logo">
   <p>是时候使用更简洁的方式开发单片机了</p>
   <p>
     <!-- <img src="https://img.shields.io/github/license/xrobot-org/XRobot.svg" alt="License">
@@ -87,4 +87,7 @@
 使用以下命令构建可执行文件：
 
 ```bash
-pyinstaller --onefile --windowed 
+pyinstaller --onefile --windowed 
+pyinstaller MR_Toolbox.py --onefile --noconsole --icon=img\M.ico --add-data "mr_tool_img\MRobot.png;mr_tool_img"
+
+pyinstaller MR_Tool.py --onefile --noconsole --icon=img\M.ico --add-data "mr_tool_img\MRobot.png;mr_tool_img" --add-data "src;src" --add-data "User;User"

User/bsp/can.c

@@ -0,0 +1,141 @@
+/* Includes ----------------------------------------------------------------- */
+#include "bsp\can.h"
+
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+static void (*CAN_Callback[BSP_CAN_NUM][BSP_CAN_CB_NUM])(void);
+
+/* Private function  -------------------------------------------------------- */
+static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan) {
+  if (hcan->Instance == CAN2)
+    return BSP_CAN_2;
+  else if (hcan->Instance == CAN1)
+    return BSP_CAN_1;
+  else
+    return BSP_CAN_ERR;
+}
+
+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 ------------------------------------------------------- */
+CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can) {
+  switch (can) {
+    case BSP_CAN_2:
+      return &hcan2;
+    case BSP_CAN_1:
+      return &hcan1;
+    default:
+      return NULL;
+  }
+}
+
+int8_t BSP_CAN_RegisterCallback(BSP_CAN_t can, BSP_CAN_Callback_t type,
+                                void (*callback)(void)) {
+  if (callback == NULL) return BSP_ERR_NULL;
+  CAN_Callback[can][type] = callback;
+  return BSP_OK;
+}

User/bsp/can.h

@@ -0,0 +1,46 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <can.h>
+
+#include "bsp/bsp.h"
+
+/* Exported constants ------------------------------------------------------- */
+/* 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;
+
+/* Exported functions prototypes -------------------------------------------- */
+CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can);
+int8_t BSP_CAN_RegisterCallback(BSP_CAN_t can, BSP_CAN_Callback_t type,
+                                void (*callback)(void));
+
+#ifdef __cplusplus
+}
+#endif

User/bsp/delay.c

@@ -0,0 +1,118 @@
+#include "bsp_delay.h"
+
+#include "cmsis_os.h"
+#include "main.h"
+
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+static uint8_t fac_us = 0;
+static uint32_t fac_ms = 0;
+/* Private function  -------------------------------------------------------- */
+static void delay_ticks(uint32_t ticks)
+{
+    uint32_t told = SysTick->VAL;
+    uint32_t tnow = 0;
+    uint32_t tcnt = 0;
+    uint32_t reload = SysTick->LOAD;
+    while (1)
+    {
+        tnow = SysTick->VAL;
+        if (tnow != told)
+        {
+            if (tnow < told)
+            {
+                tcnt += told - tnow;
+            }
+            else
+            {
+                tcnt += reload - tnow + told;
+            }
+            told = tnow;
+            if (tcnt >= ticks)
+            {
+                break;
+            }
+        }
+    }
+}
+/* Exported functions ------------------------------------------------------- */
+
+/**
+ * @brief 毫秒延时函数
+ * @param ms
+ * @return
+ */
+int8_t BSP_Delay(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;
+}
+
+/**
+ * @brief 延时初始化
+ * @param  
+ * @return 
+ */
+int8_t BSP_Delay_Init(void)
+{
+    if (SystemCoreClock == 0)
+    {
+        return BSP_ERR;
+    }
+    fac_us = SystemCoreClock / 1000000;
+    fac_ms = SystemCoreClock / 1000;
+    return BSP_OK;
+}
+
+/**
+ * @brief 微秒延时，注意：此函数会阻塞当前线程，直到延时结束，并且会被中断打断
+ * @param us 
+ * @return 
+ */
+int8_t BSP_Delay_us(uint32_t us)
+{
+    if (fac_us == 0)
+    {
+        return BSP_ERR;
+    }
+    uint32_t ticks = us * fac_us;
+    delay_ticks(ticks);
+    return BSP_OK;
+}
+
+/**
+ * @brief 毫秒延时，注意：此函数会阻塞当前线程，直到延时结束，并且会被中断打断
+ * @param ms 
+ * @return 
+ */
+int8_t BSP_Delay_ms(uint32_t ms)
+{
+    if (fac_ms == 0)
+    {
+        return BSP_ERR;
+    }
+    uint32_t ticks = ms * fac_ms;
+    delay_ticks(ticks);
+    return BSP_OK;
+}

User/bsp/delay.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdint.h>
+
+#include "bsp/bsp.h"
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+/* Exported functions prototypes -------------------------------------------- */
+int8_t BSP_Delay(uint32_t ms);
+
+int8_t BSP_Delay_Init(void);
+int8_t BSP_Delay_us(uint32_t us);
+int8_t BSP_Delay_ms(uint32_t ms);
+
+#ifdef __cplusplus
+}
+#endif

User/bsp/gpio_exti.c

@@ -0,0 +1,72 @@
+/* Includes ----------------------------------------------------------------- */
+#include "bsp\gpio.h"
+
+#include <gpio.h>
+#include <main.h>
+
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+static void (*GPIO_Callback[BSP_GPIO_NUM][BSP_GPIO_CB_NUM])(void);
+
+/* Private function  -------------------------------------------------------- */
+static BSP_GPIO_t GPIO_Get(uint16_t pin) {
+  switch (pin) {
+    case USER_KEY_Pin:
+      return BSP_GPIO_USER_KEY;
+    /* case XXX_Pin:
+      return BSP_GPIO_XXX; */
+    default:
+      return BSP_GPIO_ERR;
+  }
+}
+
+void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
+  BSP_GPIO_t gpio = GPIO_Get(GPIO_Pin);
+  if (gpio != BSP_GPIO_ERR) {
+    if (GPIO_Callback[gpio][BSP_GPIO_EXTI_CB]) {
+      GPIO_Callback[gpio][BSP_GPIO_EXTI_CB]();
+    }
+  }
+}
+
+/* Exported functions ------------------------------------------------------- */
+int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, BSP_GPIO_Callback_t type, void (*callback)(void)) {
+  if (callback == NULL || gpio >= BSP_GPIO_NUM || type >= BSP_GPIO_CB_NUM) return BSP_ERR_NULL;
+
+  GPIO_Callback[gpio][type] = callback;
+  return BSP_OK;
+}
+
+int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) {
+  switch (gpio) {
+    case BSP_GPIO_USER_KEY:
+      HAL_NVIC_EnableIRQ(USER_KEY_EXTI_IRQn);
+      break;
+
+    /* case BSP_GPIO_XXX:
+      HAL_NVIC_EnableIRQ(XXX_IRQn);
+      break; */
+
+    default:
+      return BSP_ERR;
+  }
+  return BSP_OK;
+}
+
+int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) {
+  switch (gpio) {
+    case BSP_GPIO_USER_KEY:
+      HAL_NVIC_DisableIRQ(USER_KEY_EXTI_IRQn);
+      break;
+
+    /* case BSP_GPIO_XXX:
+      HAL_NVIC_DisableIRQ(XXX_IRQn);
+      break; */
+
+    default:
+      return BSP_ERR;
+  }
+  return BSP_OK;
+}

User/bsp/gpio_exti.h

@@ -0,0 +1,37 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdint.h>
+
+#include "bsp/bsp.h"
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+/* GPIO设备枚举，与设备对应 */
+typedef enum {
+  BSP_GPIO_USER_KEY,
+  /* BSP_GPIO_XXX, */
+  BSP_GPIO_NUM,
+  BSP_GPIO_ERR,
+} BSP_GPIO_t;
+
+/* GPIO支持的中断回调函数类型 */
+typedef enum {
+  BSP_GPIO_EXTI_CB,
+  BSP_GPIO_CB_NUM,
+} BSP_GPIO_Callback_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, BSP_GPIO_Callback_t type, void (*callback)(void));
+int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio);
+int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio);
+
+#ifdef __cplusplus
+}
+#endif

User/bsp/key_gpio.c

@@ -1,48 +0,0 @@
-/* Includes ----------------------------------------------------------------- */
-#include "key_gpio.h"
-#include "bsp.h"
-#include <gpio.h>
-
-/* Private define ----------------------------------------------------------- */
-/* Private macro ------------------------------------------------------------ */
-/* Private typedef ---------------------------------------------------------- */
-/* Private variables -------------------------------------------------------- */
-
-static uint32_t key_stats; // 使用位掩码记录每个通道的状态，最多支持32LED
-/* 按键配置表（根据实际硬件修改） */
-static const BSP_Key_Config_t KEY_CONFIGS[] = {
-    {GPIOA, GPIO_PIN_7, GPIO_PIN_SET},   // KEY1按下时电平为高
-    {GPIOA, GPIO_PIN_9, GPIO_PIN_SET}, // KEY2按下时电平为低
-    // 添加更多按键...
-};
-
-#define KEY_COUNT (sizeof(KEY_CONFIGS)/sizeof(KEY_CONFIGS[0])
-	
-
-//读取按键状态（带消抖）
-int8_t BSP_Key_Read(BSP_Key_Channel_t ch) {
-    static uint32_t last_press_time[BSP_KEY_COUNT] = {0};      //上次按下时间
-    const uint32_t debounce_ms = 20;      //按键消抖时间
-    const uint32_t long_press_ms = 2000;      //按键长按时间
-    
-    if(ch >= BSP_KEY_COUNT) return BSP_KEY_RELEASED ;
-    
-    const BSP_Key_Config_t *cfg = &KEY_CONFIGS[ch];
-    GPIO_PinState state = HAL_GPIO_ReadPin(cfg->port, cfg->pin);
-    
-    if(state == cfg->active_level) {
-        uint32_t now = HAL_GetTick();      //用于记录按键按下时间（这里比较state是为了方便适应不同有效电平做出修改的，也可以改成直接检测电平高低）
-        
-        //消抖检测(只有按下超过20ms才被认为按下）
-        if((now - last_press_time[ch]) > debounce_ms) {
-            //长按检测（只有被按下超过2000ms才被认为是长按，根据实际情况可做出修改）
-            if((now - last_press_time[ch]) > long_press_ms) {
-                return BSP_KEY_LONG_PRESS;
-            }
-            return BSP_KEY_PRESSED;
-        }
-    } else {
-        last_press_time[ch] = HAL_GetTick();
-    }
-    return BSP_KEY_RELEASED;
-}	

User/bsp/key_gpio.h

@@ -1,36 +0,0 @@
-#pragma once
-
-/* Includes ----------------------------------------------------------------- */
-#include <stdint.h>
-#include "main.h"
-//#include "key_gpio.h"
-/* Exported constants ------------------------------------------------------- */
-/* Exported macro ----------------------------------------------------------- */
-/* Exported types ----------------------------------------------------------- */
-
-/* KEY按键状态，设置用 */
-typedef enum
-{
-    BSP_KEY_RELEASED,  //按键释放
-    BSP_KEY_PRESSED,   //按键按下
-    BSP_KEY_LONG_PRESS,  //按键长按
-} BSP_KEY_Status_t;
-
-
-/* 按键通道定义 */
-typedef enum {
-    BSP_KEY_1,
-    BSP_KEY_2,
-    /* 可根据需要扩展 */
-    BSP_KEY_COUNT
-} BSP_Key_Channel_t;
-
-
-/* 按键硬件配置结构体 */
-typedef struct {
-    GPIO_TypeDef *port;    // GPIO端口
-    uint16_t pin;          // 引脚编号
-    uint8_t active_level;  // 有效电平（GPIO_PIN_SET/RESET）
-} BSP_Key_Config_t;
-
-int8_t BSP_Key_Read(BSP_Key_Channel_t ch);

User/bsp/led_gpio.c

@@ -6,6 +6,7 @@
 /* Private define ----------------------------------------------------------- */
 /* Private macro ------------------------------------------------------------ */
 /* Private typedef ---------------------------------------------------------- */
+
 /* Private variables -------------------------------------------------------- */
 static uint32_t led_stats; // 使用位掩码记录每个通道的状态，最多支持32LED
 

User/bsp/led_gpio.h

@@ -2,7 +2,7 @@
 
 /* Includes ----------------------------------------------------------------- */
 #include <stdint.h>
-
+#include "gpio.h"
 /* Exported constants ------------------------------------------------------- */
 /* Exported macro ----------------------------------------------------------- */
 /* Exported types ----------------------------------------------------------- */

User/bsp/servo_pwm.c

@@ -0,0 +1,48 @@
+/* Includes ----------------------------------------------------------------- */
+#include "servo_pwm.h"
+
+#include "main.h"
+
+
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+/* Private function  -------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+
+int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch) {
+  
+  TIM_HandleTypeDef* htim = pwm_channel_config[ch].htim;
+  uint32_t channel = pwm_channel_config[ch].channel;
+
+  if(HAL_TIM_PWM_Start(htim, channel)!=HAL_OK){
+	return -1;
+  }else return 0;
+}
+
+int8_t BSP_PWM_Set(BSP_PWM_Channel_t ch, float duty_cycle) {
+  if (duty_cycle > 1.0f) return -1;
+
+  uint16_t pulse = duty_cycle/CYCLE * PWM_RESOLUTION;
+
+  if(__HAL_TIM_SET_COMPARE(pwm_channel_config[ch].htim, pwm_channel_config[ch].channel, pulse)!=HAL_OK){
+	return -1;
+  }else return 0;
+}
+
+int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch){
+
+  TIM_HandleTypeDef* htim = pwm_channel_config[ch].htim;
+  uint32_t channel = pwm_channel_config[ch].channel;
+
+  if(HAL_TIM_PWM_Stop(htim, channel)!=HAL_OK){
+	return -1;
+  }else return 0;
+
+
+};
+
+
+
+

User/bsp/servo_pwm.h

@@ -0,0 +1,45 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdint.h>
+#include "tim.h"
+#include "bsp/bsp.h"
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef struct {
+  TIM_HandleTypeDef* htim; // 定时器句柄
+  uint32_t channel;        // 定时器通道
+} PWM_Channel_Config_t;
+
+#define PWM_RESOLUTION 1000 // ARR                                      change begin
+#define CYCLE 20 //ms
+
+typedef enum {				
+  BSP_PWM_SERVO = 0,      
+  BSP_PWM_IMU_HEAT = 1,   
+} BSP_PWM_Channel_t;
+
+const PWM_Channel_Config_t pwm_channel_config[] = {		
+  [BSP_PWM_SERVO] = { &htim1, TIM_CHANNEL_1 },  // xxx 对应 TIMx 通道x
+  [BSP_PWM_IMU_HEAT] = { &htim1, TIM_CHANNEL_2 } 
+};																	  //change end
+
+/* Exported functions prototypes -------------------------------------------- */
+int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch);
+int8_t BSP_PWM_Set(BSP_PWM_Channel_t ch, float duty_cycle);
+int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+
+
+

User/component/dependencies.csv

@@ -0,0 +1,3 @@
+pid,component/filter
+pid,component/user_math
+filter,component/user_math

User/component/filter.c

@@ -0,0 +1,186 @@
+/*
+  各类滤波器。
+*/
+
+#include "filter.h"
+#include <stddef.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);
+}

User/component/filter.h

@@ -0,0 +1,102 @@
+/*
+  各类滤波器。
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* 二阶低通滤波器 */
+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;
+
+/**
+ * @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);
+
+#ifdef __cplusplus
+}
+#endif

User/component/pid.c

@@ -0,0 +1,161 @@
+/*
+   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"
+
+#include <stddef.h>
+#include "user_math.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;
+}

User/component/pid.h

@@ -0,0 +1,94 @@
+/*
+  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"
+
+/* 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);
+
+#ifdef __cplusplus
+}
+#endif

User/component/user_math.c

@@ -3,5 +3,86 @@
 */
 
 #include "user_math.h"
-#include <stdint.h>
 
+#include <string.h>
+#include <stdint.h>
+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 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
+ * 例如编码器：相差1.5PI其实等于相差-0.5PI
+ *
+ * \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-2PI内变化，1.5PI + 1.5PI = 1PI
+ *
+ * \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; }

User/component/user_math.h

@@ -4,10 +4,127 @@
 
 #pragma once
 
+#ifdef __cplusplus
+extern "C" {
+#endif
 
+#include <float.h>
 #include <math.h>
+#include <stdbool.h>
 #include <stdint.h>
+#define M_DEG2RAD_MULT (0.01745329251f)
+#define M_RAD2DEG_MULT (57.2957795131f)
 
 #ifndef M_PI
 #define M_PI 3.14159265358979323846f
 #endif
+
+#ifndef M_2PI
+#define M_2PI 6.28318530717958647692f
+#endif
+
+#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;      \
+  })
+
+/* 移动向量 */
+typedef struct {
+  float vx; /* 前后平移 */
+  float vy; /* 左右平移 */
+  float wz; /* 转动 */
+} MoveVector_t;
+
+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 计算循环值的误差，用于没有负数值，并在一定范围内变化的值
+ * 例如编码器：相差1.5PI其实等于相差-0.5PI
+ *
+ * \param sp 被操作的值
+ * \param fb 变化量
+ * \param range 被操作的值变化范围，正数时起效
+ *
+ * \return 函数运行结果
+ */
+float CircleError(float sp, float fb, float range);
+
+/**
+ * \brief 循环加法，用于没有负数值，并在一定范围内变化的值
+ * 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI
+ *
+ * \param origin 被操作的值
+ * \param delta 变化量
+ * \param range 被操作的值变化范围，正数时起效
+ */
+void CircleAdd(float *origin, float delta, float range);
+
+/**
+ * @brief 循环值取反
+ *
+ * @param origin 被操作的值
+ */
+void CircleReverse(float *origin);
+
+#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

User/device/dependencies.csv

@@ -1,3 +1,5 @@
 oled_i2c,bsp/i2c
 bmp280_i2c,bsp/i2c
-pc_uart,bsp/uart
+pc_uart,bsp/uart
+key_gpio,bsp/gpio_exti
+servo,bsp/servo_pwm

User/device/key_gpio.c

@@ -0,0 +1,65 @@
+/* Includes ----------------------------------------------------------------- */
+#include "key_gpio.h"
+#include "device.h"
+#include "bsp/gpio_exti.h"
+#include "gpio.h"
+/* Private define ----------------------------------------------------------- */
+#define DEBOUNCE_TIME_MS  20
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+/* Private function  -------------------------------------------------------- */
+/* 外部声明标志位（标志位） */
+volatile uint8_t key_flag = 0; // 1=按下，0=松开
+volatile uint8_t key_exti = 0; 
+volatile uint8_t key_pressed = 0;  // 全局标志位
+static uint32_t last_debounce_time = 0;    // 消抖
+
+/* Private function  -------------------------------------------------------- */
+static void KEY_Interrupt_Callback(void) {
+    // 切换标志位状态
+
+    key_flag = !key_flag;
+	key_exti = 1;
+
+} 
+
+/* Exported functions ------------------------------------------------------- */
+void KEY_Process(void)
+{
+     BSP_GPIO_RegisterCallback(BSP_GPIO_USER_KEY, BSP_GPIO_EXTI_CB, KEY_Interrupt_Callback);
+
+	if(key_exti == 1)
+	{
+		uint32_t now = HAL_GetTick();  
+        // 检查是否超过消抖时间
+        if ((now - last_debounce_time) > DEBOUNCE_TIME_MS) {
+            // 更新有效状态（假设按下为低电平）
+		    if(key_flag == 0)
+	        {
+		        key_pressed = DEVICE_KEY_RELEASED;
+	        }
+	        if(key_flag == 1)
+	        {
+		        key_pressed = DEVICE_KEY_PRESSED;
+	         }
+		 }
+		else
+		{
+			 HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_6);
+			
+		}
+	   last_debounce_time = now; // 重置消抖计时器
+	   key_exti = 0;
+    }
+	else
+	{
+		
+	}
+ }
+
+uint8_t KEY_Get_State(void) {
+    return key_pressed;
+}
+
+

User/device/key_gpio.h

@@ -0,0 +1,21 @@
+#ifndef KEY_GPIO_H
+#define KEY_GPIO_H
+/* Includes ----------------------------------------------------------------- */
+#include <stdint.h>
+#include "main.h"
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+///* KEY按键状态，设置用 */
+typedef enum
+{
+    DEVICE_KEY_RELEASED,  //按键释放
+    DEVICE_KEY_PRESSED,   //按键按下
+} DEVICE_KEY_Status_t;
+
+void KEY_Process(void);
+uint8_t KEY_Get_State(void);
+
+#endif

User/device/servo.c

@@ -0,0 +1,36 @@
+/* Includes ----------------------------------------------------------------- */
+#include "main.h"
+#include "servo.h"
+
+#include "bsp/servo_pwm.h"
+
+
+/* Private define ----------------------------------------------------------- */
+#define MIN_CYCLE 0.5f		//change begin
+#define MAX_CYCLE 2.5f
+#define ANGLE_LIMIT 180		//change end
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+/* Private function  -------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+int serve_Init(BSP_PWM_Channel_t ch)
+{
+	if(BSP_PWM_Start(ch)!=0){
+		return -1;
+	}else return 0;	
+}
+
+
+int set_servo_angle(BSP_PWM_Channel_t ch,float angle)
+{
+	if (angle < 0.0f || angle > ANGLE_LIMIT) {
+        return -1; // ÎÞЧµÄ½Ç¶È
+    }
+	
+	float duty_cycle=MIN_CYCLE+(MAX_CYCLE-MIN_CYCLE)*(angle/ANGLE_LIMIT);
+	if(BSP_PWM_Set(ch,duty_cycle)!=0){
+		return -1;
+	}else return 0;
+}
+

User/device/servo.h

@@ -0,0 +1,41 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdint.h>
+#include "tim.h"
+#include "bsp/bsp.h"
+#include "bsp/servo_pwm.h"
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+	
+	
+	
+extern int serve_Init(BSP_PWM_Channel_t ch);
+extern int set_servo_angle(BSP_PWM_Channel_t ch,float angle);
+	
+	
+	
+	
+	
+	
+	
+	
+	
+	
+	
+	
+	
+	
+#ifdef __cplusplus
+}
+#endif
+
+
+
+
+

pngico.py

@@ -0,0 +1,42 @@
+from PIL import Image
+import os
+
+def crop_transparent_background(input_path, output_path):
+    """
+    裁切 PNG 图片的透明背景并保存。
+    
+    :param input_path: 输入图片路径
+    :param output_path: 输出图片路径
+    """
+    try:
+        # 打开图片
+        img = Image.open(input_path)
+        
+        # 确保图片是 RGBA 模式
+        if img.mode != "RGBA":
+            img = img.convert("RGBA")
+        
+        # 获取图片的 alpha 通道
+        bbox = img.getbbox()
+        
+        if bbox:
+            # 裁切图片
+            cropped_img = img.crop(bbox)
+            # 保存裁切后的图片
+            cropped_img.save(output_path, format="PNG")
+            print(f"图片已保存到: {output_path}")
+        else:
+            print("图片没有透明背景或为空。")
+    except Exception as e:
+        print(f"处理图片时出错: {e}")
+
+if __name__ == "__main__":
+    # 示例：输入和输出路径
+    input_file = "C:\Mac\Home\Desktop\MRobot\img\M.png"  # 替换为你的输入图片路径
+    output_file = "C:\Mac\Home\Desktop\MRobot\img\M.png"  # 替换为你的输出图片路径
+
+    # 检查文件是否存在
+    if os.path.exists(input_file):
+        crop_transparent_background(input_file, output_file)
+    else:
+        print(f"输入文件不存在: {input_file}")

polynomial.py

@@ -0,0 +1,286 @@
+import sys
+import numpy as np
+import pandas as pd
+from PyQt5.QtWidgets import (
+    QApplication, QWidget, QVBoxLayout, QHBoxLayout, QPushButton, QSpinBox,
+    QLabel, QTableWidget, QTableWidgetItem, QFileDialog, QTextEdit,
+    QComboBox, QMessageBox, QHeaderView
+)
+from PyQt5.QtGui import QFont
+from PyQt5.QtCore import Qt
+import matplotlib
+from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
+from matplotlib.figure import Figure
+
+class PolyFitApp(QWidget):
+    def __init__(self):
+        super().__init__()
+        self.setWindowTitle("MRobot 多项式拟合工具")
+        self.resize(1440, 1280)
+        self.setFont(QFont("微软雅黑", 11))
+        self.center()
+
+        self.data_x = []
+        self.data_y = []
+        self.last_coeffs = None
+        self.last_xmin = None
+        self.last_xmax = None
+
+        # 主布局
+        main_layout = QHBoxLayout(self)
+        main_layout.setContentsMargins(20, 20, 20, 20)
+        main_layout.setSpacing(20)
+        left_layout = QVBoxLayout()
+        left_layout.setSpacing(12)
+        right_layout = QVBoxLayout()
+        right_layout.setSpacing(12)
+        main_layout.addLayout(left_layout, 0)
+        main_layout.addLayout(right_layout, 1)
+
+        # 数据输入区
+        self.table = QTableWidget(0, 2)
+        self.table.setFont(QFont("Consolas", 11))
+        self.table.setHorizontalHeaderLabels(["x", "y"])
+        self.table.horizontalHeader().setSectionResizeMode(QHeaderView.Stretch)
+        self.table.setSelectionBehavior(QTableWidget.SelectRows)
+        left_layout.addWidget(self.table)
+
+        btn_row = QHBoxLayout()
+        self.add_row_btn = QPushButton("添加数据")
+        self.add_row_btn.setStyleSheet("color: #333;")
+        self.add_row_btn.clicked.connect(self.add_point_row)
+        btn_row.addWidget(self.add_row_btn)
+
+        self.del_row_btn = QPushButton("删除选中行")
+        self.del_row_btn.setStyleSheet("color: #333;")
+        self.del_row_btn.clicked.connect(self.delete_selected_rows)
+        btn_row.addWidget(self.del_row_btn)
+        left_layout.addLayout(btn_row)
+
+        # 导入导出按钮区
+        file_btn_row = QHBoxLayout()
+        self.import_btn = QPushButton("导入Excel文件")
+        self.import_btn.setStyleSheet("font-weight: bold; color: #333;")
+        self.import_btn.clicked.connect(self.load_excel)
+        file_btn_row.addWidget(self.import_btn)
+
+        self.export_btn = QPushButton("导出Excel文件")
+        self.export_btn.setStyleSheet("font-weight: bold; color: #333;")
+        self.export_btn.clicked.connect(self.export_excel_and_plot)
+        file_btn_row.addWidget(self.export_btn)
+        left_layout.addLayout(file_btn_row)
+
+        # 拟合参数区
+        param_layout = QHBoxLayout()
+        param_layout.addWidget(QLabel("多项式阶数:"))
+        self.order_spin = QSpinBox()
+        self.order_spin.setRange(1, 10)
+        self.order_spin.setValue(2)
+        param_layout.addWidget(self.order_spin)
+        left_layout.addLayout(param_layout)
+
+        self.fit_btn = QPushButton("拟合并显示")
+        self.fit_btn.setStyleSheet("font-weight: bold; color: #333;")
+        self.fit_btn.clicked.connect(self.fit_and_plot)
+        left_layout.addWidget(self.fit_btn)
+
+        # 输出区
+        self.output = QTextEdit()
+        self.output.setReadOnly(False)
+        self.output.setFont(QFont("Consolas", 10))
+        self.output.setMaximumHeight(150)
+        left_layout.addWidget(self.output)
+
+        code_layout = QHBoxLayout()
+        code_layout.addWidget(QLabel("输出代码格式:"))
+        self.code_type = QComboBox()
+        self.code_type.addItems(["C", "C++", "Python"])
+        code_layout.addWidget(self.code_type)
+        self.gen_code_btn = QPushButton("生成函数代码")
+        self.gen_code_btn.setStyleSheet("color: #333;")
+        self.gen_code_btn.clicked.connect(self.generate_code)
+        code_layout.addWidget(self.gen_code_btn)
+        left_layout.addLayout(code_layout)
+
+        # 拟合曲线区
+        self.figure = Figure(figsize=(5, 4))
+        self.canvas = FigureCanvas(self.figure)
+        right_layout.addWidget(self.canvas)
+
+    def center(self):
+        qr = self.frameGeometry()
+        cp = QApplication.desktop().availableGeometry().center()
+        qr.moveCenter(cp)
+        self.move(qr.topLeft())
+
+    def add_point_row(self, x_val="", y_val=""):
+        row = self.table.rowCount()
+        self.table.insertRow(row)
+        self.table.setItem(row, 0, QTableWidgetItem(str(x_val)))
+        self.table.setItem(row, 1, QTableWidgetItem(str(y_val)))
+
+    def delete_selected_rows(self):
+        selected = self.table.selectionModel().selectedRows()
+        for idx in sorted(selected, reverse=True):
+            self.table.removeRow(idx.row())
+
+    def load_excel(self):
+        file, _ = QFileDialog.getOpenFileName(self, "选择Excel文件", "", "Excel Files (*.xlsx *.xls)")
+        if file:
+            try:
+                data = pd.read_excel(file, usecols=[0, 1])
+                new_x = data.iloc[:, 0].values.tolist()
+                new_y = data.iloc[:, 1].values.tolist()
+                for x, y in zip(new_x, new_y):
+                    self.add_point_row(x, y)
+                QMessageBox.information(self, "成功", "数据导入成功！")
+            except Exception as e:
+                QMessageBox.critical(self, "错误", f"读取Excel失败: {e}")
+
+    def export_excel_and_plot(self):
+        file, _ = QFileDialog.getSaveFileName(self, "导出Excel文件", "", "Excel Files (*.xlsx *.xls)")
+        if file:
+            x_list, y_list = [], []
+            for row in range(self.table.rowCount()):
+                try:
+                    x = float(self.table.item(row, 0).text())
+                    y = float(self.table.item(row, 1).text())
+                    x_list.append(x)
+                    y_list.append(y)
+                except Exception:
+                    continue
+            if not x_list or not y_list:
+                QMessageBox.warning(self, "导出失败", "没有可导出的数据！")
+                return
+            df = pd.DataFrame({'x': x_list, 'y': y_list})
+            try:
+                df.to_excel(file, index=False)
+                # 导出同名png图像
+                png_file = file
+                if png_file.lower().endswith('.xlsx') or png_file.lower().endswith('.xls'):
+                    png_file = png_file.rsplit('.', 1)[0] + '.png'
+                else:
+                    png_file = png_file + '.png'
+                self.figure.savefig(png_file, dpi=150, bbox_inches='tight')
+                QMessageBox.information(self, "导出成功", f"数据已成功导出到Excel文件！\n图像已导出为：{png_file}")
+            except Exception as e:
+                QMessageBox.critical(self, "导出错误", f"导出Excel或图像失败: {e}")
+
+    def get_manual_points(self):
+        x_list, y_list = [], []
+        for row in range(self.table.rowCount()):
+            try:
+                x = float(self.table.item(row, 0).text())
+                y = float(self.table.item(row, 1).text())
+                x_list.append(x)
+                y_list.append(y)
+            except Exception:
+                continue
+        return x_list, y_list
+
+    def fit_and_plot(self):
+        matplotlib.rcParams['font.sans-serif'] = ['SimHei', 'Microsoft YaHei']
+        matplotlib.rcParams['axes.unicode_minus'] = False
+        matplotlib.rcParams['font.size'] = 14
+        self.data_x, self.data_y = self.get_manual_points()
+        try:
+            order = int(self.order_spin.value())
+        except ValueError:
+            QMessageBox.warning(self, "输入错误", "阶数必须为整数！")
+            return
+        n_points = len(self.data_x)
+        if n_points < order + 1:
+            QMessageBox.warning(self, "数据不足", "数据点数量不足以拟合该阶多项式！")
+            return
+        x = np.array(self.data_x, dtype=np.float64)
+        y = np.array(self.data_y, dtype=np.float64)
+        x_min, x_max = x.min(), x.max()
+        if x_max - x_min == 0:
+            QMessageBox.warning(self, "数据错误", "所有x值都相同，无法拟合！")
+            return
+        try:
+            coeffs = np.polyfit(x, y, order)
+        except Exception as e:
+            QMessageBox.critical(self, "拟合错误", f"多项式拟合失败：{e}")
+            return
+        poly = np.poly1d(coeffs)
+        expr = "y = " + " + ".join([f"{c:.6g}*x^{order-i}" for i, c in enumerate(coeffs)])
+        self.output.setPlainText(f"{expr}\n")
+        self.figure.clear()
+        ax = self.figure.add_subplot(111)
+        ax.scatter(x, y, color='red', label='数据点')
+        x_fit = np.linspace(x_min, x_max, 200)
+        y_fit = poly(x_fit)
+        ax.plot(x_fit, y_fit, label='拟合曲线')
+        ax.legend()
+        self.canvas.draw()
+        self.last_coeffs = coeffs
+        self.last_xmin = x_min
+        self.last_xmax = x_max
+
+    def generate_code(self):
+        if self.last_coeffs is None:
+            QMessageBox.warning(self, "未拟合", "请先拟合数据！")
+            return
+        coeffs = self.last_coeffs
+        code_type = self.code_type.currentText()
+        if code_type == "C":
+            code = self.create_c_function(coeffs)
+        elif code_type == "C++":
+            code = self.create_cpp_function(coeffs)
+        else:
+            code = self.create_py_function(coeffs)
+        self.output.setPlainText(code)
+
+    def create_c_function(self, coeffs):
+        lines = ["#include <math.h>", "double polynomial(double x) {", "    return "]
+        n = len(coeffs)
+        terms = []
+        for i, c in enumerate(coeffs):
+            exp = n - i - 1
+            if exp == 0:
+                terms.append(f"{c:.8g}")
+            elif exp == 1:
+                terms.append(f"{c:.8g}*x")
+            else:
+                terms.append(f"{c:.8g}*pow(x,{exp})")
+        lines[-1] += " + ".join(terms) + ";"
+        lines.append("}")
+        return "\n".join(lines)
+
+    def create_cpp_function(self, coeffs):
+        lines = ["#include <cmath>", "double polynomial(double x) {", "    return "]
+        n = len(coeffs)
+        terms = []
+        for i, c in enumerate(coeffs):
+            exp = n - i - 1
+            if exp == 0:
+                terms.append(f"{c:.8g}")
+            elif exp == 1:
+                terms.append(f"{c:.8g}*x")
+            else:
+                terms.append(f"{c:.8g}*pow(x,{exp})")
+        lines[-1] += " + ".join(terms) + ";"
+        lines.append("}")
+        return "\n".join(lines)
+
+    def create_py_function(self, coeffs):
+        n = len(coeffs)
+        lines = ["def polynomial(x):", "    return "]
+        terms = []
+        for i, c in enumerate(coeffs):
+            exp = n - i - 1
+            if exp == 0:
+                terms.append(f"{c:.8g}")
+            elif exp == 1:
+                terms.append(f"{c:.8g}*x")
+            else:
+                terms.append(f"{c:.8g}*x**{exp}")
+        lines[-1] += " + ".join(terms)
+        return "\n".join(lines)
+
+if __name__ == "__main__":
+    app = QApplication(sys.argv)
+    win = PolyFitApp()
+    win.show()
+    sys.exit(app.exec_())

开发要求.md

@@ -1,10 +1,105 @@
-# BSP
+# 嵌入式 代码
-按照规范开发
+
-# component
+## 软件功能介绍
-按照规范开发
+
-# develop
+中心思想：
-按照规范开发
+
-# module
+- 利用好RTOS和中断，释放CPU性能，保证实时性。
-按照规范开发
+- 一个项目适配不同型号的机器人和不同的操作手。
-# task
+
-按照规范开发
+减少维护的工作量，减少出错的可能性。
+
+## 依赖&环境
+
+- Windows平台下用CubeMX生成项目，然后用Keil uvesrion进行编辑、烧写和调试。
+
+## 使用说明
+
+- 环境安装
+  - [MDK-ARM](https://www.keil.com/) （必备）
+  - [STM32CubeMX](https://www.st.com/zh/development-tools/stm32cubemx.html) （可选）
+
+
+- 针对不同板子需要到不同的CubeMX工程文件（DevA.ioc、DevC.ioc）。
+
+- （可选）利用CubeMX生成对应的外设初始化代码和Keil工程文件。忽略CAN总线相关错误。
+
+  - 每次生成代码后，请利用Git丢弃Middlewares文件夹中的所有改变。原因如下。
+
+    1. 使用了AC6，与CubeMX默认不匹配，会影响到FreeRTOS的移植。
+    2. 使用了比CubeMX更新的FreeRTOS版本，降版本会导致部分代码无法编译。
+
+  - 因为已经生成过Keil工程文件，所以只会覆盖以前生成的代码，而不会影响手写的代码。
+
+  - 每次生成代码后，请在HAL_InitTick函数中添加uwTickPrio = TickPriority;
+
+- 打开MDK-ARM中的DevC.uvprojx即可进行编辑、烧写或调试。
+
+- Keil工程中有两个Target，其中Debug用来调试，不包含编译器优化等；DevC/DevA用来编译输出最终固件。
+
+## 文件目录结构&文件用途说明
+
+| 文件夹 | 来源 | 内容 |
+| ---- | ---- | ----  |
+| Core | CubeMX | 包含核心代码，外设初始化，系统初始化等 |
+| Doc | 开发者 | 文档 |
+| Drivers | CubeMX | CMSIS相关库、STM32 HAL |
+| Image | 开发者 | 图片 |
+| MDK-ARM | CubeMX | Keil uversion 项目相关文件 |
+| Middlewares | 开发者 / CubeMX | 中间件 |
+| USB_DEVICE | CubeMX | USB相关文件 |
+| User |  开发者 | 手动编写的代码 |
+| Utils |  开发者 | 使用到的工具，如CubeMonitor, Matlab |
+
+| User内 | 内容 |
+| ---- | ----  |
+| bsp | 文件夹内包含开发板信息，基于STM32 HAL对板载的外设进行控制|
+| component | 包含各种组件，自成一体，相互依赖，但不依赖于其他文件夹|
+| device | 独立于开发板的设备，依赖于HAL和bsp|
+| module | 对机器人各模块的抽象，各模块一起组成机器人|
+| task | 独立的任务，module的运行容器，也包含通信、姿态解算等 |
+
+## 系统介绍
+
+### 硬件系统框图
+
+| ![步兵嵌入式硬件框图](./Image/步兵嵌入式硬件框图.png?raw=true "步兵嵌入式硬件框图") |
+|:--:|
+| *步兵嵌入式硬件框图* |
+
+### 软件流程图
+
+| ![步兵嵌入式硬件框图](./Image/嵌入式程序流程图.png?raw=true "步兵嵌入式硬件框图") |
+|:--:|
+| *步兵嵌入式硬件框图* |
+
+| ![嵌入式程序结构图](./Image/嵌入式程序结构图.png?raw=true "嵌入式程序结构图") |
+|:--:|
+| *嵌入式程序结构图* |
+
+## 原理介绍
+
+### 云台控制原理
+
+| ![云台控制原理（与PX类似）](./Image/云台控制原理.png?raw=true "嵌入式程序结构图") |
+|:--:|
+| *云台控制原理（与PX类似）* |
+
+### 其他参考文献
+
+- 软件架构参考[PX4 Architectural Overview](https://dev.px4.io/master/en/concept/architecture.html)
+
+- 云台控制参考[PX4 Controller Diagrams](https://dev.px4.io/master/en/flight_stack/controller_diagrams.html)
+
+- 底盘Mixer和CAN的Control Group参考[PX4 Mixing and Actuators](https://dev.px4.io/master/en/concept/mixing.html)
+
+## TODO
+- 给BSP USB print加保护，允许不同进程的使用。
+  - 给所有BSP加保护
+  - device.c里面加上一个Device_Init()，在里面初始化所有mutex
+- CAN设备代码优化。消息解析发送方向。
+  - CAN设备动态初始化，保存好几组配置。
+
+## Roadmap
+
+1. 在步兵上完成所有功能。