├── .clang-format ├── .clang-tidy ├── .clangd ├── .cmake-format.yaml ├── .editorconfig ├── .github ├── ISSUE_TEMPLATE │ ├── bug_report.md │ ├── config.yml │ └── feature_request.md ├── PULL_REQUEST_TEMPLATE.md └── workflows │ ├── ci.yml │ ├── pr-check.yml │ └── release.yml ├── .gitignore ├── .gitmodules ├── .markdownlint.json ├── .pre-commit-config.yaml ├── CMakeLists.txt ├── CONTRIBUTING.md ├── Documents ├── interlink │ ├── aresLink_UART.md │ ├── insLink_UART.html │ └── insLink_UART.md.future ├── motor │ ├── api.md │ └── devicetree.md └── zephyr │ ├── kconfig.md │ └── zephyr_app.md ├── Kconfig ├── README.md ├── README.md.eng ├── VSC_sample_configs ├── c_cpp_properties.json ├── launch.json ├── settings.json └── tasks.json ├── boards ├── damiao │ └── dm_mc02 │ │ ├── Kconfig.dm_mc02 │ │ ├── board.cmake │ │ ├── board.yml │ │ ├── dm_mc02.dts │ │ ├── dm_mc02.yaml │ │ ├── dm_mc02_defconfig │ │ └── support │ │ ├── STM32H723.svd │ │ ├── jlink.cfg │ │ └── openocd.cfg └── dji │ ├── robomaster_board_a │ ├── Kconfig.robomaster_board_a │ ├── board.cmake │ ├── board.yml │ ├── robomaster_board_a.dts │ ├── robomaster_board_a.yaml │ ├── robomaster_board_a_defconfig │ └── support │ │ ├── STM32F427.svd │ │ └── openocd.cfg │ └── robomaster_board_c │ ├── Kconfig.robomaster_board_c │ ├── board.cmake │ ├── board.yml │ ├── robomaster_board_c.dts │ ├── robomaster_board_c.yaml │ ├── robomaster_board_c_defconfig │ └── support │ ├── jlink.cfg │ ├── openocd.cfg │ └── stm32f407.svd ├── deassembly.sh ├── drivers ├── CMakeLists.txt ├── Kconfig ├── chassis │ ├── CMakeLists.txt │ ├── Kconfig │ ├── chassis.c │ └── chassis.h ├── graph │ ├── graph.h │ ├── inslink.c │ └── inslink.h ├── motor │ ├── CMakeLists.txt │ ├── Kconfig │ ├── common │ │ ├── CMakeLists.txt │ │ ├── common.c │ │ └── common.h │ ├── dji │ │ ├── CMakeLists.txt │ │ ├── dji_macros.h │ │ ├── dji_ratios.h │ │ ├── motor_dji.c │ │ └── motor_dji.h │ ├── dm │ │ ├── CMakeLists.txt │ │ ├── motor_dm.c │ │ └── motor_dm.h │ └── mi │ │ ├── CMakeLists.txt │ │ ├── motor_mi.c │ │ └── motor_mi.h ├── pid │ ├── CMakeLists.txt │ ├── Kconfig │ ├── pid_mit.c │ └── pid_single.c ├── sensor │ ├── CMakeLists.txt │ ├── Kconfig │ └── paw3395 │ │ ├── CMakeLists.txt │ │ ├── Kconfig │ │ ├── paw3395.c │ │ └── paw3395.h ├── transfer │ ├── CMakeLists.txt │ ├── Kconfig │ ├── ares_sbus.c │ └── ares_sbus.h └── wheel │ ├── CMakeLists.txt │ ├── Kconfig │ ├── mecanum.c │ └── steerwheel.c ├── dts └── bindings │ ├── canbus │ └── vnd,canbus.yaml │ ├── chassis │ └── ares,chassis.yaml │ ├── imu │ └── gyro-bias.yaml │ ├── motor │ ├── dji,motor.yaml │ ├── dm,motor.yaml │ └── mi,motor.yaml │ ├── pid │ ├── pid,single.yaml │ └── pid.mit.yaml │ ├── sensor │ └── pixart,paw3395.yaml │ ├── steerwheel │ ├── ares,mecanum.yaml │ └── ares,steerwheel.yaml │ ├── transfer │ └── ares,sbus.yaml │ └── vendor-prefixes.txt ├── example.dts ├── include ├── CMakeLists.txt ├── ares │ ├── ares_comm.h │ ├── board │ │ └── init.h │ ├── connectivity │ │ ├── uart_async_adapter.h │ │ └── uart_trans.h │ ├── ekf │ │ ├── QuaternionEKF.h │ │ ├── algorithm.h │ │ ├── imu_task.h │ │ ├── kalman_filter.h │ │ └── matrix_storage.h │ ├── interboard │ │ └── interboard.h │ ├── interface │ │ ├── ares_interface.h │ │ ├── uart │ │ │ └── uart.h │ │ └── usb │ │ │ ├── note.md │ │ │ └── usb_bulk.h │ ├── mahony │ │ ├── MahonyAHRS.h │ │ ├── algorithm.h │ │ ├── imu_task.h │ │ └── matrix_storage.h │ ├── protocol │ │ ├── ares_protocol.h │ │ ├── dual │ │ │ └── dual_protocol.h │ │ └── plotter │ │ │ └── aresplot_protocol.h │ ├── usb_bulk_trans │ │ └── usb_trans.h │ └── vofa │ │ └── justfloat.h └── zephyr │ └── drivers │ ├── chassis.h │ ├── motor.h │ ├── pid.h │ ├── sbus.h │ └── wheel.h ├── lib ├── CMakeLists.txt ├── Kconfig └── ares │ ├── CMakeLists.txt │ ├── Kconfig │ ├── ares_comm.h │ ├── board │ ├── CMakeLists.txt │ ├── init.c │ └── init.h │ ├── connectivity │ ├── CMakeLists.txt │ ├── Kconfig │ ├── uart_async_adapter.c │ ├── uart_async_adapter.h │ ├── uart_trans.c │ └── uart_trans.h │ ├── ekf │ ├── CMakeLists.txt │ ├── QuaternionEKF.c │ ├── QuaternionEKF.h │ ├── algorithm.c │ ├── algorithm.h │ ├── imu_task.c │ ├── imu_task.h │ ├── kalman_filter.c │ ├── kalman_filter.h │ ├── matrix_storage.c │ └── matrix_storage.h │ ├── interboard │ ├── CMakeLists.txt │ ├── interboard.c │ └── interboard.h │ ├── interface │ ├── CMakeLists.txt │ ├── Kconfig │ ├── ares_interface.h │ ├── uart │ │ ├── CMakeLists.txt │ │ ├── Kconfig │ │ └── uart.c │ └── usb │ │ ├── CMakeLists.txt │ │ ├── Kconfig │ │ ├── note.md │ │ ├── usb_bulk.c │ │ └── usb_bulk.h │ ├── mahony │ ├── CMakeLists.txt │ ├── MahonyAHRS.c │ ├── MahonyAHRS.h │ ├── algorithm.c │ ├── algorithm.h │ ├── imu_task.c │ ├── imu_task.h │ ├── matrix_storage.c │ └── matrix_storage.h │ ├── protocol │ ├── CMakeLists.txt │ ├── Kconfig │ ├── ares_protocol.h │ ├── dual │ │ ├── CMakeLists.txt │ │ └── dual_protocol.c │ └── plotter │ │ ├── CMakeLists.txt │ │ ├── Kconfig │ │ └── aresplot_protocol.c │ └── vofa │ ├── CMakeLists.txt │ ├── justfloat.c │ └── justfloat.h ├── samples ├── IMU │ ├── IMU │ │ ├── CMakeLists.txt │ │ ├── README.rst │ │ ├── boards │ │ │ └── robomaster_board_c.overlay │ │ ├── prj.conf │ │ ├── sample.yaml │ │ └── src │ │ │ ├── devices.h │ │ │ └── main.c │ └── IMU_Calibrate │ │ ├── CMakeLists.txt │ │ ├── README.rst │ │ ├── boards │ │ └── robomaster_board_c.overlay │ │ ├── prj.conf │ │ ├── prj.conf.old │ │ ├── sample.yaml │ │ ├── src │ │ ├── devices.h │ │ ├── main.c │ │ ├── matrix_storage.c │ │ └── matrix_storage.h │ │ └── threads.inf ├── chassis │ ├── CMakeLists.txt │ ├── README.rst │ ├── boards │ │ └── robomaster_board_c.overlay │ ├── prj.conf │ ├── sample.yaml │ ├── src │ │ ├── devices.h │ │ └── main.c │ └── threads.inf ├── communication │ ├── ares_communication │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── boards │ │ │ └── robomaster_board_c.overlay │ │ ├── prj.conf │ │ ├── sample_linux │ │ │ ├── example │ │ │ └── src │ │ │ │ ├── ares_protocol.cpp │ │ │ │ ├── ares_protocol.hpp │ │ │ │ └── easy_example.cpp │ │ └── src │ │ │ └── main.c │ ├── plotter_auto │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── prj.conf │ │ └── src │ │ │ └── main.c │ └── plotter_demo │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── prj.conf │ │ └── src │ │ └── main.c └── motor │ ├── dji_m3508_demo │ ├── CMakeLists.txt │ ├── README.md │ ├── app.overlay │ ├── prj.conf │ └── src │ │ └── main.c │ └── dm_demo │ ├── CMakeLists.txt │ ├── README.md │ ├── app.overlay │ ├── prj.conf │ └── src │ └── main.c ├── scripts ├── ares_gdb_plugin.py └── openocd.cfg ├── setup-precommit.sh ├── structure.png ├── submanifests └── externlib.yaml ├── west.yml └── zephyr └── module.yml /.clang-tidy: -------------------------------------------------------------------------------- 1 | Checks: "modernize-*,-modernize-use-nullptr,clang-analyzer-*,-clang-diagnostic-unknown-argument" 2 | WarningsAsErrors: "*" 3 | HeaderFilterRegex: "^(drivers/.*|include/zephyr/drivers/.*)$" 4 | AnalyzeTemporaryDtors: false 5 | FormatStyle: file 6 | SystemHeaders: false 7 | CompileFlags: 8 | Remove: [-mfp16-format=ieee] 9 | -------------------------------------------------------------------------------- /.clangd: -------------------------------------------------------------------------------- 1 | CompileFlags: 2 | Add: -Wno-unknown-warning-option 3 | Remove: [-m*, -f*] 4 | -------------------------------------------------------------------------------- /.cmake-format.yaml: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # CMake 格式化配置 3 | 4 | format: 5 | line_width: 100 6 | tab_size: 2 7 | use_tabchars: false 8 | max_subgroups_hwrap: 2 9 | max_pargs_hwrap: 6 10 | max_rows_cmdline: 2 11 | separate_ctrl_name_with_space: false 12 | separate_fn_name_with_space: false 13 | dangle_parens: false 14 | 15 | markup: 16 | enable_markup: false 17 | 18 | lint: 19 | disabled_codes: 20 | - C0113 # 禁用特定的警告 21 | -------------------------------------------------------------------------------- /.editorconfig: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # EditorConfig is awesome: https://EditorConfig.org 3 | 4 | # top-most EditorConfig file 5 | root = true 6 | 7 | # Unix-style newlines with a newline ending every file 8 | [*] 9 | end_of_line = lf 10 | insert_final_newline = true 11 | charset = utf-8 12 | trim_trailing_whitespace = true 13 | 14 | # C/C++ files 15 | [*.{c,cpp,h,hpp}] 16 | indent_style = tab 17 | indent_size = 8 18 | tab_width = 8 19 | 20 | # Python files 21 | [*.py] 22 | indent_style = space 23 | indent_size = 4 24 | 25 | # CMake files 26 | [CMakeLists.txt,*.cmake] 27 | indent_style = space 28 | indent_size = 2 29 | 30 | # YAML files 31 | [*.{yml,yaml}] 32 | indent_style = space 33 | indent_size = 2 34 | 35 | # Markdown files 36 | [*.md] 37 | trim_trailing_whitespace = false 38 | max_line_length = 100 39 | 40 | # Shell scripts 41 | [*.sh] 42 | indent_style = space 43 | indent_size = 2 44 | 45 | # Kconfig files 46 | [Kconfig*] 47 | indent_style = tab 48 | indent_size = 8 49 | 50 | # Device tree files 51 | [*.{dts,dtsi,overlay}] 52 | indent_style = tab 53 | indent_size = 8 54 | 55 | # JSON files 56 | [*.json] 57 | indent_style = space 58 | indent_size = 2 59 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug 报告 3 | about: 报告一个问题帮助我们改进 4 | title: '[BUG] ' 5 | labels: bug 6 | assignees: '' 7 | --- 8 | 9 | ## 问题描述 10 | 11 | 12 | 13 | ## 重现步骤 14 | 15 | 16 | 17 | 1. 执行 '...' 18 | 2. 点击 '...' 19 | 3. 滚动到 '...' 20 | 4. 看到错误 21 | 22 | ## 预期行为 23 | 24 | 25 | 26 | ## 实际行为 27 | 28 | 29 | 30 | ## 环境信息 31 | 32 | 33 | 34 | **硬件：** 35 | 36 | - 开发板：[例如：Robomaster Board C] 37 | - 电机型号：[例如：DJI M3508] 38 | - 其他硬件： 39 | 40 | **软件：** 41 | 42 | - OS：[例如：Ubuntu 22.04] 43 | - Zephyr 版本：[例如：v3.5.0] 44 | - West 版本：[输出 `west --version`] 45 | - SDK 版本：[例如：0.16.8] 46 | 47 | ## 日志/错误信息 48 | 49 | 50 | 51 | ```text 52 | 粘贴日志或错误信息 53 | ``` 54 | 55 | ## 截图 56 | 57 | 58 | 59 | ## 附加信息 60 | 61 | 62 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/config.yml: -------------------------------------------------------------------------------- 1 | blank_issues_enabled: false 2 | contact_links: 3 | - name: 📖 文档 4 | url: https://github.com/your-repo/motor/tree/master/Documents 5 | about: 查看项目文档 6 | - name: 💬 讨论 7 | url: https://github.com/your-repo/motor/discussions 8 | about: 在讨论区提问或分享想法 9 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: 功能请求 3 | about: 建议一个新功能或改进 4 | title: '[FEATURE] ' 5 | labels: enhancement 6 | assignees: '' 7 | --- 8 | 9 | ## 功能描述 10 | 11 | 12 | 13 | ## 问题/动机 14 | 15 | 16 | 17 | ## 建议的解决方案 18 | 19 | 20 | 21 | ## 替代方案 22 | 23 | 24 | 25 | ## 使用场景 26 | 27 | 28 | 29 | ## 实现建议 30 | 31 | 32 | 33 | ## 附加信息 34 | 35 | 36 | -------------------------------------------------------------------------------- /.github/PULL_REQUEST_TEMPLATE.md: -------------------------------------------------------------------------------- 1 | ## 描述 2 | 3 | 4 | 5 | ## 改动类型 6 | 7 | 8 | 9 | - [ ] 🐛 Bug 修复 10 | - [ ] ✨ 新功能 11 | - [ ] 📝 文档更新 12 | - [ ] 🎨 代码格式/风格调整 13 | - [ ] ♻️ 代码重构 14 | - [ ] ⚡ 性能优化 15 | - [ ] ✅ 测试相关 16 | - [ ] 🔧 构建/工具相关 17 | 18 | ## 相关 Issue 19 | 20 | 21 | 22 | Closes #(issue number) 23 | 24 | ## 测试 25 | 26 | 27 | 28 | - [ ] 代码通过所有 pre-commit 检查 29 | - [ ] 在以下开发板上测试通过： 30 | - [ ] Robomaster Board C 31 | - [ ] Robomaster Board A 32 | - [ ] DM MC02 33 | - [ ] 其他：___________ 34 | 35 | ## 检查清单 36 | 37 | 38 | 39 | - [ ] 我的代码遵循项目的编码规范 40 | - [ ] 我已经进行了自我审查 41 | - [ ] 我已经添加了必要的注释，特别是在难以理解的地方 42 | - [ ] 我已经更新了相关文档 43 | - [ ] 我的改动没有产生新的警告 44 | - [ ] 我已经添加了测试来证明我的修复有效或新功能正常工作 45 | - [ ] 新增和现有的单元测试在本地都通过 46 | - [ ] 所有文件都包含 SPDX 许可证头 47 | 48 | ## 截图/日志 49 | 50 | 51 | 52 | ## 其他说明 53 | 54 | 55 | -------------------------------------------------------------------------------- /.github/workflows/pr-check.yml: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | name: Pull Request 检查 3 | 4 | on: 5 | pull_request: 6 | types: [opened, synchronize, reopened] 7 | 8 | jobs: 9 | # PR 标题检查 10 | pr-title-check: 11 | name: 检查 PR 标题 12 | runs-on: ubuntu-latest 13 | steps: 14 | - name: 检查 PR 标题格式 15 | uses: amannn/action-semantic-pull-request@v5 16 | env: 17 | GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} 18 | with: 19 | types: | 20 | feat 21 | fix 22 | docs 23 | style 24 | refactor 25 | test 26 | chore 27 | requireScope: false 28 | 29 | # 代码大小检查 30 | code-size-check: 31 | name: 代码大小检查 32 | runs-on: ubuntu-latest 33 | steps: 34 | - name: 检出代码 35 | uses: actions/checkout@v4 36 | with: 37 | fetch-depth: 0 38 | 39 | - name: 检查代码变更大小 40 | run: | 41 | ADDED=$(git diff --stat origin/${{ github.base_ref }}...HEAD | tail -1 | awk '{print $4}') 42 | DELETED=$(git diff --stat origin/${{ github.base_ref }}...HEAD | tail -1 | awk '{print $6}') 43 | echo "新增行数: $ADDED" 44 | echo "删除行数: $DELETED" 45 | 46 | # 警告：如果单个 PR 修改超过 1000 行 47 | if [ "$ADDED" -gt 1000 ]; then 48 | echo "⚠️ 警告: PR 修改行数较多 ($ADDED 行)，建议拆分成多个小 PR" 49 | fi 50 | 51 | # 许可证检查 52 | license-check: 53 | name: 许可证检查 54 | runs-on: ubuntu-latest 55 | steps: 56 | - name: 检出代码 57 | uses: actions/checkout@v4 58 | 59 | - name: 检查新增文件是否包含 SPDX 许可证头 60 | run: | 61 | EXIT_CODE=0 62 | for file in $(git diff --name-only --diff-filter=A origin/${{ github.base_ref }}...HEAD); do 63 | if [[ "$file" =~ \.(c|cpp|h|py|cmake)$ ]]; then 64 | if ! grep -q "SPDX-License-Identifier" "$file"; then 65 | echo "❌ $file 缺少 SPDX 许可证头" 66 | EXIT_CODE=1 67 | fi 68 | fi 69 | done 70 | exit $EXIT_CODE 71 | 72 | # 检查是否修改了关键文件 73 | critical-files-check: 74 | name: 关键文件检查 75 | runs-on: ubuntu-latest 76 | steps: 77 | - name: 检出代码 78 | uses: actions/checkout@v4 79 | with: 80 | fetch-depth: 0 81 | 82 | - name: 检查是否修改了关键文件 83 | run: | 84 | CHANGED_FILES=$(git diff --name-only origin/${{ github.base_ref }}...HEAD) 85 | 86 | # 关键文件列表 87 | CRITICAL_FILES=".github/workflows/ci.yml west.yml .pre-commit-config.yaml" 88 | 89 | for file in $CRITICAL_FILES; do 90 | if echo "$CHANGED_FILES" | grep -q "^$file$"; then 91 | echo "⚠️ 检测到关键文件被修改: $file" 92 | echo "请确保你了解这个修改的影响，并在 PR 描述中说明修改原因" 93 | fi 94 | done 95 | 96 | # 评论 PR 97 | pr-comment: 98 | name: PR 自动评论 99 | runs-on: ubuntu-latest 100 | if: github.event.action == 'opened' 101 | steps: 102 | - name: 添加欢迎评论 103 | uses: actions/github-script@v7 104 | with: 105 | github-token: ${{ secrets.GITHUB_TOKEN }} 106 | script: | 107 | github.rest.issues.createComment({ 108 | issue_number: context.issue.number, 109 | owner: context.repo.owner, 110 | repo: context.repo.repo, 111 | body: '👋 感谢您的贡献！\n\n请确保：\n- ✅ 代码通过所有 CI 检查\n- ✅ 遵循项目编码规范\n- ✅ 添加必要的文档说明\n- ✅ 在实际硬件上测试（如果适用）\n\n详细的贡献指南请参考 [CONTRIBUTING.md](../blob/master/CONTRIBUTING.md)' 112 | }) 113 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | CMakeLists.txt.user 2 | CMakeCache.txt 3 | CMakeFiles 4 | CMakeScripts 5 | Testing 6 | Makefile 7 | cmake_install.cmake 8 | install_manifest.txt 9 | compile_commands.json 10 | CTestTestfile.cmake 11 | _deps 12 | CMakeUserPresets.json 13 | build 14 | .cache 15 | .DS_Store 16 | .VSCodeCounter 17 | app 18 | assembly.txt 19 | .vscode 20 | assembly.txt 21 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "app"] 2 | path = app 3 | url = git@github.com:ttwards/sustc_apps.git 4 | -------------------------------------------------------------------------------- /.markdownlint.json: -------------------------------------------------------------------------------- 1 | { 2 | "default": true, 3 | "MD013": false, 4 | "MD033": false, 5 | "MD041": false 6 | } 7 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | # Pre-commit hooks configuration for Zephyr RTOS motor control framework 3 | 4 | repos: 5 | # General code quality checks 6 | - repo: https://github.com/pre-commit/pre-commit-hooks 7 | rev: v4.6.0 8 | hooks: 9 | - id: trailing-whitespace 10 | name: Remove trailing whitespace 11 | - id: end-of-file-fixer 12 | name: Ensure file ends with a newline 13 | - id: check-yaml 14 | name: Check YAML file syntax 15 | args: ['--unsafe'] # Allow custom tags (required for Zephyr devicetree) 16 | - id: check-added-large-files 17 | name: Check for added large files 18 | args: ['--maxkb=1024'] 19 | - id: check-merge-conflict 20 | name: Check for merge conflict markers 21 | - id: check-case-conflict 22 | name: Check for file name case conflicts 23 | - id: mixed-line-ending 24 | name: Check for mixed line endings 25 | args: ['--fix=lf'] 26 | 27 | # C/C++ code formatting 28 | - repo: https://github.com/pre-commit/mirrors-clang-format 29 | rev: v19.1.6 30 | hooks: 31 | - id: clang-format 32 | name: C/C++ code formatting 33 | types_or: [c, c++] 34 | args: ['-i'] # Modify files in-place 35 | 36 | # CMake file formatting 37 | - repo: https://github.com/cheshirekow/cmake-format-precommit 38 | rev: v0.6.13 39 | hooks: 40 | - id: cmake-format 41 | name: CMake file formatting 42 | additional_dependencies: [pyyaml] 43 | - id: cmake-lint 44 | name: CMake file lint 45 | additional_dependencies: [pyyaml] 46 | args: ['--disabled-codes=C0113'] # Disable certain unnecessary warnings 47 | 48 | # Shell script lint 49 | - repo: https://github.com/shellcheck-py/shellcheck-py 50 | rev: v0.10.0.1 51 | hooks: 52 | - id: shellcheck 53 | name: Shell script lint 54 | 55 | # Python code formatting (for scripts) 56 | - repo: https://github.com/psf/black 57 | rev: 24.10.0 58 | hooks: 59 | - id: black 60 | name: Python code formatting 61 | language_version: python3 62 | 63 | # Python import sorting 64 | - repo: https://github.com/pycqa/isort 65 | rev: 5.13.2 66 | hooks: 67 | - id: isort 68 | name: Python import sorting 69 | args: ["--profile", "black"] 70 | 71 | # Markdown linting 72 | - repo: https://github.com/igorshubovych/markdownlint-cli 73 | rev: v0.41.0 74 | hooks: 75 | - id: markdownlint 76 | name: Markdown linting 77 | 78 | # Global configuration 79 | default_language_version: 80 | python: python3 81 | 82 | # Excluded files and directories 83 | exclude: | 84 | (?x)^( 85 | build/.*| 86 | \.git/.*| 87 | zephyr/.*| 88 | .*\.patch$| 89 | .*\.diff$ 90 | )$ 91 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 设置 Zephyr 基础路径 2 | set(ENV{ZEPHYR_BASE} "${CMAKE_CURRENT_SOURCE_DIR}/../zephyr") 3 | 4 | # 导出编译命令 5 | set(CMAKE_EXPORT_COMPILE_COMMANDS ON) 6 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -save-temps") 7 | 8 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -save-temps") 9 | 10 | set(CMAKE_C_STANDARD 11) 11 | set(CMAKE_CXX_STANDARD 14) 12 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 13 | set(CMAKE_CXX_STANDARD 14) 14 | 15 | set(CMAKE_C_EXTENSIONS OFF) 16 | set(CMAKE_CXX_EXTENSIONS OFF) 17 | 18 | # 查找 clang-tidy 可执行文件 19 | find_program(CLANG_TIDY_BIN NAMES "clang-tidy") 20 | 21 | # 如果找到 clang-tidy，则配置自动整理代码 22 | if(CLANG_TIDY_BIN) 23 | # 定义要检查的文件类型 24 | file(GLOB_RECURSE TIDY_SOURCES "${CMAKE_CURRENT_SOURCE_DIR}/drivers/**/*.c" 25 | "${CMAKE_CURRENT_SOURCE_DIR}/drivers/**/*.cpp" 26 | "${CMAKE_CURRENT_SOURCE_DIR}/include/zephyr/drivers/*.h") 27 | endif() # 结束 if(CLANG_TIDY_BIN) 28 | 29 | # 包含目录 30 | zephyr_include_directories(include) 31 | zephyr_syscall_include_directories(include) 32 | 33 | # 添加子目录 34 | add_subdirectory(drivers) 35 | add_subdirectory(lib) 36 | add_subdirectory(include) 37 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # 贡献指南 2 | 3 | ## 快速开始 4 | 5 | ### 安装 Pre-commit 6 | 7 | ```bash 8 | ./setup-precommit.sh 9 | ``` 10 | 11 | 或手动安装： 12 | 13 | ```bash 14 | pip install pre-commit 15 | pre-commit install 16 | ``` 17 | 18 | ### 提交代码 19 | 20 | ```bash 21 | git add . 22 | git commit -m "feat: 你的功能描述" # pre-commit 自动运行检查 23 | ``` 24 | 25 | ## 提交类型 26 | 27 | - `feat`: 新功能 28 | - `fix`: Bug 修复 29 | - `docs`: 文档更新 30 | - `style`: 代码格式 31 | - `refactor`: 重构 32 | - `test`: 测试 33 | - `chore`: 构建/工具 34 | 35 | ## 常用命令 36 | 37 | ```bash 38 | # Makefile 快捷命令 39 | make help # 查看所有命令 40 | make check # 运行所有检查 41 | make format # 自动格式化代码 42 | make build-all # 编译所有示例 43 | 44 | # Pre-commit 45 | pre-commit run --all-files # 检查所有文件 46 | pre-commit autoupdate # 更新 hooks 47 | 48 | # 编译测试 49 | west build -b robomaster_board_c samples/motor/dji_m3508_demo 50 | west build -b dm_mc02 samples/motor/dm_demo 51 | ``` 52 | 53 | ## 代码规范 54 | 55 | - 每个文件必须包含 SPDX 许可证头：`// SPDX-License-Identifier: Apache-2.0` 56 | - 使用 `.clang-format` 格式化 C/C++ 代码 57 | - 提交前运行 `make check` 58 | 59 | ## CI 流程 60 | 61 | 推送到 GitHub 后自动运行： 62 | 63 | 1. 代码格式检查 64 | 2. 多开发板编译测试 65 | 3. 静态代码分析 66 | 4. 文档检查 67 | 68 | ## 常见问题 69 | 70 | **Q: Pre-commit 失败？** 71 | 72 | ```bash 73 | make format # 自动修复格式问题 74 | ``` 75 | 76 | **Q: 临时跳过检查？** 77 | 78 | ```bash 79 | git commit --no-verify # 不推荐！ 80 | ``` 81 | 82 | **Q: 更新依赖？** 83 | 84 | ```bash 85 | west update 86 | west build -t pristine 87 | ``` 88 | 89 | --- 90 | 91 | 详细的 Zephyr 开发指南请参考 `Documents/` 目录。 92 | -------------------------------------------------------------------------------- /Documents/interlink/aresLink_UART.md: -------------------------------------------------------------------------------- 1 | ## ARES2025上下位机通信 2 | 3 | ### 一、执行 4 | 5 | #### 1. 发送帧 6 | 7 | ```mermaid 8 | packet-beta 9 | title 执行帧 10 | 0-1: "0xCAFE" 11 | 2-3: "ID" 12 | 4-7: "Arg1" 13 | 8-11: "Arg2" 14 | 12-15: "Arg3" 15 | 16-17: "Request ID" 16 | ``` 17 | 18 | #### 2. 接收帧 19 | 20 | ```mermaid 21 | packet-beta 22 | title 返回帧 23 | 0-1: "0x1A64" 24 | 2-3: "Func ID" 25 | 4-7: "Return Value" 26 | 8-9: "Value" 27 | ``` 28 | 29 | ### 二、数据 30 | 31 | #### 1. 接收帧 32 | 33 | ```mermaid 34 | packet-beta 35 | title 数据帧 36 | 0-1: "0xDA2A" 37 | 2-3: "Data ID" 38 | 4-5: "Num" 39 | 6-13: "Block 0" 40 | 14-21: "Block 1" 41 | 22-29: "Block..." 42 | 30-31: "CRC-16" 43 | 32-33: "0xDEAD" 44 | ``` 45 | 46 | ```mermaid 47 | packet-beta 48 | title 数据Block 49 | 0: "0x98" 50 | 1-2: "Var Code" 51 | 3-6: "Var Data" 52 | 7: "0x77" 53 | ``` 54 | 55 | #### 2. 注意事项 56 | 57 | 一帧数据中最多包含12个数据 58 | 59 | ### 三、错误 60 | 61 | #### 1. 错误帧 62 | 63 | ```mermaid 64 | packet-beta 65 | title 错误帧 66 | 0-1: "0x2BAD" 67 | 2-3: "Request ID" 68 | 4-5: "Error Code" 69 | 6-7: "CRC-8" 70 | 8-9: "0xDEAD" 71 | ``` 72 | 73 | #### 2. 错误码 74 | 75 | ```text 76 | 0xFAC2 CRC效验错误 77 | 0xF411 未知帧头 78 | 0x8848 未知帧尾 79 | 0x6666 未知变量码 80 | 0x2024 未知函数码 81 | 0x1024 参数数量错误 82 | 0xCAFE 发送超时 83 | 0x22DD 下位机忙 84 | 0x2401 下位机/上位机收到对方的错误码，但数据已经丢弃无法重发 85 | 0xBEEF 重要事件，需要对方立刻处理下一帧数据 86 | 0x90DE 下位机重启 87 | ``` 88 | 89 | 错误帧中的Request ID可以是执行帧的Request ID，也可以是数据帧的Data ID。 90 | 上位机最好保存尽可能多的数据，下位机会至少保留256Bytes 91 | 92 | #### 3. 错误处理 93 | 94 | 尽可能重发，如果重发次数超过3次，上位机应该停止发送数据，等待下位机重启。 95 | -------------------------------------------------------------------------------- /Documents/motor/api.md: -------------------------------------------------------------------------------- 1 | # 电机驱动API 2 | 3 | ```c 4 | motor_driver_api { 5 | int motor_get(const struct device *dev, motor_status_t *status); 6 | int motor_set(const struct device *dev, motor_status_t *status); 7 | void motor_control(const struct device *dev, enum motor_cmd cmd); 8 | }; 9 | ``` 10 | 11 | ```c 12 | /** 13 | * @brief 电机工作模式 14 | * 15 | * MIT: MIT模式 16 | * PV: 位置-速度控制 17 | * VO: 速度控制 18 | * ML_TORQUE: 多环扭矩控制 19 | * ML_ANGLE: 多环角度控制 20 | * ML_SPEED: 多环速度控制 21 | */ 22 | enum motor_mode { MIT, PV, VO, ML_TORQUE, ML_ANGLE, ML_SPEED }; 23 | ``` 24 | 25 | ```c 26 | /** 27 | * @brief 电机控制命令 28 | */ 29 | enum motor_cmd { ENABLE_MOTOR, DISABLE_MOTOR, SET_ZERO, CLEAR_PID, CLEAR_ERROR }; 30 | ``` 31 | 32 | ```c 33 | motor_status_t { 34 | float angle; 35 | float rpm; 36 | float torque; 37 | float temperature; /* Cannot be set in target */ 38 | int round_cnt; 39 | 40 | float speed_limit[2]; 41 | float torque_limit[2]; 42 | 43 | enum motor_mode mode; 44 | }; 45 | ``` 46 | 47 | ## 电机状态获取 48 | 49 | `motor_get(const struct device *dev, motor_status_t *status)`: 获取电机当前状态，参数为电机状态结构体。 50 | `motor_set(const struct device *dev, motor_status_t *status)`: 设置电机目标状态，参数为电机状态结构体。 51 | `motor_control(const struct device *dev, enum motor_cmd cmd)`: 控制电机，参数为控制命令。 52 | 53 | ## 示例 54 | 55 | 设置电机为多环-扭矩控制模式，并设置目标扭矩为10N*m。 56 | 57 | ```c 58 | motor_status_t status; 59 | status.mode = ML_TORQUE; 60 | status.torque = 10; 61 | motor_set(motor, &status); 62 | ``` 63 | 64 | 获取电机当前的扭矩 65 | 66 | ```c 67 | motor_status_t status; 68 | motor_get(motor, &status); 69 | LOG_INF("Motor torque: %.2f", status.torque); 70 | ``` 71 | -------------------------------------------------------------------------------- /Documents/motor/devicetree.md: -------------------------------------------------------------------------------- 1 | # 电机驱动 Device Tree 配置 2 | 3 | ## DJI Motor Device Tree Properties 4 | 5 | - `compatible`: 必须是"dji,motor" 6 | - `is_m3508`: 电机型号，可选 7 | - `is_m2006`: 电机型号，可选 8 | - `is_gm6020`: 电机型号，可选 9 | - `id`: 电机编号 10 | - `rx_id`: 电机接收ID 11 | - `tx_id`: 电机发送ID 12 | - `gear_ratio`: 减速比（需要乘100） 13 | - `status` = "okay": 启用电机 14 | - `can_channel`: CAN总线节点 15 | - `controllers`: PID控制器节点，是从前到后的串联环，暂时不支持MIT模式 16 | - `capabilities`: 指定PID环对应的控制对象，可选 17 | - "angle": 角度控制 18 | - "speed": 速度控制 19 | 20 | ```dts 21 | / { 22 | motor1 { 23 | compatible = "dji,motor"; 24 | is_m3508; 25 | id = <1>; 26 | rx_id = <0x201>; 27 | tx_id = <0x200>; 28 | gear_ratio = "19.20"; 29 | status = "okay"; 30 | can_channel = <&can1>; 31 | controllers = <&angle_pid_1 &speed_pid_1>; 32 | capabilities = "angle", "speed"; 33 | }; 34 | }; 35 | ``` 36 | 37 | ### 请注意，必须存在对应capabilities属性，否则电机驱动只能使用力矩控制 38 | 39 | ### 达妙电机 Device Tree Properties 40 | 41 | - `compatible`: 必须是"dm,motor" 42 | - `id`: 电机编号 43 | - `tx_id`: 电机发送ID 44 | - `rx_id`: 电机接收ID 45 | - `can_channel`: CAN总线节点 46 | - `gear_ratio`: 减速比 47 | - `controllers`: PID控制器节点，按顺序指定各控制环 48 | - `capabilities`: 指定控制模式，可选： 49 | - "mit": MIT力矩控制 50 | - "pv": 位置-速度双环控制 51 | - "vo": 速度-电流双环控制 52 | - `v_max`: 最大速度限制 53 | - `p_max`: 最大角度限制 54 | - `t_max`: 最大扭矩限制（ 55 | 56 | ```dts 57 | / { 58 | motor1: motor1{ 59 | compatible = "dm,motor"; 60 | id = <24>; 61 | rx_id = <24>; 62 | tx_id = <86>; 63 | gear_ratio = "1"; 64 | status = "okay"; 65 | can_channel = <&can1>; 66 | controllers = <&mit_pid_0>; 67 | capabilities = "mit"; 68 | p_max = "12.5"; 69 | v_max = "45"; 70 | t_max = "10"; 71 | }; 72 | }; 73 | ``` 74 | 75 | ### PID Device Tree Properties 76 | 77 | - `#controller-cells`: 必须是<0> 78 | - `compatible`: 必须是"pid,single" 79 | - `k_p`: 比例系数 80 | - `k_i`: 积分系数 81 | - `k_d`: 微分系数 82 | 83 | ```dts 84 | / { 85 | pid { 86 | angle_pid_1: angle_pid_1 { 87 | #controller-cells = <0>; 88 | compatible = "pid,single"; 89 | k_p = "7"; 90 | k_i = "0.3"; 91 | k_d = "12"; 92 | }; 93 | speed_pid_1: speed_pid_1 { 94 | #controller-cells = <0>; 95 | compatible = "pid,single"; 96 | k_p = "8.8"; 97 | k_i = "0.2"; 98 | k_d = "2"; 99 | }; 100 | }; 101 | }; 102 | ``` 103 | -------------------------------------------------------------------------------- /Documents/zephyr/kconfig.md: -------------------------------------------------------------------------------- 1 | ## Kconfig 2 | Kconfig是Zephyr的配置系统，用于配置代码的开关。Kconfig文件通常位于项目的根目录下，文件名为`prj.conf`。 3 | Kconfig的说明可以在drivers目录下找到，如`drivers/motor/Kconfig`。 4 | -------------------------------------------------------------------------------- /Documents/zephyr/zephyr_app.md: -------------------------------------------------------------------------------- 1 | # Zephyr 2 | zephyr是一个由Linux基金会支持的RTOS，目前对多种架构的多种MCU具有完整的支持。Zephyr的目标是提供一个小型、可扩展、实时的操作系统，适用于资源受限的设备。Zephyr的特点包括使用设备树描述外围硬件等 3 | # A Basic Zephyr Application 4 | ## 文件结构 5 | ``` 6 | app 7 | ├── CMakeLists.txt \\ CMake构建文件 8 | ├── README.rst \\ App描述，这是可选的 9 | ├── boards \\ 对应不同开发板 10 | │ └── robomaster_board_c.overlay \\ 外围设备配置文件 11 | ├── prj.conf \\ Kconfig配置文件 12 | ├── sample.yaml \\ App描述，这是可选的 13 | └── src \\ 源文件 14 | └── main.c 15 | ``` 16 | ## CMakeLists.txt 17 | ```cmake 18 | cmake_minimum_required(VERSION 3.20.0) \\ 指定cmake最低版本 19 | set(CMAKE_CXX_STANDARD 17) \\ 指定C++标准 20 | find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) \\ 引用Zephyr 21 | project(IMU) \\ 项目名 22 | 23 | FILE(GLOB app_sources src/*.c) \\ 搜索src目录下的所有.c文件 24 | 25 | target_sources(app PRIVATE ${app_sources}) \\ 将src文件添加到app目标 26 | ``` 27 | ## prj.conf 28 | 此目录下指定Kconfig设置，如： 29 | ``` 30 | # 启用dji电机驱动 31 | CONFIG_MOTOR=y 32 | CONFIG_MOTOR_DJI=y 33 | CONFIG_MOTOR_LOG_LEVEL=4 34 | CONFIG_CAN=y 35 | CONFIG_MOTOR_INIT_PRIORITY=90 36 | ``` 37 | 你可以在zephyr官网上和该项目下drivers/*/Kconfig文件中找到更多的配置选项 38 | 39 | ## boards 40 | 详见../motor/devicetree.md 41 | -------------------------------------------------------------------------------- /Kconfig: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021 Nordic Semiconductor ASA 2 | # SPDX-License-Identifier: Apache-2.0 3 | # 4 | # This Kconfig file is picked by the Zephyr build system because it is defined 5 | # as the module Kconfig entry point (see zephyr/module.yml). You can browse 6 | # module options by going to Zephyr -> Modules in Kconfig. 7 | 8 | rsource "drivers/Kconfig" 9 | rsource "lib/Kconfig" 10 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Zephyr RTOS 下位机框架 2 | ## 该项目希望以设备树形式描述机器人 3 | ### 最终的效果将如example.dts所示 4 | The structure is as shown below 5 | ![structure](https://mirrors.sustech.edu.cn/git/12411711/mambo/-/blob/main/structure.png "Structure") 6 | ## 如何在我的开发板上运行？（以Robomaster Developement Board C为例） 7 | ### Initialization 8 | 9 | First zephyr SDKs are needed. 10 | ```shell 11 | sudo apt update 12 | sudo apt upgrade 13 | sudo apt install --no-install-recommends git cmake ninja-build gperf \ 14 | ccache dfu-util device-tree-compiler wget \ 15 | python3-dev python3-pip python3-setuptools python3-tk python3-wheel xz-utils file \ 16 | make gcc gcc-multilib g++-multilib libsdl2-dev libmagic1 17 | ``` 18 | ### You should have a virtual environment if Python tells you to do so 19 | 20 | 可以提前更换PIP源: 21 | ```shell 22 | pip install --upgrade pip --index-url https://mirrors.sustech.edu.cn/pypi/web/simple 23 | pip config set global.index-url https://mirrors.sustech.edu.cn/pypi/web/simple 24 | ``` 25 | 26 | ```shell 27 | pip install west 28 | ``` 29 | 30 | Then initialize the workspace folder (``my-workspace``) where 31 | the ``example-application`` and all Zephyr modules will be cloned. Run the following 32 | command: 33 | 34 | ```shell 35 | # initialize my-workspace for the example-application (main branch) 36 | west init -m https://www.github.com/ttwards/mambo --mr master my-workspace 37 | # update Zephyr modules 38 | cd my-workspace 39 | west update 40 | ``` 41 | 42 | Then export a Zephyr CMake package. This allows CMake to automatically load boilerplate code required for building Zephyr applications. 43 | ```shell 44 | west zephyr-export 45 | pip install -r ./zephyr/scripts/requirements.txt 46 | west sdk install 47 | ``` 48 | 49 | ### Building and running 50 | 51 | To build the application, run the following command: 52 | 53 | ```shell 54 | cd mambo 55 | west build -b robomaster_board_c samples\motor\dji_m3508_demo 56 | ``` 57 | 58 | `$BOARD` is the target board. Here you can use `robomaster_board_c` 59 | 60 | A sample debug configuration is also provided. To apply it, run the following 61 | command: 62 | 63 | ```shell 64 | west build -b $BOARD samples\motor\dji_m3508_demo 65 | ``` 66 | 67 | Once you have built the application, run the following command to flash it: 68 | 69 | ```shell 70 | west flash 71 | ``` 72 | 默认为cmsis-dap, 如果使用stlink请加上`--runner stlink`, Jlink则为`--runner jlink` 73 | If everything goes well, the LED on the board should be blinking 74 | 75 | Then you can have your motors running! 76 | 77 | 另外，VSC_sample_configs下有.vscode的示例配置文件 78 | 79 | ## 开发工具 80 | 81 | ### Pre-commit 和 CI 82 | 83 | 本项目使用 pre-commit 和 GitHub Actions CI 来确保代码质量。 84 | 85 | **快速设置：** 86 | ```shell 87 | ./setup-precommit.sh 88 | make check 89 | ``` 90 | 91 | 详细说明请参考 [CONTRIBUTING.md](CONTRIBUTING.md) 92 | 93 | 详细的文档请参考`Documents`文件夹 94 | 95 | ``` 96 | 好无聊逗逗梅总吧 97 | 嘬嘬嘬𐃆 ˒˒ ͏ 98 | ͏ 99 | ͏ ╱|、 100 | (˚ˎ 。7 101 | |、˜ 〵 102 | じしˍ,_)ノ 103 | ``` 104 | -------------------------------------------------------------------------------- /README.md.eng: -------------------------------------------------------------------------------- 1 | # Motor Drivers for Zephyr RTOS 2 | ### I am totally against Linus's dictatorial behaviour of wiping out the Linux Maintainer on 10.23 on the basis of nationality. 3 | ## This a project supported by various members in SUSTech ARES the robot club. 4 | ### We are attempting to describe the robot in a device tree model. 5 | ### This helps cut down costs processing the relationships between chassis, motor or so. 6 | -------------------------------------------------------------------------------- /VSC_sample_configs/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "zephyr", 5 | "includePath": [ 6 | "${workspaceFolder}/../zephyr/**", 7 | "${workspaceFolder}/../zephyr/include/**" 8 | ], 9 | "defines": [], 10 | "compilerPath": "${userHome}/zephyr-sdk-0.17.0/arm-zephyr-eabi/bin/arm-zephyr-eabi-gcc", 11 | "cStandard": "c17", 12 | "cppStandard": "gnu++17", 13 | "intelliSenseMode": "macos-gcc-arm64" 14 | } 15 | ], 16 | "version": 4 17 | } 18 | -------------------------------------------------------------------------------- /VSC_sample_configs/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "VsCodeTaskButtons.tasks": [ 3 | { 4 | "label": "Build", 5 | "task": "Build", 6 | "tooltip": "Build the Zephyr project" 7 | }, 8 | { 9 | "label": "Flash", 10 | "task": "Flash", 11 | "tooltip": "Flash the Zephyr project to the board" 12 | }, 13 | { 14 | "label": "Build & Flash", 15 | "task": "Build & Flash", 16 | "tooltip": "Build and flash the Zephyr project" 17 | }, 18 | { 19 | "label": "Tidy", 20 | "task": "Tidy", 21 | "tooltip": "Build the Zephyr project" 22 | }, 23 | { 24 | "label": "Monitor", 25 | "task": "Monitor", 26 | "tooltip": "Monitor via RTT Console" 27 | } 28 | ], 29 | "[c]": { 30 | "editor.defaultFormatter": "llvm-vs-code-extensions.vscode-clangd", 31 | "editor.formatOnSave": true 32 | }, 33 | "C_Cpp.formatting": "clangFormat", 34 | "C_Cpp.errorSquiggles": "disabled", 35 | "cortex-debug.variableUseNaturalFormat": false, 36 | "C_Cpp.default.compileCommands": "${workspaceFolder}/.vscode/compile_commands.json", 37 | "cmake.configureOnOpen": false, 38 | 39 | /*editor*/ 40 | "editor.cursorBlinking": "smooth",//使编辑器光标的闪烁平滑，有呼吸感 41 | "editor.cursorSmoothCaretAnimation": "on",//让光标移动、插入变得平滑 42 | 43 | "editor.formatOnPaste": false,//在粘贴时格式化代码 44 | "editor.formatOnType": true,//敲完一行代码自动格式化 45 | "editor.smoothScrolling": true,//使编辑器滚动变平滑 46 | "editor.wordWrap": "on",//在文件内容溢出vscode显示区域时自动折行 47 | 48 | "editor.renderWhitespace": "boundary",//除了两个单词之间用于分隔单词的一个空格，以一个小灰点的样子使空格可见 49 | 50 | /*terminal*/ 51 | "terminal.integrated.defaultProfile.linux": "zsh", 52 | "terminal.integrated.profiles.linux": { 53 | "zsh": { 54 | "path": "/bin/zsh", 55 | "args": [] 56 | } 57 | }, 58 | "terminal.integrated.defaultProfile.osx": "zsh", 59 | "terminal.integrated.profiles.osx": { 60 | "zsh": { 61 | "path": "/bin/zsh", 62 | "args": [] 63 | } 64 | }, 65 | 66 | /*files*/ 67 | "files.autoSave": "afterDelay", 68 | "files.exclude": {//隐藏一些碍眼的文件夹 69 | "**/.git": true, 70 | "**/.svn": true, 71 | "**/.hg": true, 72 | "**/CVS": true, 73 | "**/.DS_Store": true, 74 | "**/tmp": true, 75 | "**/node_modules": true, 76 | "**/bower_components": true 77 | }, 78 | "files.watcherExclude": {//不索引一些不必要索引的大文件夹以减少内存和CPU消耗 79 | "**/.git/objects/**": true, 80 | "**/.git/subtree-cache/**": true, 81 | "**/node_modules/**": true, 82 | "**/tmp/**": true, 83 | "**/bower_components/**": true, 84 | "**/dist/**": true 85 | }, 86 | /*workbench*/ 87 | "workbench.list.smoothScrolling": true,//使文件列表滚动变平滑 88 | 89 | /*explorer*/ 90 | "explorer.confirmDelete": false,//删除文件时不弹出确认弹窗（因为很烦） 91 | "explorer.confirmDragAndDrop": false,//往左边文件资源管理器拖动东西来移动/复制时不显示确认窗口 92 | 93 | /*search*/ 94 | "search.followSymlinks": false,//据说可以减少vscode的CPU和内存占用 95 | 96 | /*debug*/ 97 | "debug.internalConsoleOptions": "openOnSessionStart",//每次调试都打开调试控制台，方便调试 98 | "debug.showBreakpointsInOverviewRuler": true,//在滚动条标尺上显示断点的位置，便于查找断点的位置 99 | "debug.saveBeforeStart": "nonUntitledEditorsInActiveGroup",//在启动调试会话前保存除了无标题文档以外的文档 100 | "debug.onTaskErrors": "showErrors", 101 | "stm32-for-vscode.armToolchainPath": false, //预启动任务出错后显示错误，并不启动调试 102 | } 103 | -------------------------------------------------------------------------------- /VSC_sample_configs/tasks.json: -------------------------------------------------------------------------------- 1 | { 2 | "version": "2.0.0", 3 | "tasks": [ 4 | { 5 | "label": "Build", 6 | "type": "shell", 7 | "command": "west build -p always -b robomaster_board_c app/IMU", 8 | "args": [], 9 | "options": { 10 | "env": { 11 | "PATH": "/opt/miniconda3/bin:${env:PATH}" 12 | } 13 | }, 14 | "group": { 15 | "kind": "build", 16 | "isDefault": false 17 | }, 18 | "problemMatcher": ["$gcc"], 19 | "detail": "Build the Zephyr project" 20 | }, 21 | { 22 | "label": "Flash", 23 | "type": "shell", 24 | "command": "west flash --runner openocd", 25 | "group": { 26 | "kind": "build", 27 | "isDefault": false 28 | }, 29 | "problemMatcher": [], 30 | "detail": "Flash the Zephyr project to the board" 31 | }, 32 | { 33 | "label": "Build & Flash", 34 | "type": "shell", 35 | "command": "west build -p always -b robomaster_board_c app/IMU && west flash --runner openocd", 36 | "group": { 37 | "kind": "build", 38 | "isDefault": false 39 | }, 40 | "problemMatcher": [], 41 | "detail": "Build and flash the Zephyr project" 42 | }, 43 | { 44 | "label": "Tidy", 45 | "type": "shell", 46 | "command": "west build -t tidy", 47 | "group": { 48 | "kind": "build", 49 | "isDefault": true 50 | }, 51 | "problemMatcher": ["$gcc"], 52 | "detail": "Tidy up codes" 53 | }, 54 | { 55 | "label": "Monitor", 56 | "type": "shell", 57 | "command": "pyocd rtt -t STM32F407IGHx", 58 | "group": { 59 | "kind": "build", 60 | "isDefault": false 61 | }, 62 | "problemMatcher": [], 63 | "detail": "Monitor via RTT Console" 64 | } 65 | ] 66 | } 67 | -------------------------------------------------------------------------------- /boards/damiao/dm_mc02/Kconfig.dm_mc02: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024, ttwards 2 | # SPDX-License-Identifier: Apache-2.0 3 | 4 | config BOARD_DM_MC02 5 | select SOC_STM32H723XX 6 | -------------------------------------------------------------------------------- /boards/damiao/dm_mc02/board.cmake: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | 3 | # keep first 4 | board_runner_args(stm32cubeprogrammer "--port=swd" "--reset-mode=hw") 5 | board_runner_args(openocd) 6 | 7 | board_runner_args(pyocd "--target=STM32H723VG") 8 | board_runner_args(jlink "--device=STM32H723VG" "--speed=12000") 9 | 10 | # keep first 11 | include(${ZEPHYR_BASE}/boards/common/stm32cubeprogrammer.board.cmake) 12 | include(${ZEPHYR_BASE}/boards/common/openocd.board.cmake) 13 | 14 | include(${ZEPHYR_BASE}/boards/common/jlink.board.cmake) 15 | include(${ZEPHYR_BASE}/boards/common/pyocd.board.cmake) 16 | -------------------------------------------------------------------------------- /boards/damiao/dm_mc02/board.yml: -------------------------------------------------------------------------------- 1 | board: 2 | name: dm_mc02 3 | vendor: damiao 4 | socs: 5 | - name: stm32h723xx 6 | -------------------------------------------------------------------------------- /boards/damiao/dm_mc02/dm_mc02.yaml: -------------------------------------------------------------------------------- 1 | identifier: dm_mc02 2 | name: Damiao MC02 3 | type: mcu 4 | arch: arm 5 | toolchain: 6 | - zephyr 7 | - gnuarmemb 8 | - xtools 9 | ram: 192 10 | flash: 512 11 | supported: 12 | - counter 13 | - gpio 14 | - watchdog 15 | - can 16 | testing: 17 | ignore_tags: 18 | - net 19 | - shell 20 | - console 21 | vendor: dji 22 | -------------------------------------------------------------------------------- /boards/damiao/dm_mc02/dm_mc02_defconfig: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025 RobotPilots-SZU 2 | 3 | # Enable MPU 4 | CONFIG_ARM_MPU=y 5 | 6 | # Enable HW stack protection 7 | CONFIG_HW_STACK_PROTECTION=y 8 | 9 | # Enable drivers 10 | CONFIG_DMA=y 11 | CONFIG_GPIO=y 12 | CONFIG_COUNTER=y 13 | CONFIG_PWM=y 14 | CONFIG_SERIAL=y 15 | CONFIG_SPI=y 16 | CONFIG_I2C=y 17 | CONFIG_CAN=y 18 | 19 | # On-board devices 20 | CONFIG_LED_STRIP=y 21 | CONFIG_SENSOR=y 22 | CONFIG_BMI08X=y 23 | -------------------------------------------------------------------------------- /boards/damiao/dm_mc02/support/jlink.cfg: -------------------------------------------------------------------------------- 1 | source [find interface/jlink.cfg] 2 | 3 | transport select swd 4 | adapter speed 4000 5 | 6 | set CHIPNAME STM32H723VG 7 | 8 | source [find target/stm32h7x.cfg] 9 | 10 | reset_config srst_only 11 | 12 | $_CHIPNAME.cpu0 configure -event gdb-attach { 13 | echo "Debugger attaching: halting execution" 14 | gdb_breakpoint_override hard 15 | reset halt 16 | } 17 | 18 | $_CHIPNAME.cpu0 configure -event gdb-detach { 19 | echo "Debugger detaching: resuming execution" 20 | resume 21 | } 22 | 23 | # rename init old_init 24 | # proc init {} { 25 | # old_init 26 | # reset halt 27 | # } 28 | -------------------------------------------------------------------------------- /boards/damiao/dm_mc02/support/openocd.cfg: -------------------------------------------------------------------------------- 1 | source [find interface/cmsis-dap.cfg] 2 | 3 | transport select swd 4 | 5 | source [find target/stm32h7x.cfg] 6 | adapter speed 10000 7 | -------------------------------------------------------------------------------- /boards/dji/robomaster_board_a/Kconfig.robomaster_board_a: -------------------------------------------------------------------------------- 1 | 2 | 3 | config BOARD_ROBOMASTER_BOARD_A 4 | select SOC_STM32F427XX 5 | -------------------------------------------------------------------------------- /boards/dji/robomaster_board_a/board.cmake: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | 3 | # keep first 4 | board_runner_args(stm32cubeprogrammer "--port=swd" "--reset-mode=hw") 5 | board_runner_args(openocd) 6 | board_runner_args(pyocd "--target=STM32F427II") 7 | board_runner_args(jlink "--device=STM32F427II" "--speed=4000") 8 | 9 | # keep first 10 | include(${ZEPHYR_BASE}/boards/common/stm32cubeprogrammer.board.cmake) 11 | include(${ZEPHYR_BASE}/boards/common/openocd.board.cmake) 12 | include(${ZEPHYR_BASE}/boards/common/jlink.board.cmake) 13 | include(${ZEPHYR_BASE}/boards/common/pyocd.board.cmake) 14 | -------------------------------------------------------------------------------- /boards/dji/robomaster_board_a/board.yml: -------------------------------------------------------------------------------- 1 | board: 2 | name: robomaster_board_a 3 | vendor: dji 4 | socs: 5 | - name: stm32f427xx 6 | -------------------------------------------------------------------------------- /boards/dji/robomaster_board_a/robomaster_board_a.yaml: -------------------------------------------------------------------------------- 1 | identifier: robomaster_board_a 2 | name: DJI Robomaster Development Board A 3 | type: mcu 4 | arch: arm 5 | toolchain: 6 | - zephyr 7 | - gnuarmemb 8 | - xtools 9 | ram: 256 10 | flash: 2048 11 | supported: 12 | - counter 13 | - gpio 14 | - watchdog 15 | - can 16 | testing: 17 | ignore_tags: 18 | - net 19 | - shell 20 | - console 21 | vendor: dji 22 | -------------------------------------------------------------------------------- /boards/dji/robomaster_board_a/robomaster_board_a_defconfig: -------------------------------------------------------------------------------- 1 | 2 | 3 | # Enable MPU 4 | CONFIG_ARM_MPU=n 5 | 6 | # Float i/o 7 | CONFIG_CBPRINTF_FP_SUPPORT=y 8 | 9 | # Enable HW stack protection 10 | CONFIG_HW_STACK_PROTECTION=n 11 | 12 | # enable GPIO 13 | CONFIG_GPIO=y 14 | 15 | #CAN 16 | CONFIG_CAN_DEFAULT_BITRATE=1000000 17 | 18 | 19 | CONFIG_ISR_STACK_SIZE=3200 20 | CONFIG_MAIN_STACK_SIZE=3200 21 | -------------------------------------------------------------------------------- /boards/dji/robomaster_board_a/support/openocd.cfg: -------------------------------------------------------------------------------- 1 | source [find interface/cmsis-dap.cfg] 2 | source [find target/stm32f4x.cfg] 3 | adapter speed 2000 4 | 5 | $_TARGETNAME configure -event gdb-attach { 6 | echo "Debugger attaching: halting execution" 7 | reset halt 8 | gdb_breakpoint_override hard 9 | } 10 | 11 | $_TARGETNAME configure -event gdb-detach { 12 | echo "Debugger detaching: resuming execution" 13 | resume 14 | } 15 | -------------------------------------------------------------------------------- /boards/dji/robomaster_board_c/Kconfig.robomaster_board_c: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024, ttwards 2 | # SPDX-License-Identifier: Apache-2.0 3 | 4 | config BOARD_ROBOMASTER_BOARD_C 5 | select SOC_STM32F407XG 6 | -------------------------------------------------------------------------------- /boards/dji/robomaster_board_c/board.cmake: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | 3 | # keep first 4 | board_runner_args(stm32cubeprogrammer "--port=swd" "--reset-mode=hw") 5 | board_runner_args(openocd) 6 | board_runner_args(pyocd "--target=STM32F407IG") 7 | board_runner_args(jlink "--device=STM32F407IG" "--speed=16000") 8 | 9 | # keep first 10 | include(${ZEPHYR_BASE}/boards/common/stm32cubeprogrammer.board.cmake) 11 | include(${ZEPHYR_BASE}/boards/common/openocd.board.cmake) 12 | include(${ZEPHYR_BASE}/boards/common/jlink.board.cmake) 13 | include(${ZEPHYR_BASE}/boards/common/pyocd.board.cmake) 14 | -------------------------------------------------------------------------------- /boards/dji/robomaster_board_c/board.yml: -------------------------------------------------------------------------------- 1 | board: 2 | name: robomaster_board_c 3 | vendor: dji 4 | socs: 5 | - name: stm32f407xx 6 | -------------------------------------------------------------------------------- /boards/dji/robomaster_board_c/robomaster_board_c.yaml: -------------------------------------------------------------------------------- 1 | identifier: robomaster_board_c 2 | name: DJI Robomaster Development Board C 3 | type: mcu 4 | arch: arm 5 | toolchain: 6 | - zephyr 7 | - gnuarmemb 8 | - xtools 9 | ram: 192 10 | flash: 1024 11 | supported: 12 | - counter 13 | - gpio 14 | - watchdog 15 | - can 16 | testing: 17 | ignore_tags: 18 | - net 19 | - shell 20 | - console 21 | vendor: dji 22 | -------------------------------------------------------------------------------- /boards/dji/robomaster_board_c/robomaster_board_c_defconfig: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024, ttwards 2 | # SPDX-License-Identifier: Apache-2.0 3 | 4 | # Enable MPU 5 | CONFIG_ARM_MPU=n 6 | 7 | # Float i/o 8 | CONFIG_CBPRINTF_FP_SUPPORT=y 9 | 10 | # Enable HW stack protection 11 | CONFIG_HW_STACK_PROTECTION=n 12 | 13 | # enable GPIO 14 | CONFIG_GPIO=y 15 | 16 | # Enable UART console 17 | CONFIG_SERIAL=y 18 | CONFIG_UART_ASYNC_API=y 19 | CONFIG_UART_INTERRUPT_DRIVEN=y 20 | 21 | # 启用 Shell 支持 22 | CONFIG_SHELL=n 23 | CONFIG_SHELL_BACKENDS=n 24 | CONFIG_SHELL_BACKEND_SERIAL=n 25 | 26 | # 启用 UART 控制台 27 | CONFIG_UART_CONSOLE=y 28 | 29 | # 禁用 SEGGER RTT 30 | CONFIG_USE_SEGGER_RTT=n 31 | 32 | # 启用事件支持 33 | CONFIG_EVENTS=y 34 | 35 | # 启用控制台支持 36 | CONFIG_CONSOLE=y 37 | 38 | CONFIG_LOG=y 39 | CONFIG_LOG_MODE_IMMEDIATE=y 40 | 41 | CONFIG_SPI=y 42 | CONFIG_SPI_STM32_DMA=y 43 | 44 | CONFIG_DMA=y 45 | 46 | CONFIG_FPU=y 47 | 48 | CONFIG_PWM=y 49 | 50 | CONFIG_SENSOR=y 51 | CONFIG_BMI08X=y 52 | CONFIG_BMI08X_ACCEL_TRIGGER_OWN_THREAD=y 53 | CONFIG_BMI08X_ACCEL_THREAD_PRIORITY=10 54 | CONFIG_BMI08X_ACCEL_THREAD_STACK_SIZE=2048 55 | CONFIG_BMI08X_GYRO_TRIGGER_OWN_THREAD=y 56 | CONFIG_BMI08X_GYRO_THREAD_PRIORITY=10 57 | CONFIG_BMI08X_GYRO_THREAD_STACK_SIZE=2048 58 | CONFIG_BMI08X_I2C_WRITE_BURST_SIZE=16 59 | 60 | CONFIG_IMU_PWM_TEMP_CTRL=y 61 | 62 | CONFIG_FLASH=y 63 | CONFIG_FLASH_MAP=y 64 | 65 | CONFIG_ZMS=y 66 | CONFIG_LOG=y 67 | 68 | CONFIG_DMA_STM32=y # STM32 UART 驱动 69 | 70 | CONFIG_THREAD_ANALYZER=y 71 | CONFIG_SCHED_THREAD_USAGE=y 72 | 73 | CONFIG_CMSIS_DSP=y 74 | CONFIG_CMSIS_DSP_MATRIX=y 75 | CONFIG_CMSIS_DSP_CONTROLLER=y 76 | CONFIG_CMSIS_DSP_FASTMATH=y 77 | 78 | CONFIG_PID=y 79 | 80 | CONFIG_ISR_STACK_SIZE=3200 81 | CONFIG_MAIN_STACK_SIZE=3200 82 | 83 | CONFIG_CAN_COUNT=2 84 | -------------------------------------------------------------------------------- /boards/dji/robomaster_board_c/support/jlink.cfg: -------------------------------------------------------------------------------- 1 | source [find interface/jlink.cfg] 2 | 3 | transport select swd 4 | adapter speed 12000 5 | 6 | source [find target/stm32f4x.cfg] 7 | -------------------------------------------------------------------------------- /boards/dji/robomaster_board_c/support/openocd.cfg: -------------------------------------------------------------------------------- 1 | source [find interface/cmsis-dap.cfg] 2 | source [find target/stm32f4x.cfg] 3 | adapter speed 10000 4 | 5 | $_TARGETNAME configure -event gdb-attach { 6 | echo "Debugger attaching: halting execution" 7 | reset halt 8 | gdb_breakpoint_override hard 9 | } 10 | 11 | $_TARGETNAME configure -event gdb-detach { 12 | echo "Debugger detaching: resuming execution" 13 | resume 14 | } 15 | -------------------------------------------------------------------------------- /deassembly.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | objdump -S build/zephyr/zephyr.elf > assembly.txt 3 | -------------------------------------------------------------------------------- /drivers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | # SPDX-License-Identifier: Apache-2.0 3 | 4 | # FIXME: SHADOW_VARS: Remove this once we have enabled -Wshadow globally. 5 | # add_compile_options($) 6 | 7 | # add_definitions(-D__ZEPHYR_SUPERVISOR__) 8 | 9 | add_subdirectory_ifdef(CONFIG_MOTOR_COMMON motor) 10 | 11 | add_subdirectory_ifdef(CONFIG_PID pid) 12 | 13 | add_subdirectory_ifdef(CONFIG_ARES_SBUS transfer) 14 | 15 | add_subdirectory_ifdef(CONFIG_CHASSIS chassis) 16 | 17 | add_subdirectory_ifdef(CONFIG_WHEEL wheel) 18 | 19 | add_subdirectory_ifdef(CONFIG_SENSOR sensor) 20 | -------------------------------------------------------------------------------- /drivers/Kconfig: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | # SPDX-License-Identifier: Apache-2.0 3 | 4 | menu "Device Drivers" 5 | rsource "motor/Kconfig" 6 | rsource "transfer/Kconfig" 7 | rsource "chassis/Kconfig" 8 | rsource "wheel/Kconfig" 9 | rsource "pid/Kconfig" 10 | rsource "sensor/Kconfig" 11 | endmenu 12 | -------------------------------------------------------------------------------- /drivers/chassis/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | 3 | zephyr_library() 4 | zephyr_library_sources(chassis.c) 5 | -------------------------------------------------------------------------------- /drivers/chassis/Kconfig: -------------------------------------------------------------------------------- 1 | # Chassis driver configuration options 2 | 3 | # Copyright (c) 2025 ttwards <12411711@mail.sustech.edu.cn> 4 | # SPDX-License-Identifier: Apache-2.0 5 | 6 | menuconfig CHASSIS 7 | bool "Chassis drivers" 8 | select WHEEL 9 | help 10 | Enable support for chassis. 11 | 12 | if CHASSIS 13 | 14 | module = CHASSIS 15 | module-str = CHASSIS 16 | source "subsys/logging/Kconfig.template.log_config" 17 | 18 | config CHASSIS_LOG_LEVEL 19 | int "Motor log level" 20 | default 4 21 | help 22 | The log level of chassis driver 23 | 24 | config CHASSIS_INIT_PRIORITY 25 | int "Chassis init priority" 26 | default 90 27 | help 28 | The init priority of chassis driver 29 | 30 | endif 31 | -------------------------------------------------------------------------------- /drivers/graph/graph.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | typedef enum { 5 | STRING, 6 | INTEGER, 7 | FLOAT, 8 | DOUBLE, 9 | RAW 10 | } VarType; 11 | 12 | typedef struct { 13 | VarType type; 14 | int16_t size; 15 | void *ptr; 16 | } feedbackVar; 17 | 18 | #define DT_GRAPH_DEFINE(node_id) \ 19 | DEVICE_DT_DEFINE(node_id, init_fn, pm, data, config, level, prio, api, __VA_ARGS__) 20 | -------------------------------------------------------------------------------- /drivers/graph/inslink.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 3 | * 4 | * SPDX-License-Identifier: Apache-2.0 5 | */ 6 | #include 7 | #include 8 | #include "graph.h" 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #define DT_DRV_COMPAT ares_rttgraph 16 | 17 | LOG_MODULE_REGISTER(insLink, CONFIG_INS_LINK_LOG_LEVEL); 18 | 19 | DT_INST_FOREACH_STATUS_OKAY(RTT_GRAPH_INST) 20 | -------------------------------------------------------------------------------- /drivers/graph/inslink.h: -------------------------------------------------------------------------------- 1 | #include "graph.h" 2 | #include 3 | #include 4 | #include 5 | #define DT_DRV_COMPAT ares_rttgraph 6 | 7 | struct rtt_graph_data { 8 | feedbackVar vars[16]; 9 | int16_t num; 10 | }; 11 | 12 | struct rtt_graph_cfg { 13 | const char name[12]; 14 | const struct device *dev; 15 | int time_gap_ms; 16 | }; 17 | 18 | static void rtt_graph_init(const struct device *dev); 19 | static int rtt_graph_add(const struct device *dev, feedbackVar var); 20 | static void rtt_graph_stop(const struct device *dev); 21 | 22 | #define DT_RTT_GRAPH_DEFINE_INST(inst, ...) DT_GRAPH_DEFINE(DT_DRV_INST(inst), __VA_ARGS__) 23 | 24 | #define RTT_GRAPH_DEFINE_INST(node_id) \ 25 | DT_RTT_GRAPH_DEFINE_INST(inst, rtt_graph_init, NULL, &rtt_graph_data_##inst, \ 26 | &rtt_graph_cfg_##inst, POST_KERNEL, CONFIG_RTT_GRAPH_INIT_PRIO, \ 27 | &rtt_graph_api_funcs); 28 | 29 | #define RTT_GRAPH_DATA_INST(inst) \ 30 | struct rtt_graph_data rtt_graph_data_##inst = { \ 31 | .num = 0, \ 32 | }; 33 | 34 | #define RTT_GRAPH_CONFIG_INST(inst) \ 35 | struct rtt_graph_cfg rtt_graph_cfg_##inst = { \ 36 | .name = DT_PROP(DT_DRV_INST(inst), name), \ 37 | .dev = DEVICE_DT_GET(DT_PROP(DT_DRV_INST(inst), phy)), \ 38 | .time_gap_ms = DT_PROP(DT_DRV_INST(inst), time_gap_ms), \ 39 | }; 40 | 41 | #define RTT_GRAPH_INST(inst) \ 42 | RTT_GRAPH_CONFIG_INST(inst) \ 43 | RTT_GRAPH_DATA_INST(inst) \ 44 | RTT_GRAPH_DEFINE_INST(inst) 45 | -------------------------------------------------------------------------------- /drivers/motor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | # SPDX-License-Identifier: Apache-2.0 3 | 4 | add_subdirectory_ifdef(CONFIG_MOTOR_DJI dji) 5 | 6 | add_subdirectory_ifdef(CONFIG_MOTOR_MI mi) 7 | 8 | add_subdirectory_ifdef(CONFIG_MOTOR_DM dm) 9 | 10 | add_subdirectory_ifdef(CONFIG_MOTOR_COMMON common) 11 | -------------------------------------------------------------------------------- /drivers/motor/Kconfig: -------------------------------------------------------------------------------- 1 | # Motor driver configuration options 2 | 3 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 4 | # SPDX-License-Identifier: Apache-2.0 5 | 6 | menuconfig MOTOR 7 | bool "Motor drivers" 8 | help 9 | Enable support for motors. 10 | 11 | if MOTOR 12 | 13 | module = MOTOR 14 | module-str = MOTOR 15 | source "subsys/logging/Kconfig.template.log_config" 16 | 17 | config MOTOR_COMMON 18 | bool "MOTOR COMMON" 19 | help 20 | Add support for common motor drivers 21 | select CAN 22 | 23 | config MOTOR_DJI 24 | bool "MOTOR DJI" 25 | help 26 | Add support for dji motors 27 | select PID 28 | select MOTOR_COMMON 29 | 30 | config MOTOR_DM 31 | bool "MOTOR DM" 32 | help 33 | Add support for dm motors 34 | select PID 35 | select MOTOR_COMMON 36 | 37 | config MOTOR_MI 38 | bool "MOTOR MI" 39 | help 40 | Add support for mi motors 41 | select PID 42 | select MOTOR_COMMON 43 | 44 | config MOTOR_LOG_LEVEL 45 | int "Motor log level" 46 | default 4 47 | help 48 | The log level of motor driver 49 | 50 | config MOTOR_INIT_PRIORITY 51 | int "Motor init priority" 52 | default 90 53 | help 54 | The init priority of motor driver 55 | 56 | config CAN_COUNT 57 | int "CAN count" 58 | default 2 59 | help 60 | The number of CAN devices 61 | endif 62 | -------------------------------------------------------------------------------- /drivers/motor/common/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | # SPDX-License-Identifier: Apache-2.0 3 | 4 | zephyr_library() 5 | zephyr_library_sources_ifdef(CONFIG_MOTOR_COMMON common.c) 6 | -------------------------------------------------------------------------------- /drivers/motor/common/common.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | LOG_MODULE_REGISTER(motor_common, CONFIG_MOTOR_LOG_LEVEL); 9 | 10 | #include "common.h" 11 | 12 | static struct k_sem tx_queue_sem[CONFIG_CAN_COUNT]; 13 | static struct device *can_devices[CONFIG_CAN_COUNT]; 14 | 15 | K_MSGQ_DEFINE(can_tx_msgq, sizeof(struct tx_frame), 4 * CONFIG_CAN_COUNT, 4); 16 | 17 | static void can_tx_entry(void *arg1, void *arg2, void *arg3); 18 | K_THREAD_DEFINE(can_tx_thread, 1024, can_tx_entry, NULL, NULL, NULL, 0, 0, 0); 19 | 20 | static bool initialized = false; 21 | int can_work_init(void) 22 | { 23 | k_thread_start(can_tx_thread); 24 | initialized = true; 25 | return 0; 26 | } 27 | 28 | int8_t reg_can_dev(const struct device *dev) 29 | { 30 | if (!initialized) { 31 | can_work_init(); 32 | initialized = true; 33 | } 34 | int8_t can_id = get_can_id(dev); 35 | if (can_id != -1) { 36 | return can_id; 37 | } 38 | for (int i = 0; i < CONFIG_CAN_COUNT; i++) { 39 | if (can_devices[i] == NULL) { 40 | can_devices[i] = (struct device *)dev; 41 | k_sem_init(&tx_queue_sem[i], 3, 3); 42 | can_start(can_devices[i]); 43 | return i; 44 | } 45 | } 46 | return -1; 47 | } 48 | 49 | int8_t get_can_id(const struct device *dev) 50 | { 51 | for (int i = 0; i < CONFIG_CAN_COUNT; i++) { 52 | if (can_devices[i] == dev) { 53 | return i; 54 | } 55 | } 56 | return -1; 57 | } 58 | 59 | static void can_tx_callback(const struct device *can_dev, int error, void *user_data) 60 | { 61 | struct k_sem *queue_sem = user_data; 62 | k_sem_give(queue_sem); 63 | } 64 | 65 | int can_send_queued(const struct device *can_dev, struct can_frame *frame) 66 | { 67 | if (!initialized) { 68 | return -ENOSYS; 69 | } 70 | 71 | int err = k_sem_take(&tx_queue_sem[get_can_id(can_dev)], K_NO_WAIT); 72 | if (err == 0) { 73 | err = can_send(can_dev, frame, K_NO_WAIT, can_tx_callback, 74 | &tx_queue_sem[get_can_id(can_dev)]); 75 | // LOG_ERR("Send CAN frame: %d", err); 76 | if (err) { 77 | LOG_ERR("TX queue full, will be put into msgq: %d", err); 78 | } 79 | } else if (err < 0) { 80 | // LOG_ERR("CAN hardware TX queue is full. (err %d)", err); 81 | struct tx_frame q_frame = { 82 | .can_dev = can_dev, 83 | .sem = &tx_queue_sem[get_can_id(can_dev)], 84 | .frame = *frame, 85 | }; 86 | err = k_msgq_put(&can_tx_msgq, &q_frame, K_NO_WAIT); 87 | } 88 | // if (err) { 89 | // LOG_ERR("Failed to send CAN frame: %d", err); 90 | // } 91 | return err; 92 | } 93 | 94 | void can_tx_entry(void *arg1, void *arg2, void *arg3) 95 | { 96 | struct tx_frame frame; 97 | int err = 0; 98 | uint32_t last_time = 0; 99 | uint16_t failed_times = 0; 100 | while (1) { 101 | k_msgq_get(&can_tx_msgq, &frame, K_FOREVER); 102 | err = k_sem_take(frame.sem, K_MSEC(2)); 103 | if (err == 0) { 104 | err = can_send(frame.can_dev, &(frame.frame), K_USEC(100), can_tx_callback, 105 | frame.sem); 106 | if (err && k_uptime_get() - last_time > 400) { 107 | LOG_ERR("Failed to send CAN frame: %d", err); 108 | last_time = k_uptime_get(); 109 | } 110 | } else { 111 | if (failed_times > 127) { 112 | k_msgq_purge(&can_tx_msgq); 113 | LOG_ERR("Failed too many times, purge msgq"); 114 | k_sem_give(frame.sem); 115 | failed_times = 0; 116 | continue; 117 | } 118 | k_sleep(K_USEC(50)); 119 | failed_times++; 120 | } 121 | } 122 | } 123 | 124 | SYS_INIT(can_work_init, APPLICATION, CONFIG_MOTOR_INIT_PRIORITY); 125 | -------------------------------------------------------------------------------- /drivers/motor/common/common.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | struct tx_frame { 7 | const struct device *can_dev; 8 | struct k_sem *sem; 9 | struct can_frame frame; 10 | }; 11 | 12 | int8_t get_can_id(const struct device *dev); 13 | int8_t reg_can_dev(const struct device *dev); 14 | 15 | int can_send_queued(const struct device *can_dev, struct can_frame *frame); 16 | -------------------------------------------------------------------------------- /drivers/motor/dji/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | # SPDX-License-Identifier: Apache-2.0 3 | 4 | zephyr_library() 5 | zephyr_library_sources_ifdef(CONFIG_MOTOR_DJI motor_dji.c) 6 | -------------------------------------------------------------------------------- /drivers/motor/dji/dji_ratios.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 3 | * 4 | * SPDX-License-Identifier: Apache-2.0 5 | */ 6 | #define M3508_CURRENT2TORQUE 0.00002200f 7 | #define M3508_TORQUE2CURRENT 45000.0422411f 8 | #define M3508_SPEED2RPM 1.0f 9 | #define M3508_RPM2SPEED 1.0f 10 | #define M3508_ANGLE2DEGREE 0.043945312f 11 | #define M3508_DEGREE2ANGLE 22.75277778f 12 | 13 | #define GM6020_CURRENT2TORQUE 0.00010986f 14 | #define GM6020_TORQUE2CURRENT 9102.22222f 15 | #define GM6020_SPEED2RPM 1.0f 16 | #define GM6020_RPM2SPEED 1.0f 17 | #define GM6020_ANGLE2DEGREE 0.043950678f 18 | #define GM6020_DEGREE2ANGLE 22.75277778f 19 | 20 | #define M2006_CURRENT2TORQUE 0.0000089606f 21 | #define M2006_TORQUE2CURRENT 111600.0f 22 | #define M2006_SPEED2RPM 1.0f 23 | #define M2006_RPM2SPEED 1.0f 24 | #define M2006_ANGLE2DEGREE 0.043950678f 25 | #define M2006_DEGREE2ANGLE 22.75277778f 26 | 27 | #define DM_MOTOR_SPEED2RPM 0.001f 28 | #define DM_MOTOR_RPM2SPEED 1.0f 29 | #define DM_MOTOR_ANGLE2DEGREE 0.001f 30 | 31 | #define M3508_CONVERT_NUM 0 32 | #define GM6020_CONVERT_NUM 1 33 | #define M2006_CONVERT_NUM 2 34 | #define DM_MOTOR_CONVERT_NUM 3 35 | 36 | #define CURRENT2TORQUE 0 37 | #define TORQUE2CURRENT 1 38 | #define SPEED2RPM 2 39 | #define RPM2SPEED 3 40 | #define ANGLE2DEGREE 4 41 | #define DEGREE2ANGLE 5 42 | 43 | const float convert[4][6] = { 44 | {M3508_CURRENT2TORQUE, M3508_TORQUE2CURRENT, M3508_SPEED2RPM, M3508_RPM2SPEED, 45 | M3508_ANGLE2DEGREE, M3508_DEGREE2ANGLE}, 46 | 47 | {GM6020_CURRENT2TORQUE, GM6020_TORQUE2CURRENT, GM6020_SPEED2RPM, GM6020_RPM2SPEED, 48 | GM6020_ANGLE2DEGREE, GM6020_DEGREE2ANGLE}, 49 | 50 | {M2006_CURRENT2TORQUE, M2006_TORQUE2CURRENT, M2006_SPEED2RPM, M2006_RPM2SPEED, 51 | M2006_ANGLE2DEGREE, M2006_DEGREE2ANGLE}, 52 | 53 | {0.0f, 0.0f, DM_MOTOR_SPEED2RPM, DM_MOTOR_RPM2SPEED, DM_MOTOR_ANGLE2DEGREE, 0.0}, 54 | }; 55 | -------------------------------------------------------------------------------- /drivers/motor/dm/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | # SPDX-License-Identifier: Apache-2.0 3 | 4 | zephyr_library() 5 | zephyr_library_sources_ifdef(CONFIG_MOTOR_DM motor_dm.c) 6 | -------------------------------------------------------------------------------- /drivers/motor/mi/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | zephyr_library() 2 | zephyr_library_sources_ifdef(CONFIG_MOTOR_MI motor_mi.c) 3 | -------------------------------------------------------------------------------- /drivers/pid/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | 3 | zephyr_library() 4 | zephyr_library_sources_ifdef(CONFIG_PID pid_mit.c) 5 | zephyr_library_sources_ifdef(CONFIG_PID pid_single.c) 6 | -------------------------------------------------------------------------------- /drivers/pid/Kconfig: -------------------------------------------------------------------------------- 1 | # Motor driver configuration options 2 | 3 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 4 | # SPDX-License-Identifier: Apache-2.0 5 | 6 | menuconfig PID_MUNU 7 | bool "PID drivers" 8 | help 9 | Enable support for PID devices. 10 | 11 | 12 | 13 | module = PID_MUNU 14 | module-str = PID_MUNU 15 | source "subsys/logging/Kconfig.template.log_config" 16 | 17 | config PID 18 | bool "PID" 19 | help 20 | Add support for PID 21 | 22 | if PID_MUNU 23 | 24 | endif 25 | -------------------------------------------------------------------------------- /drivers/pid/pid_mit.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 3 | * 4 | * SPDX-License-Identifier: Apache-2.0 5 | */ 6 | #ifndef PID_MIT_H 7 | #define PID_MIT_H 8 | 9 | #include "zephyr/devicetree.h" 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #define DT_DRV_COMPAT pid_mit 20 | 21 | #define PID_SINGLE_DT_DRIVER_CONFIG_GET(node_id) \ 22 | { \ 23 | .k_p = DT_STRING_UNQUOTED(node_id, k_p), \ 24 | .integral_limit = DT_STRING_UNQUOTED_OR(node_id, i_max, 0), \ 25 | .output_limit = DT_STRING_UNQUOTED_OR(node_id, out_max, 0), \ 26 | .detri_lpf = DT_STRING_UNQUOTED_OR(node_id, detri_lpf, NAN), \ 27 | .k_i = DT_STRING_UNQUOTED_OR(node_id, k_i, NAN), \ 28 | .k_d = DT_STRING_UNQUOTED_OR(node_id, k_d, NAN), \ 29 | .mit = true, \ 30 | .output_offset = DT_STRING_UNQUOTED_OR(node_id, offset, 0), \ 31 | } 32 | 33 | #define PID_CONFIG_DEFINE(inst) \ 34 | static const struct pid_config mit_config_##inst = \ 35 | PID_SINGLE_DT_DRIVER_CONFIG_GET(DT_DRV_INST(inst)); 36 | 37 | #define PID_INST(inst) \ 38 | PID_CONFIG_DEFINE(inst) \ 39 | PID_DEVICE_DT_DEFINE(DT_DRV_INST(inst), NULL, NULL, NULL, &mit_config_##inst, POST_KERNEL, \ 40 | 90, NULL); 41 | 42 | DT_INST_FOREACH_STATUS_OKAY(PID_INST) 43 | 44 | #endif // PID_MIT_H 45 | -------------------------------------------------------------------------------- /drivers/pid/pid_single.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 3 | * 4 | * SPDX-License-Identifier: Apache-2.0 5 | */ 6 | #ifndef PID_SINGLE_H 7 | #define PID_SINGLE_H 8 | 9 | #include "zephyr/devicetree.h" 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | #define DT_DRV_COMPAT pid_single 19 | 20 | #define PID_SINGLE_DT_DRIVER_CONFIG_GET(node_id) \ 21 | { \ 22 | .k_p = DT_STRING_UNQUOTED(node_id, k_p), \ 23 | .integral_limit = DT_STRING_UNQUOTED_OR(node_id, i_max, 0), \ 24 | .output_limit = DT_STRING_UNQUOTED_OR(node_id, out_max, 0), \ 25 | .detri_lpf = DT_STRING_UNQUOTED_OR(node_id, detri_lpf, NAN), \ 26 | .k_i = DT_STRING_UNQUOTED_OR(node_id, k_i, NAN), \ 27 | .k_d = DT_STRING_UNQUOTED_OR(node_id, k_d, NAN), \ 28 | .mit = false, \ 29 | .output_offset = DT_STRING_UNQUOTED_OR(node_id, offset, 0), \ 30 | } 31 | 32 | #define PID_CONFIG_DEFINE(inst) \ 33 | static struct pid_config pid_config_##inst = \ 34 | PID_SINGLE_DT_DRIVER_CONFIG_GET(DT_DRV_INST(inst)); 35 | 36 | #define PID_INST(inst) \ 37 | PID_CONFIG_DEFINE(inst) \ 38 | PID_DEVICE_DT_DEFINE(DT_DRV_INST(inst), NULL, NULL, NULL, &pid_config_##inst, POST_KERNEL, \ 39 | 90, NULL); 40 | 41 | DT_INST_FOREACH_STATUS_OKAY(PID_INST) 42 | 43 | #endif // PID_SINGLE_H 44 | -------------------------------------------------------------------------------- /drivers/sensor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory_ifdef(CONFIG_PAW3395 paw3395) 2 | -------------------------------------------------------------------------------- /drivers/sensor/Kconfig: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | # SPDX-License-Identifier: Apache-2.0 3 | 4 | menu "Device Drivers" 5 | rsource "paw3395/Kconfig" 6 | endmenu 7 | -------------------------------------------------------------------------------- /drivers/sensor/paw3395/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | 3 | zephyr_library() 4 | zephyr_library_sources_ifdef(CONFIG_PAW3395 paw3395.c) 5 | -------------------------------------------------------------------------------- /drivers/sensor/paw3395/Kconfig: -------------------------------------------------------------------------------- 1 | # PAW3395 optical mouse sensor configuration options 2 | 3 | # Copyright (c) 2023, Your Name 4 | # SPDX-License-Identifier: Apache-2.0 5 | 6 | menuconfig PAW3395 7 | bool "PAW3395 optical mouse sensor" 8 | default y 9 | depends on DT_HAS_PIXART_PAW3395_ENABLED 10 | select SPI 11 | help 12 | Enable driver for PAW3395 optical mouse sensor. 13 | 14 | if PAW3395 15 | 16 | config PAW3395_TRIGGER 17 | bool "Trigger support" 18 | help 19 | Enable trigger support for PAW3395 sensor. 20 | 21 | endif # PAW3395 22 | -------------------------------------------------------------------------------- /drivers/transfer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | 3 | zephyr_library() 4 | zephyr_library_sources(ares_sbus.c) 5 | -------------------------------------------------------------------------------- /drivers/transfer/Kconfig: -------------------------------------------------------------------------------- 1 | # Motor driver configuration options 2 | 3 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 4 | # SPDX-License-Identifier: Apache-2.0 5 | 6 | menuconfig ARES 7 | bool "ARES drivers" 8 | help 9 | Enable support for ARES devices. 10 | 11 | if ARES 12 | 13 | module = ARES 14 | module-str = ARES 15 | source "subsys/logging/Kconfig.template.log_config" 16 | 17 | config SBUS_LOG_LEVEL 18 | int "sbus log level" 19 | default 4 20 | help 21 | The log level of sbus driver 22 | 23 | config SBUS_INIT_PRIORITY 24 | int "Sbus init priority" 25 | default 90 26 | help 27 | The init priority of sbus driver 28 | 29 | config ARES_SBUS 30 | bool "ARES_SBUS" 31 | help 32 | Add support for sbus 33 | 34 | endif 35 | -------------------------------------------------------------------------------- /drivers/transfer/ares_sbus.h: -------------------------------------------------------------------------------- 1 | #ifndef SBUS_H_ 2 | #define SBUS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | // SBUS 常量定义 10 | #define SBUS_MIN 353 11 | #define SBUS_MAX 1695 12 | 13 | struct sbus_driver_config { 14 | uint8_t frameSize; 15 | uint8_t inputChannels; 16 | uint8_t startByte; 17 | uint8_t endByte; 18 | }; 19 | 20 | struct sbus_driver_data { 21 | uint8_t data[25]; 22 | uint16_t channels[16]; 23 | bool frameLost; 24 | bool failSafe; 25 | bool digitalChannels[2]; 26 | int recv_time; 27 | }; 28 | 29 | #endif // SBUS_H 30 | -------------------------------------------------------------------------------- /drivers/wheel/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | 3 | zephyr_library() 4 | zephyr_library_sources(mecanum.c) 5 | zephyr_library_sources(steerwheel.c) 6 | -------------------------------------------------------------------------------- /drivers/wheel/Kconfig: -------------------------------------------------------------------------------- 1 | # Chassis driver configuration options 2 | 3 | # Copyright (c) 2025 ttwards <12411711@mail.sustech.edu.cn> 4 | # SPDX-License-Identifier: Apache-2.0 5 | 6 | menuconfig WHEEL 7 | bool "Wheel drivers" 8 | help 9 | Enable support for wheels. 10 | 11 | if WHEEL 12 | 13 | module = WHEEL 14 | module-str = WHEEL 15 | source "subsys/logging/Kconfig.template.log_config" 16 | 17 | endif 18 | -------------------------------------------------------------------------------- /dts/bindings/canbus/vnd,canbus.yaml: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | 3 | # SPDX-License-Identifier: Apache-2.0 4 | 5 | description: Motor CAN bus 6 | 7 | compatible: "vnd,canbus" 8 | 9 | include: [base.yaml] 10 | 11 | properties: 12 | can_device: 13 | type: phandle 14 | required: true 15 | -------------------------------------------------------------------------------- /dts/bindings/chassis/ares,chassis.yaml: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025 ttwards <12411711@mail.sustech.edu.cn> 2 | 3 | # SPDX-License-Identifier: Apache-2.0 4 | 5 | description: Multi Wheel Chassis 6 | 7 | compatible: "ares,chassis" 8 | 9 | include: [base.yaml] 10 | 11 | properties: 12 | wheels: 13 | type: phandle-array 14 | required: true 15 | specifier-space: wheel 16 | speed-limit: 17 | type: string 18 | angle-pid: 19 | type: phandle 20 | required: true 21 | track_angle: 22 | type: boolean 23 | max_gyro: 24 | type: string 25 | max_linear_accel: 26 | type: string 27 | -------------------------------------------------------------------------------- /dts/bindings/imu/gyro-bias.yaml: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | 3 | # SPDX-License-Identifier: Apache-2.0 4 | 5 | description: Biases of IMU 6 | 7 | compatible: "gyro-bias" 8 | 9 | properties: 10 | gyro-bias: 11 | description: Gyroscope bias 12 | type: string-array 13 | required: true 14 | -------------------------------------------------------------------------------- /dts/bindings/motor/dji,motor.yaml: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | 3 | # SPDX-License-Identifier: Apache-2.0 4 | 5 | description: DJI Motor 6 | 7 | compatible: "dji,motor" 8 | 9 | include: [base.yaml] 10 | 11 | properties: 12 | id: 13 | type: int 14 | required: true 15 | tx_id: 16 | type: int 17 | required: true 18 | rx_id: 19 | type: int 20 | required: true 21 | can_channel: 22 | type: phandle 23 | required: true 24 | gear_ratio: 25 | type: string 26 | required: true 27 | controllers: 28 | type: phandles 29 | required: true 30 | capabilities: 31 | type: string-array 32 | required: true 33 | is_m3508: 34 | type: boolean 35 | required: false 36 | is_m2006: 37 | type: boolean 38 | required: false 39 | is_gm6020: 40 | type: boolean 41 | required: false 42 | is_dm_motor: 43 | type: boolean 44 | required: false 45 | dm_i_max: 46 | type: string 47 | required: false 48 | dm_torque_ratio: 49 | type: string 50 | required: false 51 | minor_arc: 52 | type: boolean 53 | required: false 54 | inverse: 55 | type: boolean 56 | required: false 57 | follow: 58 | type: phandle 59 | required: false 60 | -------------------------------------------------------------------------------- /dts/bindings/motor/dm,motor.yaml: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | 3 | # SPDX-License-Identifier: Apache-2.0 4 | 5 | description: Da Miao Motor 6 | 7 | compatible: "dm,motor" 8 | 9 | include: [base.yaml] 10 | 11 | properties: 12 | id: 13 | type: int 14 | tx_id: 15 | type: int 16 | required: true 17 | rx_id: 18 | type: int 19 | required: true 20 | can_channel: 21 | type: phandle 22 | required: true 23 | controllers: 24 | type: phandles 25 | required: true 26 | capabilities: 27 | type: string-array 28 | required: true 29 | v_max: 30 | type: string 31 | p_max: 32 | type: string 33 | t_max: 34 | type: string 35 | freq: 36 | type: int 37 | -------------------------------------------------------------------------------- /dts/bindings/motor/mi,motor.yaml: -------------------------------------------------------------------------------- 1 | description: Xiao Mi Motor 2 | 3 | compatible: "mi,motor" 4 | 5 | include: [base.yaml] 6 | 7 | properties: 8 | id: 9 | type: int 10 | tx_id: 11 | type: int 12 | required: true 13 | rx_id: 14 | type: int 15 | required: true 16 | can_channel: 17 | type: phandle 18 | required: true 19 | gear_ratio: 20 | type: string 21 | required: true 22 | controllers: 23 | type: phandles 24 | required: true 25 | capabilities: 26 | type: string-array 27 | required: true 28 | -------------------------------------------------------------------------------- /dts/bindings/pid/pid,single.yaml: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | 3 | # SPDX-License-Identifier: Apache-2.0 4 | 5 | description: Single PID Controller 6 | 7 | compatible: "pid,single" 8 | 9 | include: [base.yaml] 10 | 11 | properties: 12 | k_p: 13 | type: string 14 | required: true 15 | k_i: 16 | type: string 17 | k_d: 18 | type: string 19 | i_max: 20 | type: string 21 | out_max: 22 | type: string 23 | offset: 24 | type: string 25 | detri_lpf: 26 | type: string 27 | -------------------------------------------------------------------------------- /dts/bindings/pid/pid.mit.yaml: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | 3 | # SPDX-License-Identifier: Apache-2.0 4 | 5 | description: PID MIT Controller 6 | 7 | compatible: "pid,mit" 8 | 9 | include: [base.yaml] 10 | 11 | properties: 12 | k_p: 13 | type: string 14 | required: true 15 | k_i: 16 | type: string 17 | k_d: 18 | type: string 19 | i_max: 20 | type: string 21 | out_max: 22 | type: string 23 | offset: 24 | type: string 25 | detri_lpf: 26 | type: string 27 | -------------------------------------------------------------------------------- /dts/bindings/sensor/pixart,paw3395.yaml: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2023, Your Name 2 | # SPDX-License-Identifier: Apache-2.0 3 | 4 | description: PAW3395 optical mouse sensor 5 | 6 | compatible: "pixart,paw3395" 7 | 8 | include: [spi-device.yaml, "sensor-device.yaml"] 9 | 10 | properties: 11 | cs-gpios: 12 | type: phandle-array 13 | description: CS pin 14 | required: true 15 | ripple: 16 | type: boolean 17 | description: Enable ripple mode 18 | dpi: 19 | type: int 20 | description: DPI setting 21 | default: 20000 22 | -------------------------------------------------------------------------------- /dts/bindings/steerwheel/ares,mecanum.yaml: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025 ttwards <12411711@mail.sustech.edu.cn> 2 | 3 | # SPDX-License-Identifier: Apache-2.0 4 | 5 | description: Steer Wheel 6 | 7 | compatible: "ares,mecanum" 8 | 9 | include: [base.yaml] 10 | 11 | properties: 12 | wheel-motor: 13 | type: phandle 14 | required: true 15 | wheel-radius: 16 | type: string 17 | required: true 18 | angle-offset: 19 | type: string 20 | required: true 21 | inverse-wheel: 22 | type: boolean 23 | free-angle: 24 | type: string 25 | 26 | wheel-cells: 27 | - offset_x 28 | - offset_y 29 | -------------------------------------------------------------------------------- /dts/bindings/steerwheel/ares,steerwheel.yaml: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025 ttwards <12411711@mail.sustech.edu.cn> 2 | 3 | # SPDX-License-Identifier: Apache-2.0 4 | 5 | description: Steer Wheel 6 | 7 | compatible: "ares,steerwheel" 8 | 9 | include: [base.yaml] 10 | 11 | properties: 12 | steer-motor: 13 | type: phandle 14 | required: true 15 | wheel-motor: 16 | type: phandle 17 | required: true 18 | wheel-radius: 19 | type: string 20 | required: true 21 | angle-offset: 22 | type: string 23 | required: true 24 | inverse-steer: 25 | type: boolean 26 | inverse-wheel: 27 | type: boolean 28 | 29 | wheel-cells: 30 | - offset_x 31 | - offset_y 32 | -------------------------------------------------------------------------------- /dts/bindings/transfer/ares,sbus.yaml: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | 3 | # SPDX-License-Identifier: Apache-2.0 4 | 5 | description: SBUS 6 | 7 | compatible: "ares,sbus" 8 | 9 | properties: 10 | frame_size: 11 | description: Frame size 12 | type: int 13 | default: 25 14 | input_channels: 15 | description: Number of input channels 16 | type: int 17 | default: 16 18 | start_byte: 19 | description: Start byte 20 | type: int 21 | default: 0x0F 22 | end_byte: 23 | description: End byte 24 | type: int 25 | default: 0x00 26 | -------------------------------------------------------------------------------- /dts/bindings/vendor-prefixes.txt: -------------------------------------------------------------------------------- 1 | dji DaJiang Innovation Inc. 2 | dm DaMiao Bot Inc. 3 | mi Xiaomi Corp. 4 | pid PID Controller 5 | ares ARES Devices 6 | pixart PixArt Imaging Inc. 7 | -------------------------------------------------------------------------------- /include/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Include目录只包含头文件，不需要编译源文件 2 | -------------------------------------------------------------------------------- /include/ares/ares_comm.h: -------------------------------------------------------------------------------- 1 | #ifndef ARES_COMM_H__ 2 | #define ARES_COMM_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | static inline int ares_bind_interface(struct AresInterface *interface, 10 | struct AresProtocol *protocol) 11 | { 12 | // Notice that we initialize interface before protocol 13 | if (interface == NULL || protocol == NULL) { 14 | return -EINVAL; 15 | } 16 | interface->protocol = protocol; 17 | protocol->interface = interface; 18 | int ret = interface->api->init(interface); 19 | if (ret) { 20 | return ret; 21 | } 22 | ret = protocol->api->init(protocol); 23 | if (ret) { 24 | return ret; 25 | } 26 | return 0; 27 | } 28 | 29 | #endif 30 | -------------------------------------------------------------------------------- /include/ares/board/init.h: -------------------------------------------------------------------------------- 1 | // init.h 2 | #ifndef INIT_H 3 | #define INIT_H 4 | 5 | #include 6 | #include 7 | 8 | struct led_rgb; 9 | 10 | #endif /* INIT_H */ 11 | -------------------------------------------------------------------------------- /include/ares/connectivity/uart_trans.h: -------------------------------------------------------------------------------- 1 | #ifndef uart_TRANS_H_ 2 | #define uart_TRANS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #define SYNC_FRAME_HEAD 0x5A5A 9 | #define SYNC_FRAME_TAIL 0x55AA 10 | #define SYNC_HEAD_IDX 0 11 | #define SYNC_ID_IDX 2 12 | #define SYNC_DATA_IDX 4 13 | #define SYNC_CRC_OFFSET 4 14 | #define SYNC_TAIL_OFFSET 6 15 | 16 | #define FUNC_FRAME_HEAD 0xFECA 17 | #define FUNC_FRAME_TAIL 0xBEBA 18 | #define FUNC_HEAD_IDX 0 19 | #define FUNC_ID_IDX 2 20 | #define FUNC_ARG1_IDX 4 21 | #define FUNC_ARG2_IDX 8 22 | #define FUNC_ARG3_IDX 12 23 | #define FUNC_REQ_IDX 16 24 | #define FUNC_CRC_IDX 17 25 | #define FUNC_TAIL_IDX 18 26 | 27 | #define ERROR_FRAME_HEAD 0xDECA 28 | #define ERROR_FRAME_TAIL 0xADDE 29 | #define ERROR_HEAD_IDX 0 30 | #define ERROR_ID_IDX 2 31 | #define ERROR_CODE_IDX 4 32 | #define ERROR_TAIL_IDX 6 33 | 34 | #define REPL_FRAME_HEAD 0xDEC0 35 | #define REPL_FRAME_TAIL 0xEFBE 36 | #define REPL_HEAD_IDX 0 37 | #define REPL_FUNC_ID_IDX 2 38 | #define REPL_RET_IDX 4 39 | #define REPL_REQ_ID_IDX 8 40 | #define REPL_CRC_IDX 9 41 | #define REPL_TAIL_IDX 10 42 | 43 | #define HEARTBEAT_ID 0xFF 44 | 45 | #define UNMATCH_CRCx sys_cpu_to_be16(BIT(0)) 46 | #define UNKNOWN_HEAD sys_cpu_to_be16(BIT(1)) 47 | #define UNKNOWN_TAIL sys_cpu_to_be16(BIT(2)) 48 | #define UNKNOWN_FUNC sys_cpu_to_be16(BIT(3)) 49 | #define UNKNOWN_SYNC sys_cpu_to_be16(BIT(4)) 50 | #define TOOHIGH_FREQ sys_cpu_to_be16(BIT(5)) 51 | #define CLEANED_PACK sys_cpu_to_be16(BIT(6)) 52 | #define REQUEST_SYNC sys_cpu_to_be16(BIT(7)) 53 | #define HEART_BEAT sys_cpu_to_be16(BIT(8)) 54 | 55 | #define SYNC_PACK_STATUS_READ BIT(0) 56 | #define SYNC_PACK_STATUS_WRITE BIT(1) 57 | 58 | #define GET_8BITS(buf, n_byte) (*(uint8_t *)(buf + n_byte)) 59 | #define GET_16BITS(buf, n_byte) (*(uint16_t *)(buf + n_byte)) 60 | #define GET_32BITS(buf, n_byte) (*(uint32_t *)(buf + n_byte)) 61 | 62 | typedef struct id_mapping func_mapping_t; 63 | 64 | typedef uint8_t uart_trans_data_t; 65 | 66 | typedef struct sync_pack uart_sync_pack_t; 67 | 68 | typedef void (*uart_trans_cb_t)(int status); 69 | 70 | typedef uint32_t (*uart_trans_func_t)(uint32_t arg1, uint32_t arg2, uint32_t arg3); 71 | 72 | enum head_type { 73 | HEAD_TYPE_SYNC = 0, 74 | HEAD_TYPE_FUNC, 75 | HEAD_TYPE_ERROR, 76 | HEAD_TYPE_REPL, 77 | HEAD_TYPE_NONE, 78 | }; 79 | 80 | struct sync_pack { 81 | uint16_t ID; 82 | uart_trans_data_t *data; 83 | uint8_t crc; 84 | uint8_t request_id; 85 | size_t len; 86 | uart_trans_cb_t cb; 87 | uint8_t *buf; 88 | }; 89 | 90 | struct id_mapping { 91 | uint16_t id; 92 | uart_trans_func_t cb; 93 | }; 94 | 95 | struct error_frame { 96 | uint16_t head; 97 | uint8_t request_id; 98 | uint8_t reserved; 99 | uint16_t error_code; 100 | uint16_t tail; 101 | }; 102 | 103 | // 定义一个上下文结构体来存储不完整的数据 104 | typedef struct { 105 | uint8_t buffer[256]; // 与process_buffer大小相同 106 | enum head_type head; // 当前数据的类型 107 | uint16_t length; // 当前缓冲区中的数据长度 108 | } PartialDataContext; 109 | 110 | int uart_trans_init(); 111 | 112 | void uart_trans_func_add(uint16_t header, uart_trans_func_t cb); 113 | 114 | void uart_trans_func_remove(uint16_t header); 115 | 116 | uart_sync_pack_t *uart_trans_sync_add(uart_trans_data_t *data, uint16_t ID, size_t len, 117 | uart_trans_cb_t cb); 118 | 119 | void uart_trans_sync_flush(uart_sync_pack_t *pack); 120 | 121 | void uart_trans_call_func(uint16_t ID, void *arg1, void *arg2, void *arg3, uart_trans_cb_t cb); 122 | 123 | #endif /* uart_TRANS_H_ */ 124 | -------------------------------------------------------------------------------- /include/ares/ekf/QuaternionEKF.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file QuaternionEKF.h 4 | * @author Wang Hongxi 5 | * @version V1.2.0 6 | * @date 2022/3/8 7 | * @brief attitude update with gyro bias estimate and chi-square test 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | ****************************************************************************** 12 | */ 13 | #ifndef _QUAT_EKF_H 14 | #define _QUAT_EKF_H 15 | #include "kalman_filter.h" 16 | #include 17 | 18 | /* boolean type definitions */ 19 | #ifndef TRUE 20 | #define TRUE 1 /**< boolean true */ 21 | #endif 22 | 23 | #ifndef FALSE 24 | #define FALSE 0 /**< boolean fails */ 25 | #endif 26 | 27 | #define PHY_G 9.7887f 28 | 29 | typedef struct { 30 | uint8_t Initialized; 31 | KalmanFilter_t IMU_QuaternionEKF; 32 | uint8_t ConvergeFlag; 33 | uint8_t StableFlag; 34 | uint64_t ErrorCount; 35 | uint64_t UpdateCount; 36 | 37 | float q[4]; // 四元数估计值 38 | float GyroBias[3]; // 陀螺仪零偏估计值 39 | float AccelBias[3]; // 加速度计零偏估计值 40 | float AccelBeta[3]; // 加速度计标定系数 41 | 42 | float accel_dt; // 加速度计采样周期 43 | float gyro_dt; // 陀螺仪采样周期 44 | 45 | float g; // 重力加速度 46 | 47 | float Gyro[3]; 48 | float Accel[3]; 49 | 50 | float OrientationCosine[3]; 51 | 52 | float accLPFcoef; 53 | float gyro_norm; 54 | float accl_norm; 55 | float AdaptiveGainScale; 56 | 57 | float Roll; 58 | float Pitch; 59 | float Yaw; 60 | 61 | float YawTotalAngle; 62 | 63 | float Q1; // 四元数更新过程噪声 64 | float Q2; // 陀螺仪零偏过程噪声 65 | float R; // 加速度计量测噪声 66 | 67 | mat ChiSquare; 68 | float ChiSquare_Data[1]; // 卡方检验检测函数 69 | float ChiSquareTestThreshold; // 卡方检验阈值 70 | float lambda; // 渐消因子 71 | 72 | int16_t YawRoundCount; 73 | 74 | float YawAngleLast; 75 | 76 | uint64_t UpdateTime; 77 | 78 | bool hasStoredBias; // 是否存储过偏置 79 | 80 | // New fields for split update 81 | float RawGyro[3]; // Store raw gyro for bias estimation 82 | } QEKF_INS_t; 83 | 84 | extern QEKF_INS_t QEKF_INS; 85 | extern float chiSquare; 86 | extern float ChiSquareTestThreshold; 87 | void IMU_QuaternionEKF_Init(float *init_quaternion, float process_noise1, float process_noise2, 88 | float measure_noise, float lambda, float lpf); 89 | 90 | void IMU_QuaternionEKF_Predict_Update(float gx, float gy, float gz, float gyro_dt); 91 | void IMU_QuaternionEKF_Measurement_Update(float gx, float gy, float gz, float gyro_dt, float ax, 92 | float ay, float az, float accel_dt); 93 | void CalcBias(float *q, float *accel, float g, float *bias); 94 | 95 | #define default_EKF_F \ 96 | { \ 97 | 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, \ 98 | } 99 | 100 | #define default_EKF_P \ 101 | { \ 102 | 100000, 0.1, 0.1, 0.1, 0.1, 100000, 0.1, 0.1, \ 103 | 0.1, 0.1, 100000, 0.1, 0.1, 0.1, 0.1, 100000, \ 104 | } 105 | 106 | #endif 107 | -------------------------------------------------------------------------------- /include/ares/ekf/imu_task.h: -------------------------------------------------------------------------------- 1 | #ifndef IMU_TASK_H 2 | #define IMU_TASK_H 3 | 4 | #include 5 | #include 6 | #include "QuaternionEKF.h" 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | static const int X = 0; 15 | static const int Y = 1; 16 | static const int Z = 2; 17 | 18 | #define FREQ 800 19 | 20 | typedef void (*update_cb_t)(QEKF_INS_t *QEKF); 21 | 22 | typedef struct { 23 | // IMU量测值 24 | float Gyro[3]; // 角速度 25 | float Accel[3]; // 加速度 26 | 27 | float lpf_Accel[3]; // 加速度低通滤波 28 | 29 | uint32_t gyro_prev_cyc; 30 | uint32_t accel_prev_cyc; 31 | 32 | float AccelLPF; 33 | 34 | uint32_t gyro_curr_cyc; 35 | uint32_t accel_curr_cyc; 36 | 37 | update_cb_t update_cb; 38 | 39 | uint32_t temp_update_ms; 40 | float imu_temp; 41 | 42 | const struct device *accel_dev; 43 | const struct device *gyro_dev; 44 | } INS_t; 45 | 46 | typedef struct { 47 | bool flag; 48 | float Gyro[3]; // 角速度 49 | } IMU_Bias_t; 50 | 51 | /* 用于修正安装误差的参数 */ 52 | typedef struct { 53 | uint8_t flag; 54 | 55 | float scale[3]; 56 | 57 | float Yaw; 58 | float Pitch; 59 | float Roll; 60 | } IMU_Param_t; 61 | 62 | float IMU_temp_read(const struct device *dev); 63 | void IMU_Sensor_set_update_cb(update_cb_t cb); 64 | #ifdef CONFIG_IMU_PWM_TEMP_CTRL 65 | void IMU_Sensor_set_IMU_temp(float temp); 66 | #endif // CONFIG_IMU_PWM_TEMP_CTRL 67 | void IMU_Sensor_trig_init(const struct device *accel_dev, const struct device *gyro_dev); 68 | 69 | #define MIN_PERIOD PWM_SEC(1U) / 128U 70 | #define MAX_PERIOD PWM_SEC(1U) 71 | 72 | extern INS_t INS; 73 | 74 | #if DT_HAS_CHOSEN(ares_bias) 75 | #define GYRO_BIAS_NODE DT_CHOSEN(ares_bias) 76 | #if DT_NODE_HAS_PROP(GYRO_BIAS_NODE, gyro_bias) 77 | #endif // DT_NODE_HAS_PROP(GYRO_BIAS_NODE, gyro_bias) 78 | #endif // DT_HAS_CHOSEN(ares_bias) 79 | 80 | #endif // IMU_TASK_H 81 | -------------------------------------------------------------------------------- /include/ares/ekf/matrix_storage.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2024 BayLibre SAS 3 | * Copyright (c) 2024 Your Name/Company // Add your copyright 4 | * 5 | * SPDX-License-Identifier: Apache-2.0 6 | */ 7 | 8 | #ifndef MATRIX_STORAGE_H_ 9 | #define MATRIX_STORAGE_H_ 10 | 11 | #include 12 | #include // For size_t 13 | 14 | #define MATRIX_ROWS 3 15 | #define MATRIX_COLS 3 16 | 17 | #define ZMS_MIN_SECTOR_COUNT 2 18 | 19 | /** 20 | * @brief Saves two 3x3 float matrices to ZMS storage. 21 | * 22 | * This function attempts to initialize the ZMS filesystem if not already done, 23 | * then writes the provided matrices under a predefined internal ID. 24 | * 25 | * @param matrix1 Pointer to the first 3x3 float matrix. 26 | * @param matrix2 Pointer to the second 3x3 float matrix. 27 | * @return 0 on success. 28 | * @return -errno code on failure (e.g., -ENOSPC, -EIO, -EINVAL). 29 | */ 30 | int matrix_storage_save(const float matrix1[MATRIX_ROWS][MATRIX_COLS], 31 | const float matrix2[MATRIX_ROWS][MATRIX_COLS]); 32 | 33 | /** 34 | * @brief Reads two 3x3 float matrices from ZMS storage. 35 | * 36 | * This function attempts to initialize the ZMS filesystem if not already done, 37 | * then reads the matrices associated with a predefined internal ID. 38 | * 39 | * @param matrix1 Pointer to a buffer to store the first 3x3 float matrix. 40 | * @param matrix2 Pointer to a buffer to store the second 3x3 float matrix. 41 | * @return 0 on success. 42 | * @return -ENOENT if the matrices are not found in storage. 43 | * @return -EIO if the read data size does not match the expected size. 44 | * @return Other -errno codes on ZMS or flash driver failures. 45 | */ 46 | int matrix_storage_read(float matrix1[MATRIX_ROWS][MATRIX_COLS], 47 | float matrix2[MATRIX_ROWS][MATRIX_COLS]); 48 | 49 | /** 50 | * @brief Checks if the matrices exist in ZMS storage. 51 | * 52 | * This function attempts to initialize the ZMS filesystem if not already done, 53 | * then checks for the presence and correct size of the matrix data. 54 | * 55 | * @return true if the matrices exist and have the correct size. 56 | * @return false if the matrices do not exist, have an incorrect size, 57 | * or an error occurred during initialization or check. 58 | */ 59 | bool matrix_storage_exists(void); 60 | 61 | /** 62 | * @brief Deletes the stored matrices from ZMS storage. 63 | * 64 | * This function attempts to initialize the ZMS filesystem if not already done, 65 | * then deletes the matrices associated with a predefined internal ID. 66 | * 67 | * @return 0 on success or if the item was already deleted. 68 | * @return -errno code on failure during deletion (e.g., -EIO). Note that 69 | * -ENOENT from the underlying zms_delete is treated as success here. 70 | */ 71 | int matrix_storage_delete(void); 72 | 73 | #endif /* MATRIX_STORAGE_H_ */ 74 | -------------------------------------------------------------------------------- /include/ares/interboard/interboard.h: -------------------------------------------------------------------------------- 1 | #ifndef INTERBOARD_H_ 2 | #define INTERBOARD_H_ 3 | 4 | #include 5 | #include 6 | 7 | #ifdef __cplusplus 8 | extern "C" { 9 | #endif 10 | #define datalen 16 11 | #define RECEIVE_ACK 0x0A 12 | #define SPI_BUF_SIZE 24 13 | #define MSG_QUEUE_SIZE 10 14 | #define SPI_SEND_STACK_SIZE 2048 15 | #define SPI_SEND_PRIORITY -1 16 | typedef struct { 17 | uint8_t sync; // 同步头0x5A（固定值） 18 | uint8_t ctrl; // [7]主1从0 [6]是否为重发 19 | uint8_t msg_type; // 消息类型 20 | uint8_t ack; // 应答 21 | } Frame_Header; 22 | // Frame structure 23 | typedef struct { 24 | Frame_Header frame_header; // Start byte to indicate the beginning of a frame 25 | uint8_t datas[datalen]; // 数据区 26 | 27 | uint16_t crc16; // CRC16校验（覆盖帧头到datas） 28 | uint16_t end_mark; // 帧尾标识0x0D0A（固定值 29 | } Interboard_Frame; 30 | 31 | typedef enum { 32 | INTERBOARD_MSG_CAN, 33 | INTERBOARD_MSG_IMU, 34 | 35 | } InterboardMsgType; 36 | // 为消息队列设计的数据结构 37 | typedef struct { 38 | uint8_t msg_type; 39 | uint8_t data[16]; 40 | } Frame_Data; 41 | // 为信号量和回调函数和线程传递数据的数据结构 42 | typedef struct { 43 | struct k_sem data_ready; 44 | uint8_t *rxdata; 45 | } Interboard_Context; 46 | 47 | uint16_t calculate_crc16(const uint8_t *data, size_t length); 48 | void build_txbuf(uint8_t *tx_buffer, InterboardMsgType msg_type, uint8_t *data, bool is_retransmit); 49 | void process_rxbuf(uint8_t *rx_buffer, InterboardMsgType msg_type, uint8_t *data, 50 | bool is_retransmit); 51 | 52 | void interboard_rx_data_handler(struct k_work *work); 53 | 54 | void interboardrx_callback(const struct device *spi_dev, int error_code, void *data); 55 | 56 | void interboard_tx_data_handler(struct k_work *work); 57 | void interboard_tx_isr_handler(struct k_timer *dummy); 58 | 59 | void interboard_init(struct device *spi_dev); 60 | int interboard_transceive(Frame_Data *tx_data, Frame_Data *rx_data); 61 | 62 | #ifdef __cplusplus 63 | } 64 | #endif 65 | 66 | #endif // INTERBOARD_H_ 67 | -------------------------------------------------------------------------------- /include/ares/interface/ares_interface.h: -------------------------------------------------------------------------------- 1 | #ifndef _ARES_INTERFACE_H_ 2 | #define _ARES_INTERFACE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #ifdef __cplusplus 10 | extern "C" { 11 | #endif 12 | 13 | struct AresProtocol; 14 | struct AresInterface; 15 | 16 | /** 17 | * @brief API that an interface must implement. 18 | * 19 | * This structure defines the set of functions that a specific interface 20 | * (e.g., a UART or USB driver) must provide. It is primarily used by the 21 | * protocol layer to send data. 22 | */ 23 | struct AresInterfaceAPI { 24 | int (*send)(struct AresInterface *interface, struct net_buf *buf); 25 | int (*send_with_lock)(struct AresInterface *interface, struct net_buf *buf, 26 | struct k_mutex *mutex); 27 | int (*send_raw)(struct AresInterface *interface, uint8_t *data, uint16_t len); 28 | 29 | int (*connect)(struct AresInterface *interface); 30 | int (*disconnect)(struct AresInterface *interface); 31 | bool (*is_connected)(struct AresInterface *interface); 32 | 33 | struct net_buf *(*alloc_buf)(struct AresInterface *interface); 34 | struct net_buf *(*alloc_buf_with_data)(struct AresInterface *interface, void *data, 35 | size_t size); 36 | 37 | int (*init)(struct AresInterface *interface); 38 | }; 39 | 40 | /** 41 | * @brief Represents a specific communication interface instance (e.g., a UART port). 42 | * 43 | * This structure holds the state of the interface, including the loaded protocol 44 | * and any driver-specific data. 45 | */ 46 | struct AresInterface { 47 | const char *name; 48 | const struct AresInterfaceAPI *api; 49 | struct AresProtocol *protocol; // The protocol loaded onto this interface 50 | 51 | void *priv_data; // Interface-specific private data (e.g., UART device ptr) 52 | }; 53 | 54 | #ifdef __cplusplus 55 | } 56 | #endif 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /include/ares/interface/uart/uart.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2025 ttwards <12411711@mail.sustech.edu.cn> 3 | * 4 | * SPDX-License-Identifier: Apache-2.0 5 | */ 6 | 7 | #ifndef ARES_UART_H__ 8 | #define ARES_UART_H__ 9 | 10 | #include "zephyr/sys/ring_buffer.h" 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include "ares/interface/ares_interface.h" 16 | 17 | int ares_uart_init(struct AresInterface *interface); 18 | int ares_uart_send(struct AresInterface *interface, struct net_buf *buf); 19 | int ares_uart_send_raw(struct AresInterface *interface, uint8_t *data, uint16_t len); 20 | struct net_buf *ares_uart_interface_alloc_buf(struct AresInterface *interface); 21 | 22 | void ares_uart_init_dev(struct AresInterface *interface, const struct device *uart_dev); 23 | 24 | #define ARES_UART_PROCESSING_THREAD_STACK_SIZE CONFIG_ARES_UART_THREAD_STACK_SIZE 25 | #define ARES_UART_TX_THREAD_STACK_SIZE CONFIG_ARES_UART_THREAD_STACK_SIZE 26 | 27 | #define ARES_UART_BLOCK_SIZE 72 28 | #define MAX_FRAME_PAYLOAD_SIZE ARES_UART_BLOCK_SIZE 29 | #define ARES_UART_TX_QUEUE_SIZE 32 30 | 31 | struct AresUartInterface { 32 | struct AresInterface *interface; 33 | 34 | atomic_t state; 35 | 36 | struct ring_buf uart_rb; 37 | uint8_t uart_rb_buf[4 * ARES_UART_BLOCK_SIZE]; 38 | 39 | struct device *uart_dev; 40 | 41 | struct k_sem sem; 42 | 43 | struct k_thread thread; 44 | k_thread_stack_t thread_stack[ARES_UART_PROCESSING_THREAD_STACK_SIZE]; 45 | 46 | struct k_msgq tx_msgq; // 发送消息队列 47 | char __aligned(4) tx_msgq_buffer[sizeof(struct net_buf *) * ARES_UART_TX_QUEUE_SIZE]; 48 | struct k_sem tx_sem; // 用于发送流控制的信号量 49 | struct net_buf *current_tx_buf; // 指向当前正在发送的buf 50 | struct k_thread tx_thread; // 发送线程的句柄 51 | k_thread_stack_t tx_thread_stack[ARES_UART_TX_THREAD_STACK_SIZE]; // 发送线程的栈 52 | }; 53 | 54 | #define ARES_UART_INTERFACE_DEFINE(Interface_name) \ 55 | struct AresInterfaceAPI ares_uart_interface_api = { \ 56 | .init = ares_uart_init, \ 57 | .send = ares_uart_send, \ 58 | .send_raw = ares_uart_send_raw, \ 59 | .alloc_buf = ares_uart_interface_alloc_buf, \ 60 | }; \ 61 | struct AresUartInterface Internal_##Interface_name = {NULL}; \ 62 | struct AresInterface Interface_name = { \ 63 | .name = #Interface_name, \ 64 | .api = &ares_uart_interface_api, \ 65 | .protocol = NULL, \ 66 | .priv_data = &Internal_##Interface_name, \ 67 | }; 68 | 69 | #endif // ARES_UART_H__ 70 | -------------------------------------------------------------------------------- /include/ares/interface/usb/note.md: -------------------------------------------------------------------------------- 1 | # USBD调用关系及结构笔记 2 | ## 接收数据 3 | ```mermaid 4 | sequenceDiagram 5 | participant HW as Hardware Interrupt 6 | participant ISR as ISR Context (HAL) 7 | participant UDC_Thread as UDC Driver Thread 8 | participant USBD_Thread as USBD Worker Thread 9 | participant User_Code as User Class Driver 10 | 11 | %% 1. Hardware Interrupt -> HAL Callback (ISR Context) 12 | HW->>ISR: IN Transfer Complete Interrupt 13 | activate ISR 14 | Note right of ISR: HAL_PCD_DataInStageCallback() 15 | ISR->>+UDC_Thread: k_msgq_put(&priv->msgq_data, ...) 16 | deactivate ISR 17 | 18 | %% 2. UDC Driver Thread wakes up and processes the message 19 | Note over UDC_Thread: Wakes up, now in Thread Context 20 | UDC_Thread->>UDC_Thread: k_msgq_get() 21 | UDC_Thread->>UDC_Thread: handle_msg_data_in(epnum) 22 | alt If transfer is not complete 23 | UDC_Thread->>HW: Continues sending next packet 24 | else If transfer is complete 25 | UDC_Thread->>+USBD_Thread: udc_submit_ep_event() -> usbd_event_carrier() -> k_msgq_put(&usbd_msgq, ...) 26 | end 27 | deactivate UDC_Thread 28 | 29 | %% 3. USBD Worker Thread wakes up and handles the event 30 | Note over USBD_Thread: Wakes up, now in Thread Context 31 | USBD_Thread->>USBD_Thread: k_msgq_get() 32 | USBD_Thread->>USBD_Thread: usbd_event_handler(event) 33 | Note right of USBD_Thread: switch (event->type)
case UDC_EVT_EP_REQUEST 34 | USBD_Thread->>USBD_Thread: event_handler_ep_request(event) 35 | USBD_Thread->>USBD_Thread: usbd_class_handle_xfer(buf, err) 36 | USBD_Thread->>+User_Code: usbd_class_request(c_data, buf, err) 37 | deactivate USBD_Thread 38 | 39 | %% 4. User Code (your class driver) is called 40 | Note over User_Code: Your api->request() is called 41 | User_Code->>User_Code: Process completion (check err, etc.) 42 | User_Code->>User_Code: net_buf_unref(buf) 43 | Note right of User_Code: Final ownership handled,
buffer is released. 44 | deactivate User_Code 45 | ``` 46 | -------------------------------------------------------------------------------- /include/ares/interface/usb/usb_bulk.h: -------------------------------------------------------------------------------- 1 | // usb_bulk.h 2 | #ifndef ARES_USB_BULK_H__ 3 | #define ARES_USB_BULK_H__ 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | int ares_usbd_init(struct AresInterface *interface); 12 | int ares_usbd_write(struct AresInterface *interface, struct net_buf *buf); 13 | struct net_buf *ares_interface_alloc_buf(struct AresInterface *interface); 14 | struct net_buf *ares_interface_alloc_buf_with_data(struct AresInterface *interface, void *data, 15 | size_t len); 16 | int ares_usbd_write_with_lock(struct AresInterface *interface, struct net_buf *buf, 17 | struct k_mutex *mutex); 18 | 19 | struct AresBulkInterface { 20 | struct AresInterface *interface; 21 | 22 | struct usbd_class_data *c_data; 23 | struct usbd_context *usbd_ctx; 24 | 25 | atomic_t state; 26 | 27 | struct k_msgq *incoming_data_msgq; 28 | struct k_thread *processing_thread_data; 29 | }; 30 | 31 | #define ARES_BULK_INTERFACE_DEFINE(Interface_name) \ 32 | struct AresInterfaceAPI ares_bulk_interface_api = { \ 33 | .init = ares_usbd_init, \ 34 | .send = ares_usbd_write, \ 35 | .send_with_lock = ares_usbd_write_with_lock, \ 36 | .alloc_buf = ares_interface_alloc_buf, \ 37 | .alloc_buf_with_data = ares_interface_alloc_buf_with_data, \ 38 | }; \ 39 | struct AresBulkInterface Internal_##Interface_name = {NULL}; \ 40 | struct AresInterface Interface_name = { \ 41 | .name = #Interface_name, \ 42 | .api = &ares_bulk_interface_api, \ 43 | .protocol = NULL, \ 44 | .priv_data = &Internal_##Interface_name, \ 45 | }; 46 | 47 | #endif // ARES_USB_BULK_H__ 48 | -------------------------------------------------------------------------------- /include/ares/mahony/MahonyAHRS.h: -------------------------------------------------------------------------------- 1 | //===================================================================================================== 2 | // MahonyAHRS.h 3 | //===================================================================================================== 4 | // 5 | // Madgwick's implementation of Mayhony's AHRS algorithm. 6 | // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 7 | // 8 | // Date Author Notes 9 | // 29/09/2011 SOH Madgwick Initial release 10 | // 02/10/2011 SOH Madgwick Optimised for reduced CPU load 11 | // 12 | //===================================================================================================== 13 | 14 | #include 15 | 16 | #ifndef MahonyAHRS_h 17 | #define MahonyAHRS_h 18 | 19 | typedef struct { 20 | float q[4]; 21 | float Accel[3]; 22 | float Gyro[3]; 23 | float Mag[3]; 24 | float Yaw, Pitch, Roll; 25 | float g; 26 | } MahonyAHRS_INS_t; 27 | 28 | //---------------------------------------------------------------------------------------------------- 29 | // Variable declaration 30 | 31 | extern volatile float twoKp; // 2 * proportional gain (Kp) 32 | extern volatile float twoKi; // 2 * integral gain (Ki) 33 | extern volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame 34 | 35 | extern MahonyAHRS_INS_t MahonyAHRS_INS; 36 | 37 | //--------------------------------------------------------------------------------------------------- 38 | // Function declarations 39 | 40 | void MahonyAHRSupdate(float gx, float gy, float gz, float gyro_dt, float ax, float ay, float az, 41 | float mx, float my, float mz, float accel_dt); 42 | void MahonyAHRSupdateIMU(float gx, float gy, float gz, float gyro_dt, float ax, float ay, float az, 43 | float accel_dt); 44 | void MahonyAHRSupdateGyro(float gx, float gy, float gz, float gyro_dt); 45 | 46 | #endif 47 | //===================================================================================================== 48 | // End of file 49 | //===================================================================================================== 50 | -------------------------------------------------------------------------------- /include/ares/mahony/imu_task.h: -------------------------------------------------------------------------------- 1 | #ifndef IMU_TASK_H 2 | #define IMU_TASK_H 3 | 4 | #include 5 | #include 6 | #include "MahonyAHRS.h" 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | static const int X = 0; 15 | static const int Y = 1; 16 | static const int Z = 2; 17 | 18 | #ifndef PHY_G 19 | #define PHY_G 9.80665f 20 | #endif 21 | 22 | typedef void (*update_cb_t)(MahonyAHRS_INS_t *MahonyAHRS); 23 | 24 | typedef struct { 25 | // IMU量测值 26 | float Gyro[3]; // 角速度 27 | float Accel[3]; // 加速度 28 | float Mag[3]; // 磁场 29 | 30 | float AccelBias[3]; 31 | float AccelBeta[3]; 32 | float GyroBias[3]; 33 | float GyroBeta[3]; 34 | 35 | float MagBias[3]; 36 | float MagBeta[3]; 37 | 38 | bool hasStoredBias; 39 | 40 | float lpf_Accel[3]; // 加速度低通滤波 41 | 42 | uint32_t gyro_prev_cyc; 43 | uint32_t accel_prev_cyc; 44 | 45 | float AccelLPF; 46 | 47 | update_cb_t update_cb; 48 | 49 | uint32_t temp_update_ms; 50 | float imu_temp; 51 | 52 | struct device *accel_dev; 53 | struct device *gyro_dev; 54 | struct device *mag_dev; 55 | } INS_t; 56 | 57 | typedef struct { 58 | bool flag; 59 | float Gyro[3]; // 角速度 60 | } IMU_Bias_t; 61 | 62 | /* 用于修正安装误差的参数 */ 63 | typedef struct { 64 | uint8_t flag; 65 | 66 | float scale[3]; 67 | 68 | float Yaw; 69 | float Pitch; 70 | float Roll; 71 | } IMU_Param_t; 72 | 73 | float IMU_temp_read(const struct device *dev); 74 | void IMU_Sensor_set_update_cb(update_cb_t cb); 75 | #ifdef CONFIG_IMU_PWM_TEMP_CTRL 76 | void IMU_Sensor_set_IMU_temp(float temp); 77 | #endif // CONFIG_IMU_PWM_TEMP_CTRL 78 | void IMU_Sensor_trig_init(const struct device *accel_dev, const struct device *gyro_dev, 79 | const struct device *mag_dev); 80 | 81 | #define MIN_PERIOD PWM_SEC(1U) / 128U 82 | #define MAX_PERIOD PWM_SEC(1U) 83 | 84 | extern INS_t INS; 85 | 86 | #if DT_HAS_CHOSEN(ares_bias) 87 | #define GYRO_BIAS_NODE DT_CHOSEN(ares_bias) 88 | #if DT_NODE_HAS_PROP(GYRO_BIAS_NODE, gyro_bias) 89 | #endif // DT_NODE_HAS_PROP(GYRO_BIAS_NODE, gyro_bias) 90 | #endif // DT_HAS_CHOSEN(ares_bias) 91 | 92 | #endif // IMU_TASK_H 93 | -------------------------------------------------------------------------------- /include/ares/mahony/matrix_storage.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2024 BayLibre SAS 3 | * Copyright (c) 2024 Your Name/Company // Add your copyright 4 | * 5 | * SPDX-License-Identifier: Apache-2.0 6 | */ 7 | 8 | #ifndef MATRIX_STORAGE_H_ 9 | #define MATRIX_STORAGE_H_ 10 | 11 | #include 12 | #include // For size_t 13 | 14 | #define MATRIX_ROWS 3 15 | #define MATRIX_COLS 3 16 | 17 | #define ZMS_MIN_SECTOR_COUNT 2 18 | 19 | /** 20 | * @brief Saves two 3x3 float matrices to ZMS storage. 21 | * 22 | * This function attempts to initialize the ZMS filesystem if not already done, 23 | * then writes the provided matrices under a predefined internal ID. 24 | * 25 | * @param matrix1 Pointer to the first 3x3 float matrix. 26 | * @param matrix2 Pointer to the second 3x3 float matrix. 27 | * @return 0 on success. 28 | * @return -errno code on failure (e.g., -ENOSPC, -EIO, -EINVAL). 29 | */ 30 | int matrix_storage_save(const float matrix1[MATRIX_ROWS][MATRIX_COLS], 31 | const float matrix2[MATRIX_ROWS][MATRIX_COLS]); 32 | 33 | /** 34 | * @brief Reads two 3x3 float matrices from ZMS storage. 35 | * 36 | * This function attempts to initialize the ZMS filesystem if not already done, 37 | * then reads the matrices associated with a predefined internal ID. 38 | * 39 | * @param matrix1 Pointer to a buffer to store the first 3x3 float matrix. 40 | * @param matrix2 Pointer to a buffer to store the second 3x3 float matrix. 41 | * @return 0 on success. 42 | * @return -ENOENT if the matrices are not found in storage. 43 | * @return -EIO if the read data size does not match the expected size. 44 | * @return Other -errno codes on ZMS or flash driver failures. 45 | */ 46 | int matrix_storage_read(float matrix1[MATRIX_ROWS][MATRIX_COLS], 47 | float matrix2[MATRIX_ROWS][MATRIX_COLS]); 48 | 49 | /** 50 | * @brief Checks if the matrices exist in ZMS storage. 51 | * 52 | * This function attempts to initialize the ZMS filesystem if not already done, 53 | * then checks for the presence and correct size of the matrix data. 54 | * 55 | * @return true if the matrices exist and have the correct size. 56 | * @return false if the matrices do not exist, have an incorrect size, 57 | * or an error occurred during initialization or check. 58 | */ 59 | bool matrix_storage_exists(void); 60 | 61 | /** 62 | * @brief Deletes the stored matrices from ZMS storage. 63 | * 64 | * This function attempts to initialize the ZMS filesystem if not already done, 65 | * then deletes the matrices associated with a predefined internal ID. 66 | * 67 | * @return 0 on success or if the item was already deleted. 68 | * @return -errno code on failure during deletion (e.g., -EIO). Note that 69 | * -ENOENT from the underlying zms_delete is treated as success here. 70 | */ 71 | int matrix_storage_delete(void); 72 | 73 | #endif /* MATRIX_STORAGE_H_ */ 74 | -------------------------------------------------------------------------------- /include/ares/protocol/ares_protocol.h: -------------------------------------------------------------------------------- 1 | #ifndef ARES_PROTOCOL_H 2 | #define ARES_PROTOCOL_H 3 | 4 | #include 5 | #include 6 | 7 | #ifdef __cplusplus 8 | extern "C" { 9 | #endif 10 | 11 | enum AresProtocolEvent { 12 | ARES_PROTOCOL_EVENT_CONNECTED, 13 | ARES_PROTOCOL_EVENT_DISCONNECTED, 14 | }; 15 | 16 | struct AresProtocol; 17 | 18 | /** 19 | * @brief API that a protocol must implement. 20 | * 21 | * This structure defines the set of functions that a specific protocol 22 | * (e.g., MAVLink, CANopen) must provide. These functions are called by the 23 | * interface layer to process data and manage the protocol's lifecycle. 24 | */ 25 | struct AresProtocolAPI { 26 | void (*handle)(struct AresProtocol *protocol, struct net_buf *buf); 27 | void (*handle_byte)(struct AresProtocol *protocol, uint8_t byte); 28 | void (*event)(struct AresProtocol *protocol, enum AresProtocolEvent event); 29 | int (*init)(struct AresProtocol *protocol); 30 | }; 31 | 32 | /** 33 | * @brief Represents a stateless definition of a communication protocol. 34 | */ 35 | struct AresProtocol { 36 | const char *name; 37 | const struct AresProtocolAPI *api; 38 | struct AresInterface *interface; 39 | 40 | void *priv_data; 41 | }; 42 | 43 | #ifdef __cplusplus 44 | } 45 | #endif 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /include/ares/vofa/justfloat.h: -------------------------------------------------------------------------------- 1 | #ifndef JUSTFLOAT_H 2 | #define JUSTFLOAT_H 3 | 4 | #include 5 | #include 6 | 7 | #define aresMaxChannel 24 8 | 9 | enum JF_Types { 10 | PTR_INT = 0, 11 | PTR_FLOAT = 1, 12 | PTR_DOUBLE = 2, 13 | PTR_INT8 = 3, 14 | PTR_INT16 = 4, 15 | PTR_UINT8 = 5, 16 | PTR_UINT16 = 6, 17 | PTR_UINT = 7, 18 | RAW = 8, 19 | }; 20 | 21 | struct JFData { 22 | struct device *uart_dev; 23 | 24 | void *data_ptr[aresMaxChannel]; 25 | 26 | float fdata[aresMaxChannel]; 27 | 28 | enum JF_Types types[aresMaxChannel]; 29 | 30 | int channel; 31 | }; 32 | 33 | struct JFData *jf_send_init(const struct device *uart_dev, int delay); 34 | void jf_channel_add(struct JFData *data, void *value, enum JF_Types type); 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /include/zephyr/drivers/sbus.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 3 | * 4 | * SPDX-License-Identifier: Apache-2.0 5 | */ 6 | #ifndef ZEPHYR_DRIVERS_SBUS_H_ 7 | #define ZEPHYR_DRIVERS_SBUS_H_ 8 | 9 | #include 10 | #include 11 | 12 | #ifdef __cplusplus 13 | extern "C" { 14 | #endif 15 | 16 | /** 17 | * @brief SBUS driver API 18 | */ 19 | struct sbus_driver_api { 20 | float (*getchannel_percentage)(const struct device *dev, uint8_t channelid); 21 | int (*getchannel_digital)(const struct device *dev, uint8_t channelid); 22 | }; 23 | 24 | /** 25 | * @brief Get the percentage value of a SBUS channel 26 | * 27 | * @param dev Pointer to the SBUS device 28 | * @param channelid Channel ID to read 29 | * @return float Percentage value (-1.0 to 1.0) 30 | */ 31 | __syscall float sbus_get_percent(const struct device *dev, uint8_t channelid); 32 | 33 | static inline float z_impl_sbus_get_percent(const struct device *dev, uint8_t channelid) 34 | { 35 | const struct sbus_driver_api *api = (const struct sbus_driver_api *)dev->api; 36 | return api->getchannel_percentage(dev, channelid); 37 | } 38 | 39 | /** 40 | * @brief Get the digital value of a SBUS channel 41 | * 42 | * @param dev Pointer to the SBUS device 43 | * @param channelid Channel ID to read 44 | * @return int Digital value 45 | */ 46 | __syscall int sbus_get_digit(const struct device *dev, uint8_t channelid); 47 | 48 | static inline int z_impl_sbus_get_digit(const struct device *dev, uint8_t channelid) 49 | { 50 | const struct sbus_driver_api *api = (const struct sbus_driver_api *)dev->api; 51 | return api->getchannel_digital(dev, channelid); 52 | } 53 | 54 | #ifdef __cplusplus 55 | } 56 | #endif 57 | 58 | #include 59 | 60 | #endif /* ZEPHYR_DRIVERS_SBUS_H_ */ 61 | -------------------------------------------------------------------------------- /lib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # ARES库文件编译配置 2 | add_subdirectory(ares) 3 | -------------------------------------------------------------------------------- /lib/Kconfig: -------------------------------------------------------------------------------- 1 | 2 | rsource "ares/Kconfig" 3 | -------------------------------------------------------------------------------- /lib/ares/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory_ifdef(CONFIG_EKF_LIB ekf) 2 | add_subdirectory_ifdef(CONFIG_MAHONY_LIB mahony) 3 | add_subdirectory_ifdef(CONFIG_VOFA_LIB vofa) 4 | add_subdirectory_ifdef(CONFIG_MASTER_BOARD interboard) 5 | add_subdirectory_ifdef(CONFIG_SLAVE_BOARD interboard) 6 | add_subdirectory_ifdef(CONFIG_UART_TRANS_LIB connectivity) 7 | add_subdirectory_ifdef(CONFIG_EXPERIMENTAL_BULK_LIB usb_bulk_trans) 8 | add_subdirectory(board) 9 | add_subdirectory_ifdef(CONFIG_ARES_COMM_LIB interface) 10 | add_subdirectory_ifdef(CONFIG_ARES_COMM_LIB protocol) 11 | -------------------------------------------------------------------------------- /lib/ares/Kconfig: -------------------------------------------------------------------------------- 1 | # ARES configuration options 2 | 3 | # Copyright (c) 2025 ttwards <12411711@mail.sustech.edu.cn> 4 | # SPDX-License-Identifier: Apache-2.0 5 | 6 | rsource "connectivity/Kconfig" 7 | rsource "interface/Kconfig" 8 | rsource "protocol/Kconfig" 9 | 10 | menuconfig ARES 11 | bool "ARES options" 12 | help 13 | Enable support for ares modules. 14 | 15 | module = ARES 16 | module-str = ARES 17 | 18 | config UART_INTERFACE 19 | bool "UART_INTERFACE" 20 | help 21 | Enable UART_INTERFACE 22 | 23 | config USB_BULK_INTERFACE 24 | bool "USB_BULK_INTERFACE" 25 | help 26 | Enable USB_BULK_INTERFACE 27 | 28 | config BOARD_LOG_LEVEL 29 | int "BOARD_LOG_LEVEL" 30 | range 0 4 31 | default 3 32 | help 33 | Set BOARD_LOG_LEVEL 34 | 35 | config IMU_PWM_TEMP_CTRL 36 | select PID 37 | bool "IMU_PWM_TEMP_CTRL" 38 | help 39 | Enable IMU PWM TEMP CTRL 40 | 41 | config AUTO_PROBE_GYRO_BIAS 42 | bool "AUTO_PROBE_GYRO_BIAS" 43 | help 44 | Enable AUTO PROBE GYRO BIAS 45 | 46 | config EKF_LIB 47 | bool "EKF_LIB" 48 | select CMSIS_DSP 49 | select CONFIG_CMSIS_DSP_MATRIX 50 | help 51 | Enable EKF_LIB 52 | 53 | config MAHONY_LIB 54 | bool "MAHONY_LIB" 55 | help 56 | Enable MAHONY_LIB 57 | 58 | config VOFA_LIB 59 | bool "VOFA_LIB" 60 | help 61 | Enable VOFA_LIB 62 | 63 | config MASTER_BOARD 64 | bool "MASTER_BOARD" 65 | help 66 | Enable MASTER_BOARD 67 | 68 | config SLAVE_BOARD 69 | bool "SLAVE_BOARD" 70 | help 71 | Enable SLAVE_BOARD 72 | 73 | config UART_TRANS_LIB 74 | bool "UART_TRANS_LIB" 75 | help 76 | Enable UART_TRANS_LIB 77 | 78 | config EXPERIMENTAL_BULK_LIB 79 | bool "EXPERIMENTAL_BULK_LIB" 80 | help 81 | Enable EXPERIMENTAL_BULK_LIB 82 | 83 | config ARES_COMM_LIB 84 | bool "ARES_COMM" 85 | help 86 | Enable ARES_COMM_LIB 87 | -------------------------------------------------------------------------------- /lib/ares/ares_comm.h: -------------------------------------------------------------------------------- 1 | #ifndef ARES_COMM_H__ 2 | #define ARES_COMM_H__ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | int ares_bind_interface(struct AresInterface *interface, struct AresProtocol *protocol) 10 | { 11 | // Notice that we initialize interface before protocol 12 | if (interface == NULL || protocol == NULL) { 13 | return -EINVAL; 14 | } 15 | interface->protocol = protocol; 16 | protocol->interface = interface; 17 | int ret = interface->api->init(interface); 18 | if (ret) { 19 | return ret; 20 | } 21 | ret = protocol->api->init(protocol); 22 | if (ret) { 23 | return ret; 24 | } 25 | return 0; 26 | } 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /lib/ares/board/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | # SPDX-License-Identifier: Apache-2.0 3 | 4 | zephyr_library() 5 | 6 | set(INIT_SOURCES init.c) 7 | 8 | zephyr_library_sources(${INIT_SOURCES}) 9 | -------------------------------------------------------------------------------- /lib/ares/board/init.h: -------------------------------------------------------------------------------- 1 | // init.h 2 | #ifndef INIT_H 3 | #define INIT_H 4 | 5 | #include 6 | #include 7 | 8 | struct led_rgb; 9 | 10 | #endif /* INIT_H */ 11 | -------------------------------------------------------------------------------- /lib/ares/connectivity/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | # SPDX-License-Identifier: Apache-2.0 3 | 4 | zephyr_library() 5 | 6 | set(CONNECT_SOURCES uart_trans.c uart_async_adapter.c) 7 | 8 | zephyr_library_sources(${CONNECT_SOURCES}) 9 | -------------------------------------------------------------------------------- /lib/ares/connectivity/Kconfig: -------------------------------------------------------------------------------- 1 | 2 | config USB_UART_ASYNC_ADAPTER 3 | bool "Enable USB UART async adapter" 4 | select SERIAL_SUPPORT_ASYNC 5 | help 6 | Enables asynchronous adapter for UART drives that supports only 7 | IRQ interface. 8 | 9 | config UART_ASYNC_ADAPTER_LOG_LEVEL 10 | int "Log level for UART async adapter" 11 | range 0 7 12 | default 0 13 | help 14 | Set the log level for the UART async adapter. The higher the number, 15 | the more verbose the logs. The default is 4, which is a good balance 16 | between verbosity and performance. 17 | -------------------------------------------------------------------------------- /lib/ares/connectivity/uart_trans.h: -------------------------------------------------------------------------------- 1 | #ifndef uart_TRANS_H_ 2 | #define uart_TRANS_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #define SYNC_FRAME_HEAD 0x5A5A 9 | #define SYNC_FRAME_TAIL 0x55AA 10 | #define SYNC_HEAD_IDX 0 11 | #define SYNC_ID_IDX 2 12 | #define SYNC_DATA_IDX 4 13 | #define SYNC_CRC_OFFSET 4 14 | #define SYNC_TAIL_OFFSET 6 15 | 16 | #define FUNC_FRAME_HEAD 0xFECA 17 | #define FUNC_FRAME_TAIL 0xBEBA 18 | #define FUNC_HEAD_IDX 0 19 | #define FUNC_ID_IDX 2 20 | #define FUNC_ARG1_IDX 4 21 | #define FUNC_ARG2_IDX 8 22 | #define FUNC_ARG3_IDX 12 23 | #define FUNC_REQ_IDX 16 24 | #define FUNC_CRC_IDX 17 25 | #define FUNC_TAIL_IDX 18 26 | 27 | #define ERROR_FRAME_HEAD 0xDECA 28 | #define ERROR_FRAME_TAIL 0xADDE 29 | #define ERROR_HEAD_IDX 0 30 | #define ERROR_ID_IDX 2 31 | #define ERROR_CODE_IDX 4 32 | #define ERROR_TAIL_IDX 6 33 | 34 | #define REPL_FRAME_HEAD 0xDEC0 35 | #define REPL_FRAME_TAIL 0xEFBE 36 | #define REPL_HEAD_IDX 0 37 | #define REPL_FUNC_ID_IDX 2 38 | #define REPL_RET_IDX 4 39 | #define REPL_REQ_ID_IDX 8 40 | #define REPL_CRC_IDX 9 41 | #define REPL_TAIL_IDX 10 42 | 43 | #define HEARTBEAT_ID 0xFF 44 | 45 | #define UNMATCH_CRCx sys_cpu_to_be16(BIT(0)) 46 | #define UNKNOWN_HEAD sys_cpu_to_be16(BIT(1)) 47 | #define UNKNOWN_TAIL sys_cpu_to_be16(BIT(2)) 48 | #define UNKNOWN_FUNC sys_cpu_to_be16(BIT(3)) 49 | #define UNKNOWN_SYNC sys_cpu_to_be16(BIT(4)) 50 | #define TOOHIGH_FREQ sys_cpu_to_be16(BIT(5)) 51 | #define CLEANED_PACK sys_cpu_to_be16(BIT(6)) 52 | #define REQUEST_SYNC sys_cpu_to_be16(BIT(7)) 53 | #define HEART_BEAT sys_cpu_to_be16(BIT(8)) 54 | 55 | #define SYNC_PACK_STATUS_READ BIT(0) 56 | #define SYNC_PACK_STATUS_WRITE BIT(1) 57 | 58 | #define GET_8BITS(buf, n_byte) (*(uint8_t *)(buf + n_byte)) 59 | #define GET_16BITS(buf, n_byte) (*(uint16_t *)(buf + n_byte)) 60 | #define GET_32BITS(buf, n_byte) (*(uint32_t *)(buf + n_byte)) 61 | 62 | typedef struct id_mapping func_mapping_t; 63 | 64 | typedef uint8_t uart_trans_data_t; 65 | 66 | typedef struct sync_pack uart_sync_pack_t; 67 | 68 | typedef void (*uart_trans_cb_t)(int status); 69 | 70 | typedef uint32_t (*uart_trans_func_t)(uint32_t arg1, uint32_t arg2, uint32_t arg3); 71 | 72 | enum head_type { 73 | HEAD_TYPE_SYNC = 0, 74 | HEAD_TYPE_FUNC, 75 | HEAD_TYPE_ERROR, 76 | HEAD_TYPE_REPL, 77 | HEAD_TYPE_NONE, 78 | }; 79 | 80 | struct sync_pack { 81 | uint16_t ID; 82 | uart_trans_data_t *data; 83 | uint8_t crc; 84 | uint8_t request_id; 85 | size_t len; 86 | uart_trans_cb_t cb; 87 | uint8_t *buf; 88 | }; 89 | 90 | struct id_mapping { 91 | uint16_t id; 92 | uart_trans_func_t cb; 93 | }; 94 | 95 | struct error_frame { 96 | uint16_t head; 97 | uint8_t request_id; 98 | uint8_t reserved; 99 | uint16_t error_code; 100 | uint16_t tail; 101 | }; 102 | 103 | // 定义一个上下文结构体来存储不完整的数据 104 | typedef struct { 105 | uint8_t buffer[256]; // 与process_buffer大小相同 106 | enum head_type head; // 当前数据的类型 107 | uint16_t length; // 当前缓冲区中的数据长度 108 | } PartialDataContext; 109 | 110 | int uart_trans_init(); 111 | 112 | void uart_trans_func_add(uint16_t header, uart_trans_func_t cb); 113 | 114 | void uart_trans_func_remove(uint16_t header); 115 | 116 | uart_sync_pack_t *uart_trans_sync_add(uart_trans_data_t *data, uint16_t ID, size_t len, 117 | uart_trans_cb_t cb); 118 | 119 | void uart_trans_sync_flush(uart_sync_pack_t *pack); 120 | 121 | void uart_trans_call_func(uint16_t ID, void *arg1, void *arg2, void *arg3, uart_trans_cb_t cb); 122 | 123 | #endif /* uart_TRANS_H_ */ 124 | -------------------------------------------------------------------------------- /lib/ares/ekf/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | # SPDX-License-Identifier: Apache-2.0 3 | 4 | zephyr_library() 5 | 6 | file(GLOB EKF_SOURCES "${CMAKE_CURRENT_SOURCE_DIR}/*.c") 7 | 8 | zephyr_library_sources(${EKF_SOURCES}) 9 | 10 | zephyr_library_compile_options( 11 | -O3 12 | -fno-strict-overflow 13 | -fno-common 14 | -ffunction-sections 15 | -fdata-sections 16 | -ffreestanding 17 | -fno-builtin) 18 | -------------------------------------------------------------------------------- /lib/ares/ekf/QuaternionEKF.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file QuaternionEKF.h 4 | * @author Wang Hongxi 5 | * @version V1.2.0 6 | * @date 2022/3/8 7 | * @brief attitude update with gyro bias estimate and chi-square test 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | ****************************************************************************** 12 | */ 13 | #ifndef _QUAT_EKF_H 14 | #define _QUAT_EKF_H 15 | #include "kalman_filter.h" 16 | #include 17 | 18 | /* boolean type definitions */ 19 | #ifndef TRUE 20 | #define TRUE 1 /**< boolean true */ 21 | #endif 22 | 23 | #ifndef FALSE 24 | #define FALSE 0 /**< boolean fails */ 25 | #endif 26 | 27 | #define PHY_G 9.7887f 28 | 29 | typedef struct { 30 | uint8_t Initialized; 31 | KalmanFilter_t IMU_QuaternionEKF; 32 | uint8_t ConvergeFlag; 33 | uint8_t StableFlag; 34 | uint64_t ErrorCount; 35 | uint64_t UpdateCount; 36 | 37 | float q[4]; // 四元数估计值 38 | float GyroBias[3]; // 陀螺仪零偏估计值 39 | float AccelBias[3]; // 加速度计零偏估计值 40 | float AccelBeta[3]; // 加速度计标定系数 41 | 42 | float accel_dt; // 加速度计采样周期 43 | float gyro_dt; // 陀螺仪采样周期 44 | 45 | float g; // 重力加速度 46 | 47 | float Gyro[3]; 48 | float Accel[3]; 49 | 50 | float OrientationCosine[3]; 51 | 52 | float accLPFcoef; 53 | float gyro_norm; 54 | float accl_norm; 55 | float AdaptiveGainScale; 56 | 57 | float Roll; 58 | float Pitch; 59 | float Yaw; 60 | 61 | float YawTotalAngle; 62 | 63 | float Q1; // 四元数更新过程噪声 64 | float Q2; // 陀螺仪零偏过程噪声 65 | float R; // 加速度计量测噪声 66 | 67 | mat ChiSquare; 68 | float ChiSquare_Data[1]; // 卡方检验检测函数 69 | float ChiSquareTestThreshold; // 卡方检验阈值 70 | float lambda; // 渐消因子 71 | 72 | int16_t YawRoundCount; 73 | 74 | float YawAngleLast; 75 | 76 | uint64_t UpdateTime; 77 | 78 | bool hasStoredBias; // 是否存储过偏置 79 | 80 | // New fields for split update 81 | float RawGyro[3]; // Store raw gyro for bias estimation 82 | } QEKF_INS_t; 83 | 84 | extern QEKF_INS_t QEKF_INS; 85 | extern float chiSquare; 86 | extern float ChiSquareTestThreshold; 87 | void IMU_QuaternionEKF_Init(float *init_quaternion, float process_noise1, float process_noise2, 88 | float measure_noise, float lambda, float lpf); 89 | 90 | void IMU_QuaternionEKF_Predict_Update(float gx, float gy, float gz, float gyro_dt); 91 | void IMU_QuaternionEKF_Measurement_Update(float gx, float gy, float gz, float gyro_dt, float ax, 92 | float ay, float az, float accel_dt); 93 | void CalcBias(float *q, float *accel, float g, float *bias); 94 | 95 | #define default_EKF_F \ 96 | { \ 97 | 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, \ 98 | } 99 | 100 | #define default_EKF_P \ 101 | { \ 102 | 100000, 0.1, 0.1, 0.1, 0.1, 100000, 0.1, 0.1, \ 103 | 0.1, 0.1, 100000, 0.1, 0.1, 0.1, 0.1, 100000, \ 104 | } 105 | 106 | #endif 107 | -------------------------------------------------------------------------------- /lib/ares/ekf/imu_task.h: -------------------------------------------------------------------------------- 1 | #ifndef IMU_TASK_H 2 | #define IMU_TASK_H 3 | 4 | #include 5 | #include 6 | #include "QuaternionEKF.h" 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | static const int X = 0; 15 | static const int Y = 1; 16 | static const int Z = 2; 17 | 18 | #define FREQ 800 19 | 20 | typedef void (*update_cb_t)(QEKF_INS_t *QEKF); 21 | 22 | typedef struct { 23 | // IMU量测值 24 | float Gyro[3]; // 角速度 25 | float Accel[3]; // 加速度 26 | 27 | float lpf_Accel[3]; // 加速度低通滤波 28 | 29 | uint32_t gyro_prev_cyc; 30 | uint32_t accel_prev_cyc; 31 | 32 | float AccelLPF; 33 | 34 | uint32_t gyro_curr_cyc; 35 | uint32_t accel_curr_cyc; 36 | 37 | update_cb_t update_cb; 38 | 39 | uint32_t temp_update_ms; 40 | float imu_temp; 41 | 42 | const struct device *accel_dev; 43 | const struct device *gyro_dev; 44 | } INS_t; 45 | 46 | typedef struct { 47 | bool flag; 48 | float Gyro[3]; // 角速度 49 | } IMU_Bias_t; 50 | 51 | /* 用于修正安装误差的参数 */ 52 | typedef struct { 53 | uint8_t flag; 54 | 55 | float scale[3]; 56 | 57 | float Yaw; 58 | float Pitch; 59 | float Roll; 60 | } IMU_Param_t; 61 | 62 | float IMU_temp_read(const struct device *dev); 63 | void IMU_Sensor_set_update_cb(update_cb_t cb); 64 | #ifdef CONFIG_IMU_PWM_TEMP_CTRL 65 | void IMU_Sensor_set_IMU_temp(float temp); 66 | #endif // CONFIG_IMU_PWM_TEMP_CTRL 67 | void IMU_Sensor_trig_init(const struct device *accel_dev, const struct device *gyro_dev); 68 | 69 | #define MIN_PERIOD PWM_SEC(1U) / 128U 70 | #define MAX_PERIOD PWM_SEC(1U) 71 | 72 | extern INS_t INS; 73 | 74 | #if DT_HAS_CHOSEN(ares_bias) 75 | #define GYRO_BIAS_NODE DT_CHOSEN(ares_bias) 76 | #if DT_NODE_HAS_PROP(GYRO_BIAS_NODE, gyro_bias) 77 | #endif // DT_NODE_HAS_PROP(GYRO_BIAS_NODE, gyro_bias) 78 | #endif // DT_HAS_CHOSEN(ares_bias) 79 | 80 | #endif // IMU_TASK_H 81 | -------------------------------------------------------------------------------- /lib/ares/ekf/kalman_filter.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file kalman filter.h 4 | * @author Wang Hongxi 5 | * @version V1.2.2 6 | * @date 2022/1/8 7 | * @brief 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | ****************************************************************************** 12 | */ 13 | #ifndef __KALMAN_FILTER_H 14 | #define __KALMAN_FILTER_H 15 | 16 | #include 17 | #include "arm_math.h" 18 | 19 | // 若运算速度不够,可以使用q31代替f32,但是精度会降低 20 | #define mat arm_matrix_instance_f32 21 | #define Matrix_Init arm_mat_init_f32 22 | #define Matrix_Add arm_mat_add_f32 23 | #define Matrix_Subtract arm_mat_sub_f32 24 | #define Matrix_Multiply arm_mat_mult_f32 25 | #define Matrix_Transpose arm_mat_trans_f32 26 | #define Matrix_Inverse arm_mat_inverse_f32 27 | 28 | typedef struct kf_t { 29 | float *FilteredValue; 30 | float *MeasuredVector; 31 | float *ControlVector; 32 | 33 | uint8_t xhatSize; 34 | uint8_t uSize; 35 | uint8_t zSize; 36 | 37 | uint8_t UseAutoAdjustment; 38 | uint8_t MeasurementValidNum; 39 | 40 | uint8_t *MeasurementMap; // 量测与状态的关系 how measurement relates to the state 41 | float *MeasurementDegree; // 测量值对应H矩阵元素值 elements of each measurement in H 42 | float *MatR_DiagonalElements; // 量测方差 variance for each measurement 43 | float *StateMinVariance; // 最小方差 避免方差过度收敛 suppress filter excessive convergence 44 | uint8_t *temp; 45 | 46 | // 配合用户定义函数使用,作为标志位用于判断是否要跳过标准KF中五个环节中的任意一个 47 | uint8_t SkipEq1, SkipEq2, SkipEq3, SkipEq4, SkipEq5; 48 | 49 | // definiion of struct mat: rows & cols & pointer to vars 50 | mat xhat; // x(k|k) 51 | mat xhatminus; // x(k|k-1) 52 | mat u; // control vector u 53 | mat z; // measurement vector z 54 | mat P; // covariance matrix P(k|k) 55 | mat Pminus; // covariance matrix P(k|k-1) 56 | mat F, FT; // state transition matrix F FT 57 | mat B; // control matrix B 58 | mat H, HT; // measurement matrix H 59 | mat Q; // process noise covariance matrix Q 60 | mat R; // measurement noise covariance matrix R 61 | mat K; // kalman gain K 62 | mat S, temp_matrix, temp_matrix1, temp_vector, temp_vector1; 63 | 64 | int8_t MatStatus; 65 | 66 | // 用户定义函数,可以替换或扩展基准KF的功能 67 | void (*User_Func0_f)(struct kf_t *kf); 68 | void (*User_Func1_f)(struct kf_t *kf); 69 | void (*User_Func2_f)(struct kf_t *kf); 70 | void (*User_Func3_f)(struct kf_t *kf); 71 | void (*User_Func4_f)(struct kf_t *kf); 72 | void (*User_Func5_f)(struct kf_t *kf); 73 | void (*User_Func6_f)(struct kf_t *kf); 74 | 75 | // 矩阵存储空间指针 76 | float *xhat_data, *xhatminus_data; 77 | float *u_data; 78 | float *z_data; 79 | float *P_data, *Pminus_data; 80 | float *F_data, *FT_data; 81 | float *B_data; 82 | float *H_data, *HT_data; 83 | float *Q_data; 84 | float *R_data; 85 | float *K_data; 86 | float *S_data, *temp_matrix_data, *temp_matrix_data1, *temp_vector_data, *temp_vector_data1; 87 | } KalmanFilter_t; 88 | 89 | extern uint16_t sizeof_float, sizeof_double; 90 | 91 | void Kalman_Filter_Init(KalmanFilter_t *kf, uint8_t xhatSize, uint8_t uSize, uint8_t zSize); 92 | void Kalman_Filter_Measure(KalmanFilter_t *kf); 93 | void Kalman_Filter_xhatMinusUpdate(KalmanFilter_t *kf); 94 | void Kalman_Filter_PminusUpdate(KalmanFilter_t *kf); 95 | void Kalman_Filter_SetK(KalmanFilter_t *kf); 96 | void Kalman_Filter_xhatUpdate(KalmanFilter_t *kf); 97 | void Kalman_Filter_P_Update(KalmanFilter_t *kf); 98 | float *Kalman_Filter_Update(KalmanFilter_t *kf); 99 | 100 | #endif //__KALMAN_FILTER_H 101 | -------------------------------------------------------------------------------- /lib/ares/ekf/matrix_storage.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2024 BayLibre SAS 3 | * Copyright (c) 2024 Your Name/Company // Add your copyright 4 | * 5 | * SPDX-License-Identifier: Apache-2.0 6 | */ 7 | 8 | #ifndef MATRIX_STORAGE_H_ 9 | #define MATRIX_STORAGE_H_ 10 | 11 | #include 12 | #include // For size_t 13 | 14 | #define MATRIX_ROWS 3 15 | #define MATRIX_COLS 3 16 | 17 | #define ZMS_MIN_SECTOR_COUNT 2 18 | 19 | /** 20 | * @brief Saves two 3x3 float matrices to ZMS storage. 21 | * 22 | * This function attempts to initialize the ZMS filesystem if not already done, 23 | * then writes the provided matrices under a predefined internal ID. 24 | * 25 | * @param matrix1 Pointer to the first 3x3 float matrix. 26 | * @param matrix2 Pointer to the second 3x3 float matrix. 27 | * @return 0 on success. 28 | * @return -errno code on failure (e.g., -ENOSPC, -EIO, -EINVAL). 29 | */ 30 | int matrix_storage_save(const float matrix1[MATRIX_ROWS][MATRIX_COLS], 31 | const float matrix2[MATRIX_ROWS][MATRIX_COLS]); 32 | 33 | /** 34 | * @brief Reads two 3x3 float matrices from ZMS storage. 35 | * 36 | * This function attempts to initialize the ZMS filesystem if not already done, 37 | * then reads the matrices associated with a predefined internal ID. 38 | * 39 | * @param matrix1 Pointer to a buffer to store the first 3x3 float matrix. 40 | * @param matrix2 Pointer to a buffer to store the second 3x3 float matrix. 41 | * @return 0 on success. 42 | * @return -ENOENT if the matrices are not found in storage. 43 | * @return -EIO if the read data size does not match the expected size. 44 | * @return Other -errno codes on ZMS or flash driver failures. 45 | */ 46 | int matrix_storage_read(float matrix1[MATRIX_ROWS][MATRIX_COLS], 47 | float matrix2[MATRIX_ROWS][MATRIX_COLS]); 48 | 49 | /** 50 | * @brief Checks if the matrices exist in ZMS storage. 51 | * 52 | * This function attempts to initialize the ZMS filesystem if not already done, 53 | * then checks for the presence and correct size of the matrix data. 54 | * 55 | * @return true if the matrices exist and have the correct size. 56 | * @return false if the matrices do not exist, have an incorrect size, 57 | * or an error occurred during initialization or check. 58 | */ 59 | bool matrix_storage_exists(void); 60 | 61 | /** 62 | * @brief Deletes the stored matrices from ZMS storage. 63 | * 64 | * This function attempts to initialize the ZMS filesystem if not already done, 65 | * then deletes the matrices associated with a predefined internal ID. 66 | * 67 | * @return 0 on success or if the item was already deleted. 68 | * @return -errno code on failure during deletion (e.g., -EIO). Note that 69 | * -ENOENT from the underlying zms_delete is treated as success here. 70 | */ 71 | int matrix_storage_delete(void); 72 | 73 | #endif /* MATRIX_STORAGE_H_ */ 74 | -------------------------------------------------------------------------------- /lib/ares/interboard/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | zephyr_library() 2 | 3 | set(INTERBOARD_SOURCES interboard.c) 4 | 5 | zephyr_library_sources(${INTERBOARD_SOURCES}) 6 | -------------------------------------------------------------------------------- /lib/ares/interboard/interboard.h: -------------------------------------------------------------------------------- 1 | #ifndef INTERBOARD_H_ 2 | #define INTERBOARD_H_ 3 | 4 | #include 5 | #include 6 | 7 | #ifdef __cplusplus 8 | extern "C" { 9 | #endif 10 | #define datalen 16 11 | #define RECEIVE_ACK 0x0A 12 | #define SPI_BUF_SIZE 24 13 | #define MSG_QUEUE_SIZE 10 14 | #define SPI_SEND_STACK_SIZE 2048 15 | #define SPI_SEND_PRIORITY -1 16 | typedef struct { 17 | uint8_t sync; // 同步头0x5A（固定值） 18 | uint8_t ctrl; // [7]主1从0 [6]是否为重发 19 | uint8_t msg_type; // 消息类型 20 | uint8_t ack; // 应答 21 | } Frame_Header; 22 | // Frame structure 23 | typedef struct { 24 | Frame_Header frame_header; // Start byte to indicate the beginning of a frame 25 | uint8_t datas[datalen]; // 数据区 26 | 27 | uint16_t crc16; // CRC16校验（覆盖帧头到datas） 28 | uint16_t end_mark; // 帧尾标识0x0D0A（固定值 29 | } Interboard_Frame; 30 | 31 | typedef enum { 32 | INTERBOARD_MSG_CAN, 33 | INTERBOARD_MSG_IMU, 34 | 35 | } InterboardMsgType; 36 | // 为消息队列设计的数据结构 37 | typedef struct { 38 | uint8_t msg_type; 39 | uint8_t data[16]; 40 | } Frame_Data; 41 | // 为信号量和回调函数和线程传递数据的数据结构 42 | typedef struct { 43 | struct k_sem data_ready; 44 | uint8_t *rxdata; 45 | } Interboard_Context; 46 | 47 | uint16_t calculate_crc16(const uint8_t *data, size_t length); 48 | void build_txbuf(uint8_t *tx_buffer, InterboardMsgType msg_type, uint8_t *data, bool is_retransmit); 49 | void process_rxbuf(uint8_t *rx_buffer, InterboardMsgType msg_type, uint8_t *data, 50 | bool is_retransmit); 51 | 52 | void interboard_rx_data_handler(struct k_work *work); 53 | 54 | void interboardrx_callback(const struct device *spi_dev, int error_code, void *data); 55 | 56 | void interboard_tx_data_handler(struct k_work *work); 57 | void interboard_tx_isr_handler(struct k_timer *dummy); 58 | 59 | void interboard_init(struct device *spi_dev); 60 | int interboard_transceive(Frame_Data *tx_data, Frame_Data *rx_data); 61 | 62 | #ifdef __cplusplus 63 | } 64 | #endif 65 | 66 | #endif // INTERBOARD_H_ 67 | -------------------------------------------------------------------------------- /lib/ares/interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory_ifdef(CONFIG_USB_BULK_INTERFACE usb) 2 | add_subdirectory_ifdef(CONFIG_UART_INTERFACE uart) 3 | -------------------------------------------------------------------------------- /lib/ares/interface/Kconfig: -------------------------------------------------------------------------------- 1 | rsource "usb/Kconfig" 2 | rsource "uart/Kconfig" 3 | 4 | config USB_BULK_INTERFACE 5 | bool "USB_BULK_INTERFACE" 6 | help 7 | Enable USB_BULK_INTERFACE 8 | select ARES_COMM_LIB 9 | select USB_DEVICE_STACK_NEXT 10 | select UDC_WORKQUEUE 11 | 12 | config UART_INTERFACE 13 | bool "UART_INTERFACE" 14 | help 15 | Enable UART_INTERFACE 16 | select NET_BUF 17 | -------------------------------------------------------------------------------- /lib/ares/interface/ares_interface.h: -------------------------------------------------------------------------------- 1 | #ifndef _ARES_INTERFACE_H_ 2 | #define _ARES_INTERFACE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #ifdef __cplusplus 10 | extern "C" { 11 | #endif 12 | 13 | struct AresProtocol; 14 | struct AresInterface; 15 | 16 | /** 17 | * @brief API that an interface must implement. 18 | * 19 | * This structure defines the set of functions that a specific interface 20 | * (e.g., a UART or USB driver) must provide. It is primarily used by the 21 | * protocol layer to send data. 22 | */ 23 | struct AresInterfaceAPI { 24 | int (*send)(struct AresInterface *interface, struct net_buf *buf); 25 | int (*send_with_lock)(struct AresInterface *interface, struct net_buf *buf, 26 | struct k_mutex *mutex); 27 | int (*send_raw)(struct AresInterface *interface, uint8_t *data, uint16_t len); 28 | 29 | int (*connect)(struct AresInterface *interface); 30 | int (*disconnect)(struct AresInterface *interface); 31 | bool (*is_connected)(struct AresInterface *interface); 32 | 33 | struct net_buf *(*alloc_buf)(struct AresInterface *interface); 34 | struct net_buf *(*alloc_buf_with_data)(struct AresInterface *interface, void *data, 35 | size_t size); 36 | 37 | int (*init)(struct AresInterface *interface); 38 | }; 39 | 40 | /** 41 | * @brief Represents a specific communication interface instance (e.g., a UART port). 42 | * 43 | * This structure holds the state of the interface, including the loaded protocol 44 | * and any driver-specific data. 45 | */ 46 | struct AresInterface { 47 | const char *name; 48 | const struct AresInterfaceAPI *api; 49 | struct AresProtocol *protocol; // The protocol loaded onto this interface 50 | 51 | void *priv_data; // Interface-specific private data (e.g., UART device ptr) 52 | }; 53 | 54 | #ifdef __cplusplus 55 | } 56 | #endif 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /lib/ares/interface/uart/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | # SPDX-License-Identifier: Apache-2.0 3 | 4 | zephyr_library() 5 | 6 | set(UART_INTERFACE_SOURCES uart.c) 7 | 8 | zephyr_library_sources(${UART_INTERFACE_SOURCES}) 9 | -------------------------------------------------------------------------------- /lib/ares/interface/uart/Kconfig: -------------------------------------------------------------------------------- 1 | # Kconfig for ARES Custom USB Application 2 | 3 | menu "ARES UART Application" 4 | 5 | config ARES_UART_LOG_LEVEL 6 | int "Log level for ARES UART protocol" 7 | default 3 8 | help 9 | Select the log level for the ARES UART protocol. 10 | 0=OFF, 1=ERR, 2=WRN, 3=INF, 4=DBG 11 | 12 | config ARES_UART_THREAD_STACK_SIZE 13 | int "Stack size for ARES UART processing thread" 14 | default 1024 15 | help 16 | Specifies the stack size for the thread that processes incoming UART data. 17 | 18 | endmenu # ARES UART Application 19 | -------------------------------------------------------------------------------- /lib/ares/interface/usb/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | # SPDX-License-Identifier: Apache-2.0 3 | 4 | zephyr_library() 5 | 6 | set(USB_BULK_INTERFACE_SOURCES usb_bulk.c) 7 | 8 | zephyr_library_sources(${USB_BULK_INTERFACE_SOURCES}) 9 | -------------------------------------------------------------------------------- /lib/ares/interface/usb/Kconfig: -------------------------------------------------------------------------------- 1 | # Kconfig for ARES Custom USB Application 2 | 3 | # 顶层菜单，用于组织所有应用相关的配置 4 | menu "ARES Custom USB Application" 5 | 6 | config ARES_USB_BULK_LOG_LEVEL 7 | int "Log level for ARES USB Bulk driver" 8 | default 3 9 | help 10 | Select the log level for the ARES USB Bulk driver. 11 | 0=OFF, 1=ERR, 2=WRN, 3=INF, 4=DBG 12 | 13 | config ARES_USB_THREAD_STACK_SIZE 14 | int "Stack size for ARES USB processing thread" 15 | default 1024 16 | help 17 | Specifies the stack size for the thread that processes incoming USB data. 18 | 19 | endmenu # ARES Custom USB Application 20 | -------------------------------------------------------------------------------- /lib/ares/interface/usb/note.md: -------------------------------------------------------------------------------- 1 | # USBD调用关系及结构笔记 2 | ## 接收数据 3 | ```mermaid 4 | sequenceDiagram 5 | participant HW as Hardware Interrupt 6 | participant ISR as ISR Context (HAL) 7 | participant UDC_Thread as UDC Driver Thread 8 | participant USBD_Thread as USBD Worker Thread 9 | participant User_Code as User Class Driver 10 | 11 | %% 1. Hardware Interrupt -> HAL Callback (ISR Context) 12 | HW->>ISR: IN Transfer Complete Interrupt 13 | activate ISR 14 | Note right of ISR: HAL_PCD_DataInStageCallback() 15 | ISR->>+UDC_Thread: k_msgq_put(&priv->msgq_data, ...) 16 | deactivate ISR 17 | 18 | %% 2. UDC Driver Thread wakes up and processes the message 19 | Note over UDC_Thread: Wakes up, now in Thread Context 20 | UDC_Thread->>UDC_Thread: k_msgq_get() 21 | UDC_Thread->>UDC_Thread: handle_msg_data_in(epnum) 22 | alt If transfer is not complete 23 | UDC_Thread->>HW: Continues sending next packet 24 | else If transfer is complete 25 | UDC_Thread->>+USBD_Thread: udc_submit_ep_event() -> usbd_event_carrier() -> k_msgq_put(&usbd_msgq, ...) 26 | end 27 | deactivate UDC_Thread 28 | 29 | %% 3. USBD Worker Thread wakes up and handles the event 30 | Note over USBD_Thread: Wakes up, now in Thread Context 31 | USBD_Thread->>USBD_Thread: k_msgq_get() 32 | USBD_Thread->>USBD_Thread: usbd_event_handler(event) 33 | Note right of USBD_Thread: switch (event->type)
case UDC_EVT_EP_REQUEST 34 | USBD_Thread->>USBD_Thread: event_handler_ep_request(event) 35 | USBD_Thread->>USBD_Thread: usbd_class_handle_xfer(buf, err) 36 | USBD_Thread->>+User_Code: usbd_class_request(c_data, buf, err) 37 | deactivate USBD_Thread 38 | 39 | %% 4. User Code (your class driver) is called 40 | Note over User_Code: Your api->request() is called 41 | User_Code->>User_Code: Process completion (check err, etc.) 42 | User_Code->>User_Code: net_buf_unref(buf) 43 | Note right of User_Code: Final ownership handled,
buffer is released. 44 | deactivate User_Code 45 | ``` 46 | -------------------------------------------------------------------------------- /lib/ares/interface/usb/usb_bulk.h: -------------------------------------------------------------------------------- 1 | // usb_bulk.h 2 | #ifndef ARES_USB_BULK_H__ 3 | #define ARES_USB_BULK_H__ 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | int ares_usbd_init(struct AresInterface *interface); 12 | int ares_usbd_write(struct AresInterface *interface, struct net_buf *buf); 13 | struct net_buf *ares_interface_alloc_buf(struct AresInterface *interface); 14 | struct net_buf *ares_interface_alloc_buf_with_data(struct AresInterface *interface, void *data, 15 | size_t len); 16 | int ares_usbd_write_with_lock(struct AresInterface *interface, struct net_buf *buf, 17 | struct k_mutex *mutex); 18 | 19 | struct AresBulkInterface { 20 | struct AresInterface *interface; 21 | 22 | struct usbd_class_data *c_data; 23 | struct usbd_context *usbd_ctx; 24 | 25 | atomic_t state; 26 | 27 | struct k_msgq *incoming_data_msgq; 28 | struct k_thread *processing_thread_data; 29 | }; 30 | 31 | #define ARES_BULK_INTERFACE_DEFINE(Interface_name) \ 32 | struct AresInterfaceAPI ares_bulk_interface_api = { \ 33 | .init = ares_usbd_init, \ 34 | .send = ares_usbd_write, \ 35 | .send_with_lock = ares_usbd_write_with_lock, \ 36 | .alloc_buf = ares_interface_alloc_buf, \ 37 | .alloc_buf_with_data = ares_interface_alloc_buf_with_data, \ 38 | }; \ 39 | struct AresBulkInterface Internal_##Interface_name = {NULL}; \ 40 | struct AresInterface Interface_name = { \ 41 | .name = #Interface_name, \ 42 | .api = &ares_bulk_interface_api, \ 43 | .protocol = NULL, \ 44 | .priv_data = &Internal_##Interface_name, \ 45 | }; 46 | 47 | #endif // ARES_USB_BULK_H__ 48 | -------------------------------------------------------------------------------- /lib/ares/mahony/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | # SPDX-License-Identifier: Apache-2.0 3 | 4 | zephyr_library() 5 | 6 | file(GLOB MAHONY_SOURCES "${CMAKE_CURRENT_SOURCE_DIR}/*.c") 7 | 8 | zephyr_library_sources(${MAHONY_SOURCES}) 9 | 10 | zephyr_library_compile_options( 11 | -O0 12 | # -fno-strict-overflow 13 | # -fno-common 14 | # -ffunction-sections 15 | # -fdata-sections 16 | # -ffreestanding 17 | # -fno-builtin 18 | ) 19 | -------------------------------------------------------------------------------- /lib/ares/mahony/MahonyAHRS.h: -------------------------------------------------------------------------------- 1 | //===================================================================================================== 2 | // MahonyAHRS.h 3 | //===================================================================================================== 4 | // 5 | // Madgwick's implementation of Mayhony's AHRS algorithm. 6 | // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 7 | // 8 | // Date Author Notes 9 | // 29/09/2011 SOH Madgwick Initial release 10 | // 02/10/2011 SOH Madgwick Optimised for reduced CPU load 11 | // 12 | //===================================================================================================== 13 | 14 | #include 15 | 16 | #ifndef MahonyAHRS_h 17 | #define MahonyAHRS_h 18 | 19 | typedef struct { 20 | float q[4]; 21 | float Accel[3]; 22 | float Gyro[3]; 23 | float Mag[3]; 24 | float Yaw, Pitch, Roll; 25 | float g; 26 | } MahonyAHRS_INS_t; 27 | 28 | //---------------------------------------------------------------------------------------------------- 29 | // Variable declaration 30 | 31 | extern volatile float twoKp; // 2 * proportional gain (Kp) 32 | extern volatile float twoKi; // 2 * integral gain (Ki) 33 | extern volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame 34 | 35 | extern MahonyAHRS_INS_t MahonyAHRS_INS; 36 | 37 | //--------------------------------------------------------------------------------------------------- 38 | // Function declarations 39 | 40 | void MahonyAHRSupdate(float gx, float gy, float gz, float gyro_dt, float ax, float ay, float az, 41 | float mx, float my, float mz, float accel_dt); 42 | void MahonyAHRSupdateIMU(float gx, float gy, float gz, float gyro_dt, float ax, float ay, float az, 43 | float accel_dt); 44 | void MahonyAHRSupdateGyro(float gx, float gy, float gz, float gyro_dt); 45 | 46 | #endif 47 | //===================================================================================================== 48 | // End of file 49 | //===================================================================================================== 50 | -------------------------------------------------------------------------------- /lib/ares/mahony/imu_task.h: -------------------------------------------------------------------------------- 1 | #ifndef IMU_TASK_H 2 | #define IMU_TASK_H 3 | 4 | #include 5 | #include 6 | #include "MahonyAHRS.h" 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | static const int X = 0; 15 | static const int Y = 1; 16 | static const int Z = 2; 17 | 18 | #ifndef PHY_G 19 | #define PHY_G 9.80665f 20 | #endif 21 | 22 | typedef void (*update_cb_t)(MahonyAHRS_INS_t *MahonyAHRS); 23 | 24 | typedef struct { 25 | // IMU量测值 26 | float Gyro[3]; // 角速度 27 | float Accel[3]; // 加速度 28 | float Mag[3]; // 磁场 29 | 30 | float AccelBias[3]; 31 | float AccelBeta[3]; 32 | float GyroBias[3]; 33 | float GyroBeta[3]; 34 | 35 | float MagBias[3]; 36 | float MagBeta[3]; 37 | 38 | bool hasStoredBias; 39 | 40 | float lpf_Accel[3]; // 加速度低通滤波 41 | 42 | uint32_t gyro_prev_cyc; 43 | uint32_t accel_prev_cyc; 44 | 45 | float AccelLPF; 46 | 47 | update_cb_t update_cb; 48 | 49 | uint32_t temp_update_ms; 50 | float imu_temp; 51 | 52 | struct device *accel_dev; 53 | struct device *gyro_dev; 54 | struct device *mag_dev; 55 | } INS_t; 56 | 57 | typedef struct { 58 | bool flag; 59 | float Gyro[3]; // 角速度 60 | } IMU_Bias_t; 61 | 62 | /* 用于修正安装误差的参数 */ 63 | typedef struct { 64 | uint8_t flag; 65 | 66 | float scale[3]; 67 | 68 | float Yaw; 69 | float Pitch; 70 | float Roll; 71 | } IMU_Param_t; 72 | 73 | float IMU_temp_read(const struct device *dev); 74 | void IMU_Sensor_set_update_cb(update_cb_t cb); 75 | #ifdef CONFIG_IMU_PWM_TEMP_CTRL 76 | void IMU_Sensor_set_IMU_temp(float temp); 77 | #endif // CONFIG_IMU_PWM_TEMP_CTRL 78 | void IMU_Sensor_trig_init(const struct device *accel_dev, const struct device *gyro_dev, 79 | const struct device *mag_dev); 80 | 81 | #define MIN_PERIOD PWM_SEC(1U) / 128U 82 | #define MAX_PERIOD PWM_SEC(1U) 83 | 84 | extern INS_t INS; 85 | 86 | #if DT_HAS_CHOSEN(ares_bias) 87 | #define GYRO_BIAS_NODE DT_CHOSEN(ares_bias) 88 | #if DT_NODE_HAS_PROP(GYRO_BIAS_NODE, gyro_bias) 89 | #endif // DT_NODE_HAS_PROP(GYRO_BIAS_NODE, gyro_bias) 90 | #endif // DT_HAS_CHOSEN(ares_bias) 91 | 92 | #endif // IMU_TASK_H 93 | -------------------------------------------------------------------------------- /lib/ares/mahony/matrix_storage.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2024 BayLibre SAS 3 | * Copyright (c) 2024 Your Name/Company // Add your copyright 4 | * 5 | * SPDX-License-Identifier: Apache-2.0 6 | */ 7 | 8 | #ifndef MATRIX_STORAGE_H_ 9 | #define MATRIX_STORAGE_H_ 10 | 11 | #include 12 | #include // For size_t 13 | 14 | #define MATRIX_ROWS 3 15 | #define MATRIX_COLS 3 16 | 17 | #define ZMS_MIN_SECTOR_COUNT 2 18 | 19 | /** 20 | * @brief Saves two 3x3 float matrices to ZMS storage. 21 | * 22 | * This function attempts to initialize the ZMS filesystem if not already done, 23 | * then writes the provided matrices under a predefined internal ID. 24 | * 25 | * @param matrix1 Pointer to the first 3x3 float matrix. 26 | * @param matrix2 Pointer to the second 3x3 float matrix. 27 | * @return 0 on success. 28 | * @return -errno code on failure (e.g., -ENOSPC, -EIO, -EINVAL). 29 | */ 30 | int matrix_storage_save(const float matrix1[MATRIX_ROWS][MATRIX_COLS], 31 | const float matrix2[MATRIX_ROWS][MATRIX_COLS]); 32 | 33 | /** 34 | * @brief Reads two 3x3 float matrices from ZMS storage. 35 | * 36 | * This function attempts to initialize the ZMS filesystem if not already done, 37 | * then reads the matrices associated with a predefined internal ID. 38 | * 39 | * @param matrix1 Pointer to a buffer to store the first 3x3 float matrix. 40 | * @param matrix2 Pointer to a buffer to store the second 3x3 float matrix. 41 | * @return 0 on success. 42 | * @return -ENOENT if the matrices are not found in storage. 43 | * @return -EIO if the read data size does not match the expected size. 44 | * @return Other -errno codes on ZMS or flash driver failures. 45 | */ 46 | int matrix_storage_read(float matrix1[MATRIX_ROWS][MATRIX_COLS], 47 | float matrix2[MATRIX_ROWS][MATRIX_COLS]); 48 | 49 | /** 50 | * @brief Checks if the matrices exist in ZMS storage. 51 | * 52 | * This function attempts to initialize the ZMS filesystem if not already done, 53 | * then checks for the presence and correct size of the matrix data. 54 | * 55 | * @return true if the matrices exist and have the correct size. 56 | * @return false if the matrices do not exist, have an incorrect size, 57 | * or an error occurred during initialization or check. 58 | */ 59 | bool matrix_storage_exists(void); 60 | 61 | /** 62 | * @brief Deletes the stored matrices from ZMS storage. 63 | * 64 | * This function attempts to initialize the ZMS filesystem if not already done, 65 | * then deletes the matrices associated with a predefined internal ID. 66 | * 67 | * @return 0 on success or if the item was already deleted. 68 | * @return -errno code on failure during deletion (e.g., -EIO). Note that 69 | * -ENOENT from the underlying zms_delete is treated as success here. 70 | */ 71 | int matrix_storage_delete(void); 72 | 73 | #endif /* MATRIX_STORAGE_H_ */ 74 | -------------------------------------------------------------------------------- /lib/ares/protocol/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory_ifdef(CONFIG_DUAL_PROPOSE_PROTOCOL dual) 2 | add_subdirectory_ifdef(CONFIG_ARES_PLOTTER_PROTOCOL plotter) 3 | -------------------------------------------------------------------------------- /lib/ares/protocol/Kconfig: -------------------------------------------------------------------------------- 1 | # source "dual/Kconfig" 2 | 3 | config DUAL_PROPOSE_PROTOCOL 4 | bool "DUAL_PROPOSE_PROTOCOL" 5 | help 6 | Enable DUAL_PROPOSE_PROTOCOL 7 | select ARES_COMM_LIB 8 | 9 | rsource "plotter/Kconfig" 10 | -------------------------------------------------------------------------------- /lib/ares/protocol/ares_protocol.h: -------------------------------------------------------------------------------- 1 | #ifndef ARES_PROTOCOL_H 2 | #define ARES_PROTOCOL_H 3 | 4 | #include 5 | #include 6 | 7 | #ifdef __cplusplus 8 | extern "C" { 9 | #endif 10 | 11 | enum AresProtocolEvent { 12 | ARES_PROTOCOL_EVENT_CONNECTED, 13 | ARES_PROTOCOL_EVENT_DISCONNECTED, 14 | }; 15 | 16 | struct AresProtocol; 17 | 18 | /** 19 | * @brief API that a protocol must implement. 20 | * 21 | * This structure defines the set of functions that a specific protocol 22 | * (e.g., MAVLink, CANopen) must provide. These functions are called by the 23 | * interface layer to process data and manage the protocol's lifecycle. 24 | */ 25 | struct AresProtocolAPI { 26 | void (*handle)(struct AresProtocol *protocol, struct net_buf *buf); 27 | void (*handle_byte)(struct AresProtocol *protocol, uint8_t byte); 28 | void (*event)(struct AresProtocol *protocol, enum AresProtocolEvent event); 29 | int (*init)(struct AresProtocol *protocol); 30 | }; 31 | 32 | /** 33 | * @brief Represents a stateless definition of a communication protocol. 34 | */ 35 | struct AresProtocol { 36 | const char *name; 37 | const struct AresProtocolAPI *api; 38 | struct AresInterface *interface; 39 | 40 | void *priv_data; 41 | }; 42 | 43 | #ifdef __cplusplus 44 | } 45 | #endif 46 | 47 | #endif 48 | -------------------------------------------------------------------------------- /lib/ares/protocol/dual/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2025 ttwards <12411711@mail.sustech.edu.cn> 2 | # SPDX-License-Identifier: Apache-2.0 3 | 4 | zephyr_library() 5 | 6 | set(DUAL_PROTOCOL_SOURCES dual_protocol.c) 7 | 8 | zephyr_library_sources(${DUAL_PROTOCOL_SOURCES}) 9 | -------------------------------------------------------------------------------- /lib/ares/protocol/plotter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | if(CONFIG_ARES_PLOTTER_PROTOCOL) 2 | zephyr_library() 3 | zephyr_library_sources(aresplot_protocol.c) 4 | zephyr_library_include_directories(${CMAKE_CURRENT_SOURCE_DIR}) 5 | endif() 6 | -------------------------------------------------------------------------------- /lib/ares/protocol/plotter/Kconfig: -------------------------------------------------------------------------------- 1 | config ARES_PLOTTER_PROTOCOL 2 | bool "AresPlot Protocol" 3 | default n 4 | help 5 | Enable AresPlot Protocol for real-time variable monitoring and plotting. 6 | This protocol allows remote monitoring and control of MCU variables 7 | through any Ares interface (UART, USB, etc.). 8 | 9 | config PLOTTER 10 | bool "PLOTTER" 11 | select ARES_COMM_LIB 12 | select UART_INTERFACE 13 | select ARES_PLOTTER_PROTOCOL 14 | help 15 | Enable PLOTTER 16 | 17 | if ARES_PLOTTER_PROTOCOL 18 | 19 | config ARESPLOT_MAX_VARS_TO_MONITOR 20 | int "Maximum number of variables to monitor" 21 | default 10 22 | range 1 50 23 | help 24 | Maximum number of variables that can be monitored simultaneously. 25 | Increase this value if you need to monitor more variables, but be 26 | aware that it will increase memory usage. 27 | 28 | config ARESPLOT_SHARED_BUFFER_SIZE 29 | int "Shared buffer size for protocol frames" 30 | default 256 31 | range 64 512 32 | help 33 | Size of the buffer used for assembling and parsing protocol frames. 34 | Should be large enough to contain the largest possible frame. 35 | 36 | config ARESPLOT_ENABLE_ERROR_REPORT 37 | bool "Enable error reporting" 38 | default n 39 | help 40 | Enable optional error reporting functionality. When enabled, 41 | the protocol can send error reports to the host PC. 42 | 43 | config ARESPLOT_FREQ 44 | int "Sampling frequency" 45 | default 500 46 | range 1 1000 47 | help 48 | Sampling frequency in Hz. 49 | This is the frequency at which the protocol will sample the variables. 50 | 51 | endif # ARES_PLOTTER_PROTOCOL 52 | -------------------------------------------------------------------------------- /lib/ares/vofa/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 2 | # SPDX-License-Identifier: Apache-2.0 3 | 4 | zephyr_library() 5 | 6 | set(VOFA_SOURCES justfloat.c) 7 | 8 | zephyr_library_sources(${VOFA_SOURCES}) 9 | -------------------------------------------------------------------------------- /lib/ares/vofa/justfloat.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include "justfloat.h" 6 | #include "zephyr/kernel/thread.h" 7 | #include 8 | #include 9 | 10 | // LOG_MODULE_REGISTER(vofa, LOG_LEVEL_DBG); 11 | 12 | const uint8_t tail[4] = {0x00, 0x00, 0x80, 0x7f}; 13 | struct JFData aresPlotData; 14 | 15 | K_SEM_DEFINE(jf_sem, 0, 3); 16 | 17 | static struct k_timer jf_timer; 18 | 19 | static void jf_timer_call(struct k_timer *timer_id) 20 | { 21 | k_sem_give(&jf_sem); 22 | } 23 | 24 | static void jf_send_float(struct JFData *data) 25 | { 26 | const struct device *uart_dev = data->uart_dev; 27 | 28 | for (int i = 0; i < data->channel; i++) { 29 | if (data->fdata[i] == *(float *)tail) { 30 | data->fdata[i] = 1e+6; 31 | } 32 | } 33 | 34 | if (data->fdata[data->channel] != *(float *)tail) { 35 | memcpy(&(data->fdata[data->channel]), tail, 4 * sizeof(uint8_t)); 36 | } 37 | 38 | uart_tx(uart_dev, (const uint8_t *)data->fdata, data->channel * 4 + 4, SYS_FOREVER_US); 39 | } 40 | 41 | /* JustFloat Feedback to console*/ 42 | static K_THREAD_STACK_DEFINE(jf_stack_area, 768); // 定义线程栈 43 | static struct k_thread jf_thread_data; 44 | 45 | static void jf_feedback(void *arg1, void *arg2, void *arg3) 46 | { 47 | struct JFData *data = (struct JFData *)arg2; 48 | 49 | while (1) { 50 | k_sem_take(&jf_sem, K_FOREVER); 51 | if (data->channel == 0) { 52 | continue; 53 | } 54 | for (int i = 0; i < data->channel; i++) { 55 | switch (data->types[i]) { 56 | case PTR_INT: 57 | data->fdata[i] = *(int *)data->data_ptr[i]; 58 | break; 59 | case PTR_FLOAT: 60 | data->fdata[i] = *(float *)data->data_ptr[i]; 61 | break; 62 | case PTR_DOUBLE: 63 | data->fdata[i] = *(double *)data->data_ptr[i]; 64 | break; 65 | case PTR_INT8: 66 | data->fdata[i] = *(int8_t *)data->data_ptr[i]; 67 | break; 68 | case PTR_INT16: 69 | data->fdata[i] = *(int16_t *)data->data_ptr[i]; 70 | break; 71 | default: 72 | break; 73 | } 74 | } 75 | jf_send_float(data); 76 | } 77 | } 78 | 79 | void jf_channel_add(struct JFData *data, void *value, enum JF_Types type) 80 | { 81 | if (data->channel < 24) { 82 | if (type == RAW) { 83 | data->fdata[data->channel] = *(float *)value; 84 | } else { 85 | data->fdata[data->channel] = *(float *)value; 86 | data->data_ptr[data->channel] = value; 87 | } 88 | } 89 | data->types[data->channel] = type; 90 | data->channel++; 91 | data->types[data->channel] = RAW; 92 | memcpy(&(data->fdata[data->channel]), tail, 4 * sizeof(uint8_t)); 93 | } 94 | 95 | struct JFData *jf_send_init(const struct device *uart_dev, int delay) 96 | { 97 | aresPlotData.channel = 0; 98 | if (!device_is_ready(uart_dev)) { 99 | return NULL; 100 | } 101 | aresPlotData.uart_dev = (struct device *)uart_dev; 102 | memcpy(&(aresPlotData.fdata[aresPlotData.channel]), tail, 4 * sizeof(uint8_t)); 103 | aresPlotData.types[aresPlotData.channel] = RAW; 104 | delay = delay > 0 ? delay : 10; 105 | /* Start JustFloat thread*/ 106 | k_tid_t tid = k_thread_create(&jf_thread_data, jf_stack_area, 107 | K_THREAD_STACK_SIZEOF(jf_stack_area), jf_feedback, 108 | (void *)delay, &aresPlotData, NULL, 1, 0, K_MSEC(200)); 109 | k_thread_name_set(tid, "just_float"); 110 | k_timer_init(&jf_timer, jf_timer_call, NULL); 111 | k_timer_start(&jf_timer, K_MSEC(200), K_MSEC(delay)); 112 | return &aresPlotData; 113 | } 114 | -------------------------------------------------------------------------------- /lib/ares/vofa/justfloat.h: -------------------------------------------------------------------------------- 1 | #ifndef JUSTFLOAT_H 2 | #define JUSTFLOAT_H 3 | 4 | #include 5 | #include 6 | 7 | #define aresMaxChannel 24 8 | 9 | enum JF_Types { 10 | PTR_INT = 0, 11 | PTR_FLOAT = 1, 12 | PTR_DOUBLE = 2, 13 | PTR_INT8 = 3, 14 | PTR_INT16 = 4, 15 | PTR_UINT8 = 5, 16 | PTR_UINT16 = 6, 17 | PTR_UINT = 7, 18 | RAW = 8, 19 | }; 20 | 21 | struct JFData { 22 | struct device *uart_dev; 23 | 24 | void *data_ptr[aresMaxChannel]; 25 | 26 | float fdata[aresMaxChannel]; 27 | 28 | enum JF_Types types[aresMaxChannel]; 29 | 30 | int channel; 31 | }; 32 | 33 | struct JFData *jf_send_init(const struct device *uart_dev, int delay); 34 | void jf_channel_add(struct JFData *data, void *value, enum JF_Types type); 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /samples/IMU/IMU/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | 3 | cmake_minimum_required(VERSION 3.20.0) 4 | set(CMAKE_CXX_STANDARD 17) 5 | find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) 6 | project(motor) 7 | 8 | file(GLOB app_sources src/*.c) 9 | 10 | include_directories(${ZEPHYR_BASE}/include) 11 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) 12 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/dts) 13 | target_sources(app PRIVATE ${app_sources}) 14 | target_include_directories(app PRIVATE ${ZEPHYR_BASE}/include) 15 | target_include_directories(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include) 16 | target_include_directories(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/dts) 17 | -------------------------------------------------------------------------------- /samples/IMU/IMU/README.rst: -------------------------------------------------------------------------------- 1 | .. zephyr:code-sample:: imu-quaternion-ekf 2 | :name: IMU四元数扩展卡尔曼滤波器示例 3 | 4 | 使用四元数EKF算法处理IMU传感器数据并估计姿态角度。 5 | 6 | 概述 7 | **** 8 | 9 | 这个示例应用演示了如何使用ARES框架中的四元数扩展卡尔曼滤波器(Quaternion EKF)来处理IMU传感器数据。该示例集成了BMI08x系列的加速度计和陀螺仪，通过Hongxi Wong的QEKF融合算法实现精确的姿态估计。 10 | 11 | 传感器配置 12 | ========== 13 | 14 | 该示例使用以下传感器： 15 | 16 | * **BMI08x加速度计**: 测量线性加速度，用于重力方向估计 17 | * **BMI08x陀螺仪**: 测量角速度，提供高频率姿态更新 18 | 19 | 传感器通过SPI接口连接，支持高频率数据采集(最高2000Hz)。 20 | 21 | 硬件要求 22 | ******** 23 | 24 | * RoboMaster Board C (STM32F407) 25 | * DM MC02开发板 26 | 27 | 构建和运行 28 | ********** 29 | 30 | 以RoboMaster Board C为例的构建命令: 31 | 32 | `west build -b robomaster_board_c samples/IMU` 33 | 34 | 配置选项 35 | ======== 36 | 37 | 主要的Kconfig配置选项： 38 | 39 | * ``CONFIG_ARES=y`` - 启用ARES框架 40 | * ``CONFIG_MAHONY_LIB=y`` - 启用Mahony算法库 41 | * ``CONFIG_AUTO_PROBE_GYRO_BIAS=y`` - 启用陀螺仪零偏自动估计 42 | * ``CONFIG_CMSIS_DSP=y`` - 启用CMSIS DSP库用于矩阵运算 43 | * ``CONFIG_PLOTTER=y`` - 启用数据绘图功能 44 | 45 | 示例输出 46 | ======== 47 | 48 | 控制台输出示例: 49 | 50 | .. code-block:: console 51 | 52 | *** Booting Zephyr OS build *** 53 | [00:00:01.000,000] main: q: 1.000000, 0.000000, 0.000000, 0.000000 54 | [00:00:01.200,000] main: Yaw: 0.123456, Pitch: 0.654321, Roll: 0.987654 55 | [00:00:01.400,000] main: q: 0.999123, 0.001234, 0.005678, 0.009876 56 | [00:00:01.600,000] main: Yaw: 1.234567, Pitch: 0.765432, Roll: 1.098765 57 | 58 | 输出数据说明: 59 | 60 | * **q**: 四元数分量 [q0, q1, q2, q3]，表示当前姿态 61 | * **Yaw/Pitch/Roll**: 欧拉角(度)，分别表示偏航角、俯仰角、横滚角 62 | -------------------------------------------------------------------------------- /samples/IMU/IMU/boards/robomaster_board_c.overlay: -------------------------------------------------------------------------------- 1 | / { 2 | }; 3 | -------------------------------------------------------------------------------- /samples/IMU/IMU/prj.conf: -------------------------------------------------------------------------------- 1 | CONFIG_ARES=y 2 | CONFIG_EKF_LIB=y 3 | 4 | CONFIG_CMSIS_DSP=y 5 | CONFIG_CMSIS_DSP_MATRIX=y 6 | CONFIG_CMSIS_DSP_CONTROLLER=y 7 | CONFIG_CMSIS_DSP_FASTMATH=y 8 | 9 | CONFIG_PLOTTER=y 10 | CONFIG_ARESPLOT_FREQ=500 11 | 12 | CONFIG_CHASSIS=n 13 | -------------------------------------------------------------------------------- /samples/IMU/IMU/sample.yaml: -------------------------------------------------------------------------------- 1 | sample: 2 | name: IMU四元数EKF姿态估计示例 3 | description: 使用ARES框架的四元数扩展卡尔曼滤波器处理IMU传感器数据并估计姿态角度 4 | tests: 5 | sample.ares.imu.quaternion_ekf: 6 | tags: sensor imu ekf ares 7 | depends_on: spi sensor 8 | filter: dt_nodelabel_enabled("bmi08x_accel") and dt_nodelabel_enabled("bmi08x_gyro") 9 | integration_platforms: 10 | - robomaster_board_c 11 | - dm_mc02 12 | harness: console 13 | harness_config: 14 | type: multi_line 15 | regex: 16 | - "q: [0-9.-]+, [0-9.-]+, [0-9.-]+, [0-9.-]+" 17 | - "Yaw: [0-9.-]+, Pitch: [0-9.-]+, Roll: [0-9.-]+" 18 | -------------------------------------------------------------------------------- /samples/IMU/IMU/src/devices.h: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | /* Devicetree */ 5 | #define ACCEL_NODE DT_NODELABEL(bmi08x_accel) 6 | const struct device *accel_dev = DEVICE_DT_GET(ACCEL_NODE); 7 | 8 | #define GYRO_NODE DT_NODELABEL(bmi08x_gyro) 9 | const struct device *gyro_dev = DEVICE_DT_GET(GYRO_NODE); 10 | 11 | #define MAG_NODE DT_NODELABEL(ist8310) 12 | const struct device *mag_dev = DEVICE_DT_GET(MAG_NODE); 13 | -------------------------------------------------------------------------------- /samples/IMU/IMU/src/main.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 3 | * 4 | * SPDX-License-Identifier: Apache-2.0 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include "ares/ekf/QuaternionEKF.h" 20 | #include "devices.h" 21 | #include 22 | #include 23 | 24 | LOG_MODULE_REGISTER(main, LOG_LEVEL_DBG); 25 | 26 | #ifndef PI 27 | #define PI 3.14159265f 28 | #endif 29 | 30 | /* CAN Feedback to console*/ 31 | void console_feedback(void *arg1, void *arg2, void *arg3) 32 | { 33 | while (1) { 34 | LOG_INF("q: %f, %f, %f, %f", QEKF_INS.q[0], QEKF_INS.q[1], QEKF_INS.q[2], 35 | QEKF_INS.q[3]); 36 | LOG_INF("Yaw: %f, Pitch: %f, Roll: %f", QEKF_INS.Yaw, QEKF_INS.Pitch, 37 | QEKF_INS.Roll); 38 | k_msleep(200); 39 | } 40 | } 41 | K_THREAD_DEFINE(feedback_thread, 4096, console_feedback, NULL, NULL, NULL, -1, 0, 100); 42 | 43 | float vel[3] = {0}; 44 | float pos[3] = {0}; 45 | float turned_accel[3] = {0}; 46 | void Sensor_update_cb(QEKF_INS_t *QEKF) 47 | { 48 | } 49 | 50 | float vel_data[4] = {0}; 51 | void trans_cb(int arg) 52 | { 53 | return; 54 | } 55 | 56 | int main(void) 57 | { 58 | k_msleep(2000); 59 | 60 | IMU_Sensor_trig_init(accel_dev, gyro_dev); 61 | IMU_Sensor_set_update_cb(Sensor_update_cb); 62 | 63 | while (1) { 64 | k_msleep(10); 65 | } 66 | } 67 | -------------------------------------------------------------------------------- /samples/IMU/IMU_Calibrate/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | 3 | cmake_minimum_required(VERSION 3.20.0) 4 | set(CMAKE_CXX_STANDARD 17) 5 | find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) 6 | project(IMU) 7 | 8 | file(GLOB app_sources src/*.c) 9 | 10 | target_sources(app PRIVATE ${app_sources}) 11 | -------------------------------------------------------------------------------- /samples/IMU/IMU_Calibrate/README.rst: -------------------------------------------------------------------------------- 1 | .. zephyr:code-sample:: can-babbling 2 | :name: Controller Area Network (CAN) babbling node 3 | :relevant-api: can_interface 4 | 5 | Simulate a babbling CAN node. 6 | 7 | Overview 8 | ******** 9 | 10 | In a Controller Area Network a babbling node is a node continuously (and usually erroneously) 11 | transmitting CAN frames with identical - often high - priority. This constant babbling blocks CAN 12 | bus access for any CAN frame with lower priority as these frames will loose the bus arbitration. 13 | 14 | This sample application simulates a babbling CAN node. The properties of the CAN frames sent are 15 | configurable via :ref:`Kconfig `. The frames carry no data as only the arbitration part of 16 | the CAN frame is of interest. 17 | 18 | Being able to simulate a babbling CAN node is useful when examining the behavior of other nodes on 19 | the same CAN bus when they constantly loose bus arbitration. 20 | 21 | The source code for this sample application can be found at: 22 | :zephyr_file:`samples/drivers/can/babbling`. 23 | 24 | Requirements 25 | ************ 26 | 27 | This sample requires a board with a CAN controller. The CAN controller must be configured using the 28 | ``zephyr,canbus`` :ref:`devicetree ` chosen node property. 29 | 30 | The sample supports an optional button for stopping the babbling. If present, the button must be 31 | configured using the ``sw0`` :ref:`devicetree ` alias, usually in the :ref:`BOARD.dts file 32 | `. 33 | 34 | Building and Running 35 | ******************** 36 | 37 | Example building for :ref:`twr_ke18f`: 38 | 39 | .. zephyr-app-commands:: 40 | :zephyr-app: samples/drivers/can/babbling 41 | :board: twr_ke18f 42 | :goals: build flash 43 | 44 | Sample output 45 | ============= 46 | 47 | .. code-block:: console 48 | 49 | *** Booting Zephyr OS build zephyr-v3.1.0-4606-g8c1efa8b96bb *** 50 | babbling on can@40024000 with standard (11-bit) CAN ID 0x010, RTR 0, CAN FD 0 51 | abort by pressing User SW3 button 52 | -------------------------------------------------------------------------------- /samples/IMU/IMU_Calibrate/boards/robomaster_board_c.overlay: -------------------------------------------------------------------------------- 1 | /* SPDX-License-Identifier: Apache-2.0 */ 2 | 3 | /* 定位到你的板子的 flash controller 节点，通常是 &flash0 或类似名称 */ 4 | /* 对于 stm32f407vg_disc1 或类似板子，通常是 &flash0 */ 5 | &flash0 { 6 | partitions { 7 | compatible = "fixed-partitions"; 8 | #address-cells = <1>; 9 | #size-cells = <1>; 10 | 11 | /* Sector 10 base: 0x080C0000, size: 128KB */ 12 | /* Sector 11 base: 0x080E0000, size: 128KB */ 13 | /* 我们使用 Sector 10 和 Sector 11，总共 256KB */ 14 | storage_partition: partition@c0000 { 15 | label = "storage"; 16 | /* 从 Sector 10 开始 (0x080C0000)，大小为 256KB (0x40000) */ 17 | reg = <0x00C0000 0x00040000>; 18 | }; 19 | }; 20 | }; 21 | -------------------------------------------------------------------------------- /samples/IMU/IMU_Calibrate/prj.conf: -------------------------------------------------------------------------------- 1 | 2 | # 启用线程分析器 3 | CONFIG_THREAD_ANALYZER=y 4 | CONFIG_THREAD_ANALYZER_USE_LOG=y 5 | CONFIG_THREAD_ANALYZER_USE_PRINTK=n 6 | CONFIG_THREAD_NAME=y 7 | # CONFIG_THREAD_ANALYZER_AUTO=y 8 | # CONFIG_THREAD_ANALYZER_AUTO_INTERVAL=5 9 | CONFIG_THREAD_ANALYZER_LOG_LEVEL_INF=y 10 | CONFIG_THREAD_ANALYZER_AUTO_SEPARATE_CORES=n 11 | 12 | # 启用 Shell 支持 13 | CONFIG_SHELL=n 14 | CONFIG_SHELL_BACKENDS=n 15 | CONFIG_SHELL_BACKEND_SERIAL=n 16 | 17 | # 启用 UART 控制台 18 | CONFIG_UART_CONSOLE=y 19 | 20 | # 禁用 SEGGER RTT 21 | CONFIG_USE_SEGGER_RTT=n 22 | 23 | # 启用事件支持 24 | CONFIG_EVENTS=y 25 | 26 | # 启用控制台支持 27 | CONFIG_CONSOLE=y 28 | 29 | # 启用日志系统 30 | CONFIG_LOG=y 31 | CONFIG_LOG_MODE_IMMEDIATE=y 32 | CONFIG_LOG_PRINTK=y 33 | CONFIG_LOG_BACKEND_UART=y 34 | # CONFIG_LOG_MODE_IMMEDIATE=y 35 | 36 | # 启用串行支持 37 | CONFIG_SERIAL=y 38 | 39 | # 禁用优化 40 | CONFIG_NO_OPTIMIZATIONS=n 41 | 42 | CONFIG_DMA=y 43 | CONFIG_DMA_STM32=y # STM32 DMA 驱动 44 | CONFIG_UART_STM32=y # STM32 UART 驱动 45 | # CONFIG_STM32_UART_DMA=y 46 | 47 | # CONFIG_HAS_STM32CUBE=y 48 | # CONFIG_USE_STM32_HAL_DMA=y 49 | CONFIG_SPI=y 50 | CONFIG_SPI_STM32=y 51 | 52 | CONFIG_CMSIS_DSP=y 53 | CONFIG_CMSIS_DSP_MATRIX=y 54 | CONFIG_CMSIS_DSP_CONTROLLER=y 55 | CONFIG_CMSIS_DSP_FASTMATH=y 56 | 57 | CONFIG_AUTO_PROBE_GYRO_BIAS=y 58 | 59 | # NVS 60 | CONFIG_NVS=y 61 | CONFIG_NVS_LOG_LEVEL_INF=y # 可选：启用一些NVS信息日志 62 | 63 | # Flash and Flash Map 64 | CONFIG_FLASH=y 65 | CONFIG_FLASH_MAP=y 66 | 67 | # **重要：为了数据完整性检查，启用数据CRC** 68 | CONFIG_NVS_DATA_CRC=y 69 | 70 | # 如果你的应用使用了MPU，并且NVS操作导致MPU fault或-ETIMEDOUT，需要启用这个 71 | # CONFIG_MPU_ALLOW_FLASH_WRITE=y 72 | 73 | # 其他配置... 74 | CONFIG_PRINTK=y 75 | CONFIG_SERIAL=y 76 | # 根据你的板子配置 UART 77 | # CONFIG_UART_CONSOLE=y 78 | 79 | CONFIG_FLASH=y 80 | CONFIG_FLASH_MAP=y 81 | 82 | CONFIG_ZMS=y 83 | CONFIG_LOG=y 84 | CONFIG_LOG_BLOCK_IN_THREAD=y 85 | 86 | # CONFIG_VOFA_LIB=y 87 | 88 | CONFIG_MAIN_STACK_SIZE=4096 89 | -------------------------------------------------------------------------------- /samples/IMU/IMU_Calibrate/prj.conf.old: -------------------------------------------------------------------------------- 1 | # 启用 Shell 支持 2 | CONFIG_SHELL=n 3 | CONFIG_SHELL_BACKENDS=n 4 | CONFIG_SHELL_BACKEND_SERIAL=n 5 | 6 | # 启用 UART 控制台 7 | CONFIG_UART_CONSOLE=y 8 | 9 | # 禁用 SEGGER RTT 10 | CONFIG_USE_SEGGER_RTT=n 11 | 12 | # 启用事件支持 13 | CONFIG_EVENTS=y 14 | 15 | # 启用控制台支持 16 | CONFIG_CONSOLE=y 17 | 18 | # 启用日志系统 19 | CONFIG_LOG=y 20 | CONFIG_LOG_MODE_IMMEDIATE=y 21 | CONFIG_LOG_PRINTK=y 22 | CONFIG_LOG_BACKEND_UART=y 23 | CONFIG_LOG_MODE_IMMEDIATE=y 24 | 25 | # 启用串行支持 26 | CONFIG_SERIAL=y 27 | 28 | # 禁用优化 29 | CONFIG_NO_OPTIMIZATIONS=n 30 | 31 | # use ASYNC uart API 32 | CONFIG_SERIAL=y 33 | CONFIG_UART_ASYNC_API=y 34 | CONFIG_UART_INTERRUPT_DRIVEN=y 35 | 36 | CONFIG_DMA=y 37 | CONFIG_DMA_STM32=y # STM32 DMA 驱动 38 | CONFIG_UART_STM32=y # STM32 UART 驱动 39 | # CONFIG_STM32_UART_DMA=y 40 | 41 | # CONFIG_HAS_STM32CUBE=y 42 | # CONFIG_USE_STM32_HAL_DMA=y 43 | CONFIG_SPI=y 44 | CONFIG_SPI_STM32=y 45 | 46 | # 47 | # Device Drivers 48 | # 49 | CONFIG_SENSOR=y 50 | CONFIG_BMI08X=y 51 | CONFIG_BMI08X_ACCEL_TRIGGER_OWN_THREAD=y 52 | CONFIG_BMI08X_GYRO_TRIGGER_OWN_THREAD=y 53 | CONFIG_BMI08X_I2C_WRITE_BURST_SIZE=16 54 | 55 | CONFIG_GPIO=y 56 | 57 | 58 | CONFIG_CMSIS_DSP=y 59 | CONFIG_CMSIS_DSP_MATRIX=y 60 | -------------------------------------------------------------------------------- /samples/IMU/IMU_Calibrate/sample.yaml: -------------------------------------------------------------------------------- 1 | sample: 2 | name: CAN bus babbling node 3 | tests: 4 | sample.drivers.can.babbling: 5 | tags: can 6 | depends_on: can 7 | filter: dt_chosen_enabled("zephyr,canbus") 8 | integration_platforms: 9 | - native_sim 10 | harness: console 11 | harness_config: 12 | type: one_line 13 | regex: 14 | - "babbling on .*" 15 | -------------------------------------------------------------------------------- /samples/IMU/IMU_Calibrate/src/devices.h: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | /* Devicetree */ 8 | // #define UART_NODE DT_ALIAS(vofa) 9 | // const struct device *uart_dev = DEVICE_DT_GET(UART_NODE); 10 | 11 | #define ACCEL_NODE DT_NODELABEL(bmi08x_accel) 12 | const struct device *accel_dev = DEVICE_DT_GET(ACCEL_NODE); 13 | 14 | #define GYRO_NODE DT_NODELABEL(bmi08x_gyro) 15 | const struct device *gyro_dev = DEVICE_DT_GET(GYRO_NODE); 16 | 17 | #define BUTTON_NODE DT_CHOSEN(zephyr_button) 18 | const struct device *button_dev = DEVICE_DT_GET(BUTTON_NODE); 19 | 20 | static const struct gpio_dt_spec button = GPIO_DT_SPEC_GET_OR(BUTTON_NODE, gpios, {0}); 21 | static struct gpio_callback button_cb_data; 22 | -------------------------------------------------------------------------------- /samples/IMU/IMU_Calibrate/src/matrix_storage.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2024 BayLibre SAS 3 | * Copyright (c) 2024 Your Name/Company // Add your copyright 4 | * 5 | * SPDX-License-Identifier: Apache-2.0 6 | */ 7 | 8 | #ifndef MATRIX_STORAGE_H_ 9 | #define MATRIX_STORAGE_H_ 10 | 11 | #include 12 | #include // For size_t 13 | 14 | #define MATRIX_ROWS 3 15 | #define MATRIX_COLS 3 16 | 17 | #define ZMS_MIN_SECTOR_COUNT 2 18 | 19 | /** 20 | * @brief Saves two 3x3 float matrices to ZMS storage. 21 | * 22 | * This function attempts to initialize the ZMS filesystem if not already done, 23 | * then writes the provided matrices under a predefined internal ID. 24 | * 25 | * @param matrix1 Pointer to the first 3x3 float matrix. 26 | * @param matrix2 Pointer to the second 3x3 float matrix. 27 | * @return 0 on success. 28 | * @return -errno code on failure (e.g., -ENOSPC, -EIO, -EINVAL). 29 | */ 30 | int matrix_storage_save(const float matrix1[MATRIX_ROWS][MATRIX_COLS], 31 | const float matrix2[MATRIX_ROWS][MATRIX_COLS]); 32 | 33 | /** 34 | * @brief Reads two 3x3 float matrices from ZMS storage. 35 | * 36 | * This function attempts to initialize the ZMS filesystem if not already done, 37 | * then reads the matrices associated with a predefined internal ID. 38 | * 39 | * @param matrix1 Pointer to a buffer to store the first 3x3 float matrix. 40 | * @param matrix2 Pointer to a buffer to store the second 3x3 float matrix. 41 | * @return 0 on success. 42 | * @return -ENOENT if the matrices are not found in storage. 43 | * @return -EIO if the read data size does not match the expected size. 44 | * @return Other -errno codes on ZMS or flash driver failures. 45 | */ 46 | int matrix_storage_read(float matrix1[MATRIX_ROWS][MATRIX_COLS], 47 | float matrix2[MATRIX_ROWS][MATRIX_COLS]); 48 | 49 | /** 50 | * @brief Checks if the matrices exist in ZMS storage. 51 | * 52 | * This function attempts to initialize the ZMS filesystem if not already done, 53 | * then checks for the presence and correct size of the matrix data. 54 | * 55 | * @return true if the matrices exist and have the correct size. 56 | * @return false if the matrices do not exist, have an incorrect size, 57 | * or an error occurred during initialization or check. 58 | */ 59 | bool matrix_storage_exists(void); 60 | 61 | /** 62 | * @brief Deletes the stored matrices from ZMS storage. 63 | * 64 | * This function attempts to initialize the ZMS filesystem if not already done, 65 | * then deletes the matrices associated with a predefined internal ID. 66 | * 67 | * @return 0 on success or if the item was already deleted. 68 | * @return -errno code on failure during deletion (e.g., -EIO). Note that 69 | * -ENOENT from the underlying zms_delete is treated as success here. 70 | */ 71 | int matrix_storage_delete(void); 72 | 73 | #endif /* MATRIX_STORAGE_H_ */ 74 | -------------------------------------------------------------------------------- /samples/IMU/IMU_Calibrate/threads.inf: -------------------------------------------------------------------------------- 1 | [00:00:08.570,000] Thread analyze: 2 | [00:00:08.576,000] feedback : STACK: unused 400 usage 1648 / 2048 (80 %); CPU: 76 % 3 | [00:00:08.596,000] can_recv : STACK: unused 164 usage 604 / 768 (78 %); CPU: 1 % 4 | [00:00:08.616,000] can_send : STACK: unused 248 usage 776 / 1024 (75 %); CPU: 21 % 5 | [00:00:08.637,000] thread_analyzer : STACK: unused 2000 usage 48 / 2048 (2 %); CPU: 0 % 6 | [00:00:08.656,000] shell_uart : STACK: unused 1224 usage 824 / 2048 (40 %); CPU: 0 % 7 | [00:00:08.676,000] sysworkq : STACK: unused 452 usage 572 / 1024 (55 %); CPU: 0 % 8 | [00:00:08.696,000] idle : STACK: unused 272 usage 48 / 320 (15 %); CPU: 0 % 9 | [00:00:08.715,000] main : STACK: unused 0 usage 1024 / 1024 (100 %); CPU: 0 % 10 | [00:00:08.735,000] ISR0 : STACK: unused 1308 usage 740 / 2048 (36 %) 11 | -------------------------------------------------------------------------------- /samples/chassis/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # SPDX-License-Identifier: Apache-2.0 2 | 3 | cmake_minimum_required(VERSION 3.20.0) 4 | set(CMAKE_CXX_STANDARD 17) 5 | find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) 6 | project(motor) 7 | 8 | file(GLOB app_sources src/*.c) 9 | 10 | include_directories(${ZEPHYR_BASE}/include) 11 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/include) 12 | include_directories(${CMAKE_CURRENT_SOURCE_DIR}/dts) 13 | target_sources(app PRIVATE ${app_sources}) 14 | target_include_directories(app PRIVATE ${ZEPHYR_BASE}/include) 15 | target_include_directories(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include) 16 | target_include_directories(app PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/dts) 17 | -------------------------------------------------------------------------------- /samples/chassis/README.rst: -------------------------------------------------------------------------------- 1 | .. zephyr:code-sample:: can-babbling 2 | :name: Controller Area Network (CAN) babbling node 3 | :relevant-api: can_interface 4 | 5 | Simulate a babbling CAN node. 6 | 7 | Overview 8 | ******** 9 | 10 | In a Controller Area Network a babbling node is a node continuously (and usually erroneously) 11 | transmitting CAN frames with identical - often high - priority. This constant babbling blocks CAN 12 | bus access for any CAN frame with lower priority as these frames will loose the bus arbitration. 13 | 14 | This sample application simulates a babbling CAN node. The properties of the CAN frames sent are 15 | configurable via :ref:`Kconfig `. The frames carry no data as only the arbitration part of 16 | the CAN frame is of interest. 17 | 18 | Being able to simulate a babbling CAN node is useful when examining the behavior of other nodes on 19 | the same CAN bus when they constantly loose bus arbitration. 20 | 21 | The source code for this sample application can be found at: 22 | :zephyr_file:`samples/drivers/can/babbling`. 23 | 24 | Requirements 25 | ************ 26 | 27 | This sample requires a board with a CAN controller. The CAN controller must be configured using the 28 | ``zephyr,canbus`` :ref:`devicetree ` chosen node property. 29 | 30 | The sample supports an optional button for stopping the babbling. If present, the button must be 31 | configured using the ``sw0`` :ref:`devicetree ` alias, usually in the :ref:`BOARD.dts file 32 | `. 33 | 34 | Building and Running 35 | ******************** 36 | 37 | Example building for :ref:`twr_ke18f`: 38 | 39 | .. zephyr-app-commands:: 40 | :zephyr-app: samples/drivers/can/babbling 41 | :board: twr_ke18f 42 | :goals: build flash 43 | 44 | Sample output 45 | ============= 46 | 47 | .. code-block:: console 48 | 49 | *** Booting Zephyr OS build zephyr-v3.1.0-4606-g8c1efa8b96bb *** 50 | babbling on can@40024000 with standard (11-bit) CAN ID 0x010, RTR 0, CAN FD 0 51 | abort by pressing User SW3 button 52 | -------------------------------------------------------------------------------- /samples/chassis/prj.conf: -------------------------------------------------------------------------------- 1 | 2 | # 启用线程分析器 3 | CONFIG_THREAD_ANALYZER=y 4 | CONFIG_THREAD_ANALYZER_USE_LOG=y 5 | CONFIG_THREAD_ANALYZER_USE_PRINTK=n 6 | CONFIG_THREAD_NAME=y 7 | # CONFIG_THREAD_ANALYZER_AUTO=y 8 | # CONFIG_THREAD_ANALYZER_AUTO_INTERVAL=5 9 | CONFIG_THREAD_ANALYZER_LOG_LEVEL_INF=y 10 | CONFIG_THREAD_ANALYZER_AUTO_SEPARATE_CORES=n 11 | 12 | # 启用 Shell 支持 13 | CONFIG_SHELL=n 14 | CONFIG_SHELL_BACKENDS=n 15 | CONFIG_SHELL_BACKEND_SERIAL=n 16 | 17 | # 启用 UART 控制台 18 | CONFIG_UART_CONSOLE=y 19 | 20 | # 禁用 SEGGER RTT 21 | CONFIG_USE_SEGGER_RTT=n 22 | 23 | # 启用事件支持 24 | CONFIG_EVENTS=y 25 | 26 | # 启用控制台支持 27 | CONFIG_CONSOLE=y 28 | 29 | # 启用日志系统 30 | CONFIG_LOG=y 31 | CONFIG_LOG_MODE_IMMEDIATE=y 32 | CONFIG_LOG_PRINTK=y 33 | CONFIG_LOG_BACKEND_UART=y 34 | # CONFIG_LOG_MODE_IMMEDIATE=y 35 | 36 | # 启用串行支持 37 | CONFIG_SERIAL=y 38 | 39 | # 禁用优化 40 | CONFIG_NO_OPTIMIZATIONS=n 41 | 42 | # 启用驱动 43 | CONFIG_MOTOR=y 44 | CONFIG_MOTOR_DJI=y 45 | CONFIG_MOTOR_LOG_LEVEL=4 46 | CONFIG_CAN=y 47 | CONFIG_MOTOR_INIT_PRIORITY=90 48 | 49 | CONFIG_ARES=y 50 | CONFIG_ARES_SBUS=y 51 | CONFIG_EKF_LIB=y 52 | CONFIG_VOFA_LIB=y 53 | 54 | CONFIG_AUTO_PROBE_GYRO_BIAS=y 55 | 56 | # use ASYNC uart API 57 | CONFIG_SERIAL=y 58 | CONFIG_UART_ASYNC_API=y 59 | CONFIG_UART_INTERRUPT_DRIVEN=y 60 | 61 | CONFIG_DMA=y 62 | CONFIG_DMA_STM32=y # STM32 DMA 驱动 63 | CONFIG_UART_STM32=y # STM32 UART 驱动 64 | # CONFIG_STM32_UART_DMA=y 65 | 66 | # CONFIG_HAS_STM32CUBE=y 67 | # CONFIG_USE_STM32_HAL_DMA=y 68 | CONFIG_SPI=y 69 | CONFIG_SPI_STM32=y 70 | 71 | CONFIG_CMSIS_DSP=y 72 | CONFIG_CMSIS_DSP_MATRIX=y 73 | CONFIG_CMSIS_DSP_CONTROLLER=y 74 | CONFIG_CMSIS_DSP_FASTMATH=y 75 | 76 | CONFIG_ZBUS=y 77 | CONFIG_ZBUS_LOG_LEVEL_OFF=y 78 | CONFIG_ZBUS_MSG_SUBSCRIBER=y 79 | CONFIG_ZBUS_RUNTIME_OBSERVERS=y 80 | 81 | CONFIG_CHASSIS=y 82 | -------------------------------------------------------------------------------- /samples/chassis/sample.yaml: -------------------------------------------------------------------------------- 1 | sample: 2 | name: CAN bus babbling node 3 | tests: 4 | sample.drivers.can.babbling: 5 | tags: can 6 | depends_on: can 7 | filter: dt_chosen_enabled("zephyr,canbus") 8 | integration_platforms: 9 | - native_sim 10 | harness: console 11 | harness_config: 12 | type: one_line 13 | regex: 14 | - "babbling on .*" 15 | -------------------------------------------------------------------------------- /samples/chassis/src/devices.h: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | /* Devicetree */ 5 | #define UART_NODE DT_ALIAS(vofa) 6 | const struct device *uart_dev = DEVICE_DT_GET(UART_NODE); 7 | 8 | #define ACCEL_NODE DT_NODELABEL(bmi08x_accel) 9 | const struct device *accel_dev = DEVICE_DT_GET(ACCEL_NODE); 10 | 11 | #define GYRO_NODE DT_NODELABEL(bmi08x_gyro) 12 | const struct device *gyro_dev = DEVICE_DT_GET(GYRO_NODE); 13 | 14 | #define WHEELMOTOR1_NODE DT_NODELABEL(motor_wheel0) 15 | #define WHEELMOTOR2_NODE DT_NODELABEL(motor_wheel1) 16 | #define WHEELMOTOR3_NODE DT_NODELABEL(motor_wheel2) 17 | 18 | #define STEERMOTOR1_NODE DT_NODELABEL(motor_steer0) 19 | #define STEERMOTOR2_NODE DT_NODELABEL(motor_steer1) 20 | #define STEERMOTOR3_NODE DT_NODELABEL(motor_steer2) 21 | 22 | #define CHASSIS_NODE DT_NODELABEL(chassis) 23 | 24 | const struct device *wheel_motor1 = DEVICE_DT_GET(WHEELMOTOR1_NODE); 25 | const struct device *wheel_motor2 = DEVICE_DT_GET(WHEELMOTOR2_NODE); 26 | const struct device *wheel_motor3 = DEVICE_DT_GET(WHEELMOTOR3_NODE); 27 | 28 | const struct device *steer_motor1 = DEVICE_DT_GET(STEERMOTOR1_NODE); 29 | const struct device *steer_motor2 = DEVICE_DT_GET(STEERMOTOR2_NODE); 30 | const struct device *steer_motor3 = DEVICE_DT_GET(STEERMOTOR3_NODE); 31 | 32 | const struct device *chassis = DEVICE_DT_GET(CHASSIS_NODE); 33 | 34 | #define SBUS_NODE DT_NODELABEL(sbus0) 35 | const struct device *sbus = DEVICE_DT_GET(SBUS_NODE); 36 | -------------------------------------------------------------------------------- /samples/chassis/src/main.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2024 ttwards <12411711@mail.sustech.edu.cn> 3 | * 4 | * SPDX-License-Identifier: Apache-2.0 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include "devices.h" 19 | #include 20 | 21 | LOG_MODULE_REGISTER(main, LOG_LEVEL_DBG); 22 | 23 | /* CAN Feedback to console*/ 24 | void console_feedback(void *arg1, void *arg2, void *arg3) 25 | { 26 | float angle = 0; 27 | int n = 0; 28 | while (1) { 29 | float X = sbus_get_percent(sbus, 3); 30 | float Y = sbus_get_percent(sbus, 1); 31 | 32 | chassis_set_speed(chassis, X * 7, Y * 7); 33 | chassis_set_gyro(chassis, -sbus_get_percent(sbus, 0) * 2.0f); 34 | 35 | k_msleep(20); 36 | } 37 | } 38 | K_THREAD_DEFINE(feedback_thread, 4096, console_feedback, NULL, NULL, NULL, 1, 0, 100); 39 | 40 | int pub_cnt = 0; 41 | 42 | int main(void) 43 | { 44 | 45 | k_sleep(K_MSEC(50)); 46 | chassis_set_enabled(chassis, false); 47 | IMU_Sensor_trig_init(accel_dev, gyro_dev); 48 | chassis_set_enabled(chassis, true); 49 | 50 | IMU_Sensor_set_update_cb(Sensor_update_cb); 51 | 52 | chassis_set_angle(chassis, 0); 53 | 54 | while (1) { 55 | k_sleep(K_MSEC(500)); 56 | } 57 | } 58 | -------------------------------------------------------------------------------- /samples/chassis/threads.inf: -------------------------------------------------------------------------------- 1 | [00:00:08.570,000] Thread analyze: 2 | [00:00:08.576,000] feedback : STACK: unused 400 usage 1648 / 2048 (80 %); CPU: 76 % 3 | [00:00:08.596,000] can_recv : STACK: unused 164 usage 604 / 768 (78 %); CPU: 1 % 4 | [00:00:08.616,000] can_send : STACK: unused 248 usage 776 / 1024 (75 %); CPU: 21 % 5 | [00:00:08.637,000] thread_analyzer : STACK: unused 2000 usage 48 / 2048 (2 %); CPU: 0 % 6 | [00:00:08.656,000] shell_uart : STACK: unused 1224 usage 824 / 2048 (40 %); CPU: 0 % 7 | [00:00:08.676,000] sysworkq : STACK: unused 452 usage 572 / 1024 (55 %); CPU: 0 % 8 | [00:00:08.696,000] idle : STACK: unused 272 usage 48 / 320 (15 %); CPU: 0 % 9 | [00:00:08.715,000] main : STACK: unused 0 usage 1024 / 1024 (100 %); CPU: 0 % 10 | [00:00:08.735,000] ISR0 : STACK: unused 1308 usage 740 / 2048 (36 %) 11 | -------------------------------------------------------------------------------- /samples/communication/ares_communication/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20.0) 2 | 3 | find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) 4 | project(usb_bulk_test) 5 | 6 | target_sources(app PRIVATE src/main.c) 7 | -------------------------------------------------------------------------------- /samples/communication/ares_communication/README.md: -------------------------------------------------------------------------------- 1 | # ARES 双接口通信示例 2 | 3 | 这个示例演示了如何同时使用 USB Bulk 和 UART 两种接口进行 ARES 协议通信。 4 | 5 | ## 功能特性 6 | 7 | - **双接口支持**: 同时支持 USB Bulk 和 UART 接口 8 | - **ARES 协议**: 使用 ARES dual protocol 进行数据传输 9 | - **交替发送**: 程序会交替通过 USB 和 UART 发送数据 10 | - **回调机制**: 支持函数调用回调和同步数据回调 11 | 12 | ## 硬件要求 13 | 14 | - 支持 USB 设备功能的开发板 15 | - USART6 接口用于 UART 通信 16 | - RoboMaster Board C (或兼容的开发板) 17 | 18 | ## 配置说明 19 | 20 | ### prj.conf 主要配置项： 21 | - `CONFIG_USB_BULK_INTERFACE=y`: 启用 USB Bulk 接口 22 | - `CONFIG_UART_INTERFACE=y`: 启用 UART 接口 23 | - `CONFIG_DUAL_PROPOSE_PROTOCOL=y`: 启用双向协议 24 | 25 | ## 构建和运行 26 | 27 | ```bash 28 | cd samples/ares_communication 29 | west build -b robomaster_board_c 30 | west flash 31 | ``` 32 | 33 | ## 工作原理 34 | 35 | 1. **初始化阶段**: 程序启动时会初始化 USB Bulk 和 UART 两个接口 36 | 2. **协议绑定**: 将双向协议分别绑定到两个接口上 37 | 3. **回调设置**: 设置函数调用回调和同步数据回调 38 | 4. **数据发送**: 主循环中交替通过两个接口发送数据 39 | 40 | ## 日志输出 41 | 42 | 程序运行时会输出以下信息： 43 | - 接口初始化状态 44 | - 数据发送确认 45 | - 回调函数执行情况 46 | 47 | ## 注意事项 48 | 49 | - 确保 USB 和 UART 接口都正确连接 50 | - 如果某个接口初始化失败，程序仍会继续运行，只使用成功初始化的接口 51 | 52 | ## 接线 53 | ### 串口通信 54 | 两块C板通过 UART1 进行通信的接线（TX/RX 交叉连接） 55 | 56 | ### USB通信 57 | 一块C板通过USB进行通信的接线 58 | 59 | ## 上位机测试 60 | 在sample_linux下有上位机的协议代码和一个example，可以自行编译或者使用预编译代码 61 | 编译：`g++ -std=c++11 -o example src/easy_example.cpp src/ares_protocol.cpp -lusb-1.0 -pthread` 62 | -------------------------------------------------------------------------------- /samples/communication/ares_communication/boards/robomaster_board_c.overlay: -------------------------------------------------------------------------------- 1 | / { 2 | 3 | }; 4 | &usart6 { 5 | }; 6 | -------------------------------------------------------------------------------- /samples/communication/ares_communication/prj.conf: -------------------------------------------------------------------------------- 1 | # ARES双接口示例配置 - USB Bulk + UART 2 | 3 | # 启用ARES接口和协议 4 | CONFIG_DUAL_PROPOSE_PROTOCOL=y 5 | CONFIG_USB_BULK_INTERFACE=y 6 | CONFIG_UART_INTERFACE=y 7 | 8 | # 线程优先级配置 9 | CONFIG_MAIN_THREAD_PRIORITY=10 10 | 11 | # 网络缓冲区支持 12 | CONFIG_NET_BUF=y 13 | 14 | # 日志级别配置 15 | CONFIG_UDC_DRIVER_LOG_LEVEL_INF=y 16 | CONFIG_USBD_LOG_LEVEL_INF=y 17 | CONFIG_LOG_DEFAULT_LEVEL=3 18 | 19 | # 禁用绘图器（如果不需要） 20 | CONFIG_PLOTTER=n 21 | -------------------------------------------------------------------------------- /samples/communication/ares_communication/sample_linux/example: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ttwards/Mambo/2d10ce57ba33fa7515a2ea1f93959790a8f57b85/samples/communication/ares_communication/sample_linux/example -------------------------------------------------------------------------------- /samples/communication/ares_communication/sample_linux/src/easy_example.cpp: -------------------------------------------------------------------------------- 1 | #include "ares_protocol.hpp" 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | // 简单的同步帧回调，收到数据直接打印 8 | void sync_handler(uint16_t data_id, const uint8_t *data, size_t len) 9 | { 10 | printf("[同步帧] DataID=%u, 长度=%zu, 内容=", data_id, len); 11 | for (size_t i = 0; i < len; ++i) { 12 | printf("%02X ", data[i]); 13 | } 14 | printf("\n"); 15 | } 16 | 17 | // 简单的执行帧回调，收到数据直接打印 18 | uint32_t exec_handler(uint16_t func_id, uint32_t arg1, uint32_t arg2, uint32_t arg3, 19 | uint8_t request_id) 20 | { 21 | printf("[执行帧] FuncID=%u, Arg1=%u, Arg2=%u, Arg3=%u, ReqID=%u\n", func_id, arg1, arg2, 22 | arg3, request_id); 23 | // 返回一个简单的结果 24 | return arg1 + arg2 + arg3; 25 | } 26 | 27 | int main() 28 | { 29 | ares::Protocol proto; 30 | 31 | // 注册回调 32 | proto.register_sync_callback(sync_handler); 33 | proto.register_exec_callback(exec_handler); 34 | 35 | printf("正在连接设备...\n"); 36 | if (!proto.connect()) { 37 | printf("连接失败！\n"); 38 | return 1; 39 | } 40 | printf("设备已连接。\n"); 41 | 42 | while (true) { 43 | printf("\n请选择操作：\n"); 44 | printf("1. 发送同步帧(sync)\n"); 45 | printf("2. 发送执行帧(exec)\n"); 46 | printf("0. 退出\n"); 47 | printf("输入选项: "); 48 | int op = 0; 49 | if (!(std::cin >> op)) { 50 | std::cin.clear(); 51 | std::cin.ignore(1024, '\n'); 52 | continue; 53 | } 54 | if (op == 0) 55 | break; 56 | if (op == 1) { 57 | // 发送同步帧 58 | uint16_t data_id; 59 | size_t len; 60 | printf("请输入DataID (uint16): "); 61 | std::cin >> data_id; 62 | printf("请输入数据长度 (1~64): "); 63 | std::cin >> len; 64 | if (len == 0 || len > 64) { 65 | printf("长度非法！\n"); 66 | continue; 67 | } 68 | std::vector buf(len); 69 | printf("请输入数据内容（以空格分隔，每个字节十进制或十六进制0x开头）：\n"); 70 | for (size_t i = 0; i < len; ++i) { 71 | unsigned int val; 72 | std::cin >> std::hex >> val; 73 | buf[i] = static_cast(val); 74 | } 75 | if (proto.send_sync(data_id, buf.data(), len)) { 76 | printf("同步帧已发送。\n"); 77 | } else { 78 | printf("发送失败！\n"); 79 | } 80 | } else if (op == 2) { 81 | // 发送执行帧 82 | uint16_t func_id; 83 | uint32_t arg1, arg2, arg3; 84 | uint8_t req_id; 85 | printf("请输入FuncID (uint16): "); 86 | std::cin >> func_id; 87 | printf("请输入Arg1 (uint32): "); 88 | std::cin >> arg1; 89 | printf("请输入Arg2 (uint32): "); 90 | std::cin >> arg2; 91 | printf("请输入Arg3 (uint32): "); 92 | std::cin >> arg3; 93 | printf("请输入RequestID (uint8): "); 94 | unsigned int tmp; 95 | std::cin >> tmp; 96 | req_id = static_cast(tmp); 97 | if (proto.send_exec(func_id, arg1, arg2, arg3, req_id)) { 98 | printf("执行帧已发送。\n"); 99 | } else { 100 | printf("发送失败！\n"); 101 | } 102 | } else { 103 | printf("无效选项。\n"); 104 | } 105 | // 清理输入缓冲区 106 | std::cin.ignore(1024, '\n'); 107 | } 108 | 109 | printf("正在断开连接...\n"); 110 | proto.disconnect(); 111 | printf("程序结束。\n"); 112 | return 0; 113 | } 114 | -------------------------------------------------------------------------------- /samples/communication/ares_communication/src/main.c: -------------------------------------------------------------------------------- 1 | // main.c 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | LOG_MODULE_REGISTER(main_app, LOG_LEVEL_INF); 13 | 14 | #define UART_DEV DT_NODELABEL(usart6) 15 | static const struct device *uart_dev = DEVICE_DT_GET(UART_DEV); 16 | 17 | DUAL_PROPOSE_PROTOCOL_DEFINE(dual_protocol); 18 | DUAL_PROPOSE_PROTOCOL_DEFINE(uart_protocol); 19 | // 建议对UART使用CRC校验 20 | // DUAL_PROPOSE_PROTOCOL_DEFINE_CRC(uart_protocol); 21 | ARES_BULK_INTERFACE_DEFINE(usb_bulk_interface); 22 | ARES_UART_INTERFACE_DEFINE(uart_interface); 23 | 24 | int cnt = 0; 25 | int func_cb(uint32_t param1, uint32_t param2, uint32_t param3) 26 | { 27 | cnt++; 28 | LOG_INF("func_cb"); 29 | LOG_INF("params: %2x,%2x,%2x", param1, param2, param3); 30 | return cnt; 31 | } 32 | 33 | void sync_cb(uint32_t status) 34 | { 35 | LOG_INF("sync_cb: %d", status); 36 | } 37 | 38 | void func_ret_cb(uint16_t id, uint16_t req_id, uint32_t ret) 39 | { 40 | LOG_INF("func_ret_cb: ID %x, req_id %x, ret %x", id, req_id, ret); 41 | } 42 | 43 | uint8_t test[59] = {0}; 44 | int main(void) 45 | { 46 | LOG_INF("ARES USB Bulk & UART device is ready."); 47 | 48 | // Initialize UART interface 49 | ares_uart_init_dev(&uart_interface, uart_dev); 50 | int err_uart = ares_bind_interface(&uart_interface, &uart_protocol); 51 | if (err_uart) { 52 | LOG_ERR("Failed to initialize ARES UART device (%d)", err_uart); 53 | } else { 54 | LOG_INF("ARES UART interface initialized successfully"); 55 | } 56 | 57 | // Initialize USB bulk interface 58 | int err_usb = ares_bind_interface(&usb_bulk_interface, &dual_protocol); 59 | if (err_usb) { 60 | LOG_ERR("Failed to initialize ARES USB device (%d)", err_usb); 61 | } else { 62 | LOG_INF("ARES USB Bulk interface initialized successfully"); 63 | } 64 | 65 | // Setup callbacks and functions for both protocols 66 | dual_ret_cb_set(&dual_protocol, (dual_func_ret_cb_t)func_ret_cb); 67 | dual_func_add(&dual_protocol, 0x1, (dual_trans_func_t)func_cb); 68 | sync_table_t *usb_pack = 69 | dual_sync_add(&dual_protocol, 0x1, test, sizeof(test), (dual_trans_cb_t)sync_cb); 70 | 71 | dual_ret_cb_set(&uart_protocol, (dual_func_ret_cb_t)func_ret_cb); 72 | dual_func_add(&uart_protocol, 0x1, (dual_trans_func_t)func_cb); 73 | sync_table_t *uart_pack = 74 | dual_sync_add(&uart_protocol, 0x1, test, sizeof(test), (dual_trans_cb_t)sync_cb); 75 | 76 | LOG_INF("ARES Dual Interface (USB Bulk + UART) device is ready."); 77 | 78 | // The main thread is now free to do other things. 79 | // All USB and UART data processing happens in the background. 80 | int cnt = 0; 81 | while (1) { 82 | k_sleep(K_MSEC(1000)); 83 | 84 | // Send data through both interfaces alternately 85 | if (cnt % 2 == 0) { 86 | // Send through USB 87 | if (err_usb == 0) { 88 | dual_func_call(&dual_protocol, 0x1, 0x1, 0x2, cnt); 89 | dual_sync_flush(&dual_protocol, usb_pack); 90 | LOG_INF("Data sent via USB: %d", cnt); 91 | } 92 | } else { 93 | // Send through UART 94 | if (err_uart == 0) { 95 | dual_func_call(&uart_protocol, 0x1, 0x1, 0x2, cnt); 96 | dual_sync_flush(&uart_protocol, uart_pack); 97 | LOG_INF("Data sent via UART: %d", cnt); 98 | } 99 | } 100 | cnt++; 101 | } 102 | 103 | return 0; 104 | } 105 | -------------------------------------------------------------------------------- /samples/communication/plotter_auto/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20.0) 2 | 3 | find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) 4 | project(plotter_demo) 5 | 6 | target_sources(app PRIVATE src/main.c) 7 | -------------------------------------------------------------------------------- /samples/communication/plotter_auto/README.md: -------------------------------------------------------------------------------- 1 | # AresPlot 协议示例 2 | 3 | 这个示例展示了如何使用重构后的 AresPlot 协议与 UART 接口配合工作。 4 | 5 | ## 功能特性 6 | 7 | - **实时数据监控**: 监控多个变量（正弦波、计数器、随机值、布尔标志） 8 | - **UART 通信**: 通过 UART 接口发送数据到上位机 9 | - **自动数据发送**: 定时器驱动的数据采样和发送 10 | 11 | ## 硬件要求 12 | 13 | - 支持的开发板（如 robomaster_board_c） 14 | - UART 连接到上位机 15 | 16 | ## 构建和运行 17 | 18 | ```bash 19 | # 构建项目 20 | west build -b robomaster_board_c samples/plotter_demo 21 | 22 | # 烧录到开发板 23 | west flash 24 | ``` 25 | 26 | ## 使用方法 27 | 28 | 1. 将开发板通过 UART 连接到上位机 29 | 2. 上位机浏览器打开`https://captainkaz.github.io/web-serial-plotter/` (必须使用chromium内核) 30 | 3. 选择串口，设置为921600，其余不用修改，然后选择协议为AresPlot 31 | 3. 开发板会自动开始发送监控数据 32 | 4. 可以将`build/zephyr/zephyr.elf`拖入工具，输入变量名查找变量并重新开始采集，这一操作覆盖之前的正在监视的变量 33 | 34 | ## 监控的变量 35 | 36 | - `sine_wave`: 正弦波值 (float) 37 | - `counter`: 递增计数器 (int32_t) 38 | - `random_value`: 随机值 (int32_t) 39 | - `toggle_flag`: 布尔标志 (bool) 40 | 41 | ## 协议配置 42 | 43 | 此demo中不需要手动初始化协议，可以自动初始化 44 | 45 | ## 代码结构 46 | 47 | - `src/main.c`: 主应用程序 48 | - `prj.conf`: 项目配置 49 | - `boards/robomaster_board_c.overlay`: 设备树覆盖文件 50 | 51 | ## 与原版本的差异 52 | 53 | 1. **框架集成**: 现在使用 Ares 协议框架 54 | 2. **接口抽象**: 支持任何 Ares 接口（UART、USB 等） 55 | 3. **更好的错误处理**: 标准化的错误码和处理 56 | 4. **线程安全**: 使用互斥锁保护共享数据 57 | 5. **定时器驱动**: 使用 Zephyr 内核定时器 58 | -------------------------------------------------------------------------------- /samples/communication/plotter_auto/prj.conf: -------------------------------------------------------------------------------- 1 | # ARES Plotter Protocol 示例配置 2 | 3 | # 开启自动初始化 4 | CONFIG_PLOTTER=y 5 | 6 | CONFIG_ARESPLOT_FREQ=1000 7 | -------------------------------------------------------------------------------- /samples/communication/plotter_auto/src/main.c: -------------------------------------------------------------------------------- 1 | /* 2 | * ARES Plotter Protocol Example 3 | * Demonstrates how to use the new plotter protocol and interface for variable monitoring 4 | * 5 | * Copyright (c) 2024 ttwards 6 | * SPDX-License-Identifier: Apache-2.0 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include "ares/protocol/plotter/aresplot_protocol.h" 15 | 16 | LOG_MODULE_REGISTER(plotter_demo, LOG_LEVEL_INF); 17 | 18 | #ifndef PI 19 | #define PI 3.14159265f 20 | #endif 21 | 22 | // Variables to monitor 23 | static float sine_wave = 0.0f; 24 | static float cosine_wave = 0.0f; 25 | static int32_t counter = 0; 26 | static uint16_t random_value = 0; 27 | static bool toggle_flag = false; 28 | 29 | /** 30 | * @brief Variable update thread 31 | */ 32 | void variables_update_thread(void *arg1, void *arg2, void *arg3) 33 | { 34 | ARG_UNUSED(arg1); 35 | ARG_UNUSED(arg2); 36 | ARG_UNUSED(arg3); 37 | 38 | uint32_t time_ms = 0; 39 | 40 | while (1) { 41 | k_msleep(1); // 50Hz update rate 42 | time_ms += 1; 43 | 44 | // Update various types of variables 45 | float time_sec = time_ms / 1000.0f; 46 | 47 | // Sine and cosine wave (1Hz) 48 | sine_wave = sinf(2.0f * PI * time_sec); 49 | cosine_wave = cosf(2.0f * PI * time_sec); 50 | 51 | // Counter 52 | counter++; 53 | 54 | // Simulate random value 55 | random_value = (uint16_t)(sine_wave * 1000 + 1000); 56 | 57 | // Toggle boolean value (toggle every 2 seconds) 58 | if ((time_ms % 2000) < 50) { 59 | toggle_flag = !toggle_flag; 60 | } 61 | } 62 | } 63 | 64 | K_THREAD_DEFINE(variables_update, 1024, variables_update_thread, NULL, NULL, NULL, -1, 0, 0); 65 | 66 | /** 67 | * @brief Main function 68 | */ 69 | int main(void) 70 | { 71 | LOG_INF("=== ARES Plotter Protocol Example ==="); 72 | LOG_INF("Demonstrating variable monitoring feature"); 73 | 74 | LOG_INF("Start monitoring variables"); 75 | LOG_INF("Connect serial tool to view data transmission"); 76 | LOG_INF("Baudrate: 921600, Data format: 8N1"); 77 | 78 | // Main loop 79 | uint32_t last_status_time = 0; 80 | while (1) { 81 | k_msleep(1000); 82 | 83 | uint32_t now = k_uptime_get_32(); 84 | if (now - last_status_time >= 5000) { 85 | LOG_INF("Status update - Sine: %.2f, Counter: %d, Random: %d, Flag: %s", 86 | sine_wave, counter, random_value, toggle_flag ? "true" : "false"); 87 | last_status_time = now; 88 | } 89 | } 90 | 91 | return 0; 92 | } 93 | -------------------------------------------------------------------------------- /samples/communication/plotter_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20.0) 2 | 3 | find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) 4 | project(plotter_demo) 5 | 6 | target_sources(app PRIVATE src/main.c) 7 | -------------------------------------------------------------------------------- /samples/communication/plotter_demo/README.md: -------------------------------------------------------------------------------- 1 | # AresPlot 协议示例 2 | 3 | 这个示例展示了如何使用重构后的 AresPlot 协议与 UART 接口配合工作。 4 | 5 | ## 功能特性 6 | 7 | - **实时数据监控**: 监控多个变量（正弦波、计数器、随机值、布尔标志） 8 | - **UART 通信**: 通过 UART 接口发送数据到上位机 9 | - **自动数据发送**: 定时器驱动的数据采样和发送 10 | 11 | ## 硬件要求 12 | 13 | - 支持的开发板（如 robomaster_board_c） 14 | - UART 连接到上位机 15 | 16 | ## 构建和运行 17 | 18 | ```bash 19 | # 构建项目 20 | west build -b robomaster_board_c samples/plotter_demo 21 | 22 | # 烧录到开发板 23 | west flash 24 | ``` 25 | 26 | ## 使用方法 27 | 28 | 1. 将开发板通过 UART 连接到上位机 29 | 2. 上位机浏览器打开`https://captainkaz.github.io/web-serial-plotter/` (必须使用chromium内核) 30 | 3. 选择串口，设置为921600，其余不用修改，然后选择协议为AresPlot 31 | 3. 开发板会自动开始发送监控数据 32 | 4. 可以将`build/zephyr/zephyr.elf`拖入工具，输入变量名查找变量并重新开始采集，这一操作覆盖之前的正在监视的变量 33 | 34 | ## 监控的变量 35 | 36 | - `sine_wave`: 正弦波值 (float) 37 | - `counter`: 递增计数器 (int32_t) 38 | - `random_value`: 随机值 (int32_t) 39 | - `toggle_flag`: 布尔标志 (bool) 40 | 41 | ## 协议配置 42 | 43 | 协议配置可以通过 Kconfig 选项调整： 44 | 45 | - `CONFIG_ARESPLOT_MAX_VARS_TO_MONITOR`: 最大监控变量数量 46 | - `CONFIG_ARESPLOT_SHARED_BUFFER_SIZE`: 缓冲区大小 47 | - `CONFIG_ARESPLOT_DEFAULT_SAMPLE_PERIOD_MS`: 默认采样周期 48 | 49 | ## 代码结构 50 | 51 | - `src/main.c`: 主应用程序 52 | - `prj.conf`: 项目配置 53 | - `boards/robomaster_board_c.overlay`: 设备树覆盖文件 54 | 55 | ## 与原版本的差异 56 | 57 | 1. **框架集成**: 现在使用 Ares 协议框架 58 | 2. **接口抽象**: 支持任何 Ares 接口（UART、USB 等） 59 | 3. **更好的错误处理**: 标准化的错误码和处理 60 | 4. **线程安全**: 使用互斥锁保护共享数据 61 | 5. **定时器驱动**: 使用 Zephyr 内核定时器 62 | -------------------------------------------------------------------------------- /samples/communication/plotter_demo/prj.conf: -------------------------------------------------------------------------------- 1 | # ARES Plotter Protocol 示例配置 2 | 3 | # ARES通信库 4 | CONFIG_ARES_COMM_LIB=y 5 | 6 | # 接口支持 7 | CONFIG_UART_INTERFACE=y 8 | 9 | # 协议支持 10 | CONFIG_ARES_PLOTTER_PROTOCOL=y 11 | CONFIG_ARESPLOT_FREQ=1000 12 | -------------------------------------------------------------------------------- /samples/motor/dji_m3508_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20.0) 2 | 3 | find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) 4 | project(dji_m3508_demo) 5 | 6 | target_sources(app PRIVATE src/main.c) 7 | -------------------------------------------------------------------------------- /samples/motor/dji_m3508_demo/README.md: -------------------------------------------------------------------------------- 1 | # DJI M3508电机控制示例 - 简化版 2 | 3 | 这个示例演示了DJI M3508电机的基本控制功能，通过自动循环执行简单的测试序列。 4 | 5 | ## 功能特性 6 | 7 | - **自动循环测试**: 无需人工干预，自动执行测试序列 8 | - **双环控制**: 实现角度环->速度环的串联控制 9 | - **实时监控**: 监控电机状态包括温度、电流、在线状态 10 | - **简单可靠**: 去除复杂交互，专注于电机控制演示 11 | - **安全保护**: 包含温度、电流过载保护 12 | 13 | ## 测试序列 14 | 15 | 程序会自动循环执行以下测试序列： 16 | 17 | 1. **设置零点1**: 将当前位置设为零点 18 | 2. **速度测试**: 以100 RPM恒定速度转动10秒 19 | 3. **停止**: 停止电机运行2秒 20 | 4. **设置零点2**: 重新将当前位置设为零点 21 | 5. **角度测试**: 正转180度 22 | 6. **循环**: 回到步骤1继续循环 23 | 24 | ## 硬件要求 25 | 26 | ### 必需硬件 27 | - RoboMaster Board C (或兼容的STM32F4开发板) 28 | - DJI M3508电机 + C620电调 29 | - CAN收发器模块 30 | - 24V电源供应 31 | 32 | ### 连接说明 33 | ``` 34 | 开发板 电调/电机 35 | CAN1_RX (PD0) -> CAN_H (通过收发器) 36 | CAN1_TX (PD1) -> CAN_L (通过收发器) 37 | GND -> GND 38 | ``` 39 | 40 | ### CAN总线配置 41 | - 波特率: 1Mbps 42 | - 电机ID: 1 (0x201) 43 | - 控制帧ID: 0x200 44 | 45 | ## PID参数配置 46 | 47 | ### 速度环PID 48 | - Kp: 8.00 49 | - Ki: 0.50 50 | - Kd: 0.10 51 | - 输出限制: ±16384 (电流值) 52 | - 积分限制: ±5000 53 | 54 | ### 角度环PID 55 | - Kp: 12.00 56 | - Ki: 0.00 (纯PD控制) 57 | - Kd: 2.00 58 | - 输出限制: ±8000 (速度目标值) 59 | 60 | ## 构建和运行 61 | 62 | ```bash 63 | # 进入示例目录 64 | cd samples/dji_m3508_demo 65 | 66 | # 构建项目 67 | west build -b robomaster_board_c 68 | 69 | # 烧录程序 70 | west flash 71 | ``` 72 | 73 | ## 日志输出示例 74 | 75 | ``` 76 | [00:00:00.100,000] dji_m3508_demo: === DJI M3508电机控制示例 - 简化版 === 77 | [00:00:00.150,000] dji_m3508_demo: 测试序列：设置零点 -> 100rpm转10s -> 停下 -> 设置零点 -> 正转180度 -> 循环 78 | [00:00:00.200,000] dji_m3508_demo: 电机已启用 79 | [00:00:00.250,000] dji_m3508_demo: === 步骤1: 设置零点1 === 80 | [00:00:00.800,000] dji_m3508_demo: === 步骤2: 开始速度测试 (100 RPM, 10秒) === 81 | [00:00:01.850,000] dji_m3508_demo: 速度测试 - 目标: 100.0 RPM, 实际: 98.5 RPM, 剩余: 9秒 82 | [00:00:02.850,000] dji_m3508_demo: 速度测试 - 目标: 100.0 RPM, 实际: 99.8 RPM, 剩余: 8秒 83 | ... 84 | [00:00:10.800,000] dji_m3508_demo: === 步骤3: 停止电机 === 85 | [00:00:12.800,000] dji_m3508_demo: === 步骤4: 设置零点2 === 86 | [00:00:13.300,000] dji_m3508_demo: === 步骤5: 开始角度测试 (正转180度) === 87 | [00:00:13.800,000] dji_m3508_demo: 角度测试 - 目标: 180.0°, 实际: 15.2°, 速度: 245.6 RPM 88 | [00:00:14.300,000] dji_m3508_demo: 角度测试 - 目标: 180.0°, 实际: 45.8°, 速度: 198.3 RPM 89 | ... 90 | [00:00:18.500,000] dji_m3508_demo: 角度测试完成！实际角度: 179.2° 91 | [00:00:19.500,000] dji_m3508_demo: === 循环完成，重新开始 === 92 | ``` 93 | 94 | ## 测试预期结果 95 | 96 | ### 速度控制测试 97 | - 电机应在约1-2秒内达到目标速度100 RPM 98 | - 速度误差应控制在±5 RPM以内 99 | - 转动应平稳无明显振动 100 | 101 | ### 角度控制测试 102 | - 电机应在5-8秒内转动到180度位置 103 | - 角度误差应控制在±5度以内 104 | - 到达目标位置后应稳定保持 105 | 106 | ## 安全注意事项 107 | 108 | ⚠️ **重要提醒**: 109 | - 确保电机安装牢固，避免高速旋转时脱落 110 | - 检查CAN总线连接是否正确 111 | - 确保24V电源供应充足 112 | - 首次运行时建议降低PID参数进行测试 113 | - 出现异常时立即断电 114 | 115 | ## 故障排除 116 | 117 | ### 电机不响应 118 | 1. 检查CAN总线连接和波特率设置 119 | 2. 确认电机ID配置正确 (应为1，CAN ID 0x201) 120 | 3. 检查24V电源供应是否充足 121 | 4. 查看日志中的错误信息 122 | 123 | ### 速度控制不稳定 124 | 1. 适当增加速度环的Kp值 125 | 2. 检查机械负载是否过大 126 | 3. 确认电机温度是否正常 127 | 4. 检查电源电压是否稳定 128 | 129 | ### 角度控制超调或振荡 130 | 1. 适当减少角度环的Kp值 131 | 2. 增加角度环的Kd值以提高阻尼 132 | 3. 检查机械间隙和刚性 133 | 134 | ### 程序运行异常 135 | 1. 检查CAN总线是否正常工作 136 | 2. 确认电机驱动程序是否正确加载 137 | 3. 查看系统日志中的错误信息 138 | 4. 重启系统重新初始化 139 | 140 | ## 扩展功能 141 | 142 | 可以基于此示例扩展的功能： 143 | - 调整测试参数（速度、角度、时间） 144 | - 添加更多测试步骤 145 | - 记录和分析性能数据 146 | - 多电机协调控制 147 | - 远程控制接口 148 | 149 | ## 参考资料 150 | 151 | - [DJI M3508电机手册](https://www.dji.com/cn/robomaster-s1/info#specs) 152 | - [Zephyr电机驱动API](https://docs.zephyrproject.org/latest/hardware/peripherals/motor.html) 153 | - [CAN总线配置指南](https://docs.zephyrproject.org/latest/connectivity/networking/api/can.html) 154 | -------------------------------------------------------------------------------- /samples/motor/dji_m3508_demo/app.overlay: -------------------------------------------------------------------------------- 1 | // DJI M3508电机示例 - RoboMaster Board C设备树配置 2 | 3 | / { 4 | speed_pid_m3508: speed_pid_m3508 { 5 | compatible = "pid,single"; 6 | #controller-cells = <0>; 7 | k_p = "8"; 8 | k_i = "0.5"; 9 | k_d = "0.1"; 10 | out_max = "2"; 11 | }; 12 | 13 | angle_pid_m3508: angle_pid_m3508 { 14 | compatible = "pid,single"; 15 | #controller-cells = <0>; 16 | k_p = "10"; 17 | k_i = "0"; 18 | k_d = "2"; 19 | out_max = "50"; 20 | }; 21 | 22 | m3508_motor: m3508_motor { 23 | compatible = "dji,motor"; 24 | is_m3508; 25 | id = <1>; 26 | rx_id = <0x201>; 27 | tx_id = <0x200>; 28 | gear_ratio = "19.20"; 29 | status = "okay"; 30 | can_channel = <&can1>; 31 | // 应当保持外环在前，内环在后 32 | controllers = <&angle_pid_m3508 &speed_pid_m3508>; 33 | capabilities = "angle", "speed"; 34 | }; 35 | }; 36 | -------------------------------------------------------------------------------- /samples/motor/dji_m3508_demo/prj.conf: -------------------------------------------------------------------------------- 1 | # DJI M3508电机控制示例配置 - 简化版 2 | 3 | # 基础配置 4 | CONFIG_MAIN_THREAD_PRIORITY=7 5 | CONFIG_SYSTEM_WORKQUEUE_STACK_SIZE=2048 6 | 7 | # 电机驱动支持 8 | CONFIG_MOTOR=y 9 | CONFIG_MOTOR_DJI=y 10 | 11 | # CAN总线支持 12 | CONFIG_CAN=y 13 | CONFIG_CAN_INIT_PRIORITY=80 14 | 15 | # PID控制器支持 16 | CONFIG_PID=y 17 | -------------------------------------------------------------------------------- /samples/motor/dm_demo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.20.0) 2 | 3 | find_package(Zephyr REQUIRED HINTS $ENV{ZEPHYR_BASE}) 4 | project(dji_m3508_demo) 5 | 6 | target_sources(app PRIVATE src/main.c) 7 | -------------------------------------------------------------------------------- /samples/motor/dm_demo/README.md: -------------------------------------------------------------------------------- 1 | # 达妙DM电机控制示例 - 简化版 2 | 3 | 这个示例演示了达妙DM电机的基本速度控制功能，上电后直接设置电机转速为100 RPM。 4 | 5 | ## 硬件要求 6 | - RoboMaster Board C (或兼容的STM32F4开发板) 7 | - 达妙DM电机 (如DM4310, DM4340等) 8 | - CAN收发器模块 9 | - 24V电源供应 10 | 11 | ### CAN总线配置 12 | - 波特率: 1Mbps 13 | - 发送ID: 0x0A 14 | - 接收ID: 0x1A 15 | 16 | ## 电机参数配置 17 | 18 | ### 达妙DM电机参数 19 | - 最大速度: 785 rad/s (约7500 RPM) 20 | - 最大扭矩: 5 Nm 21 | - 控制频率: 1000 Hz 22 | - 控制模式: VO (速度-电流双环控制) 23 | 24 | ## 构建和运行 25 | 26 | ```bash 27 | # 进入示例目录 28 | cd samples/motor/dm_demo 29 | 30 | # 构建项目 31 | west build -b robomaster_board_c 32 | 33 | # 烧录程序 34 | west flash 35 | ``` 36 | 37 | ## 日志输出示例 38 | 39 | ``` 40 | [00:00:00.100,000] dm_motor_demo: === 达妙DM电机控制示例 - 简化版 === 41 | [00:00:00.150,000] dm_motor_demo: 上电直接设置转速100 RPM 42 | [00:00:00.200,000] dm_motor_demo: 电机已启用 43 | [00:00:01.250,000] dm_motor_demo: 电机转速已设置为100 RPM 44 | [00:00:02.300,000] dm_motor_demo: 电机状态 - 扭矩: 0.12 Nm, 速度: 98.5 RPM, 角度: 15.2° 45 | [00:00:03.300,000] dm_motor_demo: 电机状态 - 扭矩: 0.15 Nm, 速度: 99.8 RPM, 角度: 45.6° 46 | [00:00:04.300,000] dm_motor_demo: 电机状态 - 扭矩: 0.14 Nm, 速度: 100.1 RPM, 角度: 75.3° 47 | ... 48 | ``` 49 | -------------------------------------------------------------------------------- /samples/motor/dm_demo/app.overlay: -------------------------------------------------------------------------------- 1 | / { 2 | motor { 3 | dm_motor: dm_motor{ 4 | compatible = "dm,motor"; 5 | id = <1>; 6 | rx_id = <0x1A>; 7 | tx_id = <0x0A>; 8 | v_max = "785"; 9 | status = "okay"; 10 | can_channel = <&can1>; 11 | controllers = <&vo>; 12 | capabilities = "vo"; 13 | t_max = "5"; 14 | freq = <1000>; 15 | }; 16 | }; 17 | vo: vo{ 18 | compatible = "pid,single"; 19 | k_p = "0.00251"; 20 | }; 21 | }; 22 | -------------------------------------------------------------------------------- /samples/motor/dm_demo/prj.conf: -------------------------------------------------------------------------------- 1 | # 达妙DM电机控制示例配置 2 | 3 | # 基础配置 4 | CONFIG_MAIN_THREAD_PRIORITY=7 5 | CONFIG_SYSTEM_WORKQUEUE_STACK_SIZE=2048 6 | 7 | # 电机驱动支持 8 | CONFIG_MOTOR=y 9 | CONFIG_MOTOR_DM=y 10 | 11 | # CAN总线支持 12 | CONFIG_CAN=y 13 | CONFIG_CAN_INIT_PRIORITY=80 14 | 15 | # PID控制器支持 16 | CONFIG_PID=y 17 | -------------------------------------------------------------------------------- /samples/motor/dm_demo/src/main.c: -------------------------------------------------------------------------------- 1 | /* 2 | * 达妙DM电机控制示例 - 简化版 3 | * 上电直接设置转速100 RPM 4 | * 5 | * Copyright (c) 2024 ttwards 6 | * SPDX-License-Identifier: Apache-2.0 7 | */ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | LOG_MODULE_REGISTER(dm_motor_demo, LOG_LEVEL_INF); 15 | 16 | // 电机设备定义 17 | static const struct device *dm_motor = DEVICE_DT_GET(DT_PATH(motor, dm_motor)); 18 | 19 | /** 20 | * @brief 电机状态监控线程 21 | */ 22 | void motor_monitor_thread(void *arg1, void *arg2, void *arg3) 23 | { 24 | ARG_UNUSED(arg1); 25 | ARG_UNUSED(arg2); 26 | ARG_UNUSED(arg3); 27 | 28 | while (1) { 29 | k_msleep(1000); // 每秒显示一次状态 30 | 31 | motor_status_t status; 32 | int ret = motor_get(dm_motor, &status); 33 | 34 | if (ret != 0) { 35 | LOG_ERR("读取电机状态失败: %d", ret); 36 | continue; 37 | } 38 | 39 | // 显示电机状态信息 40 | LOG_INF("电机状态 - 扭矩: %.2f Nm, 速度: %.1f RPM, 角度: %.1f°", status.torque, 41 | status.rpm, status.angle); 42 | } 43 | } 44 | 45 | K_THREAD_DEFINE(motor_monitor, 1024, motor_monitor_thread, NULL, NULL, NULL, 5, 0, 0); 46 | 47 | /** 48 | * @brief 主函数 49 | */ 50 | int main(void) 51 | { 52 | LOG_INF("=== 达妙DM电机控制示例 - 简化版 ==="); 53 | LOG_INF("上电直接设置转速100 RPM"); 54 | 55 | // 检查电机设备是否就绪 56 | if (!device_is_ready(dm_motor)) { 57 | LOG_ERR("达妙DM电机设备未就绪"); 58 | return -ENODEV; 59 | } 60 | 61 | // 启用电机 62 | motor_control(dm_motor, ENABLE_MOTOR); 63 | LOG_INF("电机已启用"); 64 | 65 | // 等待电机初始化完成 66 | k_msleep(1000); 67 | 68 | // 设置电机转速为100 RPM 69 | motor_set_speed(dm_motor, 100.0f); 70 | LOG_INF("电机转速已设置为100 RPM"); 71 | 72 | // 主循环 - 保持程序运行 73 | while (1) { 74 | k_msleep(1000); 75 | } 76 | 77 | return 0; 78 | } 79 | -------------------------------------------------------------------------------- /scripts/openocd.cfg: -------------------------------------------------------------------------------- 1 | # increase more connection 2 | try { 3 | foreach tgt [target names] { 4 | set nConn [$tgt cget -gdb-max-connections] 5 | if { $nConn > 0 } { 6 | incr nConn 7 | $tgt configure -gdb-max-connections $nConn 8 | puts "[info script]: Info: Setting gdb-max-connections for target '$tgt' to $nConn" 9 | } 10 | } 11 | } on error {} { 12 | puts stderr "[info script]: Error: Failed to increase gdb-max-connections for current target. Live variables will not work" 13 | } 14 | -------------------------------------------------------------------------------- /setup-precommit.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | # SPDX-License-Identifier: Apache-2.0 3 | # Pre-commit 设置脚本 4 | 5 | set -e 6 | 7 | echo "====================================" 8 | echo "Pre-commit Hook 安装脚本" 9 | echo "====================================" 10 | echo "" 11 | 12 | # 检查 Python 是否安装 13 | if ! command -v python3 &> /dev/null; then 14 | echo "❌ 错误: 未找到 python3，请先安装 Python 3" 15 | exit 1 16 | fi 17 | 18 | echo "✓ Python 已安装: $(python3 --version)" 19 | 20 | # 检查 pip 是否安装 21 | if ! command -v pip3 &> /dev/null; then 22 | echo "❌ 错误: 未找到 pip3，请先安装 pip" 23 | exit 1 24 | fi 25 | 26 | echo "✓ pip 已安装" 27 | 28 | # 安装 pre-commit 29 | echo "" 30 | echo "正在安装 pre-commit..." 31 | pip3 install --user pre-commit 32 | 33 | # 验证安装 34 | if ! command -v pre-commit &> /dev/null; then 35 | echo "⚠️ 警告: pre-commit 未在 PATH 中，请将 ~/.local/bin 添加到 PATH" 36 | echo " 可以运行: export PATH=\"\$HOME/.local/bin:\$PATH\"" 37 | export PATH="$HOME/.local/bin:$PATH" 38 | fi 39 | 40 | echo "✓ pre-commit 已安装: $(pre-commit --version)" 41 | 42 | # 安装 git hooks 43 | echo "" 44 | echo "正在安装 git hooks..." 45 | pre-commit install 46 | 47 | echo "" 48 | echo "✓ Git hooks 已安装" 49 | 50 | # 可选：运行一次检查 51 | echo "" 52 | read -p "是否要对所有文件运行一次 pre-commit 检查？(y/N) " -n 1 -r 53 | echo 54 | if [[ $REPLY =~ ^[Yy]$ ]]; then 55 | echo "正在运行 pre-commit 检查..." 56 | pre-commit run --all-files || true 57 | fi 58 | 59 | echo "" 60 | echo "====================================" 61 | echo "✅ Pre-commit 设置完成！" 62 | echo "====================================" 63 | echo "" 64 | echo "使用说明：" 65 | echo " - 每次提交时会自动运行代码检查" 66 | echo " - 手动运行所有检查: pre-commit run --all-files" 67 | echo " - 跳过检查提交: git commit --no-verify" 68 | echo " - 更新 hooks: pre-commit autoupdate" 69 | echo "" 70 | -------------------------------------------------------------------------------- /structure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ttwards/Mambo/2d10ce57ba33fa7515a2ea1f93959790a8f57b85/structure.png -------------------------------------------------------------------------------- /submanifests/externlib.yaml: -------------------------------------------------------------------------------- 1 | manifest: 2 | # zephyr-keep-sorted-start re(^\s+\- name:) 3 | projects: 4 | - name: etl 5 | url: https://github.com/ETLCPP/etl.git 6 | revision: master 7 | path: breeze-sdk/modules/lib/etl 8 | # zephyr-keep-sorted-stop 9 | -------------------------------------------------------------------------------- /west.yml: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021 Nordic Semiconductor ASA 2 | # Copyright (c) 2025 ttwards @ SUSTech 3 | # SPDX-License-Identifier: Apache-2.0 4 | 5 | manifest: 6 | self: 7 | west-commands: scripts/west-commands.yml 8 | 9 | remotes: 10 | - name: zephyrproject-rtos 11 | url-base: https://mirrors.sustech.edu.cn/git/12411711 12 | 13 | projects: 14 | - name: zephyr 15 | remote: zephyrproject-rtos 16 | revision: main 17 | import: 18 | # By using name-allowlist we can clone only the modules that are 19 | # strictly needed by the application. 20 | name-allowlist: 21 | - cmsis # required by the ARM port 22 | - cmsis_6 23 | - cmsis-dsp 24 | - hal_st 25 | - hal_stm32 26 | - mcuboot 27 | - segger 28 | -------------------------------------------------------------------------------- /zephyr/module.yml: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2021 Nordic Semiconductor ASA 2 | # SPDX-License-Identifier: Apache-2.0 3 | 4 | name: "ARES framework" 5 | build: 6 | # Path to the Kconfig file that will be sourced into Zephyr Kconfig tree under 7 | # Zephyr > Modules > example-application. Path is relative from root of this 8 | # repository. 9 | kconfig: Kconfig 10 | # Path to the folder that contains the CMakeLists.txt file to be included by 11 | # Zephyr build system. The `.` is the root of this repository. 12 | cmake: . 13 | settings: 14 | # Additional roots for boards and DTS files. Zephyr will use the 15 | # `/boards` for additional boards. The `.` is the root of this 16 | # repository. 17 | board_root: . 18 | # Zephyr will use the `/dts` for additional dts files and 19 | # `/dts/bindings` for additional dts binding files. The `.` is 20 | # the root of this repository. 21 | dts_root: . 22 | # Additional roots for drivers. Zephyr will use the `/drivers` 23 | # for additional drivers. The `.` is the root of this repository. 24 | # drivers_root: . 25 | # Additional include directories for the Zephyr build system. The `.` is 26 | # the root of this repository. 27 | # include_root: . 28 | --------------------------------------------------------------------------------