From 2c9309ae1e192cb329e306ac3b71d6f0a86c5f6f Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Tue, 5 Aug 2025 14:22:31 +0800 Subject: [PATCH] =?UTF-8?q?bsp=E5=92=8Ccomponent=E5=86=99=E5=AE=8C?= =?UTF-8?q?=E4=BA=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- app/code_generate_interface.py | 138 +++-- app/code_page/bsp_interface.py | 240 +++++++-- app/code_page/component_interface.py | 552 ++++++++++++++++++++ app/data_interface.py | 1 + assets/User_code/bsp/describe.csv | 5 +- assets/User_code/bsp/i2c.h | 2 +- assets/User_code/bsp/servo_pwm.c | 48 -- assets/User_code/bsp/servo_pwm.h | 45 -- assets/User_code/component/ahrs.c | 405 ++++++++++++++ assets/User_code/component/ahrs.h | 98 ++++ assets/User_code/component/dependencies.csv | 4 +- assets/User_code/component/describe.csv | 3 +- assets/User_code/config.csv | 2 +- 13 files changed, 1352 insertions(+), 191 deletions(-) delete mode 100644 assets/User_code/bsp/servo_pwm.c delete mode 100644 assets/User_code/bsp/servo_pwm.h create mode 100644 assets/User_code/component/ahrs.c create mode 100644 assets/User_code/component/ahrs.h diff --git a/app/code_generate_interface.py b/app/code_generate_interface.py index 9a354bd..293e275 100644 --- a/app/code_generate_interface.py +++ b/app/code_generate_interface.py @@ -1,11 +1,13 @@ from PyQt5.QtWidgets import QWidget, QVBoxLayout, QHBoxLayout, QSizePolicy, QTreeWidget, QTreeWidgetItem, QStackedWidget from PyQt5.QtCore import Qt -from qfluentwidgets import TitleLabel, BodyLabel, PushButton +from qfluentwidgets import TitleLabel, BodyLabel, PushButton, TreeWidget, FluentIcon, InfoBar from app.tools.analyzing_ioc import analyzing_ioc from app.code_page.bsp_interface import bsp +from app.data_interface import DataInterface import os import csv +import sys import importlib class CodeGenerateInterface(QWidget): @@ -13,11 +15,13 @@ class CodeGenerateInterface(QWidget): super().__init__(parent) self.setObjectName("CodeGenerateInterface") self.project_path = project_path + + # 初始化页面缓存 + self.page_cache = {} self._init_ui() def _init_ui(self): - """初始化界面布局""" main_layout = QVBoxLayout(self) main_layout.setAlignment(Qt.AlignTop) main_layout.setContentsMargins(10, 10, 10, 10) @@ -25,15 +29,16 @@ class CodeGenerateInterface(QWidget): top_layout = self._create_top_layout() main_layout.addLayout(top_layout) - # 下方主区域，左右分栏 content_layout = QHBoxLayout() content_layout.setContentsMargins(0, 10, 0, 0) main_layout.addLayout(content_layout) - # 左侧树形列表 - self.tree = QTreeWidget() + # 左侧树形列表（使用qfluentwidgets的TreeWidget） + self.tree = TreeWidget() self.tree.setHeaderHidden(True) self.tree.setMaximumWidth(250) + self.tree.setBorderRadius(8) + self.tree.setBorderVisible(True) content_layout.addWidget(self.tree) # 右侧内容区 @@ -59,14 +64,55 @@ class CodeGenerateInterface(QWidget): freertos_label.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Preferred) top_layout.addWidget(freertos_label) + # 配置并生成FreeRTOS任务按钮，直接调用已有方法 + freertos_task_btn = PushButton(FluentIcon.SETTING, "配置并生成FreeRTOS任务") + freertos_task_btn.setSizePolicy(QSizePolicy.Fixed, QSizePolicy.Fixed) + freertos_task_btn.clicked.connect(self.on_task_code_btn_clicked) + top_layout.addWidget(freertos_task_btn, alignment=Qt.AlignRight) + # 生成代码按钮 - generate_btn = PushButton("Generate Code") + generate_btn = PushButton(FluentIcon.PROJECTOR,"生成代码") generate_btn.setSizePolicy(QSizePolicy.Fixed, QSizePolicy.Fixed) generate_btn.clicked.connect(self.generate_code) top_layout.addWidget(generate_btn, alignment=Qt.AlignRight) return top_layout + def on_task_code_btn_clicked(self): + # 检查是否开启 FreeRTOS + ioc_files = [f for f in os.listdir(self.project_path) if f.endswith('.ioc')] + if ioc_files: + ioc_path = os.path.join(self.project_path, ioc_files[0]) + if not analyzing_ioc.is_freertos_enabled_from_ioc(ioc_path): + InfoBar.error( + title="错误", + content="请先在 .ioc 文件中开启 FreeRTOS，再进行任务配置！", + parent=self, + duration=3000 + ) + return + else: + InfoBar.error( + title="错误", + content="未找到 .ioc 文件，无法检测 FreeRTOS 状态！", + parent=self, + duration=3000 + ) + return + + # 直接弹出任务配置对话框并生成代码 + dlg = DataInterface() + dlg.project_path = self.project_path + result = dlg.open_task_config_dialog() + # 生成任务成功后弹出 InfoBar 提示 + if getattr(dlg, "task_generate_success", False): + InfoBar.success( + title="任务生成成功", + content="FreeRTOS任务代码已生成！", + parent=self, + duration=2000 + ) + def generate_code(self): """生成代码逻辑""" # 收集所有已加载的页面对象 @@ -74,10 +120,24 @@ class CodeGenerateInterface(QWidget): for i in range(self.stack.count()): widget = self.stack.widget(i) pages.append(widget) - bsp.generate_bsp(self.project_path, pages) - # component.generate_component(self.project_path) - # device.generate_device(self.project_path) - + + # 生成 BSP 代码 + bsp_result = bsp.generate_bsp(self.project_path, pages) + + # 生成 Component 代码 + from app.code_page.component_interface import component + component_result = component.generate_component(self.project_path, pages) + + # 合并结果信息 + combined_result = f"BSP代码生成:\n{bsp_result}\n\nComponent代码生成:\n{component_result}" + + # 用 InfoBar 在主界面弹出 + InfoBar.success( + title="代码生成结果", + content=combined_result, + parent=self, + duration=5000 # 增加显示时间，因为内容更多 + ) def _get_freertos_status(self): """获取FreeRTOS状态""" @@ -88,7 +148,6 @@ class CodeGenerateInterface(QWidget): return "未找到.ioc文件" def _load_csv_and_build_tree(self): - # 获取脚本目录 script_dir = os.path.dirname(os.path.abspath(__file__)) csv_path = os.path.join(script_dir, "../assets/User_code/config.csv") csv_path = os.path.abspath(csv_path) @@ -120,31 +179,38 @@ class CodeGenerateInterface(QWidget): if widget: self.stack.setCurrentWidget(widget) +# ...existing code... def _get_or_create_page(self, class_name): - for i in range(self.stack.count()): - w = self.stack.widget(i) - if w.objectName() == class_name: - return w + """获取或创建页面""" + if class_name in self.page_cache: + return self.page_cache[class_name] + + # 如果是第一次创建组件页面，初始化组件管理器 + if not hasattr(self, 'component_manager'): + from app.code_page.component_interface import ComponentManager + self.component_manager = ComponentManager() + try: - module_name = f"app.code_page.{class_name.split('_')[0]}_interface" - module = importlib.import_module(module_name) - cls = getattr(module, class_name) - widget = cls(self.project_path) - widget.setObjectName(class_name) - self.stack.addWidget(widget) - print(f"加载页面类: {class_name} 来自模块: {module_name}") - return widget + if class_name.startswith('bsp_'): + # BSP页面 + from app.code_page.bsp_interface import get_bsp_page + # 提取外设名，如 bsp_delay -> delay + periph_name = class_name[len('bsp_'):].replace("_", " ") + page = get_bsp_page(periph_name, self.project_path) + elif class_name.startswith('component_'): + from app.code_page.component_interface import get_component_page + comp_name = class_name[len('component_'):].replace("_", " ") + page = get_component_page(comp_name, self.project_path, self.component_manager) + self.component_manager.register_component(page.component_name, page) + else: + print(f"未知的页面类型: {class_name}") + return None + + self.page_cache[class_name] = page + self.stack.addWidget(page) + return page except Exception as e: - # 自动识别通用外设页面 - from app.code_page.bsp_interface import BspSimplePeripheral - peripheral_name = class_name.split('_')[1] if '_' in class_name else class_name - # 模板文件名自动推断 - template_names = { - 'header': f"{peripheral_name.lower()}.h", - 'source': f"{peripheral_name.lower()}.c" - } - widget = BspSimplePeripheral(self.project_path, peripheral_name, template_names) - widget.setObjectName(class_name) - self.stack.addWidget(widget) - print(f"自动加载通用外设页面: {class_name}") - return widget \ No newline at end of file + print(f"创建页面 {class_name} 失败: {e}") + return None + +# ...existing code... \ No newline at end of file diff --git a/app/code_page/bsp_interface.py b/app/code_page/bsp_interface.py index 8de4dcc..099e9cd 100644 --- a/app/code_page/bsp_interface.py +++ b/app/code_page/bsp_interface.py @@ -1,9 +1,43 @@ -from PyQt5.QtWidgets import QWidget, QVBoxLayout, QLineEdit, QCheckBox, QComboBox, QTableWidget, QHeaderView, QMessageBox -from qfluentwidgets import TitleLabel, BodyLabel, PushButton +from PyQt5.QtWidgets import QWidget, QVBoxLayout, QLineEdit, QCheckBox, QComboBox, QTableWidget, QHeaderView, QMessageBox, QHBoxLayout +from qfluentwidgets import TitleLabel, BodyLabel, PushButton, CheckBox, TableWidget, LineEdit, ComboBox,MessageBox,SubtitleLabel,FluentIcon +from qfluentwidgets import InfoBar +from PyQt5.QtCore import Qt from app.tools.analyzing_ioc import analyzing_ioc from app.tools.code_generator import CodeGenerator import os import csv +import shutil + +def preserve_all_user_regions(new_code, old_code): + """ Preserves all user-defined regions in the new code based on the old code. + This function uses regex to find user-defined regions in the old code and replaces them in the new code. + Args: + new_code (str): The new code content. + old_code (str): The old code content. + Returns: + str: The new code with preserved user-defined regions. + """ + import re + pattern = re.compile( + r"/\*\s*(USER [A-Z0-9_ ]+)\s*BEGIN\s*\*/(.*?)/\*\s*\1\s*END\s*\*/", + re.DOTALL + ) + old_regions = {m.group(1): m.group(2) for m in pattern.finditer(old_code or "")} + def repl(m): + region = m.group(1) + old_content = old_regions.get(region) + if old_content is not None: + return m.group(0).replace(m.group(2), old_content) + return m.group(0) + return pattern.sub(repl, new_code) + +def save_with_preserve(path, new_code): + if os.path.exists(path): + with open(path, "r", encoding="utf-8") as f: + old_code = f.read() + new_code = preserve_all_user_regions(new_code, old_code) + with open(path, "w", encoding="utf-8") as f: + f.write(new_code) class BspSimplePeripheral(QWidget): def __init__(self, project_path, peripheral_name, template_names): @@ -19,13 +53,32 @@ class BspSimplePeripheral(QWidget): def _init_ui(self): layout = QVBoxLayout(self) - layout.addWidget(TitleLabel(f"{self.peripheral_name} 配置")) + + # 顶部横向布局：左侧复选框，居中标题 + top_layout = QHBoxLayout() + top_layout.setAlignment(Qt.AlignVCenter) + + self.generate_checkbox = CheckBox(f"启用 {self.peripheral_name}") + top_layout.addWidget(self.generate_checkbox, alignment=Qt.AlignLeft) + + # 弹性空间 + top_layout.addStretch() + + title = SubtitleLabel(f"{self.peripheral_name} 配置 ") + title.setAlignment(Qt.AlignHCenter) + top_layout.addWidget(title, alignment=Qt.AlignHCenter) + + # 再加一个弹性空间，保证标题居中 + top_layout.addStretch() + + layout.addLayout(top_layout) + desc = self.descriptions.get(self.peripheral_name.lower(), "") if desc: - layout.addWidget(BodyLabel(f"功能说明：{desc}")) - self.generate_checkbox = QCheckBox(f"启用 {self.peripheral_name}") - layout.addWidget(self.generate_checkbox) - + desc_label = BodyLabel(f"功能说明：{desc}") + desc_label.setWordWrap(True) + layout.addWidget(desc_label) + layout.addStretch() def is_need_generate(self): return self.generate_checkbox.isChecked() @@ -33,18 +86,17 @@ class BspSimplePeripheral(QWidget): if not self.is_need_generate(): return False template_dir = CodeGenerator.get_template_dir() - # 直接拷贝模板，无需特殊处理 for key, filename in self.template_names.items(): template_path = os.path.join(template_dir, filename) template_content = CodeGenerator.load_template(template_path) if not template_content: return False output_path = os.path.join(self.project_path, f"User/bsp/{filename}") - if not CodeGenerator.save_file(template_content, output_path): - return False + save_with_preserve(output_path, template_content) # 使用保留用户区域的写入 self._save_config() return True + def _save_config(self): config_path = os.path.join(self.project_path, "User/bsp/bsp_config.yaml") config_data = CodeGenerator.load_config(config_path) @@ -58,7 +110,6 @@ class BspSimplePeripheral(QWidget): if conf.get('enabled', False): self.generate_checkbox.setChecked(True) - class BspPeripheralBase(QWidget): def __init__(self, project_path, peripheral_name, template_names, enum_prefix, handle_prefix, yaml_key, get_available_func): super().__init__() @@ -78,14 +129,30 @@ class BspPeripheralBase(QWidget): def _init_ui(self): layout = QVBoxLayout(self) - layout.addWidget(TitleLabel(f"{self.peripheral_name} 配置")) + + top_layout = QHBoxLayout() + top_layout.setAlignment(Qt.AlignVCenter) + + self.generate_checkbox = CheckBox(f"生成 {self.peripheral_name} 代码") + self.generate_checkbox.stateChanged.connect(self._on_generate_changed) + top_layout.addWidget(self.generate_checkbox, alignment=Qt.AlignLeft) + + top_layout.addStretch() + + title = SubtitleLabel(f"{self.peripheral_name} 配置 ") + title.setAlignment(Qt.AlignHCenter) + top_layout.addWidget(title, alignment=Qt.AlignHCenter) + + top_layout.addStretch() + + layout.addLayout(top_layout) + desc = self.descriptions.get(self.peripheral_name.lower(), "") if desc: - layout.addWidget(BodyLabel(f"功能说明：{desc}")) - self.generate_checkbox = QCheckBox(f"生成 {self.peripheral_name} 代码") - self.generate_checkbox = QCheckBox(f"生成 {self.peripheral_name} 代码") - self.generate_checkbox.stateChanged.connect(self._on_generate_changed) - layout.addWidget(self.generate_checkbox) + desc_label = BodyLabel(f"功能说明：{desc}") + desc_label.setWordWrap(True) + layout.addWidget(desc_label) + self.content_widget = QWidget() content_layout = QVBoxLayout(self.content_widget) self._get_available_list() @@ -93,7 +160,8 @@ class BspPeripheralBase(QWidget): content_layout.addWidget(BodyLabel(f"在 .ioc 文件中未找到已启用的 {self.peripheral_name}")) else: content_layout.addWidget(BodyLabel(f"可用的 {self.peripheral_name}: {', '.join(self.available_list)}")) - self.table = QTableWidget(0, 3) + self.table = TableWidget() + self.table.setColumnCount(3) self.table.setHorizontalHeaderLabels(["设备名称", f"{self.peripheral_name}选择", "操作"]) self.table.horizontalHeader().setSectionResizeMode(QHeaderView.Stretch) content_layout.addWidget(self.table) @@ -103,6 +171,7 @@ class BspPeripheralBase(QWidget): layout.addWidget(self.content_widget) self.content_widget.setEnabled(False) + def _get_available_list(self): self.available_list = self.get_available_func(self.project_path) @@ -112,13 +181,13 @@ class BspPeripheralBase(QWidget): def _add_device(self): row = self.table.rowCount() self.table.insertRow(row) - name_edit = QLineEdit() + name_edit = LineEdit() name_edit.setPlaceholderText(f"输入设备名称") self.table.setCellWidget(row, 0, name_edit) - combo = QComboBox() + combo = ComboBox() # 使用 Fluent 风格 ComboBox combo.addItems(self.available_list) self.table.setCellWidget(row, 1, combo) - del_btn = PushButton("删除") + del_btn = PushButton(FluentIcon.DELETE,"删除" ) # 添加垃圾桶图标 del_btn.clicked.connect(lambda: self._delete_device(row)) self.table.setCellWidget(row, 2, del_btn) @@ -152,7 +221,12 @@ class BspPeripheralBase(QWidget): if not self._generate_source_file(configs, template_dir): return False self._save_config(configs) - QMessageBox.information(self, "成功", f"{self.peripheral_name} 代码生成成功！") + InfoBar.success( + title="任务生成成功", + content=f"{self.peripheral_name} 代码生成成功！", + parent=self, + duration=2000 + ) return True def _generate_header_file(self, configs, template_dir): @@ -165,7 +239,8 @@ class BspPeripheralBase(QWidget): template_content, f"AUTO GENERATED {self.enum_prefix}_NAME", "\n".join(enum_lines) ) output_path = os.path.join(self.project_path, f"User/bsp/{self.template_names['header']}") - return CodeGenerator.save_file(content, output_path) + save_with_preserve(output_path, content) # 使用保留用户区域的写入 + return True def _generate_source_file(self, configs, template_dir): template_path = os.path.join(template_dir, self.template_names['source']) @@ -187,12 +262,19 @@ class BspPeripheralBase(QWidget): handle_lines = [] for name, instance in configs: handle_lines.append(f" case {self.enum_prefix}_{name}:") - handle_lines.append(f" return &h{instance.lower()};") + # UART/USART统一用 huart 前缀 + if self.enum_prefix == "BSP_UART": + # 提取数字部分 + num = ''.join(filter(str.isdigit, instance)) + handle_lines.append(f" return &huart{num};") + else: + handle_lines.append(f" return &h{instance.lower()};") content = CodeGenerator.replace_auto_generated( content, f"AUTO GENERATED {self.enum_prefix}_GET_HANDLE", "\n".join(handle_lines) ) output_path = os.path.join(self.project_path, f"User/bsp/{self.template_names['source']}") - return CodeGenerator.save_file(content, output_path) + save_with_preserve(output_path, content) # 使用保留用户区域的写入 + return True def _save_config(self, configs): config_path = os.path.join(self.project_path, "User/bsp/bsp_config.yaml") @@ -345,23 +427,40 @@ class bsp_gpio(QWidget): def _init_ui(self): layout = QVBoxLayout(self) - layout.addWidget(TitleLabel("GPIO 配置")) - # 新增：显示描述 + + top_layout = QHBoxLayout() + top_layout.setAlignment(Qt.AlignVCenter) + + self.generate_checkbox = CheckBox("生成 GPIO 代码") + top_layout.addWidget(self.generate_checkbox, alignment=Qt.AlignLeft) + + top_layout.addStretch() + + title = SubtitleLabel("GPIO 配置 ") + title.setAlignment(Qt.AlignHCenter) + top_layout.addWidget(title, alignment=Qt.AlignHCenter) + + top_layout.addStretch() + + layout.addLayout(top_layout) + desc = self.descriptions.get("gpio", "") if desc: - layout.addWidget(BodyLabel(f"功能说明：{desc}")) - self.generate_checkbox = QCheckBox("生成 GPIO 代码") - layout.addWidget(self.generate_checkbox) + desc_label = BodyLabel(f"功能说明：{desc}") + desc_label.setWordWrap(True) + layout.addWidget(desc_label) if not self.available_list: layout.addWidget(BodyLabel("在 .ioc 文件中未找到可用的 GPIO")) else: - self.table = QTableWidget(len(self.available_list), 1) + self.table = TableWidget() + self.table.setColumnCount(1) + self.table.setRowCount(len(self.available_list)) self.table.setHorizontalHeaderLabels(["Label"]) self.table.horizontalHeader().setSectionResizeMode(QHeaderView.Stretch) for row, item in enumerate(self.available_list): from PyQt5.QtWidgets import QTableWidgetItem self.table.setItem(row, 0, QTableWidgetItem(item['label'])) - self.table.setEditTriggers(QTableWidget.NoEditTriggers) + self.table.setEditTriggers(TableWidget.NoEditTriggers) layout.addWidget(self.table) def is_need_generate(self): @@ -379,20 +478,19 @@ class bsp_gpio(QWidget): return True def _generate_header_file(self, template_dir): - template_path = os.path.join(template_dir, "gpio.h") - template_content = CodeGenerator.load_template(template_path) - if not template_content: - return False - # 如有需要可在此处插入自动生成内容 - output_path = os.path.join(self.project_path, "User/bsp/gpio.h") - return CodeGenerator.save_file(template_content, output_path) + template_path = os.path.join(template_dir, "gpio.h") + template_content = CodeGenerator.load_template(template_path) + if not template_content: + return False + output_path = os.path.join(self.project_path, "User/bsp/gpio.h") + save_with_preserve(output_path, template_content) # 使用保留用户区域的写入 + return True def _generate_source_file(self, template_dir): template_path = os.path.join(template_dir, "gpio.c") template_content = CodeGenerator.load_template(template_path) if not template_content: return False - # 生成 IRQ 使能/禁用代码 enable_lines = [] disable_lines = [] for item in self.available_list: @@ -410,7 +508,8 @@ class bsp_gpio(QWidget): content, "AUTO GENERATED BSP_GPIO_DISABLE_IRQ", "\n".join(disable_lines) ) output_path = os.path.join(self.project_path, "User/bsp/gpio.c") - return CodeGenerator.save_file(content, output_path) + save_with_preserve(output_path, content) # 使用保留用户区域的写入 + return True def _save_config(self): config_path = os.path.join(self.project_path, "User/bsp/bsp_config.yaml") @@ -421,7 +520,26 @@ class bsp_gpio(QWidget): } CodeGenerator.save_config(config_data, config_path) - +def get_bsp_page(peripheral_name, project_path): + """根据外设名返回对应的页面类，没有特殊类则返回默认BspSimplePeripheral""" + name_lower = peripheral_name.lower() + special_classes = { + "i2c": bsp_i2c, + "can": bsp_can, + "spi": bsp_spi, + "uart": bsp_uart, + "gpio": bsp_gpio, + # 以后可以继续添加特殊外设 + } + if name_lower in special_classes: + return special_classes[name_lower](project_path) + else: + template_names = { + 'header': f'{name_lower}.h', + 'source': f'{name_lower}.c' + } + return BspSimplePeripheral(project_path, peripheral_name, template_names) + class bsp(QWidget): def __init__(self, project_path): super().__init__() @@ -429,25 +547,37 @@ class bsp(QWidget): @staticmethod def generate_bsp(project_path, pages): + """生成所有BSP代码""" + # 自动添加 bsp.h + src_bsp_h = os.path.join(os.path.dirname(__file__), "../../assets/User_code/bsp/bsp.h") + dst_bsp_h = os.path.join(project_path, "User/bsp/bsp.h") + os.makedirs(os.path.dirname(dst_bsp_h), exist_ok=True) + if os.path.exists(src_bsp_h): + shutil.copyfile(src_bsp_h, dst_bsp_h) + total = 0 success_count = 0 fail_count = 0 fail_list = [] + for page in pages: - name = page.objectName() if hasattr(page, "objectName") else str(page) - if hasattr(page, "is_need_generate") and page.is_need_generate(): - total += 1 - try: - result = page._generate_bsp_code_internal() - if result: - success_count += 1 - else: + # 只处理BSP页面：有 is_need_generate 方法但没有 component_name 属性的页面 + if hasattr(page, 'is_need_generate') and not hasattr(page, 'component_name'): + if page.is_need_generate(): + total += 1 + try: + result = page._generate_bsp_code_internal() + if result: + success_count += 1 + else: + fail_count += 1 + fail_list.append(page.__class__.__name__) + except Exception as e: fail_count += 1 - fail_list.append(name) - except Exception as e: - fail_count += 1 - fail_list.append(f"{name} (异常: {e})") + fail_list.append(f"{page.__class__.__name__} (异常: {e})") + msg = f"总共尝试生成 {total} 项，成功 {success_count} 项，失败 {fail_count} 项。" if fail_list: msg += "\n失败项：\n" + "\n".join(fail_list) - QMessageBox.information(None, "代码生成结果", msg) \ No newline at end of file + + return msg \ No newline at end of file diff --git a/app/code_page/component_interface.py b/app/code_page/component_interface.py index e69de29..1d05449 100644 --- a/app/code_page/component_interface.py +++ b/app/code_page/component_interface.py @@ -0,0 +1,552 @@ +from PyQt5.QtWidgets import QWidget, QVBoxLayout, QLineEdit, QCheckBox, QComboBox, QTableWidget, QHeaderView, QMessageBox, QHBoxLayout, QTextEdit +from qfluentwidgets import TitleLabel, BodyLabel, PushButton, CheckBox, TableWidget, LineEdit, ComboBox, MessageBox, SubtitleLabel, FluentIcon, TextEdit +from qfluentwidgets import InfoBar +from PyQt5.QtCore import Qt, pyqtSignal +from app.tools.code_generator import CodeGenerator +import os +import csv +import shutil +import re + +def preserve_all_user_regions(new_code, old_code): + """ Preserves all user-defined regions in the new code based on the old code. + This function uses regex to find user-defined regions in the old code and replaces them in the new code. + Args: + new_code (str): The new code content. + old_code (str): The old code content. + Returns: + str: The new code with preserved user-defined regions. + """ + pattern = re.compile( + r"/\*\s*(USER [A-Z0-9_ ]+)\s*BEGIN\s*\*/(.*?)/\*\s*\1\s*END\s*\*/", + re.DOTALL + ) + old_regions = {m.group(1): m.group(2) for m in pattern.finditer(old_code or "")} + def repl(m): + region = m.group(1) + old_content = old_regions.get(region) + if old_content is not None: + return m.group(0).replace(m.group(2), old_content) + return m.group(0) + return pattern.sub(repl, new_code) + +def save_with_preserve(path, new_code): + """保存文件并保留用户区域""" + if os.path.exists(path): + with open(path, "r", encoding="utf-8") as f: + old_code = f.read() + new_code = preserve_all_user_regions(new_code, old_code) + os.makedirs(os.path.dirname(path), exist_ok=True) + with open(path, "w", encoding="utf-8") as f: + f.write(new_code) + +def load_descriptions(csv_path): + """加载组件描述信息""" + descriptions = {} + if os.path.exists(csv_path): + with open(csv_path, encoding='utf-8') as f: + reader = csv.reader(f) + for row in reader: + if len(row) >= 2: + key, desc = row[0].strip(), row[1].strip() + descriptions[key.lower()] = desc + return descriptions + +def load_dependencies(csv_path): + """加载组件依赖关系""" + dependencies = {} + if os.path.exists(csv_path): + with open(csv_path, encoding='utf-8') as f: + reader = csv.reader(f) + for row in reader: + if len(row) >= 2: + component = row[0].strip() + deps = [dep.strip() for dep in row[1:] if dep.strip()] + dependencies[component] = deps + return dependencies + + +def get_component_page(component_name, project_path, component_manager=None): + """根据组件名返回对应的页面类，没有特殊类则返回默认ComponentSimple""" + name_lower = component_name.lower() + special_classes = { + "pid": component_pid, + "filter": component_filter, + # 以后可以继续添加特殊组件 + } + if name_lower in special_classes: + return special_classes[name_lower](project_path, component_manager) + else: + template_names = { + 'header': f'{name_lower}.h', + 'source': f'{name_lower}.c' + } + return ComponentSimple(project_path, component_name, template_names, component_manager) + + + +def get_all_dependency_components(dependencies): + """获取所有被依赖的组件列表""" + dependent_components = set() + for component, deps in dependencies.items(): + for dep_path in deps: + dep_name = os.path.basename(dep_path) + dependent_components.add(dep_name.lower()) + return dependent_components + +class ComponentSimple(QWidget): + """简单组件界面 - 只有开启/关闭功能""" + + # 添加信号，用于通知其他组件状态变化 + dependency_changed = pyqtSignal(str, bool) # 组件名, 是否启用 + + def __init__(self, project_path, component_name, template_names, component_manager=None): + super().__init__() + self.project_path = project_path + self.component_name = component_name + self.template_names = template_names + self.component_manager = component_manager + + # 加载描述和依赖信息 + component_dir = os.path.join(os.path.dirname(__file__), "../../assets/User_code/component") + describe_path = os.path.join(component_dir, "describe.csv") + dependencies_path = os.path.join(component_dir, "dependencies.csv") + + self.descriptions = load_descriptions(describe_path) + self.dependencies = load_dependencies(dependencies_path) + self.all_dependent_components = get_all_dependency_components(self.dependencies) + + # 判断当前组件是否被其他组件依赖 + self.is_dependency = self.component_name.lower() in self.all_dependent_components + + # 强制启用状态相关 + self._forced_enabled = False + self._dependency_count = 0 # 有多少个组件依赖此组件 + + self._init_ui() + self._load_config() + + def _init_ui(self): + layout = QVBoxLayout(self) + + # 顶部横向布局：左侧复选框，居中标题 + top_layout = QHBoxLayout() + top_layout.setAlignment(Qt.AlignVCenter) + + # 所有组件都有复选框 + self.generate_checkbox = CheckBox(f"启用 {self.component_name}") + self.generate_checkbox.stateChanged.connect(self._on_checkbox_changed) + top_layout.addWidget(self.generate_checkbox, alignment=Qt.AlignLeft) + + # 如果是被依赖的组件，添加状态标签 + if self.is_dependency: + self.dependency_status_label = BodyLabel("") + self.dependency_status_label.setStyleSheet("color: #888888; font-style: italic; margin-left: 10px;") + top_layout.addWidget(self.dependency_status_label, alignment=Qt.AlignLeft) + + # 弹性空间 + top_layout.addStretch() + + title = SubtitleLabel(f"{self.component_name} 配置 ") + title.setAlignment(Qt.AlignHCenter) + top_layout.addWidget(title, alignment=Qt.AlignHCenter) + + # 再加一个弹性空间，保证标题居中 + top_layout.addStretch() + + layout.addLayout(top_layout) + + # 功能说明 + desc = self.descriptions.get(self.component_name.lower(), "") + if desc: + desc_label = BodyLabel(f"功能说明：{desc}") + desc_label.setWordWrap(True) + layout.addWidget(desc_label) + + # 依赖信息 + deps = self.dependencies.get(self.component_name.lower(), []) + if deps: + deps_text = f"依赖组件：{', '.join([os.path.basename(dep) for dep in deps])}" + self.deps_label = BodyLabel(deps_text) + self.deps_label.setWordWrap(True) + self.deps_label.setStyleSheet("color: #888888;") + layout.addWidget(self.deps_label) + + # 依赖状态显示 + self.deps_status_widget = QWidget() + deps_status_layout = QVBoxLayout(self.deps_status_widget) + deps_status_layout.setContentsMargins(20, 10, 20, 10) + + self.deps_checkboxes = {} + for dep in deps: + # 从路径中提取组件名 + dep_name = os.path.basename(dep) + dep_checkbox = CheckBox(f"自动启用 {dep_name}") + dep_checkbox.setEnabled(False) # 依赖项自动管理，不允许手动取消 + deps_status_layout.addWidget(dep_checkbox) + self.deps_checkboxes[dep] = dep_checkbox + + layout.addWidget(self.deps_status_widget) + + # 如果是被依赖的组件，显示被哪些组件依赖 + if self.is_dependency: + dependent_by = [] + for component, deps in self.dependencies.items(): + for dep_path in deps: + if os.path.basename(dep_path).lower() == self.component_name.lower(): + dependent_by.append(component) + + if dependent_by: + dependent_text = f"被以下组件依赖：{', '.join(dependent_by)}" + dependent_label = BodyLabel(dependent_text) + dependent_label.setWordWrap(True) + dependent_label.setStyleSheet("color: #0078d4;") + layout.addWidget(dependent_label) + + layout.addStretch() + + # 初始化界面状态 + self._update_ui_state() + + def _update_ui_state(self): + """更新界面状态""" + if self.is_dependency: + if self._dependency_count > 0: + # 有组件依赖此组件，设置为强制启用状态 + self.generate_checkbox.setEnabled(False) + self.generate_checkbox.setChecked(True) + self.dependency_status_label.setText(f"(被 {self._dependency_count} 个组件自动启用)") + self.dependency_status_label.setStyleSheet("color: #0078d4; font-style: italic; margin-left: 10px;") + else: + # 没有组件依赖此组件，恢复正常状态 + self.generate_checkbox.setEnabled(True) + self.dependency_status_label.setText("(可选组件)") + self.dependency_status_label.setStyleSheet("color: #888888; font-style: italic; margin-left: 10px;") + + def _on_checkbox_changed(self, state): + """处理复选框状态变化，自动管理依赖""" + # 如果是被强制启用的，不允许用户取消 + if self.is_dependency and self._dependency_count > 0: + return + + if state == 2: # 选中状态 + # 自动选中所有依赖项 + deps = self.dependencies.get(self.component_name.lower(), []) + for dep in deps: + if dep in self.deps_checkboxes: + self.deps_checkboxes[dep].setChecked(True) + + # 通知组件管理器启用依赖项 + if self.component_manager: + self.component_manager.enable_dependencies(self.component_name, deps) + else: # 未选中状态 + # 取消选中所有依赖项 + if hasattr(self, 'deps_checkboxes'): + for checkbox in self.deps_checkboxes.values(): + checkbox.setChecked(False) + + # 通知组件管理器禁用依赖项 + if self.component_manager: + deps = self.dependencies.get(self.component_name.lower(), []) + self.component_manager.disable_dependencies(self.component_name, deps) + + def set_forced_enabled(self, enabled: bool): + """设置强制启用状态（用于依赖自动启用）""" + self._forced_enabled = enabled + if enabled: + self.set_dependency_count(max(1, self._dependency_count)) + else: + self.set_dependency_count(0) + + def is_need_generate(self): + """检查是否需要生成代码""" + return self.generate_checkbox.isChecked() + + def set_dependency_count(self, count): + """设置依赖计数并更新UI状态""" + self._dependency_count = count + if count > 0: + self._forced_enabled = True + if not self.generate_checkbox.isChecked(): + # 阻止信号触发，直接设置状态 + self.generate_checkbox.blockSignals(True) + self.generate_checkbox.setChecked(True) + self.generate_checkbox.blockSignals(False) + else: + self._forced_enabled = False + + self._update_ui_state() + self._save_config() # 保存状态变化 + + def get_enabled_dependencies(self): + """获取已启用的依赖项列表""" + if not self.is_need_generate(): + return [] + return self.dependencies.get(self.component_name.lower(), []) + + def _generate_component_code_internal(self): + """生成组件代码""" + if not self.is_need_generate(): + return False + + template_dir = self._get_component_template_dir() + + # 生成头文件和源文件 + for key, filename in self.template_names.items(): + template_path = os.path.join(template_dir, filename) + template_content = CodeGenerator.load_template(template_path) + if not template_content: + print(f"模板文件不存在或为空: {template_path}") + continue + + output_path = os.path.join(self.project_path, f"User/component/{filename}") + save_with_preserve(output_path, template_content) + + self._save_config() + return True + + def _get_component_template_dir(self): + """获取组件模板目录""" + current_dir = os.path.dirname(os.path.abspath(__file__)) + # 向上找到 MRobot 根目录 + while os.path.basename(current_dir) != 'MRobot' and current_dir != '/': + current_dir = os.path.dirname(current_dir) + + if os.path.basename(current_dir) == 'MRobot': + return os.path.join(current_dir, "assets/User_code/component") + else: + # 如果找不到，使用相对路径作为备选 + return os.path.join(os.path.dirname(os.path.dirname(os.path.dirname(__file__))), "assets/User_code/component") + + def _save_config(self): + """保存配置""" + config_path = os.path.join(self.project_path, "User/component/component_config.yaml") + config_data = CodeGenerator.load_config(config_path) + config_data[self.component_name.lower()] = { + 'enabled': self.is_need_generate(), + 'dependencies': self.dependencies.get(self.component_name.lower(), []), + 'is_dependency': self.is_dependency, + 'dependency_count': self._dependency_count, + 'forced_enabled': self._forced_enabled + } + CodeGenerator.save_config(config_data, config_path) + + def _load_config(self): + """加载配置""" + config_path = os.path.join(self.project_path, "User/component/component_config.yaml") + config_data = CodeGenerator.load_config(config_path) + conf = config_data.get(self.component_name.lower(), {}) + + # 加载依赖计数 + self._dependency_count = conf.get('dependency_count', 0) + self._forced_enabled = conf.get('forced_enabled', False) + + # 设置复选框状态 + if conf.get('enabled', False): + self.generate_checkbox.setChecked(True) + + # 更新UI状态 + self._update_ui_state() + +class ComponentManager: + """组件依赖管理器""" + + def __init__(self): + self.component_pages = {} # 组件名 -> 页面对象 + self.dependency_count = {} # 被依赖组件 -> 依赖计数 + + def register_component(self, component_name, page): + """注册组件页面""" + self.component_pages[component_name.lower()] = page + + # 注册后立即同步状态 + self._sync_dependency_states() + + def _sync_dependency_states(self): + """同步所有依赖状态""" + # 重新计算所有依赖计数 + new_dependency_count = {} + + for page_name, page in self.component_pages.items(): + if page.is_need_generate(): + deps = page.get_enabled_dependencies() + for dep_path in deps: + dep_name = os.path.basename(dep_path).lower() + new_dependency_count[dep_name] = new_dependency_count.get(dep_name, 0) + 1 + + # 更新依赖计数 + self.dependency_count = new_dependency_count + + # 更新所有页面的状态 + for page_name, page in self.component_pages.items(): + if page.is_dependency: + count = self.dependency_count.get(page_name, 0) + page.set_dependency_count(count) + + def enable_dependencies(self, component_name, deps): + """启用依赖项""" + for dep_path in deps: + dep_name = os.path.basename(dep_path).lower() + + # 增加依赖计数 + self.dependency_count[dep_name] = self.dependency_count.get(dep_name, 0) + 1 + + # 更新被依赖的组件状态 + if dep_name in self.component_pages: + page = self.component_pages[dep_name] + page.set_dependency_count(self.dependency_count[dep_name]) + + def disable_dependencies(self, component_name, deps): + """禁用依赖项""" + for dep_path in deps: + dep_name = os.path.basename(dep_path).lower() + + # 减少依赖计数 + if dep_name in self.dependency_count: + self.dependency_count[dep_name] = max(0, self.dependency_count[dep_name] - 1) + + # 更新被依赖的组件状态 + if dep_name in self.component_pages: + page = self.component_pages[dep_name] + page.set_dependency_count(self.dependency_count[dep_name]) + +# 具体组件类 +class component_pid(ComponentSimple): + def __init__(self, project_path, component_manager=None): + super().__init__( + project_path, + "PID", + {'header': 'pid.h', 'source': 'pid.c'}, + component_manager + ) + +class component_filter(ComponentSimple): + def __init__(self, project_path, component_manager=None): + super().__init__( + project_path, + "Filter", + {'header': 'filter.h', 'source': 'filter.c'}, + component_manager + ) + +# ...existing code... (component 类的 generate_component 方法保持不变) + +class component(QWidget): + """组件管理器""" + + def __init__(self, project_path): + super().__init__() + self.project_path = project_path + + @staticmethod + def generate_component(project_path, pages): + """生成所有组件代码，处理依赖关系""" + # 自动添加 component.h + src_component_h = os.path.join(os.path.dirname(__file__), "../../assets/User_code/component/component.h") + dst_component_h = os.path.join(project_path, "User/component/component.h") + os.makedirs(os.path.dirname(dst_component_h), exist_ok=True) + if os.path.exists(src_component_h): + shutil.copyfile(src_component_h, dst_component_h) + + # 收集所有需要生成的组件和它们的依赖 + components_to_generate = set() + component_pages = {} + + for page in pages: + # 检查是否是组件页面（通过类名或者属性判断） + if hasattr(page, "component_name") and hasattr(page, "is_need_generate"): + if page.is_need_generate(): + component_name = page.component_name.lower() + components_to_generate.add(component_name) + component_pages[component_name] = page + + # 添加依赖组件，依赖格式是路径形式如 "component/filter" + deps = page.get_enabled_dependencies() + for dep_path in deps: + # 从路径中提取组件名，如 "component/filter" -> "filter" + dep_name = os.path.basename(dep_path) + components_to_generate.add(dep_name) + + # 为没有对应页面但需要生成的依赖组件创建临时页面 + user_code_dir = os.path.join(os.path.dirname(__file__), "../../assets/User_code") + for comp_name in components_to_generate: + if comp_name not in component_pages: + # 创建临时组件页面 + template_names = {'header': f'{comp_name}.h', 'source': f'{comp_name}.c'} + temp_page = ComponentSimple(project_path, comp_name.upper(), template_names) + temp_page.set_forced_enabled(True) # 自动启用依赖组件 + component_pages[comp_name] = temp_page + + # 如果没有组件需要生成，返回提示信息 + if not components_to_generate: + return "没有启用的组件需要生成代码。" + + # 生成代码和依赖文件 + success_count = 0 + fail_count = 0 + fail_list = [] + + # 处理依赖文件的复制 + all_deps = set() + for page in pages: + if hasattr(page, "component_name") and hasattr(page, "is_need_generate"): + if page.is_need_generate(): + deps = page.get_enabled_dependencies() + all_deps.update(deps) + + # 复制依赖文件 + for dep_path in all_deps: + try: + # dep_path 格式如 "component/filter" + src_dir = os.path.join(user_code_dir, dep_path) + if os.path.isdir(src_dir): + # 如果是目录，复制整个目录 + dst_dir = os.path.join(project_path, "User", dep_path) + os.makedirs(os.path.dirname(dst_dir), exist_ok=True) + if os.path.exists(dst_dir): + shutil.rmtree(dst_dir) + shutil.copytree(src_dir, dst_dir) + else: + # 如果是文件，复制单个文件 + src_file = src_dir + dst_file = os.path.join(project_path, "User", dep_path) + os.makedirs(os.path.dirname(dst_file), exist_ok=True) + if os.path.exists(src_file): + shutil.copyfile(src_file, dst_file) + success_count += 1 + print(f"成功复制依赖: {dep_path}") + except Exception as e: + fail_count += 1 + fail_list.append(f"{dep_path} (依赖复制异常: {e})") + print(f"复制依赖失败: {dep_path}, 错误: {e}") + + # 生成组件代码 + for comp_name in components_to_generate: + if comp_name in component_pages: + page = component_pages[comp_name] + try: + # 确保调用正确的方法名 + if hasattr(page, '_generate_component_code_internal'): + result = page._generate_component_code_internal() + if result: + success_count += 1 + print(f"成功生成组件: {comp_name}") + else: + fail_count += 1 + fail_list.append(f"{comp_name} (生成失败)") + print(f"生成组件失败: {comp_name}") + else: + fail_count += 1 + fail_list.append(f"{comp_name} (缺少生成方法)") + print(f"组件页面缺少生成方法: {comp_name}") + except Exception as e: + fail_count += 1 + fail_list.append(f"{comp_name} (生成异常: {e})") + print(f"生成组件异常: {comp_name}, 错误: {e}") + + total_items = len(all_deps) + len(components_to_generate) + msg = f"组件代码生成完成：总共尝试生成 {total_items} 项，成功 {success_count} 项，失败 {fail_count} 项。" + if fail_list: + msg += "\n失败项：\n" + "\n".join(fail_list) + + return msg \ No newline at end of file diff --git a/app/data_interface.py b/app/data_interface.py index 3c46bad..05050b4 100644 --- a/app/data_interface.py +++ b/app/data_interface.py @@ -553,6 +553,7 @@ class DataInterface(QWidget): parent=self, duration=2000 ) + self.task_generate_success = True # 添加这一句 except Exception as e: InfoBar.error( title="生成失败", diff --git a/assets/User_code/bsp/describe.csv b/assets/User_code/bsp/describe.csv index b54ab16..620500e 100644 --- a/assets/User_code/bsp/describe.csv +++ b/assets/User_code/bsp/describe.csv @@ -1,5 +1,6 @@ uart,要求开启dma和中断 -can,要求开启can的中断 +can,要求开启can的中断要求开启can的中断要求开启can的中断要求开启can的中断要求开启can的中断要求开启can的中断要求开启can的中断要求开启can的中断要求开启can的中断要求开启can的中断要求开启can的中断要求开启can的中断要求开启can的中断 gpio,要求设置label开启中断 spi,要求开启spi中断 -i2c,要求开始spi中断 \ No newline at end of file +i2c,要求开始spi中断 +mm,这是套了一层的内存 \ No newline at end of file diff --git a/assets/User_code/bsp/i2c.h b/assets/User_code/bsp/i2c.h index c88fa87..fecb246 100644 --- a/assets/User_code/bsp/i2c.h +++ b/assets/User_code/bsp/i2c.h @@ -18,7 +18,7 @@ extern "C" { /* I2C实体枚举，与设备对应 */ typedef enum { /* AUTO GENERATED BSP_I2C_NAME */ - /* USER BSP_I2C END*/ + /* USER BSP_I2C BEGIN*/ /* USER_I2C_XXX */ /* USER BSP_I2C END */ BSP_I2C_NUM, diff --git a/assets/User_code/bsp/servo_pwm.c b/assets/User_code/bsp/servo_pwm.c deleted file mode 100644 index 4648e7a..0000000 --- a/assets/User_code/bsp/servo_pwm.c +++ /dev/null @@ -1,48 +0,0 @@ -/* 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; - - -}; - - - - diff --git a/assets/User_code/bsp/servo_pwm.h b/assets/User_code/bsp/servo_pwm.h deleted file mode 100644 index aefb7f1..0000000 --- a/assets/User_code/bsp/servo_pwm.h +++ /dev/null @@ -1,45 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" { -#endif - -/* Includes ----------------------------------------------------------------- */ -#include -#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 - - - - - diff --git a/assets/User_code/component/ahrs.c b/assets/User_code/component/ahrs.c new file mode 100644 index 0000000..5886297 --- /dev/null +++ b/assets/User_code/component/ahrs.c @@ -0,0 +1,405 @@ +/* + 开源的AHRS算法。 + MadgwickAHRS +*/ + +#include "ahrs.h" + +#include + +#include "user_math.h" + +#define BETA_IMU (0.033f) +#define BETA_AHRS (0.041f) + +/* 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)); } diff --git a/assets/User_code/component/ahrs.h b/assets/User_code/component/ahrs.h new file mode 100644 index 0000000..add8b8b --- /dev/null +++ b/assets/User_code/component/ahrs.h @@ -0,0 +1,98 @@ +/* + 开源的AHRS算法。 + MadgwickAHRS +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "user_math.h" + +/* 欧拉角（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; + +/** + * @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); + +#ifdef __cplusplus +} +#endif diff --git a/assets/User_code/component/dependencies.csv b/assets/User_code/component/dependencies.csv index c38bef7..a39df2c 100644 --- a/assets/User_code/component/dependencies.csv +++ b/assets/User_code/component/dependencies.csv @@ -1,3 +1,3 @@ pid,component/filter -pid,component/user_math -filter,component/user_math \ No newline at end of file +ahrs,component/filter +filter,component/ahrs \ No newline at end of file diff --git a/assets/User_code/component/describe.csv b/assets/User_code/component/describe.csv index 903a492..1110872 100644 --- a/assets/User_code/component/describe.csv +++ b/assets/User_code/component/describe.csv @@ -1 +1,2 @@ -pid,好用的 \ No newline at end of file +pid,好用的 +ahrs,开源的 \ No newline at end of file diff --git a/assets/User_code/config.csv b/assets/User_code/config.csv index 4df4c5c..2aa03c1 100644 --- a/assets/User_code/config.csv +++ b/assets/User_code/config.csv @@ -1,4 +1,4 @@ bsp,can,delay,dwt,,gpio,i2c,mm,spi,uart, -component,peripheral,driver,library,hal,freertos,stm32cube +component,pid,filter,ahrs device,dr16,ai,nuc module,chassis,gimbal,arm,shoot \ No newline at end of file