robofish/MRobot
1
0
Fork 0
mirror of https://github.com/goldenfishs/MRobot.git synced 2025-11-01 20:13:11 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

bsp和component写完了

Browse Source
This commit is contained in:
Robofish 2025-08-05 14:22:31 +08:00
parent bebfbd716c
commit 2c9309ae1e
13 changed files with 1352 additions and 191 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

138
app/code_generate_interface.py
View File

@ -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
print(f"创建页面 {class_name} 失败: {e}")
return None
# ...existing code...

240
app/code_page/bsp_interface.py
View File

@ -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)
return msg

552
app/code_page/component_interface.py
View File

@ -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

1
app/data_interface.py
View File

@ -553,6 +553,7 @@ class DataInterface(QWidget):
parent=self,
duration=2000
)
self.task_generate_success = True # 添加这一句
except Exception as e:
InfoBar.error(
title="生成失败",

5
assets/User_code/bsp/describe.csv
View File

@ -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中断
i2c,要求开始spi中断
mm,这是套了一层的内存
1 uart 要求开启dma和中断
2 can 要求开启can的中断 要求开启can的中断要求开启can的中断要求开启can的中断要求开启can的中断要求开启can的中断要求开启can的中断要求开启can的中断要求开启can的中断要求开启can的中断要求开启can的中断要求开启can的中断要求开启can的中断要求开启can的中断
3 gpio 要求设置label开启中断
4 spi 要求开启spi中断
5 i2c 要求开始spi中断
6 mm 这是套了一层的内存

2
assets/User_code/bsp/i2c.h
View File

@ -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,

48
assets/User_code/bsp/servo_pwm.c
View File

@ -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;
};

45
assets/User_code/bsp/servo_pwm.h
View File

@ -1,45 +0,0 @@
#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

405
assets/User_code/component/ahrs.c Normal file
View File

@ -0,0 +1,405 @@
/*
开源的AHRS算法。
MadgwickAHRS
*/
#include "ahrs.h"
#include <string.h>
#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)); }

98
assets/User_code/component/ahrs.h Normal file
View File

@ -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

4
assets/User_code/component/dependencies.csv
View File

@ -1,3 +1,3 @@
pid,component/filter
pid,component/user_math
filter,component/user_math
ahrs,component/filter
filter,component/ahrs
1 pid component/filter
2 pid ahrs component/user_math component/filter
3 filter component/user_math component/ahrs

3
assets/User_code/component/describe.csv
View File

@ -1 +1,2 @@
pid,好用的
pid,好用的
ahrs,开源的
1 pid 好用的
2 ahrs 开源的

2
assets/User_code/config.csv
View File

@ -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
1 bsp,can,delay,dwt,,gpio,i2c,mm,spi,uart,
2 component,peripheral,driver,library,hal,freertos,stm32cube component,pid,filter,ahrs
3 device,dr16,ai,nuc
4 module,chassis,gimbal,arm,shoot