From e3179c0495893c08d0376dd229e8c06f08557262 Mon Sep 17 00:00:00 2001 From: xxxxm <2389287465@qq.com> Date: Sat, 7 Mar 2026 20:31:52 +0800 Subject: [PATCH] batch 1: core project files (exclude drivers/middlewares/arm_model) --- .clangd | 11 + .gitignore | 5 + .mxproject | 42 + .settings/bundles-lock.store.json | 147 ++ .settings/bundles.store.json | 20 + .settings/ide.store.json | 6 + .vscode/c_cpp_properties.json | 8 + .vscode/settings.json | 15 + CMakeLists.txt | 122 ++ CMakePresets.json | 38 + Core/Inc/FreeRTOSConfig.h | 171 ++ Core/Inc/can.h | 55 + Core/Inc/dma.h | 52 + Core/Inc/gpio.h | 49 + Core/Inc/main.h | 69 + Core/Inc/retarget.h | 25 + Core/Inc/stm32f4xx_hal_conf.h | 495 +++++ Core/Inc/stm32f4xx_it.h | 72 + Core/Inc/tim.h | 54 + Core/Inc/usart.h | 52 + Core/Src/can.c | 230 ++ Core/Src/dma.c | 55 + Core/Src/freertos.c | 127 ++ Core/Src/gpio.c | 56 + Core/Src/main.c | 202 ++ Core/Src/retarget.c | 111 + Core/Src/stm32f4xx_hal_msp.c | 84 + Core/Src/stm32f4xx_it.c | 302 +++ Core/Src/syscalls.c | 244 +++ Core/Src/sysmem.c | 87 + Core/Src/system_stm32f4xx.c | 747 +++++++ Core/Src/tim.c | 162 ++ Core/Src/usart.c | 136 ++ ITM_TROUBLESHOOTING.md | 0 MDK-ARM/DebugConfig/arm_STM32F407IGHx.dbgconf | 48 + .../arm_STM32F407IGHx_1.0.1.dbgconf | 48 + MDK-ARM/RTE/_arm/RTE_Components.h | 14 + MDK-ARM/arm.uvguix.Lenovo | 1860 +++++++++++++++++ MDK-ARM/arm.uvguix.yxm23 | 1860 +++++++++++++++++ MDK-ARM/arm.uvoptx | 672 ++++++ MDK-ARM/arm.uvprojx | 710 +++++++ MDK-ARM/startup_stm32f407xx.s | 422 ++++ PRINTF_OUTPUT_GUIDE.md | 176 ++ STM32F407XX_FLASH.ld | 269 +++ User/bsp/bsp.h | 28 + User/bsp/bsp_config.yaml | 26 + User/bsp/can.c | 708 +++++++ User/bsp/can.h | 259 +++ User/bsp/mm.c | 30 + User/bsp/mm.h | 32 + User/bsp/pwm.c | 112 + User/bsp/pwm.h | 50 + User/bsp/time.c | 81 + User/bsp/time.h | 43 + User/bsp/uart.c | 155 ++ User/bsp/uart.h | 68 + User/component/ARM_USAGE.md | 184 ++ User/component/PowerControl.c | 88 + User/component/PowerControl.h | 29 + User/component/ahrs.c | 417 ++++ User/component/ahrs.h | 114 + User/component/arm_kinematics/arm6dof.cpp | 250 +++ User/component/arm_kinematics/arm6dof.h | 107 + User/component/component_config.yaml | 7 + User/component/filter.c | 185 ++ User/component/filter.h | 120 ++ User/component/mixer.c | 94 + User/component/mixer.h | 76 + User/component/pid.c | 158 ++ User/component/pid.h | 107 + User/component/toolbox/matrix.cpp | 24 + User/component/toolbox/matrix.h | 262 +++ User/component/toolbox/robotics.cpp | 342 +++ User/component/toolbox/robotics.h | 407 ++++ User/component/toolbox/utils.cpp | 56 + User/component/toolbox/utils.h | 27 + User/component/user_math.c | 134 ++ User/component/user_math.h | 183 ++ User/device/at9s.c | 121 ++ User/device/at9s.h | 70 + User/device/device.h | 49 + User/device/device_config.yaml | 6 + User/device/dr16.c | 221 ++ User/device/dr16.h | 119 ++ User/device/motor.c | 52 + User/device/motor.h | 68 + User/device/motor_dm.c | 522 +++++ User/device/motor_dm.h | 105 + User/device/motor_lz.c | 440 ++++ User/device/motor_lz.h | 212 ++ User/device/motor_rm.c | 321 +++ User/device/motor_rm.h | 132 ++ User/device/supercap.c | 158 ++ User/device/supercap.h | 102 + User/module/arm.cpp | 507 +++++ User/module/arm.h | 282 +++ User/module/arm_oop.hpp | 706 +++++++ User/module/chassis.c | 314 +++ User/module/chassis.h | 239 +++ User/module/cmd/cmd.c | 502 +++++ User/module/cmd/cmd.h | 227 ++ User/module/cmd/cmd_adapter.c | 224 ++ User/module/cmd/cmd_adapter.h | 121 ++ User/module/cmd/cmd_behavior.c | 214 ++ User/module/cmd/cmd_behavior.h | 69 + User/module/cmd/cmd_example.c | 167 ++ User/module/cmd/cmd_feature.h | 53 + User/module/cmd/cmd_types.h | 273 +++ User/module/config.c | 145 ++ User/module/config.h | 37 + User/module/joint.hpp | 159 ++ User/module/motor_base.hpp | 179 ++ User/task/arm_main.cpp | 289 +++ User/task/blink.c | 59 + User/task/cmd.cpp | 73 + User/task/config.yaml | 14 + User/task/ctrl_chassis.c | 63 + User/task/init.c | 55 + User/task/rc.c | 86 + User/task/user_task.c | 36 + User/task/user_task.h | 125 ++ arm.ioc | 221 ++ cmake/gcc-arm-none-eabi.cmake | 43 + cmake/starm-clang.cmake | 65 + cmake/stm32cubemx/CMakeLists.txt | 118 ++ ozone/arm.jdebug | 361 ++++ ozone/arm.jdebug.user | 37 + scripts/calc_new_dh.py | 127 ++ scripts/quick_calc.py | 32 + scripts/urdf_rc_to_dh.py | 127 ++ startup_stm32f407xx.s | 508 +++++ 131 files changed, 24648 insertions(+) create mode 100644 .clangd create mode 100644 .gitignore create mode 100644 .mxproject create mode 100644 .settings/bundles-lock.store.json create mode 100644 .settings/bundles.store.json create mode 100644 .settings/ide.store.json create mode 100644 .vscode/c_cpp_properties.json create mode 100644 .vscode/settings.json create mode 100644 CMakeLists.txt create mode 100644 CMakePresets.json create mode 100644 Core/Inc/FreeRTOSConfig.h create mode 100644 Core/Inc/can.h create mode 100644 Core/Inc/dma.h create mode 100644 Core/Inc/gpio.h create mode 100644 Core/Inc/main.h create mode 100644 Core/Inc/retarget.h create mode 100644 Core/Inc/stm32f4xx_hal_conf.h create mode 100644 Core/Inc/stm32f4xx_it.h create mode 100644 Core/Inc/tim.h create mode 100644 Core/Inc/usart.h create mode 100644 Core/Src/can.c create mode 100644 Core/Src/dma.c create mode 100644 Core/Src/freertos.c create mode 100644 Core/Src/gpio.c create mode 100644 Core/Src/main.c create mode 100644 Core/Src/retarget.c create mode 100644 Core/Src/stm32f4xx_hal_msp.c create mode 100644 Core/Src/stm32f4xx_it.c create mode 100644 Core/Src/syscalls.c create mode 100644 Core/Src/sysmem.c create mode 100644 Core/Src/system_stm32f4xx.c create mode 100644 Core/Src/tim.c create mode 100644 Core/Src/usart.c create mode 100644 ITM_TROUBLESHOOTING.md create mode 100644 MDK-ARM/DebugConfig/arm_STM32F407IGHx.dbgconf create mode 100644 MDK-ARM/DebugConfig/arm_STM32F407IGHx_1.0.1.dbgconf create mode 100644 MDK-ARM/RTE/_arm/RTE_Components.h create mode 100644 MDK-ARM/arm.uvguix.Lenovo create mode 100644 MDK-ARM/arm.uvguix.yxm23 create mode 100644 MDK-ARM/arm.uvoptx create mode 100644 MDK-ARM/arm.uvprojx create mode 100644 MDK-ARM/startup_stm32f407xx.s create mode 100644 PRINTF_OUTPUT_GUIDE.md create mode 100644 STM32F407XX_FLASH.ld create mode 100644 User/bsp/bsp.h create mode 100644 User/bsp/bsp_config.yaml create mode 100644 User/bsp/can.c create mode 100644 User/bsp/can.h create mode 100644 User/bsp/mm.c create mode 100644 User/bsp/mm.h create mode 100644 User/bsp/pwm.c create mode 100644 User/bsp/pwm.h create mode 100644 User/bsp/time.c create mode 100644 User/bsp/time.h create mode 100644 User/bsp/uart.c create mode 100644 User/bsp/uart.h create mode 100644 User/component/ARM_USAGE.md create mode 100644 User/component/PowerControl.c create mode 100644 User/component/PowerControl.h create mode 100644 User/component/ahrs.c create mode 100644 User/component/ahrs.h create mode 100644 User/component/arm_kinematics/arm6dof.cpp create mode 100644 User/component/arm_kinematics/arm6dof.h create mode 100644 User/component/component_config.yaml create mode 100644 User/component/filter.c create mode 100644 User/component/filter.h create mode 100644 User/component/mixer.c create mode 100644 User/component/mixer.h create mode 100644 User/component/pid.c create mode 100644 User/component/pid.h create mode 100644 User/component/toolbox/matrix.cpp create mode 100644 User/component/toolbox/matrix.h create mode 100644 User/component/toolbox/robotics.cpp create mode 100644 User/component/toolbox/robotics.h create mode 100644 User/component/toolbox/utils.cpp create mode 100644 User/component/toolbox/utils.h create mode 100644 User/component/user_math.c create mode 100644 User/component/user_math.h create mode 100644 User/device/at9s.c create mode 100644 User/device/at9s.h create mode 100644 User/device/device.h create mode 100644 User/device/device_config.yaml create mode 100644 User/device/dr16.c create mode 100644 User/device/dr16.h create mode 100644 User/device/motor.c create mode 100644 User/device/motor.h create mode 100644 User/device/motor_dm.c create mode 100644 User/device/motor_dm.h create mode 100644 User/device/motor_lz.c create mode 100644 User/device/motor_lz.h create mode 100644 User/device/motor_rm.c create mode 100644 User/device/motor_rm.h create mode 100644 User/device/supercap.c create mode 100644 User/device/supercap.h create mode 100644 User/module/arm.cpp create mode 100644 User/module/arm.h create mode 100644 User/module/arm_oop.hpp create mode 100644 User/module/chassis.c create mode 100644 User/module/chassis.h create mode 100644 User/module/cmd/cmd.c create mode 100644 User/module/cmd/cmd.h create mode 100644 User/module/cmd/cmd_adapter.c create mode 100644 User/module/cmd/cmd_adapter.h create mode 100644 User/module/cmd/cmd_behavior.c create mode 100644 User/module/cmd/cmd_behavior.h create mode 100644 User/module/cmd/cmd_example.c create mode 100644 User/module/cmd/cmd_feature.h create mode 100644 User/module/cmd/cmd_types.h create mode 100644 User/module/config.c create mode 100644 User/module/config.h create mode 100644 User/module/joint.hpp create mode 100644 User/module/motor_base.hpp create mode 100644 User/task/arm_main.cpp create mode 100644 User/task/blink.c create mode 100644 User/task/cmd.cpp create mode 100644 User/task/config.yaml create mode 100644 User/task/ctrl_chassis.c create mode 100644 User/task/init.c create mode 100644 User/task/rc.c create mode 100644 User/task/user_task.c create mode 100644 User/task/user_task.h create mode 100644 arm.ioc create mode 100644 cmake/gcc-arm-none-eabi.cmake create mode 100644 cmake/starm-clang.cmake create mode 100644 cmake/stm32cubemx/CMakeLists.txt create mode 100644 ozone/arm.jdebug create mode 100644 ozone/arm.jdebug.user create mode 100644 scripts/calc_new_dh.py create mode 100644 scripts/quick_calc.py create mode 100644 scripts/urdf_rc_to_dh.py create mode 100644 startup_stm32f407xx.s diff --git a/.clangd b/.clangd new file mode 100644 index 0000000..50c62c4 --- /dev/null +++ b/.clangd @@ -0,0 +1,11 @@ +CompileFlags: + Add: + - '-ferror-limit=0' + - '-Wno-implicit-int' + CompilationDatabase: build/Debug +Diagnostics: + Suppress: + - unused-includes + - unknown_typename + - unknown_typename_suggest + - typename_requires_specqual diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..7eda24a --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +build +mx.scratch +!.settings +.venv/ +arm_model/*.zip \ No newline at end of file diff --git a/.mxproject b/.mxproject new file mode 100644 index 0000000..ebff7e7 --- /dev/null +++ b/.mxproject @@ -0,0 +1,42 @@ +[PreviousLibFiles] +LibFiles=Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_can.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_bus.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_rcc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_system.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_utils.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ramfunc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_gpio.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_dma.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_dmamux.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_pwr.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_cortex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_cortex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h;Drivers\STM32F4xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_def.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_exti.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_exti.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_tim.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_uart.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_usart.h;Middlewares\Third_Party\FreeRTOS\Source\include\croutine.h;Middlewares\Third_Party\FreeRTOS\Source\include\deprecated_definitions.h;Middlewares\Third_Party\FreeRTOS\Source\include\event_groups.h;Middlewares\Third_Party\FreeRTOS\Source\include\FreeRTOS.h;Middlewares\Third_Party\FreeRTOS\Source\include\list.h;Middlewares\Third_Party\FreeRTOS\Source\include\message_buffer.h;Middlewares\Third_Party\FreeRTOS\Source\include\mpu_prototypes.h;Middlewares\Third_Party\FreeRTOS\Source\include\mpu_wrappers.h;Middlewares\Third_Party\FreeRTOS\Source\include\portable.h;Middlewares\Third_Party\FreeRTOS\Source\include\projdefs.h;Middlewares\Third_Party\FreeRTOS\Source\include\queue.h;Middlewares\Third_Party\FreeRTOS\Source\include\semphr.h;Middlewares\Third_Party\FreeRTOS\Source\include\stack_macros.h;Middlewares\Third_Party\FreeRTOS\Source\include\StackMacros.h;Middlewares\Third_Party\FreeRTOS\Source\include\stream_buffer.h;Middlewares\Third_Party\FreeRTOS\Source\include\task.h;Middlewares\Third_Party\FreeRTOS\Source\include\timers.h;Middlewares\Third_Party\FreeRTOS\Source\include\atomic.h;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os.h;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\freertos_mpool.h;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\freertos_os2.h;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\portmacro.h;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_can.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ramfunc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_gpio.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_cortex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_exti.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_uart.c;Middlewares\Third_Party\FreeRTOS\Source\croutine.c;Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;Middlewares\Third_Party\FreeRTOS\Source\list.c;Middlewares\Third_Party\FreeRTOS\Source\queue.c;Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;Middlewares\Third_Party\FreeRTOS\Source\tasks.c;Middlewares\Third_Party\FreeRTOS\Source\timers.c;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\port.c;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_can.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_bus.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_rcc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_system.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_utils.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ramfunc.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_gpio.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_dma.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_dmamux.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_pwr.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_cortex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_cortex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h;Drivers\STM32F4xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_def.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_exti.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_exti.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_tim.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim_ex.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_uart.h;Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_ll_usart.h;Middlewares\Third_Party\FreeRTOS\Source\include\croutine.h;Middlewares\Third_Party\FreeRTOS\Source\include\deprecated_definitions.h;Middlewares\Third_Party\FreeRTOS\Source\include\event_groups.h;Middlewares\Third_Party\FreeRTOS\Source\include\FreeRTOS.h;Middlewares\Third_Party\FreeRTOS\Source\include\list.h;Middlewares\Third_Party\FreeRTOS\Source\include\message_buffer.h;Middlewares\Third_Party\FreeRTOS\Source\include\mpu_prototypes.h;Middlewares\Third_Party\FreeRTOS\Source\include\mpu_wrappers.h;Middlewares\Third_Party\FreeRTOS\Source\include\portable.h;Middlewares\Third_Party\FreeRTOS\Source\include\projdefs.h;Middlewares\Third_Party\FreeRTOS\Source\include\queue.h;Middlewares\Third_Party\FreeRTOS\Source\include\semphr.h;Middlewares\Third_Party\FreeRTOS\Source\include\stack_macros.h;Middlewares\Third_Party\FreeRTOS\Source\include\StackMacros.h;Middlewares\Third_Party\FreeRTOS\Source\include\stream_buffer.h;Middlewares\Third_Party\FreeRTOS\Source\include\task.h;Middlewares\Third_Party\FreeRTOS\Source\include\timers.h;Middlewares\Third_Party\FreeRTOS\Source\include\atomic.h;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os.h;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\freertos_mpool.h;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\freertos_os2.h;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\portmacro.h;Drivers\CMSIS\Device\ST\STM32F4xx\Include\stm32f407xx.h;Drivers\CMSIS\Device\ST\STM32F4xx\Include\stm32f4xx.h;Drivers\CMSIS\Device\ST\STM32F4xx\Include\system_stm32f4xx.h;Drivers\CMSIS\Device\ST\STM32F4xx\Include\system_stm32f4xx.h;Drivers\CMSIS\Device\ST\STM32F4xx\Source\Templates\system_stm32f4xx.c;Drivers\CMSIS\Include\cachel1_armv7.h;Drivers\CMSIS\Include\cmsis_armcc.h;Drivers\CMSIS\Include\cmsis_armclang.h;Drivers\CMSIS\Include\cmsis_armclang_ltm.h;Drivers\CMSIS\Include\cmsis_compiler.h;Drivers\CMSIS\Include\cmsis_gcc.h;Drivers\CMSIS\Include\cmsis_iccarm.h;Drivers\CMSIS\Include\cmsis_version.h;Drivers\CMSIS\Include\core_armv81mml.h;Drivers\CMSIS\Include\core_armv8mbl.h;Drivers\CMSIS\Include\core_armv8mml.h;Drivers\CMSIS\Include\core_cm0.h;Drivers\CMSIS\Include\core_cm0plus.h;Drivers\CMSIS\Include\core_cm1.h;Drivers\CMSIS\Include\core_cm23.h;Drivers\CMSIS\Include\core_cm3.h;Drivers\CMSIS\Include\core_cm33.h;Drivers\CMSIS\Include\core_cm35p.h;Drivers\CMSIS\Include\core_cm4.h;Drivers\CMSIS\Include\core_cm55.h;Drivers\CMSIS\Include\core_cm7.h;Drivers\CMSIS\Include\core_cm85.h;Drivers\CMSIS\Include\core_sc000.h;Drivers\CMSIS\Include\core_sc300.h;Drivers\CMSIS\Include\core_starmc1.h;Drivers\CMSIS\Include\mpu_armv7.h;Drivers\CMSIS\Include\mpu_armv8.h;Drivers\CMSIS\Include\pac_armv81.h;Drivers\CMSIS\Include\pmu_armv8.h;Drivers\CMSIS\Include\tz_context.h; + +[PreviousUsedKeilFiles] +SourceFiles=..\Core\Src\main.c;..\Core\Src\gpio.c;..\Core\Src\freertos.c;..\Core\Src\can.c;..\Core\Src\tim.c;..\Core\Src\stm32f4xx_it.c;..\Core\Src\stm32f4xx_hal_msp.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_can.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc_ex.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ex.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ramfunc.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_gpio.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma_ex.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr_ex.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_cortex.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_exti.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim_ex.c;..\Middlewares\Third_Party\FreeRTOS\Source\croutine.c;..\Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;..\Middlewares\Third_Party\FreeRTOS\Source\list.c;..\Middlewares\Third_Party\FreeRTOS\Source\queue.c;..\Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;..\Middlewares\Third_Party\FreeRTOS\Source\tasks.c;..\Middlewares\Third_Party\FreeRTOS\Source\timers.c;..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;..\Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;..\Middlewares\Third_Party\FreeRTOS\Source\portable\RVDS\ARM_CM4F\port.c;..\Drivers\CMSIS\Device\ST\STM32F4xx\Source\Templates\system_stm32f4xx.c;..\Core\Src\system_stm32f4xx.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_can.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc_ex.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ex.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ramfunc.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_gpio.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma_ex.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr_ex.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_cortex.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_exti.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim.c;..\Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim_ex.c;..\Middlewares\Third_Party\FreeRTOS\Source\croutine.c;..\Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;..\Middlewares\Third_Party\FreeRTOS\Source\list.c;..\Middlewares\Third_Party\FreeRTOS\Source\queue.c;..\Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;..\Middlewares\Third_Party\FreeRTOS\Source\tasks.c;..\Middlewares\Third_Party\FreeRTOS\Source\timers.c;..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;..\Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;..\Middlewares\Third_Party\FreeRTOS\Source\portable\RVDS\ARM_CM4F\port.c;..\Drivers\CMSIS\Device\ST\STM32F4xx\Source\Templates\system_stm32f4xx.c;..\Core\Src\system_stm32f4xx.c;;;..\Middlewares\Third_Party\FreeRTOS\Source\croutine.c;..\Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;..\Middlewares\Third_Party\FreeRTOS\Source\list.c;..\Middlewares\Third_Party\FreeRTOS\Source\queue.c;..\Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;..\Middlewares\Third_Party\FreeRTOS\Source\tasks.c;..\Middlewares\Third_Party\FreeRTOS\Source\timers.c;..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;..\Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;..\Middlewares\Third_Party\FreeRTOS\Source\portable\RVDS\ARM_CM4F\port.c; +HeaderPath=..\Drivers\STM32F4xx_HAL_Driver\Inc;..\Drivers\STM32F4xx_HAL_Driver\Inc\Legacy;..\Middlewares\Third_Party\FreeRTOS\Source\include;..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2;..\Middlewares\Third_Party\FreeRTOS\Source\portable\RVDS\ARM_CM4F;..\Drivers\CMSIS\Device\ST\STM32F4xx\Include;..\Drivers\CMSIS\Include;..\Middlewares\ST\ARM\DSP\Inc;..\Core\Inc; +CDefines=USE_HAL_DRIVER;STM32F407xx;USE_HAL_DRIVER;USE_HAL_DRIVER; + +[PreviousUsedCMakes] +SourceFiles=Core\Src\main.c;Core\Src\gpio.c;Core\Src\freertos.c;Core\Src\can.c;Core\Src\dma.c;Core\Src\tim.c;Core\Src\usart.c;Core\Src\stm32f4xx_it.c;Core\Src\stm32f4xx_hal_msp.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_can.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ramfunc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_gpio.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_cortex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_exti.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_uart.c;Middlewares\Third_Party\FreeRTOS\Source\croutine.c;Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;Middlewares\Third_Party\FreeRTOS\Source\list.c;Middlewares\Third_Party\FreeRTOS\Source\queue.c;Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;Middlewares\Third_Party\FreeRTOS\Source\tasks.c;Middlewares\Third_Party\FreeRTOS\Source\timers.c;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\port.c;Drivers\CMSIS\Device\ST\STM32F4xx\Source\Templates\system_stm32f4xx.c;Core\Src\system_stm32f4xx.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_can.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_rcc_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_flash_ramfunc.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_gpio.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_dma.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_pwr_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_cortex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_exti.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_tim_ex.c;Drivers\STM32F4xx_HAL_Driver\Src\stm32f4xx_hal_uart.c;Middlewares\Third_Party\FreeRTOS\Source\croutine.c;Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;Middlewares\Third_Party\FreeRTOS\Source\list.c;Middlewares\Third_Party\FreeRTOS\Source\queue.c;Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;Middlewares\Third_Party\FreeRTOS\Source\tasks.c;Middlewares\Third_Party\FreeRTOS\Source\timers.c;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\port.c;Drivers\CMSIS\Device\ST\STM32F4xx\Source\Templates\system_stm32f4xx.c;Core\Src\system_stm32f4xx.c;;;Middlewares\Third_Party\FreeRTOS\Source\croutine.c;Middlewares\Third_Party\FreeRTOS\Source\event_groups.c;Middlewares\Third_Party\FreeRTOS\Source\list.c;Middlewares\Third_Party\FreeRTOS\Source\queue.c;Middlewares\Third_Party\FreeRTOS\Source\stream_buffer.c;Middlewares\Third_Party\FreeRTOS\Source\tasks.c;Middlewares\Third_Party\FreeRTOS\Source\timers.c;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.c;Middlewares\Third_Party\FreeRTOS\Source\portable\MemMang\heap_4.c;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F\port.c; +HeaderPath=Drivers\STM32F4xx_HAL_Driver\Inc;Drivers\STM32F4xx_HAL_Driver\Inc\Legacy;Middlewares\Third_Party\FreeRTOS\Source\include;Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2;Middlewares\Third_Party\FreeRTOS\Source\portable\GCC\ARM_CM4F;Drivers\CMSIS\Device\ST\STM32F4xx\Include;Drivers\CMSIS\Include;Core\Inc; +CDefines=USE_HAL_DRIVER;STM32F407xx;USE_HAL_DRIVER;USE_HAL_DRIVER; + +[PreviousGenFiles] +AdvancedFolderStructure=true +HeaderFileListSize=9 +HeaderFiles#0=..\Core\Inc\gpio.h +HeaderFiles#1=..\Core\Inc\FreeRTOSConfig.h +HeaderFiles#2=..\Core\Inc\can.h +HeaderFiles#3=..\Core\Inc\dma.h +HeaderFiles#4=..\Core\Inc\tim.h +HeaderFiles#5=..\Core\Inc\usart.h +HeaderFiles#6=..\Core\Inc\stm32f4xx_it.h +HeaderFiles#7=..\Core\Inc\stm32f4xx_hal_conf.h +HeaderFiles#8=..\Core\Inc\main.h +HeaderFolderListSize=1 +HeaderPath#0=..\Core\Inc +HeaderFiles=; +SourceFileListSize=9 +SourceFiles#0=..\Core\Src\gpio.c +SourceFiles#1=..\Core\Src\freertos.c +SourceFiles#2=..\Core\Src\can.c +SourceFiles#3=..\Core\Src\dma.c +SourceFiles#4=..\Core\Src\tim.c +SourceFiles#5=..\Core\Src\usart.c +SourceFiles#6=..\Core\Src\stm32f4xx_it.c +SourceFiles#7=..\Core\Src\stm32f4xx_hal_msp.c +SourceFiles#8=..\Core\Src\main.c +SourceFolderListSize=1 +SourcePath#0=..\Core\Src +SourceFiles=; + diff --git a/.settings/bundles-lock.store.json b/.settings/bundles-lock.store.json new file mode 100644 index 0000000..7e6cbbe --- /dev/null +++ b/.settings/bundles-lock.store.json @@ -0,0 +1,147 @@ +{ + "resolved": [ + { + "name": "cmake", + "version": "4.0.1+st.3", + "platform": "darwin", + "selected_by": [ + { + "name": "cmake", + "version": "4.0.1+st.3" + } + ] + }, + { + "name": "cmake", + "version": "4.0.1+st.3", + "platform": "x86_64-linux", + "selected_by": [ + { + "name": "cmake", + "version": "4.0.1+st.3" + } + ] + }, + { + "name": "cmake", + "version": "4.0.1+st.3", + "platform": "x86_64-windows", + "selected_by": [ + { + "name": "cmake", + "version": "4.0.1+st.3" + } + ] + }, + { + "name": "gnu-tools-for-stm32", + "version": "13.3.1+st.9", + "platform": "darwin", + "selected_by": [ + { + "name": "gnu-tools-for-stm32", + "version": "13.3.1+st.9" + } + ] + }, + { + "name": "gnu-tools-for-stm32", + "version": "13.3.1+st.9", + "platform": "x86_64-linux", + "selected_by": [ + { + "name": "gnu-tools-for-stm32", + "version": "13.3.1+st.9" + } + ] + }, + { + "name": "gnu-tools-for-stm32", + "version": "13.3.1+st.9", + "platform": "x86_64-windows", + "selected_by": [ + { + "name": "gnu-tools-for-stm32", + "version": "13.3.1+st.9" + } + ] + }, + { + "name": "gnu-tools-for-stm32-13_3_1-description", + "version": "1.0.1+st.1", + "platform": "all", + "selected_by": [ + { + "name": "gnu-tools-for-stm32-13_3_1-description", + "version": ">=0.0.1" + } + ] + }, + { + "name": "ninja", + "version": "1.13.1+st.1", + "platform": "darwin", + "selected_by": [ + { + "name": "ninja", + "version": "1.13.1+st.1" + } + ] + }, + { + "name": "ninja", + "version": "1.13.1+st.1", + "platform": "x86_64-linux", + "selected_by": [ + { + "name": "ninja", + "version": "1.13.1+st.1" + } + ] + }, + { + "name": "ninja", + "version": "1.13.1+st.1", + "platform": "x86_64-windows", + "selected_by": [ + { + "name": "ninja", + "version": "1.13.1+st.1" + } + ] + }, + { + "name": "st-arm-clangd", + "version": "19.1.2+st.3", + "platform": "darwin", + "selected_by": [ + { + "name": "st-arm-clangd", + "version": "19.1.2+st.3" + } + ] + }, + { + "name": "st-arm-clangd", + "version": "19.1.2+st.3", + "platform": "x86_64-linux", + "selected_by": [ + { + "name": "st-arm-clangd", + "version": "19.1.2+st.3" + } + ] + }, + { + "name": "st-arm-clangd", + "version": "19.1.2+st.3", + "platform": "x86_64-windows", + "selected_by": [ + { + "name": "st-arm-clangd", + "version": "19.1.2+st.3" + } + ] + } + ] +} diff --git a/.settings/bundles.store.json b/.settings/bundles.store.json new file mode 100644 index 0000000..d036e56 --- /dev/null +++ b/.settings/bundles.store.json @@ -0,0 +1,20 @@ +{ + "bundles": [ + { + "name": "cmake", + "version": "4.0.1+st.3" + }, + { + "name": "ninja", + "version": "1.13.1+st.1" + }, + { + "name": "gnu-tools-for-stm32", + "version": "13.3.1+st.9" + }, + { + "name": "st-arm-clangd", + "version": "19.1.2+st.3" + } + ] +} diff --git a/.settings/ide.store.json b/.settings/ide.store.json new file mode 100644 index 0000000..6ffc181 --- /dev/null +++ b/.settings/ide.store.json @@ -0,0 +1,6 @@ +{ + "device": "STM32F407IGH6", + "core": "Cortex-M4", + "order": 0, + "toolchain": "GCC" +} \ No newline at end of file diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..233ee1f --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,8 @@ +{ + "configurations": [ + { + "name": "STM32", + "compileCommands": "${workspaceFolder}/build/Debug/compile_commands.json" + } + ] +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..77d682d --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,15 @@ +{ + "cmake.cmakePath": "cube-cmake", + "cmake.configureArgs": [ + "-DCMAKE_COMMAND=cube-cmake" + ], + "cmake.preferredGenerators": [ + "Ninja" + ], + "stm32cube-ide-clangd.path": "cube", + "stm32cube-ide-clangd.arguments": [ + "starm-clangd", + "--query-driver=${env:CUBE_BUNDLE_PATH}/gnu-tools-for-stm32/13.3.1+st.9/bin/arm-none-eabi-gcc.exe", + "--query-driver=${env:CUBE_BUNDLE_PATH}/gnu-tools-for-stm32/13.3.1+st.9/bin/arm-none-eabi-g++.exe" + ] +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..905a2a2 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,122 @@ +cmake_minimum_required(VERSION 3.22) + +# +# This file is generated only once, +# and is not re-generated if converter is called multiple times. +# +# User is free to modify the file as much as necessary +# + +# Setup compiler settings +set(CMAKE_C_STANDARD 11) +set(CMAKE_C_STANDARD_REQUIRED ON) +set(CMAKE_C_EXTENSIONS ON) + + +# Define the build type +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE "Debug") +endif() + +# Set the project name +set(CMAKE_PROJECT_NAME arm) + +# Enable compile command to ease indexing with e.g. clangd +set(CMAKE_EXPORT_COMPILE_COMMANDS TRUE) + +# Core project settings +project(${CMAKE_PROJECT_NAME}) +message("Build type: " ${CMAKE_BUILD_TYPE}) + +# Enable CMake support for ASM and C languages +enable_language(C ASM) + +# Create an executable object type +add_executable(${CMAKE_PROJECT_NAME}) + +# Add STM32CubeMX generated sources +add_subdirectory(cmake/stm32cubemx) + +# Link directories setup +target_link_directories(${CMAKE_PROJECT_NAME} PRIVATE + # Add user defined library search paths + Middlewares/ST/ARM/DSP/Lib +) + +# Add sources to executable +target_sources(${CMAKE_PROJECT_NAME} PRIVATE + # Add user sources here + # User/bsp sources + User/bsp/can.c + User/bsp/mm.c + User/bsp/pwm.c + User/bsp/time.c + User/bsp/uart.c + + + # User/component sources + User/component/pid.c + User/component/user_math.c + User/component/filter.c + User/component/PowerControl.c + # User/component/toolbox sources (C++) + User/component/toolbox/matrix.cpp + User/component/toolbox/robotics.cpp + User/component/toolbox/utils.cpp + # User/component/arm sources (C++) + User/component/arm_kinematics/arm6dof.cpp + + # User/device sources + User/device/motor.c + User/device/motor_dm.c + User/device/motor_lz.c + User/device/motor_rm.c + User/device/dr16.c + # User/module sources (C++) + User/module/arm.cpp + User/module/config.c + User/module/chassis.c + User/module/cmd/cmd.c + User/module/cmd/cmd_adapter.c + User/module/cmd/cmd_behavior.c + User/module/cmd/cmd_example.c + # User/task sources + User/task/arm_main.cpp + User/task/blink.c + User/task/init.c + User/task/user_task.c + User/task/cmd.cpp + User/task/rc.c + User/task/ctrl_chassis.c +) + +# Add include paths +target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE + # Add user defined include paths + User + User/component + User/component/toolbox + # ARM CMSIS DSP library include + Middlewares/ST/ARM/DSP/Inc +) + +# Add project symbols (macros) +target_compile_definitions(${CMAKE_PROJECT_NAME} PRIVATE + ARM_MATH_CM4 + ARM_MATH_MATRIX_CHECK + ARM_MATH_ROUNDING + # Add user defined symbols +) + +# Remove wrong libob.a library dependency when using cpp files +list(REMOVE_ITEM CMAKE_C_IMPLICIT_LINK_LIBRARIES ob) + +# Add linked libraries +target_link_libraries(${CMAKE_PROJECT_NAME} + + # ARM CMSIS DSP library (Cortex-M4 with FPU) + ${CMAKE_SOURCE_DIR}/Middlewares/ST/ARM/DSP/Lib/libarm_cortexM4lf_math.a + stm32cubemx + + # Add user defined libraries +) diff --git a/CMakePresets.json b/CMakePresets.json new file mode 100644 index 0000000..9a0c120 --- /dev/null +++ b/CMakePresets.json @@ -0,0 +1,38 @@ +{ + "version": 3, + "configurePresets": [ + { + "name": "default", + "hidden": true, + "generator": "Ninja", + "binaryDir": "${sourceDir}/build/${presetName}", + "toolchainFile": "${sourceDir}/cmake/gcc-arm-none-eabi.cmake", + "cacheVariables": { + } + }, + { + "name": "Debug", + "inherits": "default", + "cacheVariables": { + "CMAKE_BUILD_TYPE": "Debug" + } + }, + { + "name": "Release", + "inherits": "default", + "cacheVariables": { + "CMAKE_BUILD_TYPE": "Release" + } + } + ], + "buildPresets": [ + { + "name": "Debug", + "configurePreset": "Debug" + }, + { + "name": "Release", + "configurePreset": "Release" + } + ] +} \ No newline at end of file diff --git a/Core/Inc/FreeRTOSConfig.h b/Core/Inc/FreeRTOSConfig.h new file mode 100644 index 0000000..eee0451 --- /dev/null +++ b/Core/Inc/FreeRTOSConfig.h @@ -0,0 +1,171 @@ +/* USER CODE BEGIN Header */ +/* + * FreeRTOS Kernel V10.3.1 + * Portion Copyright (C) 2017 Amazon.com, Inc. or its affiliates. All Rights Reserved. + * Portion Copyright (C) 2019 StMicroelectronics, Inc. All Rights Reserved. + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + * + * http://www.FreeRTOS.org + * http://aws.amazon.com/freertos + * + * 1 tab == 4 spaces! + */ +/* USER CODE END Header */ + +#ifndef FREERTOS_CONFIG_H +#define FREERTOS_CONFIG_H + +/*----------------------------------------------------------- + * Application specific definitions. + * + * These definitions should be adjusted for your particular hardware and + * application requirements. + * + * These parameters and more are described within the 'configuration' section of the + * FreeRTOS API documentation available on the FreeRTOS.org web site. + * + * See http://www.freertos.org/a00110.html + *----------------------------------------------------------*/ + +/* USER CODE BEGIN Includes */ +/* Section where include file can be added */ +/* USER CODE END Includes */ + +/* Ensure definitions are only used by the compiler, and not by the assembler. */ +#if defined(__ICCARM__) || defined(__CC_ARM) || defined(__GNUC__) + #include + extern uint32_t SystemCoreClock; + void xPortSysTickHandler(void); +#endif +#ifndef CMSIS_device_header +#define CMSIS_device_header "stm32f4xx.h" +#endif /* CMSIS_device_header */ + +#define configENABLE_FPU 0 +#define configENABLE_MPU 0 + +#define configUSE_PREEMPTION 1 +#define configSUPPORT_STATIC_ALLOCATION 1 +#define configSUPPORT_DYNAMIC_ALLOCATION 1 +#define configUSE_IDLE_HOOK 0 +#define configUSE_TICK_HOOK 0 +#define configCPU_CLOCK_HZ ( SystemCoreClock ) +#define configTICK_RATE_HZ ((TickType_t)1000) +#define configMAX_PRIORITIES ( 56 ) +#define configMINIMAL_STACK_SIZE ((uint16_t)128) +#define configTOTAL_HEAP_SIZE ((size_t)20480) +#define configMAX_TASK_NAME_LEN ( 16 ) +#define configUSE_TRACE_FACILITY 1 +#define configUSE_16_BIT_TICKS 0 +#define configUSE_MUTEXES 1 +#define configQUEUE_REGISTRY_SIZE 8 +#define configUSE_RECURSIVE_MUTEXES 1 +#define configUSE_COUNTING_SEMAPHORES 1 +#define configUSE_PORT_OPTIMISED_TASK_SELECTION 0 +/* USER CODE BEGIN MESSAGE_BUFFER_LENGTH_TYPE */ +/* Defaults to size_t for backward compatibility, but can be changed + if lengths will always be less than the number of bytes in a size_t. */ +#define configMESSAGE_BUFFER_LENGTH_TYPE size_t +/* USER CODE END MESSAGE_BUFFER_LENGTH_TYPE */ + +/* Co-routine definitions. */ +#define configUSE_CO_ROUTINES 0 +#define configMAX_CO_ROUTINE_PRIORITIES ( 2 ) + +/* Software timer definitions. */ +#define configUSE_TIMERS 1 +#define configTIMER_TASK_PRIORITY ( 2 ) +#define configTIMER_QUEUE_LENGTH 10 +#define configTIMER_TASK_STACK_DEPTH 256 + +/* CMSIS-RTOS V2 flags */ +#define configUSE_OS2_THREAD_SUSPEND_RESUME 1 +#define configUSE_OS2_THREAD_ENUMERATE 1 +#define configUSE_OS2_EVENTFLAGS_FROM_ISR 1 +#define configUSE_OS2_THREAD_FLAGS 1 +#define configUSE_OS2_TIMER 1 +#define configUSE_OS2_MUTEX 1 + +/* Set the following definitions to 1 to include the API function, or zero +to exclude the API function. */ +#define INCLUDE_vTaskPrioritySet 1 +#define INCLUDE_uxTaskPriorityGet 1 +#define INCLUDE_vTaskDelete 1 +#define INCLUDE_vTaskCleanUpResources 0 +#define INCLUDE_vTaskSuspend 1 +#define INCLUDE_vTaskDelayUntil 1 +#define INCLUDE_vTaskDelay 1 +#define INCLUDE_xTaskGetSchedulerState 1 +#define INCLUDE_xTimerPendFunctionCall 1 +#define INCLUDE_xQueueGetMutexHolder 1 +#define INCLUDE_uxTaskGetStackHighWaterMark 1 +#define INCLUDE_xTaskGetCurrentTaskHandle 1 +#define INCLUDE_eTaskGetState 1 + +/* + * The CMSIS-RTOS V2 FreeRTOS wrapper is dependent on the heap implementation used + * by the application thus the correct define need to be enabled below + */ +#define USE_FreeRTOS_HEAP_4 + +/* Cortex-M specific definitions. */ +#ifdef __NVIC_PRIO_BITS + /* __BVIC_PRIO_BITS will be specified when CMSIS is being used. */ + #define configPRIO_BITS __NVIC_PRIO_BITS +#else + #define configPRIO_BITS 4 +#endif + +/* The lowest interrupt priority that can be used in a call to a "set priority" +function. */ +#define configLIBRARY_LOWEST_INTERRUPT_PRIORITY 15 + +/* The highest interrupt priority that can be used by any interrupt service +routine that makes calls to interrupt safe FreeRTOS API functions. DO NOT CALL +INTERRUPT SAFE FREERTOS API FUNCTIONS FROM ANY INTERRUPT THAT HAS A HIGHER +PRIORITY THAN THIS! (higher priorities are lower numeric values. */ +#define configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY 5 + +/* Interrupt priorities used by the kernel port layer itself. These are generic +to all Cortex-M ports, and do not rely on any particular library functions. */ +#define configKERNEL_INTERRUPT_PRIORITY ( configLIBRARY_LOWEST_INTERRUPT_PRIORITY << (8 - configPRIO_BITS) ) +/* !!!! configMAX_SYSCALL_INTERRUPT_PRIORITY must not be set to zero !!!! +See http://www.FreeRTOS.org/RTOS-Cortex-M3-M4.html. */ +#define configMAX_SYSCALL_INTERRUPT_PRIORITY ( configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY << (8 - configPRIO_BITS) ) + +/* Normal assert() semantics without relying on the provision of an assert.h +header file. */ +/* USER CODE BEGIN 1 */ +#define configASSERT( x ) if ((x) == 0) {taskDISABLE_INTERRUPTS(); for( ;; );} +/* USER CODE END 1 */ + +/* Definitions that map the FreeRTOS port interrupt handlers to their CMSIS +standard names. */ +#define vPortSVCHandler SVC_Handler +#define xPortPendSVHandler PendSV_Handler + +/* IMPORTANT: After 10.3.1 update, Systick_Handler comes from NVIC (if SYS timebase = systick), otherwise from cmsis_os2.c */ + +#define USE_CUSTOM_SYSTICK_HANDLER_IMPLEMENTATION 1 + +/* USER CODE BEGIN Defines */ +/* Section where parameter definitions can be added (for instance, to override default ones in FreeRTOS.h) */ +/* USER CODE END Defines */ + +#endif /* FREERTOS_CONFIG_H */ diff --git a/Core/Inc/can.h b/Core/Inc/can.h new file mode 100644 index 0000000..2889c8c --- /dev/null +++ b/Core/Inc/can.h @@ -0,0 +1,55 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file can.h + * @brief This file contains all the function prototypes for + * the can.c file + ****************************************************************************** + * @attention + * + * Copyright (c) 2026 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __CAN_H__ +#define __CAN_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "main.h" + +/* USER CODE BEGIN Includes */ + +/* USER CODE END Includes */ + +extern CAN_HandleTypeDef hcan1; + +extern CAN_HandleTypeDef hcan2; + +/* USER CODE BEGIN Private defines */ + +/* USER CODE END Private defines */ + +void MX_CAN1_Init(void); +void MX_CAN2_Init(void); + +/* USER CODE BEGIN Prototypes */ + +/* USER CODE END Prototypes */ + +#ifdef __cplusplus +} +#endif + +#endif /* __CAN_H__ */ + diff --git a/Core/Inc/dma.h b/Core/Inc/dma.h new file mode 100644 index 0000000..afad30d --- /dev/null +++ b/Core/Inc/dma.h @@ -0,0 +1,52 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file dma.h + * @brief This file contains all the function prototypes for + * the dma.c file + ****************************************************************************** + * @attention + * + * Copyright (c) 2026 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __DMA_H__ +#define __DMA_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "main.h" + +/* DMA memory to memory transfer handles -------------------------------------*/ + +/* USER CODE BEGIN Includes */ + +/* USER CODE END Includes */ + +/* USER CODE BEGIN Private defines */ + +/* USER CODE END Private defines */ + +void MX_DMA_Init(void); + +/* USER CODE BEGIN Prototypes */ + +/* USER CODE END Prototypes */ + +#ifdef __cplusplus +} +#endif + +#endif /* __DMA_H__ */ + diff --git a/Core/Inc/gpio.h b/Core/Inc/gpio.h new file mode 100644 index 0000000..3c91c4d --- /dev/null +++ b/Core/Inc/gpio.h @@ -0,0 +1,49 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file gpio.h + * @brief This file contains all the function prototypes for + * the gpio.c file + ****************************************************************************** + * @attention + * + * Copyright (c) 2026 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __GPIO_H__ +#define __GPIO_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "main.h" + +/* USER CODE BEGIN Includes */ + +/* USER CODE END Includes */ + +/* USER CODE BEGIN Private defines */ + +/* USER CODE END Private defines */ + +void MX_GPIO_Init(void); + +/* USER CODE BEGIN Prototypes */ + +/* USER CODE END Prototypes */ + +#ifdef __cplusplus +} +#endif +#endif /*__ GPIO_H__ */ + diff --git a/Core/Inc/main.h b/Core/Inc/main.h new file mode 100644 index 0000000..06878af --- /dev/null +++ b/Core/Inc/main.h @@ -0,0 +1,69 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file : main.h + * @brief : Header for main.c file. + * This file contains the common defines of the application. + ****************************************************************************** + * @attention + * + * Copyright (c) 2026 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __MAIN_H +#define __MAIN_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "stm32f4xx_hal.h" + +/* Private includes ----------------------------------------------------------*/ +/* USER CODE BEGIN Includes */ + +/* USER CODE END Includes */ + +/* Exported types ------------------------------------------------------------*/ +/* USER CODE BEGIN ET */ + +/* USER CODE END ET */ + +/* Exported constants --------------------------------------------------------*/ +/* USER CODE BEGIN EC */ + +/* USER CODE END EC */ + +/* Exported macro ------------------------------------------------------------*/ +/* USER CODE BEGIN EM */ + +/* USER CODE END EM */ + +/* Exported functions prototypes ---------------------------------------------*/ +void Error_Handler(void); + +/* USER CODE BEGIN EFP */ + +/* USER CODE END EFP */ + +/* Private defines -----------------------------------------------------------*/ + +/* USER CODE BEGIN Private defines */ + +/* USER CODE END Private defines */ + +#ifdef __cplusplus +} +#endif + +#endif /* __MAIN_H */ diff --git a/Core/Inc/retarget.h b/Core/Inc/retarget.h new file mode 100644 index 0000000..10671af --- /dev/null +++ b/Core/Inc/retarget.h @@ -0,0 +1,25 @@ +/** + ****************************************************************************** + * @file retarget.h + * @brief Printf 重定向头文件 + ****************************************************************************** + */ + +#ifndef __RETARGET_H +#define __RETARGET_H + +#ifdef __cplusplus +extern "C" { +#endif + +/** + * @brief 初始化 ITM（用于 SWO/printf 输出） + * @note 必须在使用 printf 之前调用 + */ +void ITM_Init(void); + +#ifdef __cplusplus +} +#endif + +#endif /* __RETARGET_H */ diff --git a/Core/Inc/stm32f4xx_hal_conf.h b/Core/Inc/stm32f4xx_hal_conf.h new file mode 100644 index 0000000..f02e031 --- /dev/null +++ b/Core/Inc/stm32f4xx_hal_conf.h @@ -0,0 +1,495 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file stm32f4xx_hal_conf_template.h + * @author MCD Application Team + * @brief HAL configuration template file. + * This file should be copied to the application folder and renamed + * to stm32f4xx_hal_conf.h. + ****************************************************************************** + * @attention + * + * Copyright (c) 2017 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F4xx_HAL_CONF_H +#define __STM32F4xx_HAL_CONF_H + +#ifdef __cplusplus + extern "C" { +#endif + +/* Exported types ------------------------------------------------------------*/ +/* Exported constants --------------------------------------------------------*/ + +/* ########################## Module Selection ############################## */ +/** + * @brief This is the list of modules to be used in the HAL driver + */ +#define HAL_MODULE_ENABLED + + /* #define HAL_CRYP_MODULE_ENABLED */ +/* #define HAL_ADC_MODULE_ENABLED */ +#define HAL_CAN_MODULE_ENABLED +/* #define HAL_CRC_MODULE_ENABLED */ +/* #define HAL_CAN_LEGACY_MODULE_ENABLED */ +/* #define HAL_DAC_MODULE_ENABLED */ +/* #define HAL_DCMI_MODULE_ENABLED */ +/* #define HAL_DMA2D_MODULE_ENABLED */ +/* #define HAL_ETH_MODULE_ENABLED */ +/* #define HAL_ETH_LEGACY_MODULE_ENABLED */ +/* #define HAL_NAND_MODULE_ENABLED */ +/* #define HAL_NOR_MODULE_ENABLED */ +/* #define HAL_PCCARD_MODULE_ENABLED */ +/* #define HAL_SRAM_MODULE_ENABLED */ +/* #define HAL_SDRAM_MODULE_ENABLED */ +/* #define HAL_HASH_MODULE_ENABLED */ +/* #define HAL_I2C_MODULE_ENABLED */ +/* #define HAL_I2S_MODULE_ENABLED */ +/* #define HAL_IWDG_MODULE_ENABLED */ +/* #define HAL_LTDC_MODULE_ENABLED */ +/* #define HAL_RNG_MODULE_ENABLED */ +/* #define HAL_RTC_MODULE_ENABLED */ +/* #define HAL_SAI_MODULE_ENABLED */ +/* #define HAL_SD_MODULE_ENABLED */ +/* #define HAL_MMC_MODULE_ENABLED */ +/* #define HAL_SPI_MODULE_ENABLED */ +#define HAL_TIM_MODULE_ENABLED +#define HAL_UART_MODULE_ENABLED +/* #define HAL_USART_MODULE_ENABLED */ +/* #define HAL_IRDA_MODULE_ENABLED */ +/* #define HAL_SMARTCARD_MODULE_ENABLED */ +/* #define HAL_SMBUS_MODULE_ENABLED */ +/* #define HAL_WWDG_MODULE_ENABLED */ +/* #define HAL_PCD_MODULE_ENABLED */ +/* #define HAL_HCD_MODULE_ENABLED */ +/* #define HAL_DSI_MODULE_ENABLED */ +/* #define HAL_QSPI_MODULE_ENABLED */ +/* #define HAL_QSPI_MODULE_ENABLED */ +/* #define HAL_CEC_MODULE_ENABLED */ +/* #define HAL_FMPI2C_MODULE_ENABLED */ +/* #define HAL_FMPSMBUS_MODULE_ENABLED */ +/* #define HAL_SPDIFRX_MODULE_ENABLED */ +/* #define HAL_DFSDM_MODULE_ENABLED */ +/* #define HAL_LPTIM_MODULE_ENABLED */ +#define HAL_GPIO_MODULE_ENABLED +#define HAL_EXTI_MODULE_ENABLED +#define HAL_DMA_MODULE_ENABLED +#define HAL_RCC_MODULE_ENABLED +#define HAL_FLASH_MODULE_ENABLED +#define HAL_PWR_MODULE_ENABLED +#define HAL_CORTEX_MODULE_ENABLED + +/* ########################## HSE/HSI Values adaptation ##################### */ +/** + * @brief Adjust the value of External High Speed oscillator (HSE) used in your application. + * This value is used by the RCC HAL module to compute the system frequency + * (when HSE is used as system clock source, directly or through the PLL). + */ +#if !defined (HSE_VALUE) + #define HSE_VALUE 12000000U /*!< Value of the External oscillator in Hz */ +#endif /* HSE_VALUE */ + +#if !defined (HSE_STARTUP_TIMEOUT) + #define HSE_STARTUP_TIMEOUT 100U /*!< Time out for HSE start up, in ms */ +#endif /* HSE_STARTUP_TIMEOUT */ + +/** + * @brief Internal High Speed oscillator (HSI) value. + * This value is used by the RCC HAL module to compute the system frequency + * (when HSI is used as system clock source, directly or through the PLL). + */ +#if !defined (HSI_VALUE) + #define HSI_VALUE ((uint32_t)16000000U) /*!< Value of the Internal oscillator in Hz*/ +#endif /* HSI_VALUE */ + +/** + * @brief Internal Low Speed oscillator (LSI) value. + */ +#if !defined (LSI_VALUE) + #define LSI_VALUE 32000U /*!< LSI Typical Value in Hz*/ +#endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz + The real value may vary depending on the variations + in voltage and temperature.*/ +/** + * @brief External Low Speed oscillator (LSE) value. + */ +#if !defined (LSE_VALUE) + #define LSE_VALUE 32768U /*!< Value of the External Low Speed oscillator in Hz */ +#endif /* LSE_VALUE */ + +#if !defined (LSE_STARTUP_TIMEOUT) + #define LSE_STARTUP_TIMEOUT 5000U /*!< Time out for LSE start up, in ms */ +#endif /* LSE_STARTUP_TIMEOUT */ + +/** + * @brief External clock source for I2S peripheral + * This value is used by the I2S HAL module to compute the I2S clock source + * frequency, this source is inserted directly through I2S_CKIN pad. + */ +#if !defined (EXTERNAL_CLOCK_VALUE) + #define EXTERNAL_CLOCK_VALUE 12288000U /*!< Value of the External audio frequency in Hz*/ +#endif /* EXTERNAL_CLOCK_VALUE */ + +/* Tip: To avoid modifying this file each time you need to use different HSE, + === you can define the HSE value in your toolchain compiler preprocessor. */ + +/* ########################### System Configuration ######################### */ +/** + * @brief This is the HAL system configuration section + */ +#define VDD_VALUE 3300U /*!< Value of VDD in mv */ +#define TICK_INT_PRIORITY 15U /*!< tick interrupt priority */ +#define USE_RTOS 0U +#define PREFETCH_ENABLE 1U +#define INSTRUCTION_CACHE_ENABLE 1U +#define DATA_CACHE_ENABLE 1U + +#define USE_HAL_ADC_REGISTER_CALLBACKS 0U /* ADC register callback disabled */ +#define USE_HAL_CAN_REGISTER_CALLBACKS 0U /* CAN register callback disabled */ +#define USE_HAL_CEC_REGISTER_CALLBACKS 0U /* CEC register callback disabled */ +#define USE_HAL_CRYP_REGISTER_CALLBACKS 0U /* CRYP register callback disabled */ +#define USE_HAL_DAC_REGISTER_CALLBACKS 0U /* DAC register callback disabled */ +#define USE_HAL_DCMI_REGISTER_CALLBACKS 0U /* DCMI register callback disabled */ +#define USE_HAL_DFSDM_REGISTER_CALLBACKS 0U /* DFSDM register callback disabled */ +#define USE_HAL_DMA2D_REGISTER_CALLBACKS 0U /* DMA2D register callback disabled */ +#define USE_HAL_DSI_REGISTER_CALLBACKS 0U /* DSI register callback disabled */ +#define USE_HAL_ETH_REGISTER_CALLBACKS 0U /* ETH register callback disabled */ +#define USE_HAL_HASH_REGISTER_CALLBACKS 0U /* HASH register callback disabled */ +#define USE_HAL_HCD_REGISTER_CALLBACKS 0U /* HCD register callback disabled */ +#define USE_HAL_I2C_REGISTER_CALLBACKS 0U /* I2C register callback disabled */ +#define USE_HAL_FMPI2C_REGISTER_CALLBACKS 0U /* FMPI2C register callback disabled */ +#define USE_HAL_FMPSMBUS_REGISTER_CALLBACKS 0U /* FMPSMBUS register callback disabled */ +#define USE_HAL_I2S_REGISTER_CALLBACKS 0U /* I2S register callback disabled */ +#define USE_HAL_IRDA_REGISTER_CALLBACKS 0U /* IRDA register callback disabled */ +#define USE_HAL_LPTIM_REGISTER_CALLBACKS 0U /* LPTIM register callback disabled */ +#define USE_HAL_LTDC_REGISTER_CALLBACKS 0U /* LTDC register callback disabled */ +#define USE_HAL_MMC_REGISTER_CALLBACKS 0U /* MMC register callback disabled */ +#define USE_HAL_NAND_REGISTER_CALLBACKS 0U /* NAND register callback disabled */ +#define USE_HAL_NOR_REGISTER_CALLBACKS 0U /* NOR register callback disabled */ +#define USE_HAL_PCCARD_REGISTER_CALLBACKS 0U /* PCCARD register callback disabled */ +#define USE_HAL_PCD_REGISTER_CALLBACKS 0U /* PCD register callback disabled */ +#define USE_HAL_QSPI_REGISTER_CALLBACKS 0U /* QSPI register callback disabled */ +#define USE_HAL_RNG_REGISTER_CALLBACKS 0U /* RNG register callback disabled */ +#define USE_HAL_RTC_REGISTER_CALLBACKS 0U /* RTC register callback disabled */ +#define USE_HAL_SAI_REGISTER_CALLBACKS 0U /* SAI register callback disabled */ +#define USE_HAL_SD_REGISTER_CALLBACKS 0U /* SD register callback disabled */ +#define USE_HAL_SMARTCARD_REGISTER_CALLBACKS 0U /* SMARTCARD register callback disabled */ +#define USE_HAL_SDRAM_REGISTER_CALLBACKS 0U /* SDRAM register callback disabled */ +#define USE_HAL_SRAM_REGISTER_CALLBACKS 0U /* SRAM register callback disabled */ +#define USE_HAL_SPDIFRX_REGISTER_CALLBACKS 0U /* SPDIFRX register callback disabled */ +#define USE_HAL_SMBUS_REGISTER_CALLBACKS 0U /* SMBUS register callback disabled */ +#define USE_HAL_SPI_REGISTER_CALLBACKS 0U /* SPI register callback disabled */ +#define USE_HAL_TIM_REGISTER_CALLBACKS 0U /* TIM register callback disabled */ +#define USE_HAL_UART_REGISTER_CALLBACKS 0U /* UART register callback disabled */ +#define USE_HAL_USART_REGISTER_CALLBACKS 0U /* USART register callback disabled */ +#define USE_HAL_WWDG_REGISTER_CALLBACKS 0U /* WWDG register callback disabled */ + +/* ########################## Assert Selection ############################## */ +/** + * @brief Uncomment the line below to expanse the "assert_param" macro in the + * HAL drivers code + */ +/* #define USE_FULL_ASSERT 1U */ + +/* ################## Ethernet peripheral configuration ##################### */ + +/* Section 1 : Ethernet peripheral configuration */ + +/* MAC ADDRESS: MAC_ADDR0:MAC_ADDR1:MAC_ADDR2:MAC_ADDR3:MAC_ADDR4:MAC_ADDR5 */ +#define MAC_ADDR0 2U +#define MAC_ADDR1 0U +#define MAC_ADDR2 0U +#define MAC_ADDR3 0U +#define MAC_ADDR4 0U +#define MAC_ADDR5 0U + +/* Definition of the Ethernet driver buffers size and count */ +#define ETH_RX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for receive */ +#define ETH_TX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for transmit */ +#define ETH_RXBUFNB 4U /* 4 Rx buffers of size ETH_RX_BUF_SIZE */ +#define ETH_TXBUFNB 4U /* 4 Tx buffers of size ETH_TX_BUF_SIZE */ + +/* Section 2: PHY configuration section */ + +/* DP83848_PHY_ADDRESS Address*/ +#define DP83848_PHY_ADDRESS +/* PHY Reset delay these values are based on a 1 ms Systick interrupt*/ +#define PHY_RESET_DELAY 0x000000FFU +/* PHY Configuration delay */ +#define PHY_CONFIG_DELAY 0x00000FFFU + +#define PHY_READ_TO 0x0000FFFFU +#define PHY_WRITE_TO 0x0000FFFFU + +/* Section 3: Common PHY Registers */ + +#define PHY_BCR ((uint16_t)0x0000U) /*!< Transceiver Basic Control Register */ +#define PHY_BSR ((uint16_t)0x0001U) /*!< Transceiver Basic Status Register */ + +#define PHY_RESET ((uint16_t)0x8000U) /*!< PHY Reset */ +#define PHY_LOOPBACK ((uint16_t)0x4000U) /*!< Select loop-back mode */ +#define PHY_FULLDUPLEX_100M ((uint16_t)0x2100U) /*!< Set the full-duplex mode at 100 Mb/s */ +#define PHY_HALFDUPLEX_100M ((uint16_t)0x2000U) /*!< Set the half-duplex mode at 100 Mb/s */ +#define PHY_FULLDUPLEX_10M ((uint16_t)0x0100U) /*!< Set the full-duplex mode at 10 Mb/s */ +#define PHY_HALFDUPLEX_10M ((uint16_t)0x0000U) /*!< Set the half-duplex mode at 10 Mb/s */ +#define PHY_AUTONEGOTIATION ((uint16_t)0x1000U) /*!< Enable auto-negotiation function */ +#define PHY_RESTART_AUTONEGOTIATION ((uint16_t)0x0200U) /*!< Restart auto-negotiation function */ +#define PHY_POWERDOWN ((uint16_t)0x0800U) /*!< Select the power down mode */ +#define PHY_ISOLATE ((uint16_t)0x0400U) /*!< Isolate PHY from MII */ + +#define PHY_AUTONEGO_COMPLETE ((uint16_t)0x0020U) /*!< Auto-Negotiation process completed */ +#define PHY_LINKED_STATUS ((uint16_t)0x0004U) /*!< Valid link established */ +#define PHY_JABBER_DETECTION ((uint16_t)0x0002U) /*!< Jabber condition detected */ + +/* Section 4: Extended PHY Registers */ +#define PHY_SR ((uint16_t)) /*!< PHY status register Offset */ + +#define PHY_SPEED_STATUS ((uint16_t)) /*!< PHY Speed mask */ +#define PHY_DUPLEX_STATUS ((uint16_t)) /*!< PHY Duplex mask */ + +/* ################## SPI peripheral configuration ########################## */ + +/* CRC FEATURE: Use to activate CRC feature inside HAL SPI Driver +* Activated: CRC code is present inside driver +* Deactivated: CRC code cleaned from driver +*/ + +#define USE_SPI_CRC 0U + +/* Includes ------------------------------------------------------------------*/ +/** + * @brief Include module's header file + */ + +#ifdef HAL_RCC_MODULE_ENABLED + #include "stm32f4xx_hal_rcc.h" +#endif /* HAL_RCC_MODULE_ENABLED */ + +#ifdef HAL_GPIO_MODULE_ENABLED + #include "stm32f4xx_hal_gpio.h" +#endif /* HAL_GPIO_MODULE_ENABLED */ + +#ifdef HAL_EXTI_MODULE_ENABLED + #include "stm32f4xx_hal_exti.h" +#endif /* HAL_EXTI_MODULE_ENABLED */ + +#ifdef HAL_DMA_MODULE_ENABLED + #include "stm32f4xx_hal_dma.h" +#endif /* HAL_DMA_MODULE_ENABLED */ + +#ifdef HAL_CORTEX_MODULE_ENABLED + #include "stm32f4xx_hal_cortex.h" +#endif /* HAL_CORTEX_MODULE_ENABLED */ + +#ifdef HAL_ADC_MODULE_ENABLED + #include "stm32f4xx_hal_adc.h" +#endif /* HAL_ADC_MODULE_ENABLED */ + +#ifdef HAL_CAN_MODULE_ENABLED + #include "stm32f4xx_hal_can.h" +#endif /* HAL_CAN_MODULE_ENABLED */ + +#ifdef HAL_CAN_LEGACY_MODULE_ENABLED + #include "stm32f4xx_hal_can_legacy.h" +#endif /* HAL_CAN_LEGACY_MODULE_ENABLED */ + +#ifdef HAL_CRC_MODULE_ENABLED + #include "stm32f4xx_hal_crc.h" +#endif /* HAL_CRC_MODULE_ENABLED */ + +#ifdef HAL_CRYP_MODULE_ENABLED + #include "stm32f4xx_hal_cryp.h" +#endif /* HAL_CRYP_MODULE_ENABLED */ + +#ifdef HAL_DMA2D_MODULE_ENABLED + #include "stm32f4xx_hal_dma2d.h" +#endif /* HAL_DMA2D_MODULE_ENABLED */ + +#ifdef HAL_DAC_MODULE_ENABLED + #include "stm32f4xx_hal_dac.h" +#endif /* HAL_DAC_MODULE_ENABLED */ + +#ifdef HAL_DCMI_MODULE_ENABLED + #include "stm32f4xx_hal_dcmi.h" +#endif /* HAL_DCMI_MODULE_ENABLED */ + +#ifdef HAL_ETH_MODULE_ENABLED + #include "stm32f4xx_hal_eth.h" +#endif /* HAL_ETH_MODULE_ENABLED */ + +#ifdef HAL_ETH_LEGACY_MODULE_ENABLED + #include "stm32f4xx_hal_eth_legacy.h" +#endif /* HAL_ETH_LEGACY_MODULE_ENABLED */ + +#ifdef HAL_FLASH_MODULE_ENABLED + #include "stm32f4xx_hal_flash.h" +#endif /* HAL_FLASH_MODULE_ENABLED */ + +#ifdef HAL_SRAM_MODULE_ENABLED + #include "stm32f4xx_hal_sram.h" +#endif /* HAL_SRAM_MODULE_ENABLED */ + +#ifdef HAL_NOR_MODULE_ENABLED + #include "stm32f4xx_hal_nor.h" +#endif /* HAL_NOR_MODULE_ENABLED */ + +#ifdef HAL_NAND_MODULE_ENABLED + #include "stm32f4xx_hal_nand.h" +#endif /* HAL_NAND_MODULE_ENABLED */ + +#ifdef HAL_PCCARD_MODULE_ENABLED + #include "stm32f4xx_hal_pccard.h" +#endif /* HAL_PCCARD_MODULE_ENABLED */ + +#ifdef HAL_SDRAM_MODULE_ENABLED + #include "stm32f4xx_hal_sdram.h" +#endif /* HAL_SDRAM_MODULE_ENABLED */ + +#ifdef HAL_HASH_MODULE_ENABLED + #include "stm32f4xx_hal_hash.h" +#endif /* HAL_HASH_MODULE_ENABLED */ + +#ifdef HAL_I2C_MODULE_ENABLED + #include "stm32f4xx_hal_i2c.h" +#endif /* HAL_I2C_MODULE_ENABLED */ + +#ifdef HAL_SMBUS_MODULE_ENABLED + #include "stm32f4xx_hal_smbus.h" +#endif /* HAL_SMBUS_MODULE_ENABLED */ + +#ifdef HAL_I2S_MODULE_ENABLED + #include "stm32f4xx_hal_i2s.h" +#endif /* HAL_I2S_MODULE_ENABLED */ + +#ifdef HAL_IWDG_MODULE_ENABLED + #include "stm32f4xx_hal_iwdg.h" +#endif /* HAL_IWDG_MODULE_ENABLED */ + +#ifdef HAL_LTDC_MODULE_ENABLED + #include "stm32f4xx_hal_ltdc.h" +#endif /* HAL_LTDC_MODULE_ENABLED */ + +#ifdef HAL_PWR_MODULE_ENABLED + #include "stm32f4xx_hal_pwr.h" +#endif /* HAL_PWR_MODULE_ENABLED */ + +#ifdef HAL_RNG_MODULE_ENABLED + #include "stm32f4xx_hal_rng.h" +#endif /* HAL_RNG_MODULE_ENABLED */ + +#ifdef HAL_RTC_MODULE_ENABLED + #include "stm32f4xx_hal_rtc.h" +#endif /* HAL_RTC_MODULE_ENABLED */ + +#ifdef HAL_SAI_MODULE_ENABLED + #include "stm32f4xx_hal_sai.h" +#endif /* HAL_SAI_MODULE_ENABLED */ + +#ifdef HAL_SD_MODULE_ENABLED + #include "stm32f4xx_hal_sd.h" +#endif /* HAL_SD_MODULE_ENABLED */ + +#ifdef HAL_SPI_MODULE_ENABLED + #include "stm32f4xx_hal_spi.h" +#endif /* HAL_SPI_MODULE_ENABLED */ + +#ifdef HAL_TIM_MODULE_ENABLED + #include "stm32f4xx_hal_tim.h" +#endif /* HAL_TIM_MODULE_ENABLED */ + +#ifdef HAL_UART_MODULE_ENABLED + #include "stm32f4xx_hal_uart.h" +#endif /* HAL_UART_MODULE_ENABLED */ + +#ifdef HAL_USART_MODULE_ENABLED + #include "stm32f4xx_hal_usart.h" +#endif /* HAL_USART_MODULE_ENABLED */ + +#ifdef HAL_IRDA_MODULE_ENABLED + #include "stm32f4xx_hal_irda.h" +#endif /* HAL_IRDA_MODULE_ENABLED */ + +#ifdef HAL_SMARTCARD_MODULE_ENABLED + #include "stm32f4xx_hal_smartcard.h" +#endif /* HAL_SMARTCARD_MODULE_ENABLED */ + +#ifdef HAL_WWDG_MODULE_ENABLED + #include "stm32f4xx_hal_wwdg.h" +#endif /* HAL_WWDG_MODULE_ENABLED */ + +#ifdef HAL_PCD_MODULE_ENABLED + #include "stm32f4xx_hal_pcd.h" +#endif /* HAL_PCD_MODULE_ENABLED */ + +#ifdef HAL_HCD_MODULE_ENABLED + #include "stm32f4xx_hal_hcd.h" +#endif /* HAL_HCD_MODULE_ENABLED */ + +#ifdef HAL_DSI_MODULE_ENABLED + #include "stm32f4xx_hal_dsi.h" +#endif /* HAL_DSI_MODULE_ENABLED */ + +#ifdef HAL_QSPI_MODULE_ENABLED + #include "stm32f4xx_hal_qspi.h" +#endif /* HAL_QSPI_MODULE_ENABLED */ + +#ifdef HAL_CEC_MODULE_ENABLED + #include "stm32f4xx_hal_cec.h" +#endif /* HAL_CEC_MODULE_ENABLED */ + +#ifdef HAL_FMPI2C_MODULE_ENABLED + #include "stm32f4xx_hal_fmpi2c.h" +#endif /* HAL_FMPI2C_MODULE_ENABLED */ + +#ifdef HAL_FMPSMBUS_MODULE_ENABLED + #include "stm32f4xx_hal_fmpsmbus.h" +#endif /* HAL_FMPSMBUS_MODULE_ENABLED */ + +#ifdef HAL_SPDIFRX_MODULE_ENABLED + #include "stm32f4xx_hal_spdifrx.h" +#endif /* HAL_SPDIFRX_MODULE_ENABLED */ + +#ifdef HAL_DFSDM_MODULE_ENABLED + #include "stm32f4xx_hal_dfsdm.h" +#endif /* HAL_DFSDM_MODULE_ENABLED */ + +#ifdef HAL_LPTIM_MODULE_ENABLED + #include "stm32f4xx_hal_lptim.h" +#endif /* HAL_LPTIM_MODULE_ENABLED */ + +#ifdef HAL_MMC_MODULE_ENABLED + #include "stm32f4xx_hal_mmc.h" +#endif /* HAL_MMC_MODULE_ENABLED */ + +/* Exported macro ------------------------------------------------------------*/ +#ifdef USE_FULL_ASSERT +/** + * @brief The assert_param macro is used for function's parameters check. + * @param expr If expr is false, it calls assert_failed function + * which reports the name of the source file and the source + * line number of the call that failed. + * If expr is true, it returns no value. + * @retval None + */ + #define assert_param(expr) ((expr) ? (void)0U : assert_failed((uint8_t *)__FILE__, __LINE__)) +/* Exported functions ------------------------------------------------------- */ + void assert_failed(uint8_t* file, uint32_t line); +#else + #define assert_param(expr) ((void)0U) +#endif /* USE_FULL_ASSERT */ + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F4xx_HAL_CONF_H */ diff --git a/Core/Inc/stm32f4xx_it.h b/Core/Inc/stm32f4xx_it.h new file mode 100644 index 0000000..7a7a9a9 --- /dev/null +++ b/Core/Inc/stm32f4xx_it.h @@ -0,0 +1,72 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file stm32f4xx_it.h + * @brief This file contains the headers of the interrupt handlers. + ****************************************************************************** + * @attention + * + * Copyright (c) 2026 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ + +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __STM32F4xx_IT_H +#define __STM32F4xx_IT_H + +#ifdef __cplusplus +extern "C" { +#endif + +/* Private includes ----------------------------------------------------------*/ +/* USER CODE BEGIN Includes */ + +/* USER CODE END Includes */ + +/* Exported types ------------------------------------------------------------*/ +/* USER CODE BEGIN ET */ + +/* USER CODE END ET */ + +/* Exported constants --------------------------------------------------------*/ +/* USER CODE BEGIN EC */ + +/* USER CODE END EC */ + +/* Exported macro ------------------------------------------------------------*/ +/* USER CODE BEGIN EM */ + +/* USER CODE END EM */ + +/* Exported functions prototypes ---------------------------------------------*/ +void NMI_Handler(void); +void HardFault_Handler(void); +void MemManage_Handler(void); +void BusFault_Handler(void); +void UsageFault_Handler(void); +void DebugMon_Handler(void); +void SysTick_Handler(void); +void DMA1_Stream1_IRQHandler(void); +void CAN1_TX_IRQHandler(void); +void CAN1_RX0_IRQHandler(void); +void CAN1_RX1_IRQHandler(void); +void TIM5_IRQHandler(void); +void CAN2_TX_IRQHandler(void); +void CAN2_RX0_IRQHandler(void); +void CAN2_RX1_IRQHandler(void); +/* USER CODE BEGIN EFP */ + +/* USER CODE END EFP */ + +#ifdef __cplusplus +} +#endif + +#endif /* __STM32F4xx_IT_H */ diff --git a/Core/Inc/tim.h b/Core/Inc/tim.h new file mode 100644 index 0000000..d044cea --- /dev/null +++ b/Core/Inc/tim.h @@ -0,0 +1,54 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file tim.h + * @brief This file contains all the function prototypes for + * the tim.c file + ****************************************************************************** + * @attention + * + * Copyright (c) 2026 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __TIM_H__ +#define __TIM_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "main.h" + +/* USER CODE BEGIN Includes */ + +/* USER CODE END Includes */ + +extern TIM_HandleTypeDef htim5; + +/* USER CODE BEGIN Private defines */ + +/* USER CODE END Private defines */ + +void MX_TIM5_Init(void); + +void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim); + +/* USER CODE BEGIN Prototypes */ + +/* USER CODE END Prototypes */ + +#ifdef __cplusplus +} +#endif + +#endif /* __TIM_H__ */ + diff --git a/Core/Inc/usart.h b/Core/Inc/usart.h new file mode 100644 index 0000000..2ee625c --- /dev/null +++ b/Core/Inc/usart.h @@ -0,0 +1,52 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file usart.h + * @brief This file contains all the function prototypes for + * the usart.c file + ****************************************************************************** + * @attention + * + * Copyright (c) 2026 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ +/* Define to prevent recursive inclusion -------------------------------------*/ +#ifndef __USART_H__ +#define __USART_H__ + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------*/ +#include "main.h" + +/* USER CODE BEGIN Includes */ + +/* USER CODE END Includes */ + +extern UART_HandleTypeDef huart3; + +/* USER CODE BEGIN Private defines */ + +/* USER CODE END Private defines */ + +void MX_USART3_UART_Init(void); + +/* USER CODE BEGIN Prototypes */ + +/* USER CODE END Prototypes */ + +#ifdef __cplusplus +} +#endif + +#endif /* __USART_H__ */ + diff --git a/Core/Src/can.c b/Core/Src/can.c new file mode 100644 index 0000000..ae2ed97 --- /dev/null +++ b/Core/Src/can.c @@ -0,0 +1,230 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file can.c + * @brief This file provides code for the configuration + * of the CAN instances. + ****************************************************************************** + * @attention + * + * Copyright (c) 2026 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ +/* Includes ------------------------------------------------------------------*/ +#include "can.h" + +/* USER CODE BEGIN 0 */ + +/* USER CODE END 0 */ + +CAN_HandleTypeDef hcan1; +CAN_HandleTypeDef hcan2; + +/* CAN1 init function */ +void MX_CAN1_Init(void) +{ + + /* USER CODE BEGIN CAN1_Init 0 */ + + /* USER CODE END CAN1_Init 0 */ + + /* USER CODE BEGIN CAN1_Init 1 */ + + /* USER CODE END CAN1_Init 1 */ + hcan1.Instance = CAN1; + hcan1.Init.Prescaler = 3; + hcan1.Init.Mode = CAN_MODE_NORMAL; + hcan1.Init.SyncJumpWidth = CAN_SJW_1TQ; + hcan1.Init.TimeSeg1 = CAN_BS1_10TQ; + hcan1.Init.TimeSeg2 = CAN_BS2_3TQ; + hcan1.Init.TimeTriggeredMode = DISABLE; + hcan1.Init.AutoBusOff = DISABLE; + hcan1.Init.AutoWakeUp = DISABLE; + hcan1.Init.AutoRetransmission = ENABLE; + hcan1.Init.ReceiveFifoLocked = DISABLE; + hcan1.Init.TransmitFifoPriority = DISABLE; + if (HAL_CAN_Init(&hcan1) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN CAN1_Init 2 */ + + /* USER CODE END CAN1_Init 2 */ + +} +/* CAN2 init function */ +void MX_CAN2_Init(void) +{ + + /* USER CODE BEGIN CAN2_Init 0 */ + + /* USER CODE END CAN2_Init 0 */ + + /* USER CODE BEGIN CAN2_Init 1 */ + + /* USER CODE END CAN2_Init 1 */ + hcan2.Instance = CAN2; + hcan2.Init.Prescaler = 3; + hcan2.Init.Mode = CAN_MODE_NORMAL; + hcan2.Init.SyncJumpWidth = CAN_SJW_1TQ; + hcan2.Init.TimeSeg1 = CAN_BS1_10TQ; + hcan2.Init.TimeSeg2 = CAN_BS2_3TQ; + hcan2.Init.TimeTriggeredMode = DISABLE; + hcan2.Init.AutoBusOff = DISABLE; + hcan2.Init.AutoWakeUp = DISABLE; + hcan2.Init.AutoRetransmission = ENABLE; + hcan2.Init.ReceiveFifoLocked = DISABLE; + hcan2.Init.TransmitFifoPriority = DISABLE; + if (HAL_CAN_Init(&hcan2) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN CAN2_Init 2 */ + + /* USER CODE END CAN2_Init 2 */ + +} + +static uint32_t HAL_RCC_CAN1_CLK_ENABLED=0; + +void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle) +{ + + GPIO_InitTypeDef GPIO_InitStruct = {0}; + if(canHandle->Instance==CAN1) + { + /* USER CODE BEGIN CAN1_MspInit 0 */ + + /* USER CODE END CAN1_MspInit 0 */ + /* CAN1 clock enable */ + HAL_RCC_CAN1_CLK_ENABLED++; + if(HAL_RCC_CAN1_CLK_ENABLED==1){ + __HAL_RCC_CAN1_CLK_ENABLE(); + } + + __HAL_RCC_GPIOD_CLK_ENABLE(); + /**CAN1 GPIO Configuration + PD0 ------> CAN1_RX + PD1 ------> CAN1_TX + */ + GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF9_CAN1; + HAL_GPIO_Init(GPIOD, &GPIO_InitStruct); + + /* CAN1 interrupt Init */ + HAL_NVIC_SetPriority(CAN1_TX_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(CAN1_TX_IRQn); + HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn); + HAL_NVIC_SetPriority(CAN1_RX1_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(CAN1_RX1_IRQn); + /* USER CODE BEGIN CAN1_MspInit 1 */ + + /* USER CODE END CAN1_MspInit 1 */ + } + else if(canHandle->Instance==CAN2) + { + /* USER CODE BEGIN CAN2_MspInit 0 */ + + /* USER CODE END CAN2_MspInit 0 */ + /* CAN2 clock enable */ + __HAL_RCC_CAN2_CLK_ENABLE(); + HAL_RCC_CAN1_CLK_ENABLED++; + if(HAL_RCC_CAN1_CLK_ENABLED==1){ + __HAL_RCC_CAN1_CLK_ENABLE(); + } + + __HAL_RCC_GPIOB_CLK_ENABLE(); + /**CAN2 GPIO Configuration + PB5 ------> CAN2_RX + PB6 ------> CAN2_TX + */ + GPIO_InitStruct.Pin = GPIO_PIN_5|GPIO_PIN_6; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF9_CAN2; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + + /* CAN2 interrupt Init */ + HAL_NVIC_SetPriority(CAN2_TX_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(CAN2_TX_IRQn); + HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn); + HAL_NVIC_SetPriority(CAN2_RX1_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(CAN2_RX1_IRQn); + /* USER CODE BEGIN CAN2_MspInit 1 */ + + /* USER CODE END CAN2_MspInit 1 */ + } +} + +void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle) +{ + + if(canHandle->Instance==CAN1) + { + /* USER CODE BEGIN CAN1_MspDeInit 0 */ + + /* USER CODE END CAN1_MspDeInit 0 */ + /* Peripheral clock disable */ + HAL_RCC_CAN1_CLK_ENABLED--; + if(HAL_RCC_CAN1_CLK_ENABLED==0){ + __HAL_RCC_CAN1_CLK_DISABLE(); + } + + /**CAN1 GPIO Configuration + PD0 ------> CAN1_RX + PD1 ------> CAN1_TX + */ + HAL_GPIO_DeInit(GPIOD, GPIO_PIN_0|GPIO_PIN_1); + + /* CAN1 interrupt Deinit */ + HAL_NVIC_DisableIRQ(CAN1_TX_IRQn); + HAL_NVIC_DisableIRQ(CAN1_RX0_IRQn); + HAL_NVIC_DisableIRQ(CAN1_RX1_IRQn); + /* USER CODE BEGIN CAN1_MspDeInit 1 */ + + /* USER CODE END CAN1_MspDeInit 1 */ + } + else if(canHandle->Instance==CAN2) + { + /* USER CODE BEGIN CAN2_MspDeInit 0 */ + + /* USER CODE END CAN2_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_CAN2_CLK_DISABLE(); + HAL_RCC_CAN1_CLK_ENABLED--; + if(HAL_RCC_CAN1_CLK_ENABLED==0){ + __HAL_RCC_CAN1_CLK_DISABLE(); + } + + /**CAN2 GPIO Configuration + PB5 ------> CAN2_RX + PB6 ------> CAN2_TX + */ + HAL_GPIO_DeInit(GPIOB, GPIO_PIN_5|GPIO_PIN_6); + + /* CAN2 interrupt Deinit */ + HAL_NVIC_DisableIRQ(CAN2_TX_IRQn); + HAL_NVIC_DisableIRQ(CAN2_RX0_IRQn); + HAL_NVIC_DisableIRQ(CAN2_RX1_IRQn); + /* USER CODE BEGIN CAN2_MspDeInit 1 */ + + /* USER CODE END CAN2_MspDeInit 1 */ + } +} + +/* USER CODE BEGIN 1 */ + +/* USER CODE END 1 */ diff --git a/Core/Src/dma.c b/Core/Src/dma.c new file mode 100644 index 0000000..c58658f --- /dev/null +++ b/Core/Src/dma.c @@ -0,0 +1,55 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file dma.c + * @brief This file provides code for the configuration + * of all the requested memory to memory DMA transfers. + ****************************************************************************** + * @attention + * + * Copyright (c) 2026 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ + +/* Includes ------------------------------------------------------------------*/ +#include "dma.h" + +/* USER CODE BEGIN 0 */ + +/* USER CODE END 0 */ + +/*----------------------------------------------------------------------------*/ +/* Configure DMA */ +/*----------------------------------------------------------------------------*/ + +/* USER CODE BEGIN 1 */ + +/* USER CODE END 1 */ + +/** + * Enable DMA controller clock + */ +void MX_DMA_Init(void) +{ + + /* DMA controller clock enable */ + __HAL_RCC_DMA1_CLK_ENABLE(); + + /* DMA interrupt init */ + /* DMA1_Stream1_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(DMA1_Stream1_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(DMA1_Stream1_IRQn); + +} + +/* USER CODE BEGIN 2 */ + +/* USER CODE END 2 */ + diff --git a/Core/Src/freertos.c b/Core/Src/freertos.c new file mode 100644 index 0000000..a0c04eb --- /dev/null +++ b/Core/Src/freertos.c @@ -0,0 +1,127 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * File Name : freertos.c + * Description : Code for freertos applications + ****************************************************************************** + * @attention + * + * Copyright (c) 2026 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ + +/* Includes ------------------------------------------------------------------*/ +#include "FreeRTOS.h" +#include "task.h" +#include "main.h" +#include "cmsis_os.h" + +/* Private includes ----------------------------------------------------------*/ +/* USER CODE BEGIN Includes */ + +#include "task/user_task.h" +/* USER CODE END Includes */ + +/* Private typedef -----------------------------------------------------------*/ +/* USER CODE BEGIN PTD */ + +/* USER CODE END PTD */ + +/* Private define ------------------------------------------------------------*/ +/* USER CODE BEGIN PD */ + +/* USER CODE END PD */ + +/* Private macro -------------------------------------------------------------*/ +/* USER CODE BEGIN PM */ + +/* USER CODE END PM */ + +/* Private variables ---------------------------------------------------------*/ +/* USER CODE BEGIN Variables */ + +/* USER CODE END Variables */ +/* Definitions for defaultTask */ +osThreadId_t defaultTaskHandle; +const osThreadAttr_t defaultTask_attributes = { + .name = "defaultTask", + .stack_size = 128 * 4, + .priority = (osPriority_t) osPriorityNormal, +}; + +/* Private function prototypes -----------------------------------------------*/ +/* USER CODE BEGIN FunctionPrototypes */ + +/* USER CODE END FunctionPrototypes */ + +void StartDefaultTask(void *argument); + +void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */ + +/** + * @brief FreeRTOS initialization + * @param None + * @retval None + */ +void MX_FREERTOS_Init(void) { + /* USER CODE BEGIN Init */ + + /* USER CODE END Init */ + + /* USER CODE BEGIN RTOS_MUTEX */ + /* add mutexes, ... */ + /* USER CODE END RTOS_MUTEX */ + + /* USER CODE BEGIN RTOS_SEMAPHORES */ + /* add semaphores, ... */ + /* USER CODE END RTOS_SEMAPHORES */ + + /* USER CODE BEGIN RTOS_TIMERS */ + /* start timers, add new ones, ... */ + /* USER CODE END RTOS_TIMERS */ + + /* USER CODE BEGIN RTOS_QUEUES */ + /* add queues, ... */ + /* USER CODE END RTOS_QUEUES */ + + /* Create the thread(s) */ + /* creation of defaultTask */ + defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes); + + /* USER CODE BEGIN RTOS_THREADS */ + /* add threads, ... */ + osThreadNew(Task_Init, NULL, &attr_init); // 创建初始化任务 + /* USER CODE END RTOS_THREADS */ + + /* USER CODE BEGIN RTOS_EVENTS */ + /* add events, ... */ + /* USER CODE END RTOS_EVENTS */ + +} + +/* USER CODE BEGIN Header_StartDefaultTask */ +/** + * @brief Function implementing the defaultTask thread. + * @param argument: Not used + * @retval None + */ +/* USER CODE END Header_StartDefaultTask */ +void StartDefaultTask(void *argument) +{ + /* USER CODE BEGIN StartDefaultTask */ + osThreadTerminate(osThreadGetId()); + /* USER CODE END StartDefaultTask */ +} + +/* Private application code --------------------------------------------------*/ +/* USER CODE BEGIN Application */ + +/* USER CODE END Application */ + diff --git a/Core/Src/gpio.c b/Core/Src/gpio.c new file mode 100644 index 0000000..cddda2b --- /dev/null +++ b/Core/Src/gpio.c @@ -0,0 +1,56 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file gpio.c + * @brief This file provides code for the configuration + * of all used GPIO pins. + ****************************************************************************** + * @attention + * + * Copyright (c) 2026 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ + +/* Includes ------------------------------------------------------------------*/ +#include "gpio.h" + +/* USER CODE BEGIN 0 */ + +/* USER CODE END 0 */ + +/*----------------------------------------------------------------------------*/ +/* Configure GPIO */ +/*----------------------------------------------------------------------------*/ +/* USER CODE BEGIN 1 */ + +/* USER CODE END 1 */ + +/** Configure pins as + * Analog + * Input + * Output + * EVENT_OUT + * EXTI +*/ +void MX_GPIO_Init(void) +{ + + /* GPIO Ports Clock Enable */ + __HAL_RCC_GPIOB_CLK_ENABLE(); + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOD_CLK_ENABLE(); + __HAL_RCC_GPIOC_CLK_ENABLE(); + __HAL_RCC_GPIOH_CLK_ENABLE(); + +} + +/* USER CODE BEGIN 2 */ + +/* USER CODE END 2 */ diff --git a/Core/Src/main.c b/Core/Src/main.c new file mode 100644 index 0000000..c298f08 --- /dev/null +++ b/Core/Src/main.c @@ -0,0 +1,202 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file : main.c + * @brief : Main program body + ****************************************************************************** + * @attention + * + * Copyright (c) 2026 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ +/* Includes ------------------------------------------------------------------*/ +#include "main.h" +#include "cmsis_os.h" +#include "can.h" +#include "dma.h" +#include "tim.h" +#include "usart.h" +#include "gpio.h" + +/* Private includes ----------------------------------------------------------*/ +/* USER CODE BEGIN Includes */ +#include "retarget.h" +#include +/* USER CODE END Includes */ + +/* Private typedef -----------------------------------------------------------*/ +/* USER CODE BEGIN PTD */ + +/* USER CODE END PTD */ + +/* Private define ------------------------------------------------------------*/ +/* USER CODE BEGIN PD */ + +/* USER CODE END PD */ + +/* Private macro -------------------------------------------------------------*/ +/* USER CODE BEGIN PM */ + +/* USER CODE END PM */ + +/* Private variables ---------------------------------------------------------*/ + +/* USER CODE BEGIN PV */ + +/* USER CODE END PV */ + +/* Private function prototypes -----------------------------------------------*/ +void SystemClock_Config(void); +void MX_FREERTOS_Init(void); +/* USER CODE BEGIN PFP */ + +/* USER CODE END PFP */ + +/* Private user code ---------------------------------------------------------*/ +/* USER CODE BEGIN 0 */ + +/* USER CODE END 0 */ + +/** + * @brief The application entry point. + * @retval int + */ +int main(void) +{ + + /* USER CODE BEGIN 1 */ + + /* USER CODE END 1 */ + + /* MCU Configuration--------------------------------------------------------*/ + + /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ + HAL_Init(); + + /* USER CODE BEGIN Init */ + + /* USER CODE END Init */ + + /* Configure the system clock */ + SystemClock_Config(); + + /* USER CODE BEGIN SysInit */ + + /* USER CODE END SysInit */ + + /* Initialize all configured peripherals */ + MX_GPIO_Init(); + MX_DMA_Init(); + MX_CAN1_Init(); + MX_CAN2_Init(); + MX_TIM5_Init(); + MX_USART3_UART_Init(); + /* USER CODE BEGIN 2 */ + /* USER CODE END 2 */ + + /* Init scheduler */ + osKernelInitialize(); /* Call init function for freertos objects (in cmsis_os2.c) */ + MX_FREERTOS_Init(); + + /* Start scheduler */ + osKernelStart(); + + /* We should never get here as control is now taken by the scheduler */ + + /* Infinite loop */ + /* USER CODE BEGIN WHILE */ + while (1) + { + /* USER CODE END WHILE */ + + /* USER CODE BEGIN 3 */ + } + /* USER CODE END 3 */ +} + +/** + * @brief System Clock Configuration + * @retval None + */ +void SystemClock_Config(void) +{ + RCC_OscInitTypeDef RCC_OscInitStruct = {0}; + RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; + + /** Configure the main internal regulator output voltage + */ + __HAL_RCC_PWR_CLK_ENABLE(); + __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); + + /** Initializes the RCC Oscillators according to the specified parameters + * in the RCC_OscInitTypeDef structure. + */ + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; + RCC_OscInitStruct.HSEState = RCC_HSE_ON; + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; + RCC_OscInitStruct.PLL.PLLM = 6; + RCC_OscInitStruct.PLL.PLLN = 168; + RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; + RCC_OscInitStruct.PLL.PLLQ = 4; + if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) + { + Error_Handler(); + } + + /** Initializes the CPU, AHB and APB buses clocks + */ + RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK + |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2; + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4; + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2; + + if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) + { + Error_Handler(); + } +} + +/* USER CODE BEGIN 4 */ + +/* USER CODE END 4 */ + +/** + * @brief This function is executed in case of error occurrence. + * @retval None + */ +void Error_Handler(void) +{ + /* USER CODE BEGIN Error_Handler_Debug */ + /* User can add his own implementation to report the HAL error return state */ + __disable_irq(); + while (1) + { + } + /* USER CODE END Error_Handler_Debug */ +} +#ifdef USE_FULL_ASSERT +/** + * @brief Reports the name of the source file and the source line number + * where the assert_param error has occurred. + * @param file: pointer to the source file name + * @param line: assert_param error line source number + * @retval None + */ +void assert_failed(uint8_t *file, uint32_t line) +{ + /* USER CODE BEGIN 6 */ + /* User can add his own implementation to report the file name and line number, + ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */ + /* USER CODE END 6 */ +} +#endif /* USE_FULL_ASSERT */ diff --git a/Core/Src/retarget.c b/Core/Src/retarget.c new file mode 100644 index 0000000..402a800 --- /dev/null +++ b/Core/Src/retarget.c @@ -0,0 +1,111 @@ +/** + ****************************************************************************** + * @file retarget.c + * @brief Printf 重定向实现（支持 Ozone SWO/ITM 调试） + * @note 提供多种 printf 输出方式 + ****************************************************************************** + */ + +#include "stm32f4xx.h" +#include + +/* 配置选择：根据需求选择一种方式 */ +#define USE_ITM_SWO 1 // 使用 ITM/SWO (Ozone Terminal 查看) +#define USE_UART 0 // 使用 UART 串口 +#define USE_SEMIHOSTING 0 // 使用半主机模式 + +/* ========== 方案1: ITM/SWO 输出（Ozone 原生支持）========== */ +#if USE_ITM_SWO + +/** + * @brief 初始化 ITM（必须在 main 开始时调用） + * @note 在 main() 函数的 USER CODE BEGIN 2 中调用此函数 + */ +void ITM_Init(void) +{ + // 1. 启用 TRCENA (Trace Enable) + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + + // 2. 解锁 ITM 访问 + ITM->LAR = 0xC5ACCE55; + + // 3. 启用 ITM + ITM->TCR = ITM_TCR_ITMENA_Msk; + + // 4. 启用 ITM 端口0（用于 printf） + ITM->TER = 0x01; + + // 5. 设置 ITM 的 Trace Privilege + ITM->TPR = 0x00; +} + +/** + * @brief 通过 ITM 端口0 发送字符 + * @note Ozone 中需要配置：Tools -> Terminal -> Enable SWO + */ +int __io_putchar(int ch) +{ + // 直接发送，不检查（因为已经初始化过了） + while (ITM->PORT[0].u32 == 0UL) + { + __NOP(); // 等待端口就绪 + } + ITM->PORT[0].u8 = (uint8_t)ch; + + return ch; +} + +#endif // USE_ITM_SWO + + +/* ========== 方案2: UART 串口输出 ========== */ +#if USE_UART + +#include "usart.h" // 根据你的项目调整头文件 + +/** + * @brief 通过 UART 发送字符 + * @note 需要先在 CubeMX 中配置好 UART + */ +int __io_putchar(int ch) +{ + // 假设使用 UART1，根据实际情况修改 + HAL_UART_Transmit(&huart1, (uint8_t *)&ch, 1, HAL_MAX_DELAY); + return ch; +} + +#endif // USE_UART + + +/* ========== 方案3: 半主机模式（Semihosting）========== */ +#if USE_SEMIHOSTING + +/** + * @brief 半主机模式输出 + * @note 需要在链接选项中添加 --specs=rdimon.specs + * 并在 main 函数前调用 initialise_monitor_handles(); + */ +// 半主机模式使用系统默认实现，无需额外代码 +// 但需要在 main() 中调用： +// extern void initialise_monitor_handles(void); +// initialise_monitor_handles(); + +#endif // USE_SEMIHOSTING + + +/* ========== getchar 实现（可选）========== */ +#if USE_ITM_SWO || USE_UART + +int __io_getchar(void) +{ + // 简单实现：返回 EOF + return EOF; + + /* 如果需要从 UART 读取，可以这样实现： + uint8_t ch; + HAL_UART_Receive(&huart1, &ch, 1, HAL_MAX_DELAY); + return ch; + */ +} + +#endif diff --git a/Core/Src/stm32f4xx_hal_msp.c b/Core/Src/stm32f4xx_hal_msp.c new file mode 100644 index 0000000..a4e2b79 --- /dev/null +++ b/Core/Src/stm32f4xx_hal_msp.c @@ -0,0 +1,84 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file stm32f4xx_hal_msp.c + * @brief This file provides code for the MSP Initialization + * and de-Initialization codes. + ****************************************************************************** + * @attention + * + * Copyright (c) 2026 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ + +/* Includes ------------------------------------------------------------------*/ +#include "main.h" +/* USER CODE BEGIN Includes */ + +/* USER CODE END Includes */ + +/* Private typedef -----------------------------------------------------------*/ +/* USER CODE BEGIN TD */ + +/* USER CODE END TD */ + +/* Private define ------------------------------------------------------------*/ +/* USER CODE BEGIN Define */ + +/* USER CODE END Define */ + +/* Private macro -------------------------------------------------------------*/ +/* USER CODE BEGIN Macro */ + +/* USER CODE END Macro */ + +/* Private variables ---------------------------------------------------------*/ +/* USER CODE BEGIN PV */ + +/* USER CODE END PV */ + +/* Private function prototypes -----------------------------------------------*/ +/* USER CODE BEGIN PFP */ + +/* USER CODE END PFP */ + +/* External functions --------------------------------------------------------*/ +/* USER CODE BEGIN ExternalFunctions */ + +/* USER CODE END ExternalFunctions */ + +/* USER CODE BEGIN 0 */ + +/* USER CODE END 0 */ +/** + * Initializes the Global MSP. + */ +void HAL_MspInit(void) +{ + + /* USER CODE BEGIN MspInit 0 */ + + /* USER CODE END MspInit 0 */ + + __HAL_RCC_SYSCFG_CLK_ENABLE(); + __HAL_RCC_PWR_CLK_ENABLE(); + + /* System interrupt init*/ + /* PendSV_IRQn interrupt configuration */ + HAL_NVIC_SetPriority(PendSV_IRQn, 15, 0); + + /* USER CODE BEGIN MspInit 1 */ + + /* USER CODE END MspInit 1 */ +} + +/* USER CODE BEGIN 1 */ + +/* USER CODE END 1 */ diff --git a/Core/Src/stm32f4xx_it.c b/Core/Src/stm32f4xx_it.c new file mode 100644 index 0000000..8520cfb --- /dev/null +++ b/Core/Src/stm32f4xx_it.c @@ -0,0 +1,302 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file stm32f4xx_it.c + * @brief Interrupt Service Routines. + ****************************************************************************** + * @attention + * + * Copyright (c) 2026 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ + +/* Includes ------------------------------------------------------------------*/ +#include "main.h" +#include "stm32f4xx_it.h" +#include "FreeRTOS.h" +#include "task.h" +/* Private includes ----------------------------------------------------------*/ +/* USER CODE BEGIN Includes */ +/* USER CODE END Includes */ + +/* Private typedef -----------------------------------------------------------*/ +/* USER CODE BEGIN TD */ + +/* USER CODE END TD */ + +/* Private define ------------------------------------------------------------*/ +/* USER CODE BEGIN PD */ + +/* USER CODE END PD */ + +/* Private macro -------------------------------------------------------------*/ +/* USER CODE BEGIN PM */ + +/* USER CODE END PM */ + +/* Private variables ---------------------------------------------------------*/ +/* USER CODE BEGIN PV */ + +/* USER CODE END PV */ + +/* Private function prototypes -----------------------------------------------*/ +/* USER CODE BEGIN PFP */ + +/* USER CODE END PFP */ + +/* Private user code ---------------------------------------------------------*/ +/* USER CODE BEGIN 0 */ + +/* USER CODE END 0 */ + +/* External variables --------------------------------------------------------*/ +extern CAN_HandleTypeDef hcan1; +extern CAN_HandleTypeDef hcan2; +extern TIM_HandleTypeDef htim5; +extern DMA_HandleTypeDef hdma_usart3_rx; +/* USER CODE BEGIN EV */ + +/* USER CODE END EV */ + +/******************************************************************************/ +/* Cortex-M4 Processor Interruption and Exception Handlers */ +/******************************************************************************/ +/** + * @brief This function handles Non maskable interrupt. + */ +void NMI_Handler(void) +{ + /* USER CODE BEGIN NonMaskableInt_IRQn 0 */ + + /* USER CODE END NonMaskableInt_IRQn 0 */ + /* USER CODE BEGIN NonMaskableInt_IRQn 1 */ + while (1) + { + } + /* USER CODE END NonMaskableInt_IRQn 1 */ +} + +/** + * @brief This function handles Hard fault interrupt. + */ +void HardFault_Handler(void) +{ + /* USER CODE BEGIN HardFault_IRQn 0 */ + + /* USER CODE END HardFault_IRQn 0 */ + while (1) + { + /* USER CODE BEGIN W1_HardFault_IRQn 0 */ + /* USER CODE END W1_HardFault_IRQn 0 */ + } +} + +/** + * @brief This function handles Memory management fault. + */ +void MemManage_Handler(void) +{ + /* USER CODE BEGIN MemoryManagement_IRQn 0 */ + + /* USER CODE END MemoryManagement_IRQn 0 */ + while (1) + { + /* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */ + /* USER CODE END W1_MemoryManagement_IRQn 0 */ + } +} + +/** + * @brief This function handles Pre-fetch fault, memory access fault. + */ +void BusFault_Handler(void) +{ + /* USER CODE BEGIN BusFault_IRQn 0 */ + + /* USER CODE END BusFault_IRQn 0 */ + while (1) + { + /* USER CODE BEGIN W1_BusFault_IRQn 0 */ + /* USER CODE END W1_BusFault_IRQn 0 */ + } +} + +/** + * @brief This function handles Undefined instruction or illegal state. + */ +void UsageFault_Handler(void) +{ + /* USER CODE BEGIN UsageFault_IRQn 0 */ + + /* USER CODE END UsageFault_IRQn 0 */ + while (1) + { + /* USER CODE BEGIN W1_UsageFault_IRQn 0 */ + /* USER CODE END W1_UsageFault_IRQn 0 */ + } +} + +/** + * @brief This function handles Debug monitor. + */ +void DebugMon_Handler(void) +{ + /* USER CODE BEGIN DebugMonitor_IRQn 0 */ + + /* USER CODE END DebugMonitor_IRQn 0 */ + /* USER CODE BEGIN DebugMonitor_IRQn 1 */ + + /* USER CODE END DebugMonitor_IRQn 1 */ +} + +/** + * @brief This function handles System tick timer. + */ +void SysTick_Handler(void) +{ + /* USER CODE BEGIN SysTick_IRQn 0 */ + + /* USER CODE END SysTick_IRQn 0 */ + HAL_IncTick(); +#if (INCLUDE_xTaskGetSchedulerState == 1 ) + if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) + { +#endif /* INCLUDE_xTaskGetSchedulerState */ + xPortSysTickHandler(); +#if (INCLUDE_xTaskGetSchedulerState == 1 ) + } +#endif /* INCLUDE_xTaskGetSchedulerState */ + /* USER CODE BEGIN SysTick_IRQn 1 */ + + /* USER CODE END SysTick_IRQn 1 */ +} + +/******************************************************************************/ +/* STM32F4xx Peripheral Interrupt Handlers */ +/* Add here the Interrupt Handlers for the used peripherals. */ +/* For the available peripheral interrupt handler names, */ +/* please refer to the startup file (startup_stm32f4xx.s). */ +/******************************************************************************/ + +/** + * @brief This function handles DMA1 stream1 global interrupt. + */ +void DMA1_Stream1_IRQHandler(void) +{ + /* USER CODE BEGIN DMA1_Stream1_IRQn 0 */ + + /* USER CODE END DMA1_Stream1_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_usart3_rx); + /* USER CODE BEGIN DMA1_Stream1_IRQn 1 */ + + /* USER CODE END DMA1_Stream1_IRQn 1 */ +} + +/** + * @brief This function handles CAN1 TX interrupts. + */ +void CAN1_TX_IRQHandler(void) +{ + /* USER CODE BEGIN CAN1_TX_IRQn 0 */ + + /* USER CODE END CAN1_TX_IRQn 0 */ + HAL_CAN_IRQHandler(&hcan1); + /* USER CODE BEGIN CAN1_TX_IRQn 1 */ + + /* USER CODE END CAN1_TX_IRQn 1 */ +} + +/** + * @brief This function handles CAN1 RX0 interrupts. + */ +void CAN1_RX0_IRQHandler(void) +{ + /* USER CODE BEGIN CAN1_RX0_IRQn 0 */ + + /* USER CODE END CAN1_RX0_IRQn 0 */ + HAL_CAN_IRQHandler(&hcan1); + /* USER CODE BEGIN CAN1_RX0_IRQn 1 */ + + /* USER CODE END CAN1_RX0_IRQn 1 */ +} + +/** + * @brief This function handles CAN1 RX1 interrupt. + */ +void CAN1_RX1_IRQHandler(void) +{ + /* USER CODE BEGIN CAN1_RX1_IRQn 0 */ + + /* USER CODE END CAN1_RX1_IRQn 0 */ + HAL_CAN_IRQHandler(&hcan1); + /* USER CODE BEGIN CAN1_RX1_IRQn 1 */ + + /* USER CODE END CAN1_RX1_IRQn 1 */ +} + +/** + * @brief This function handles TIM5 global interrupt. + */ +void TIM5_IRQHandler(void) +{ + /* USER CODE BEGIN TIM5_IRQn 0 */ + + /* USER CODE END TIM5_IRQn 0 */ + HAL_TIM_IRQHandler(&htim5); + /* USER CODE BEGIN TIM5_IRQn 1 */ + + /* USER CODE END TIM5_IRQn 1 */ +} + +/** + * @brief This function handles CAN2 TX interrupts. + */ +void CAN2_TX_IRQHandler(void) +{ + /* USER CODE BEGIN CAN2_TX_IRQn 0 */ + + /* USER CODE END CAN2_TX_IRQn 0 */ + HAL_CAN_IRQHandler(&hcan2); + /* USER CODE BEGIN CAN2_TX_IRQn 1 */ + + /* USER CODE END CAN2_TX_IRQn 1 */ +} + +/** + * @brief This function handles CAN2 RX0 interrupts. + */ +void CAN2_RX0_IRQHandler(void) +{ + /* USER CODE BEGIN CAN2_RX0_IRQn 0 */ + + /* USER CODE END CAN2_RX0_IRQn 0 */ + HAL_CAN_IRQHandler(&hcan2); + /* USER CODE BEGIN CAN2_RX0_IRQn 1 */ + + /* USER CODE END CAN2_RX0_IRQn 1 */ +} + +/** + * @brief This function handles CAN2 RX1 interrupt. + */ +void CAN2_RX1_IRQHandler(void) +{ + /* USER CODE BEGIN CAN2_RX1_IRQn 0 */ + + /* USER CODE END CAN2_RX1_IRQn 0 */ + HAL_CAN_IRQHandler(&hcan2); + /* USER CODE BEGIN CAN2_RX1_IRQn 1 */ + + /* USER CODE END CAN2_RX1_IRQn 1 */ +} + +/* USER CODE BEGIN 1 */ + +/* USER CODE END 1 */ diff --git a/Core/Src/syscalls.c b/Core/Src/syscalls.c new file mode 100644 index 0000000..e10d76f --- /dev/null +++ b/Core/Src/syscalls.c @@ -0,0 +1,244 @@ +/** + ****************************************************************************** + * @file syscalls.c + * @author Auto-generated by STM32CubeMX + * @brief Minimal System calls file + * + * For more information about which c-functions + * need which of these lowlevel functions + * please consult the Newlib or Picolibc libc-manual + ****************************************************************************** + * @attention + * + * Copyright (c) 2020-2025 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ + +/* Includes */ +#include +#include +#include +#include +#include +#include +#include +#include + + +/* Variables */ +extern int __io_putchar(int ch) __attribute__((weak)); +extern int __io_getchar(void) __attribute__((weak)); + + +char *__env[1] = { 0 }; +char **environ = __env; + + +/* Functions */ +void initialise_monitor_handles() +{ +} + +int _getpid(void) +{ + return 1; +} + +int _kill(int pid, int sig) +{ + (void)pid; + (void)sig; + errno = EINVAL; + return -1; +} + +void _exit (int status) +{ + _kill(status, -1); + while (1) {} /* Make sure we hang here */ +} + +__attribute__((weak)) int _read(int file, char *ptr, int len) +{ + (void)file; + int DataIdx; + + for (DataIdx = 0; DataIdx < len; DataIdx++) + { + *ptr++ = __io_getchar(); + } + + return len; +} + +__attribute__((weak)) int _write(int file, char *ptr, int len) +{ + (void)file; + int DataIdx; + + for (DataIdx = 0; DataIdx < len; DataIdx++) + { + __io_putchar(*ptr++); + } + return len; +} + +int _close(int file) +{ + (void)file; + return -1; +} + + +int _fstat(int file, struct stat *st) +{ + (void)file; + st->st_mode = S_IFCHR; + return 0; +} + +int _isatty(int file) +{ + (void)file; + return 1; +} + +int _lseek(int file, int ptr, int dir) +{ + (void)file; + (void)ptr; + (void)dir; + return 0; +} + +int _open(char *path, int flags, ...) +{ + (void)path; + (void)flags; + /* Pretend like we always fail */ + return -1; +} + +int _wait(int *status) +{ + (void)status; + errno = ECHILD; + return -1; +} + +int _unlink(char *name) +{ + (void)name; + errno = ENOENT; + return -1; +} + +clock_t _times(struct tms *buf) +{ + (void)buf; + return -1; +} + +int _stat(const char *file, struct stat *st) +{ + (void)file; + st->st_mode = S_IFCHR; + return 0; +} + +int _link(char *old, char *new) +{ + (void)old; + (void)new; + errno = EMLINK; + return -1; +} + +int _fork(void) +{ + errno = EAGAIN; + return -1; +} + +int _execve(char *name, char **argv, char **env) +{ + (void)name; + (void)argv; + (void)env; + errno = ENOMEM; + return -1; +} + +// --- Picolibc Specific Section --- +#if defined(__PICOLIBC__) + +/** + * @brief Picolibc helper function to output a character to a FILE stream. + * This redirects the output to the low-level __io_putchar function. + * @param c Character to write. + * @param file FILE stream pointer (ignored). + * @retval int The character written. + */ +static int starm_putc(char c, FILE *file) +{ + (void) file; + __io_putchar(c); + return c; +} + +/** + * @brief Picolibc helper function to input a character from a FILE stream. + * This redirects the input from the low-level __io_getchar function. + * @param file FILE stream pointer (ignored). + * @retval int The character read, cast to an unsigned char then int. + */ +static int starm_getc(FILE *file) +{ + unsigned char c; + (void) file; + c = __io_getchar(); + return c; +} + +// Define and initialize the standard I/O streams for Picolibc. +// FDEV_SETUP_STREAM connects the starm_putc and starm_getc helper functions to a FILE structure. +// _FDEV_SETUP_RW indicates the stream is for reading and writing. +static FILE __stdio = FDEV_SETUP_STREAM(starm_putc, + starm_getc, + NULL, + _FDEV_SETUP_RW); + +// Assign the standard stream pointers (stdin, stdout, stderr) to the initialized stream. +// Picolibc uses these pointers for standard I/O operations (printf, scanf, etc.). +FILE *const stdin = &__stdio; +__strong_reference(stdin, stdout); +__strong_reference(stdin, stderr); + +// Create strong aliases mapping standard C library function names (without underscore) +// to the implemented system call stubs (with underscore). Picolibc uses these +// standard names internally, so this linking is required. +__strong_reference(_read, read); +__strong_reference(_write, write); +__strong_reference(_times, times); +__strong_reference(_execve, execve); +__strong_reference(_fork, fork); +__strong_reference(_link, link); +__strong_reference(_unlink, unlink); +__strong_reference(_stat, stat); +__strong_reference(_wait, wait); +__strong_reference(_open, open); +__strong_reference(_close, close); +__strong_reference(_lseek, lseek); +__strong_reference(_isatty, isatty); +__strong_reference(_fstat, fstat); +__strong_reference(_exit, exit); +__strong_reference(_kill, kill); +__strong_reference(_getpid, getpid); + +#endif //__PICOLIBC__ diff --git a/Core/Src/sysmem.c b/Core/Src/sysmem.c new file mode 100644 index 0000000..a875d42 --- /dev/null +++ b/Core/Src/sysmem.c @@ -0,0 +1,87 @@ +/** + ****************************************************************************** + * @file sysmem.c + * @author Generated by STM32CubeMX + * @brief System Memory calls file + * + * For more information about which C functions + * need which of these lowlevel functions + * please consult the Newlib or Picolibc libc manual + ****************************************************************************** + * @attention + * + * Copyright (c) 2025 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ + +/* Includes */ +#include +#include +#include + +/** + * Pointer to the current high watermark of the heap usage + */ +static uint8_t *__sbrk_heap_end = NULL; + +/** + * @brief _sbrk() allocates memory to the newlib heap and is used by malloc + * and others from the C library + * + * @verbatim + * ############################################################################ + * # .data # .bss # newlib heap # MSP stack # + * # # # # Reserved by _Min_Stack_Size # + * ############################################################################ + * ^-- RAM start ^-- _end _estack, RAM end --^ + * @endverbatim + * + * This implementation starts allocating at the '_end' linker symbol + * The '_Min_Stack_Size' linker symbol reserves a memory for the MSP stack + * The implementation considers '_estack' linker symbol to be RAM end + * NOTE: If the MSP stack, at any point during execution, grows larger than the + * reserved size, please increase the '_Min_Stack_Size'. + * + * @param incr Memory size + * @return Pointer to allocated memory + */ +void *_sbrk(ptrdiff_t incr) +{ + extern uint8_t _end; /* Symbol defined in the linker script */ + extern uint8_t _estack; /* Symbol defined in the linker script */ + extern uint32_t _Min_Stack_Size; /* Symbol defined in the linker script */ + const uint32_t stack_limit = (uint32_t)&_estack - (uint32_t)&_Min_Stack_Size; + const uint8_t *max_heap = (uint8_t *)stack_limit; + uint8_t *prev_heap_end; + + /* Initialize heap end at first call */ + if (NULL == __sbrk_heap_end) + { + __sbrk_heap_end = &_end; + } + + /* Protect heap from growing into the reserved MSP stack */ + if (__sbrk_heap_end + incr > max_heap) + { + errno = ENOMEM; + return (void *)-1; + } + + prev_heap_end = __sbrk_heap_end; + __sbrk_heap_end += incr; + + return (void *)prev_heap_end; +} + +#if defined(__PICOLIBC__) + // Picolibc expects syscalls without the leading underscore. + // This creates a strong alias so that + // calls to `sbrk()` are resolved to our `_sbrk()` implementation. + __strong_reference(_sbrk, sbrk); +#endif diff --git a/Core/Src/system_stm32f4xx.c b/Core/Src/system_stm32f4xx.c new file mode 100644 index 0000000..7a61e9c --- /dev/null +++ b/Core/Src/system_stm32f4xx.c @@ -0,0 +1,747 @@ +/** + ****************************************************************************** + * @file system_stm32f4xx.c + * @author MCD Application Team + * @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File. + * + * This file provides two functions and one global variable to be called from + * user application: + * - SystemInit(): This function is called at startup just after reset and + * before branch to main program. This call is made inside + * the "startup_stm32f4xx.s" file. + * + * - SystemCoreClock variable: Contains the core clock (HCLK), it can be used + * by the user application to setup the SysTick + * timer or configure other parameters. + * + * - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must + * be called whenever the core clock is changed + * during program execution. + * + * + ****************************************************************************** + * @attention + * + * Copyright (c) 2017 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ + +/** @addtogroup CMSIS + * @{ + */ + +/** @addtogroup stm32f4xx_system + * @{ + */ + +/** @addtogroup STM32F4xx_System_Private_Includes + * @{ + */ + + +#include "stm32f4xx.h" + +#if !defined (HSE_VALUE) + #define HSE_VALUE ((uint32_t)25000000) /*!< Default value of the External oscillator in Hz */ +#endif /* HSE_VALUE */ + +#if !defined (HSI_VALUE) + #define HSI_VALUE ((uint32_t)16000000) /*!< Value of the Internal oscillator in Hz*/ +#endif /* HSI_VALUE */ + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Private_TypesDefinitions + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Private_Defines + * @{ + */ + +/************************* Miscellaneous Configuration ************************/ +/*!< Uncomment the following line if you need to use external SRAM or SDRAM as data memory */ +#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx) || defined(STM32F417xx)\ + || defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\ + || defined(STM32F469xx) || defined(STM32F479xx) || defined(STM32F412Zx) || defined(STM32F412Vx) +/* #define DATA_IN_ExtSRAM */ +#endif /* STM32F40xxx || STM32F41xxx || STM32F42xxx || STM32F43xxx || STM32F469xx || STM32F479xx ||\ + STM32F412Zx || STM32F412Vx */ + +#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\ + || defined(STM32F446xx) || defined(STM32F469xx) || defined(STM32F479xx) +/* #define DATA_IN_ExtSDRAM */ +#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F446xx || STM32F469xx ||\ + STM32F479xx */ + +/* Note: Following vector table addresses must be defined in line with linker + configuration. */ +/*!< Uncomment the following line if you need to relocate the vector table + anywhere in Flash or Sram, else the vector table is kept at the automatic + remap of boot address selected */ +/* #define USER_VECT_TAB_ADDRESS */ + +#if defined(USER_VECT_TAB_ADDRESS) +/*!< Uncomment the following line if you need to relocate your vector Table + in Sram else user remap will be done in Flash. */ +/* #define VECT_TAB_SRAM */ +#if defined(VECT_TAB_SRAM) +#define VECT_TAB_BASE_ADDRESS SRAM_BASE /*!< Vector Table base address field. + This value must be a multiple of 0x200. */ +#else +#define VECT_TAB_BASE_ADDRESS FLASH_BASE /*!< Vector Table base address field. + This value must be a multiple of 0x200. */ +#endif /* VECT_TAB_SRAM */ +#if !defined(VECT_TAB_OFFSET) +#define VECT_TAB_OFFSET 0x00000000U /*!< Vector Table offset field. + This value must be a multiple of 0x200. */ +#endif /* VECT_TAB_OFFSET */ +#endif /* USER_VECT_TAB_ADDRESS */ +/******************************************************************************/ + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Private_Macros + * @{ + */ + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Private_Variables + * @{ + */ + /* This variable is updated in three ways: + 1) by calling CMSIS function SystemCoreClockUpdate() + 2) by calling HAL API function HAL_RCC_GetHCLKFreq() + 3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency + Note: If you use this function to configure the system clock; then there + is no need to call the 2 first functions listed above, since SystemCoreClock + variable is updated automatically. + */ +uint32_t SystemCoreClock = 16000000; +const uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9}; +const uint8_t APBPrescTable[8] = {0, 0, 0, 0, 1, 2, 3, 4}; +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Private_FunctionPrototypes + * @{ + */ + +#if defined (DATA_IN_ExtSRAM) || defined (DATA_IN_ExtSDRAM) + static void SystemInit_ExtMemCtl(void); +#endif /* DATA_IN_ExtSRAM || DATA_IN_ExtSDRAM */ + +/** + * @} + */ + +/** @addtogroup STM32F4xx_System_Private_Functions + * @{ + */ + +/** + * @brief Setup the microcontroller system + * Initialize the FPU setting, vector table location and External memory + * configuration. + * @param None + * @retval None + */ +void SystemInit(void) +{ + /* FPU settings ------------------------------------------------------------*/ + #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) + SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */ + #endif + +#if defined (DATA_IN_ExtSRAM) || defined (DATA_IN_ExtSDRAM) + SystemInit_ExtMemCtl(); +#endif /* DATA_IN_ExtSRAM || DATA_IN_ExtSDRAM */ + + /* Configure the Vector Table location -------------------------------------*/ +#if defined(USER_VECT_TAB_ADDRESS) + SCB->VTOR = VECT_TAB_BASE_ADDRESS | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */ +#endif /* USER_VECT_TAB_ADDRESS */ +} + +/** + * @brief Update SystemCoreClock variable according to Clock Register Values. + * The SystemCoreClock variable contains the core clock (HCLK), it can + * be used by the user application to setup the SysTick timer or configure + * other parameters. + * + * @note Each time the core clock (HCLK) changes, this function must be called + * to update SystemCoreClock variable value. Otherwise, any configuration + * based on this variable will be incorrect. + * + * @note - The system frequency computed by this function is not the real + * frequency in the chip. It is calculated based on the predefined + * constant and the selected clock source: + * + * - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*) + * + * - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**) + * + * - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**) + * or HSI_VALUE(*) multiplied/divided by the PLL factors. + * + * (*) HSI_VALUE is a constant defined in stm32f4xx_hal_conf.h file (default value + * 16 MHz) but the real value may vary depending on the variations + * in voltage and temperature. + * + * (**) HSE_VALUE is a constant defined in stm32f4xx_hal_conf.h file (its value + * depends on the application requirements), user has to ensure that HSE_VALUE + * is same as the real frequency of the crystal used. Otherwise, this function + * may have wrong result. + * + * - The result of this function could be not correct when using fractional + * value for HSE crystal. + * + * @param None + * @retval None + */ +void SystemCoreClockUpdate(void) +{ + uint32_t tmp, pllvco, pllp, pllsource, pllm; + + /* Get SYSCLK source -------------------------------------------------------*/ + tmp = RCC->CFGR & RCC_CFGR_SWS; + + switch (tmp) + { + case 0x00: /* HSI used as system clock source */ + SystemCoreClock = HSI_VALUE; + break; + case 0x04: /* HSE used as system clock source */ + SystemCoreClock = HSE_VALUE; + break; + case 0x08: /* PLL used as system clock source */ + + /* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N + SYSCLK = PLL_VCO / PLL_P + */ + pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22; + pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM; + + if (pllsource != 0) + { + /* HSE used as PLL clock source */ + pllvco = (HSE_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); + } + else + { + /* HSI used as PLL clock source */ + pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6); + } + + pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2; + SystemCoreClock = pllvco/pllp; + break; + default: + SystemCoreClock = HSI_VALUE; + break; + } + /* Compute HCLK frequency --------------------------------------------------*/ + /* Get HCLK prescaler */ + tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)]; + /* HCLK frequency */ + SystemCoreClock >>= tmp; +} + +#if defined (DATA_IN_ExtSRAM) && defined (DATA_IN_ExtSDRAM) +#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\ + || defined(STM32F469xx) || defined(STM32F479xx) +/** + * @brief Setup the external memory controller. + * Called in startup_stm32f4xx.s before jump to main. + * This function configures the external memories (SRAM/SDRAM) + * This SRAM/SDRAM will be used as program data memory (including heap and stack). + * @param None + * @retval None + */ +void SystemInit_ExtMemCtl(void) +{ + __IO uint32_t tmp = 0x00; + + register uint32_t tmpreg = 0, timeout = 0xFFFF; + register __IO uint32_t index; + + /* Enable GPIOC, GPIOD, GPIOE, GPIOF, GPIOG, GPIOH and GPIOI interface clock */ + RCC->AHB1ENR |= 0x000001F8; + + /* Delay after an RCC peripheral clock enabling */ + tmp = READ_BIT(RCC->AHB1ENR, RCC_AHB1ENR_GPIOCEN); + + /* Connect PDx pins to FMC Alternate function */ + GPIOD->AFR[0] = 0x00CCC0CC; + GPIOD->AFR[1] = 0xCCCCCCCC; + /* Configure PDx pins in Alternate function mode */ + GPIOD->MODER = 0xAAAA0A8A; + /* Configure PDx pins speed to 100 MHz */ + GPIOD->OSPEEDR = 0xFFFF0FCF; + /* Configure PDx pins Output type to push-pull */ + GPIOD->OTYPER = 0x00000000; + /* No pull-up, pull-down for PDx pins */ + GPIOD->PUPDR = 0x00000000; + + /* Connect PEx pins to FMC Alternate function */ + GPIOE->AFR[0] = 0xC00CC0CC; + GPIOE->AFR[1] = 0xCCCCCCCC; + /* Configure PEx pins in Alternate function mode */ + GPIOE->MODER = 0xAAAA828A; + /* Configure PEx pins speed to 100 MHz */ + GPIOE->OSPEEDR = 0xFFFFC3CF; + /* Configure PEx pins Output type to push-pull */ + GPIOE->OTYPER = 0x00000000; + /* No pull-up, pull-down for PEx pins */ + GPIOE->PUPDR = 0x00000000; + + /* Connect PFx pins to FMC Alternate function */ + GPIOF->AFR[0] = 0xCCCCCCCC; + GPIOF->AFR[1] = 0xCCCCCCCC; + /* Configure PFx pins in Alternate function mode */ + GPIOF->MODER = 0xAA800AAA; + /* Configure PFx pins speed to 50 MHz */ + GPIOF->OSPEEDR = 0xAA800AAA; + /* Configure PFx pins Output type to push-pull */ + GPIOF->OTYPER = 0x00000000; + /* No pull-up, pull-down for PFx pins */ + GPIOF->PUPDR = 0x00000000; + + /* Connect PGx pins to FMC Alternate function */ + GPIOG->AFR[0] = 0xCCCCCCCC; + GPIOG->AFR[1] = 0xCCCCCCCC; + /* Configure PGx pins in Alternate function mode */ + GPIOG->MODER = 0xAAAAAAAA; + /* Configure PGx pins speed to 50 MHz */ + GPIOG->OSPEEDR = 0xAAAAAAAA; + /* Configure PGx pins Output type to push-pull */ + GPIOG->OTYPER = 0x00000000; + /* No pull-up, pull-down for PGx pins */ + GPIOG->PUPDR = 0x00000000; + + /* Connect PHx pins to FMC Alternate function */ + GPIOH->AFR[0] = 0x00C0CC00; + GPIOH->AFR[1] = 0xCCCCCCCC; + /* Configure PHx pins in Alternate function mode */ + GPIOH->MODER = 0xAAAA08A0; + /* Configure PHx pins speed to 50 MHz */ + GPIOH->OSPEEDR = 0xAAAA08A0; + /* Configure PHx pins Output type to push-pull */ + GPIOH->OTYPER = 0x00000000; + /* No pull-up, pull-down for PHx pins */ + GPIOH->PUPDR = 0x00000000; + + /* Connect PIx pins to FMC Alternate function */ + GPIOI->AFR[0] = 0xCCCCCCCC; + GPIOI->AFR[1] = 0x00000CC0; + /* Configure PIx pins in Alternate function mode */ + GPIOI->MODER = 0x0028AAAA; + /* Configure PIx pins speed to 50 MHz */ + GPIOI->OSPEEDR = 0x0028AAAA; + /* Configure PIx pins Output type to push-pull */ + GPIOI->OTYPER = 0x00000000; + /* No pull-up, pull-down for PIx pins */ + GPIOI->PUPDR = 0x00000000; + +/*-- FMC Configuration -------------------------------------------------------*/ + /* Enable the FMC interface clock */ + RCC->AHB3ENR |= 0x00000001; + /* Delay after an RCC peripheral clock enabling */ + tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FMCEN); + + FMC_Bank5_6->SDCR[0] = 0x000019E4; + FMC_Bank5_6->SDTR[0] = 0x01115351; + + /* SDRAM initialization sequence */ + /* Clock enable command */ + FMC_Bank5_6->SDCMR = 0x00000011; + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + while((tmpreg != 0) && (timeout-- > 0)) + { + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + } + + /* Delay */ + for (index = 0; index<1000; index++); + + /* PALL command */ + FMC_Bank5_6->SDCMR = 0x00000012; + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + timeout = 0xFFFF; + while((tmpreg != 0) && (timeout-- > 0)) + { + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + } + + /* Auto refresh command */ + FMC_Bank5_6->SDCMR = 0x00000073; + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + timeout = 0xFFFF; + while((tmpreg != 0) && (timeout-- > 0)) + { + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + } + + /* MRD register program */ + FMC_Bank5_6->SDCMR = 0x00046014; + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + timeout = 0xFFFF; + while((tmpreg != 0) && (timeout-- > 0)) + { + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + } + + /* Set refresh count */ + tmpreg = FMC_Bank5_6->SDRTR; + FMC_Bank5_6->SDRTR = (tmpreg | (0x0000027C<<1)); + + /* Disable write protection */ + tmpreg = FMC_Bank5_6->SDCR[0]; + FMC_Bank5_6->SDCR[0] = (tmpreg & 0xFFFFFDFF); + +#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx) + /* Configure and enable Bank1_SRAM2 */ + FMC_Bank1->BTCR[2] = 0x00001011; + FMC_Bank1->BTCR[3] = 0x00000201; + FMC_Bank1E->BWTR[2] = 0x0fffffff; +#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx */ +#if defined(STM32F469xx) || defined(STM32F479xx) + /* Configure and enable Bank1_SRAM2 */ + FMC_Bank1->BTCR[2] = 0x00001091; + FMC_Bank1->BTCR[3] = 0x00110212; + FMC_Bank1E->BWTR[2] = 0x0fffffff; +#endif /* STM32F469xx || STM32F479xx */ + + (void)(tmp); +} +#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F469xx || STM32F479xx */ +#elif defined (DATA_IN_ExtSRAM) || defined (DATA_IN_ExtSDRAM) +/** + * @brief Setup the external memory controller. + * Called in startup_stm32f4xx.s before jump to main. + * This function configures the external memories (SRAM/SDRAM) + * This SRAM/SDRAM will be used as program data memory (including heap and stack). + * @param None + * @retval None + */ +void SystemInit_ExtMemCtl(void) +{ + __IO uint32_t tmp = 0x00; +#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\ + || defined(STM32F446xx) || defined(STM32F469xx) || defined(STM32F479xx) +#if defined (DATA_IN_ExtSDRAM) + register uint32_t tmpreg = 0, timeout = 0xFFFF; + register __IO uint32_t index; + +#if defined(STM32F446xx) + /* Enable GPIOA, GPIOC, GPIOD, GPIOE, GPIOF, GPIOG interface + clock */ + RCC->AHB1ENR |= 0x0000007D; +#else + /* Enable GPIOC, GPIOD, GPIOE, GPIOF, GPIOG, GPIOH and GPIOI interface + clock */ + RCC->AHB1ENR |= 0x000001F8; +#endif /* STM32F446xx */ + /* Delay after an RCC peripheral clock enabling */ + tmp = READ_BIT(RCC->AHB1ENR, RCC_AHB1ENR_GPIOCEN); + +#if defined(STM32F446xx) + /* Connect PAx pins to FMC Alternate function */ + GPIOA->AFR[0] |= 0xC0000000; + GPIOA->AFR[1] |= 0x00000000; + /* Configure PDx pins in Alternate function mode */ + GPIOA->MODER |= 0x00008000; + /* Configure PDx pins speed to 50 MHz */ + GPIOA->OSPEEDR |= 0x00008000; + /* Configure PDx pins Output type to push-pull */ + GPIOA->OTYPER |= 0x00000000; + /* No pull-up, pull-down for PDx pins */ + GPIOA->PUPDR |= 0x00000000; + + /* Connect PCx pins to FMC Alternate function */ + GPIOC->AFR[0] |= 0x00CC0000; + GPIOC->AFR[1] |= 0x00000000; + /* Configure PDx pins in Alternate function mode */ + GPIOC->MODER |= 0x00000A00; + /* Configure PDx pins speed to 50 MHz */ + GPIOC->OSPEEDR |= 0x00000A00; + /* Configure PDx pins Output type to push-pull */ + GPIOC->OTYPER |= 0x00000000; + /* No pull-up, pull-down for PDx pins */ + GPIOC->PUPDR |= 0x00000000; +#endif /* STM32F446xx */ + + /* Connect PDx pins to FMC Alternate function */ + GPIOD->AFR[0] = 0x000000CC; + GPIOD->AFR[1] = 0xCC000CCC; + /* Configure PDx pins in Alternate function mode */ + GPIOD->MODER = 0xA02A000A; + /* Configure PDx pins speed to 50 MHz */ + GPIOD->OSPEEDR = 0xA02A000A; + /* Configure PDx pins Output type to push-pull */ + GPIOD->OTYPER = 0x00000000; + /* No pull-up, pull-down for PDx pins */ + GPIOD->PUPDR = 0x00000000; + + /* Connect PEx pins to FMC Alternate function */ + GPIOE->AFR[0] = 0xC00000CC; + GPIOE->AFR[1] = 0xCCCCCCCC; + /* Configure PEx pins in Alternate function mode */ + GPIOE->MODER = 0xAAAA800A; + /* Configure PEx pins speed to 50 MHz */ + GPIOE->OSPEEDR = 0xAAAA800A; + /* Configure PEx pins Output type to push-pull */ + GPIOE->OTYPER = 0x00000000; + /* No pull-up, pull-down for PEx pins */ + GPIOE->PUPDR = 0x00000000; + + /* Connect PFx pins to FMC Alternate function */ + GPIOF->AFR[0] = 0xCCCCCCCC; + GPIOF->AFR[1] = 0xCCCCCCCC; + /* Configure PFx pins in Alternate function mode */ + GPIOF->MODER = 0xAA800AAA; + /* Configure PFx pins speed to 50 MHz */ + GPIOF->OSPEEDR = 0xAA800AAA; + /* Configure PFx pins Output type to push-pull */ + GPIOF->OTYPER = 0x00000000; + /* No pull-up, pull-down for PFx pins */ + GPIOF->PUPDR = 0x00000000; + + /* Connect PGx pins to FMC Alternate function */ + GPIOG->AFR[0] = 0xCCCCCCCC; + GPIOG->AFR[1] = 0xCCCCCCCC; + /* Configure PGx pins in Alternate function mode */ + GPIOG->MODER = 0xAAAAAAAA; + /* Configure PGx pins speed to 50 MHz */ + GPIOG->OSPEEDR = 0xAAAAAAAA; + /* Configure PGx pins Output type to push-pull */ + GPIOG->OTYPER = 0x00000000; + /* No pull-up, pull-down for PGx pins */ + GPIOG->PUPDR = 0x00000000; + +#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\ + || defined(STM32F469xx) || defined(STM32F479xx) + /* Connect PHx pins to FMC Alternate function */ + GPIOH->AFR[0] = 0x00C0CC00; + GPIOH->AFR[1] = 0xCCCCCCCC; + /* Configure PHx pins in Alternate function mode */ + GPIOH->MODER = 0xAAAA08A0; + /* Configure PHx pins speed to 50 MHz */ + GPIOH->OSPEEDR = 0xAAAA08A0; + /* Configure PHx pins Output type to push-pull */ + GPIOH->OTYPER = 0x00000000; + /* No pull-up, pull-down for PHx pins */ + GPIOH->PUPDR = 0x00000000; + + /* Connect PIx pins to FMC Alternate function */ + GPIOI->AFR[0] = 0xCCCCCCCC; + GPIOI->AFR[1] = 0x00000CC0; + /* Configure PIx pins in Alternate function mode */ + GPIOI->MODER = 0x0028AAAA; + /* Configure PIx pins speed to 50 MHz */ + GPIOI->OSPEEDR = 0x0028AAAA; + /* Configure PIx pins Output type to push-pull */ + GPIOI->OTYPER = 0x00000000; + /* No pull-up, pull-down for PIx pins */ + GPIOI->PUPDR = 0x00000000; +#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F469xx || STM32F479xx */ + +/*-- FMC Configuration -------------------------------------------------------*/ + /* Enable the FMC interface clock */ + RCC->AHB3ENR |= 0x00000001; + /* Delay after an RCC peripheral clock enabling */ + tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FMCEN); + + /* Configure and enable SDRAM bank1 */ +#if defined(STM32F446xx) + FMC_Bank5_6->SDCR[0] = 0x00001954; +#else + FMC_Bank5_6->SDCR[0] = 0x000019E4; +#endif /* STM32F446xx */ + FMC_Bank5_6->SDTR[0] = 0x01115351; + + /* SDRAM initialization sequence */ + /* Clock enable command */ + FMC_Bank5_6->SDCMR = 0x00000011; + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + while((tmpreg != 0) && (timeout-- > 0)) + { + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + } + + /* Delay */ + for (index = 0; index<1000; index++); + + /* PALL command */ + FMC_Bank5_6->SDCMR = 0x00000012; + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + timeout = 0xFFFF; + while((tmpreg != 0) && (timeout-- > 0)) + { + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + } + + /* Auto refresh command */ +#if defined(STM32F446xx) + FMC_Bank5_6->SDCMR = 0x000000F3; +#else + FMC_Bank5_6->SDCMR = 0x00000073; +#endif /* STM32F446xx */ + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + timeout = 0xFFFF; + while((tmpreg != 0) && (timeout-- > 0)) + { + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + } + + /* MRD register program */ +#if defined(STM32F446xx) + FMC_Bank5_6->SDCMR = 0x00044014; +#else + FMC_Bank5_6->SDCMR = 0x00046014; +#endif /* STM32F446xx */ + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + timeout = 0xFFFF; + while((tmpreg != 0) && (timeout-- > 0)) + { + tmpreg = FMC_Bank5_6->SDSR & 0x00000020; + } + + /* Set refresh count */ + tmpreg = FMC_Bank5_6->SDRTR; +#if defined(STM32F446xx) + FMC_Bank5_6->SDRTR = (tmpreg | (0x0000050C<<1)); +#else + FMC_Bank5_6->SDRTR = (tmpreg | (0x0000027C<<1)); +#endif /* STM32F446xx */ + + /* Disable write protection */ + tmpreg = FMC_Bank5_6->SDCR[0]; + FMC_Bank5_6->SDCR[0] = (tmpreg & 0xFFFFFDFF); +#endif /* DATA_IN_ExtSDRAM */ +#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F446xx || STM32F469xx || STM32F479xx */ + +#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx) || defined(STM32F417xx)\ + || defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\ + || defined(STM32F469xx) || defined(STM32F479xx) || defined(STM32F412Zx) || defined(STM32F412Vx) + +#if defined(DATA_IN_ExtSRAM) +/*-- GPIOs Configuration -----------------------------------------------------*/ + /* Enable GPIOD, GPIOE, GPIOF and GPIOG interface clock */ + RCC->AHB1ENR |= 0x00000078; + /* Delay after an RCC peripheral clock enabling */ + tmp = READ_BIT(RCC->AHB1ENR, RCC_AHB1ENR_GPIODEN); + + /* Connect PDx pins to FMC Alternate function */ + GPIOD->AFR[0] = 0x00CCC0CC; + GPIOD->AFR[1] = 0xCCCCCCCC; + /* Configure PDx pins in Alternate function mode */ + GPIOD->MODER = 0xAAAA0A8A; + /* Configure PDx pins speed to 100 MHz */ + GPIOD->OSPEEDR = 0xFFFF0FCF; + /* Configure PDx pins Output type to push-pull */ + GPIOD->OTYPER = 0x00000000; + /* No pull-up, pull-down for PDx pins */ + GPIOD->PUPDR = 0x00000000; + + /* Connect PEx pins to FMC Alternate function */ + GPIOE->AFR[0] = 0xC00CC0CC; + GPIOE->AFR[1] = 0xCCCCCCCC; + /* Configure PEx pins in Alternate function mode */ + GPIOE->MODER = 0xAAAA828A; + /* Configure PEx pins speed to 100 MHz */ + GPIOE->OSPEEDR = 0xFFFFC3CF; + /* Configure PEx pins Output type to push-pull */ + GPIOE->OTYPER = 0x00000000; + /* No pull-up, pull-down for PEx pins */ + GPIOE->PUPDR = 0x00000000; + + /* Connect PFx pins to FMC Alternate function */ + GPIOF->AFR[0] = 0x00CCCCCC; + GPIOF->AFR[1] = 0xCCCC0000; + /* Configure PFx pins in Alternate function mode */ + GPIOF->MODER = 0xAA000AAA; + /* Configure PFx pins speed to 100 MHz */ + GPIOF->OSPEEDR = 0xFF000FFF; + /* Configure PFx pins Output type to push-pull */ + GPIOF->OTYPER = 0x00000000; + /* No pull-up, pull-down for PFx pins */ + GPIOF->PUPDR = 0x00000000; + + /* Connect PGx pins to FMC Alternate function */ + GPIOG->AFR[0] = 0x00CCCCCC; + GPIOG->AFR[1] = 0x000000C0; + /* Configure PGx pins in Alternate function mode */ + GPIOG->MODER = 0x00085AAA; + /* Configure PGx pins speed to 100 MHz */ + GPIOG->OSPEEDR = 0x000CAFFF; + /* Configure PGx pins Output type to push-pull */ + GPIOG->OTYPER = 0x00000000; + /* No pull-up, pull-down for PGx pins */ + GPIOG->PUPDR = 0x00000000; + +/*-- FMC/FSMC Configuration --------------------------------------------------*/ + /* Enable the FMC/FSMC interface clock */ + RCC->AHB3ENR |= 0x00000001; + +#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx) + /* Delay after an RCC peripheral clock enabling */ + tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FMCEN); + /* Configure and enable Bank1_SRAM2 */ + FMC_Bank1->BTCR[2] = 0x00001011; + FMC_Bank1->BTCR[3] = 0x00000201; + FMC_Bank1E->BWTR[2] = 0x0fffffff; +#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx */ +#if defined(STM32F469xx) || defined(STM32F479xx) + /* Delay after an RCC peripheral clock enabling */ + tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FMCEN); + /* Configure and enable Bank1_SRAM2 */ + FMC_Bank1->BTCR[2] = 0x00001091; + FMC_Bank1->BTCR[3] = 0x00110212; + FMC_Bank1E->BWTR[2] = 0x0fffffff; +#endif /* STM32F469xx || STM32F479xx */ +#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx)|| defined(STM32F417xx)\ + || defined(STM32F412Zx) || defined(STM32F412Vx) + /* Delay after an RCC peripheral clock enabling */ + tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FSMCEN); + /* Configure and enable Bank1_SRAM2 */ + FSMC_Bank1->BTCR[2] = 0x00001011; + FSMC_Bank1->BTCR[3] = 0x00000201; + FSMC_Bank1E->BWTR[2] = 0x0FFFFFFF; +#endif /* STM32F405xx || STM32F415xx || STM32F407xx || STM32F417xx || STM32F412Zx || STM32F412Vx */ + +#endif /* DATA_IN_ExtSRAM */ +#endif /* STM32F405xx || STM32F415xx || STM32F407xx || STM32F417xx || STM32F427xx || STM32F437xx ||\ + STM32F429xx || STM32F439xx || STM32F469xx || STM32F479xx || STM32F412Zx || STM32F412Vx */ + (void)(tmp); +} +#endif /* DATA_IN_ExtSRAM && DATA_IN_ExtSDRAM */ +/** + * @} + */ + +/** + * @} + */ + +/** + * @} + */ diff --git a/Core/Src/tim.c b/Core/Src/tim.c new file mode 100644 index 0000000..9a68eb8 --- /dev/null +++ b/Core/Src/tim.c @@ -0,0 +1,162 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file tim.c + * @brief This file provides code for the configuration + * of the TIM instances. + ****************************************************************************** + * @attention + * + * Copyright (c) 2026 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ +/* Includes ------------------------------------------------------------------*/ +#include "tim.h" + +/* USER CODE BEGIN 0 */ + +/* USER CODE END 0 */ + +TIM_HandleTypeDef htim5; + +/* TIM5 init function */ +void MX_TIM5_Init(void) +{ + + /* USER CODE BEGIN TIM5_Init 0 */ + + /* USER CODE END TIM5_Init 0 */ + + TIM_ClockConfigTypeDef sClockSourceConfig = {0}; + TIM_MasterConfigTypeDef sMasterConfig = {0}; + TIM_OC_InitTypeDef sConfigOC = {0}; + + /* USER CODE BEGIN TIM5_Init 1 */ + + /* USER CODE END TIM5_Init 1 */ + htim5.Instance = TIM5; + htim5.Init.Prescaler = 0; + htim5.Init.CounterMode = TIM_COUNTERMODE_UP; + htim5.Init.Period = 65535; + htim5.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1; + htim5.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE; + if (HAL_TIM_Base_Init(&htim5) != HAL_OK) + { + Error_Handler(); + } + sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL; + if (HAL_TIM_ConfigClockSource(&htim5, &sClockSourceConfig) != HAL_OK) + { + Error_Handler(); + } + if (HAL_TIM_PWM_Init(&htim5) != HAL_OK) + { + Error_Handler(); + } + sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET; + sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE; + if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig) != HAL_OK) + { + Error_Handler(); + } + sConfigOC.OCMode = TIM_OCMODE_PWM1; + sConfigOC.Pulse = 0; + sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH; + sConfigOC.OCFastMode = TIM_OCFAST_DISABLE; + if (HAL_TIM_PWM_ConfigChannel(&htim5, &sConfigOC, TIM_CHANNEL_1) != HAL_OK) + { + Error_Handler(); + } + if (HAL_TIM_PWM_ConfigChannel(&htim5, &sConfigOC, TIM_CHANNEL_2) != HAL_OK) + { + Error_Handler(); + } + if (HAL_TIM_PWM_ConfigChannel(&htim5, &sConfigOC, TIM_CHANNEL_3) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN TIM5_Init 2 */ + + /* USER CODE END TIM5_Init 2 */ + HAL_TIM_MspPostInit(&htim5); + +} + +void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle) +{ + + if(tim_baseHandle->Instance==TIM5) + { + /* USER CODE BEGIN TIM5_MspInit 0 */ + + /* USER CODE END TIM5_MspInit 0 */ + /* TIM5 clock enable */ + __HAL_RCC_TIM5_CLK_ENABLE(); + + /* TIM5 interrupt Init */ + HAL_NVIC_SetPriority(TIM5_IRQn, 5, 0); + HAL_NVIC_EnableIRQ(TIM5_IRQn); + /* USER CODE BEGIN TIM5_MspInit 1 */ + + /* USER CODE END TIM5_MspInit 1 */ + } +} +void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle) +{ + + GPIO_InitTypeDef GPIO_InitStruct = {0}; + if(timHandle->Instance==TIM5) + { + /* USER CODE BEGIN TIM5_MspPostInit 0 */ + + /* USER CODE END TIM5_MspPostInit 0 */ + + __HAL_RCC_GPIOH_CLK_ENABLE(); + /**TIM5 GPIO Configuration + PH12 ------> TIM5_CH3 + PH11 ------> TIM5_CH2 + PH10 ------> TIM5_CH1 + */ + GPIO_InitStruct.Pin = GPIO_PIN_12|GPIO_PIN_11|GPIO_PIN_10; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; + GPIO_InitStruct.Alternate = GPIO_AF2_TIM5; + HAL_GPIO_Init(GPIOH, &GPIO_InitStruct); + + /* USER CODE BEGIN TIM5_MspPostInit 1 */ + + /* USER CODE END TIM5_MspPostInit 1 */ + } + +} + +void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle) +{ + + if(tim_baseHandle->Instance==TIM5) + { + /* USER CODE BEGIN TIM5_MspDeInit 0 */ + + /* USER CODE END TIM5_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_TIM5_CLK_DISABLE(); + + /* TIM5 interrupt Deinit */ + HAL_NVIC_DisableIRQ(TIM5_IRQn); + /* USER CODE BEGIN TIM5_MspDeInit 1 */ + + /* USER CODE END TIM5_MspDeInit 1 */ + } +} + +/* USER CODE BEGIN 1 */ + +/* USER CODE END 1 */ diff --git a/Core/Src/usart.c b/Core/Src/usart.c new file mode 100644 index 0000000..7a216f4 --- /dev/null +++ b/Core/Src/usart.c @@ -0,0 +1,136 @@ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file usart.c + * @brief This file provides code for the configuration + * of the USART instances. + ****************************************************************************** + * @attention + * + * Copyright (c) 2026 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ +/* Includes ------------------------------------------------------------------*/ +#include "usart.h" + +/* USER CODE BEGIN 0 */ + +/* USER CODE END 0 */ + +UART_HandleTypeDef huart3; +DMA_HandleTypeDef hdma_usart3_rx; + +/* USART3 init function */ + +void MX_USART3_UART_Init(void) +{ + + /* USER CODE BEGIN USART3_Init 0 */ + + /* USER CODE END USART3_Init 0 */ + + /* USER CODE BEGIN USART3_Init 1 */ + + /* USER CODE END USART3_Init 1 */ + huart3.Instance = USART3; + huart3.Init.BaudRate = 115200; + huart3.Init.WordLength = UART_WORDLENGTH_8B; + huart3.Init.StopBits = UART_STOPBITS_1; + huart3.Init.Parity = UART_PARITY_NONE; + huart3.Init.Mode = UART_MODE_TX_RX; + huart3.Init.HwFlowCtl = UART_HWCONTROL_NONE; + huart3.Init.OverSampling = UART_OVERSAMPLING_16; + if (HAL_UART_Init(&huart3) != HAL_OK) + { + Error_Handler(); + } + /* USER CODE BEGIN USART3_Init 2 */ + + /* USER CODE END USART3_Init 2 */ + +} + +void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) +{ + + GPIO_InitTypeDef GPIO_InitStruct = {0}; + if(uartHandle->Instance==USART3) + { + /* USER CODE BEGIN USART3_MspInit 0 */ + + /* USER CODE END USART3_MspInit 0 */ + /* USART3 clock enable */ + __HAL_RCC_USART3_CLK_ENABLE(); + + __HAL_RCC_GPIOC_CLK_ENABLE(); + /**USART3 GPIO Configuration + PC11 ------> USART3_RX + PC10 ------> USART3_TX + */ + GPIO_InitStruct.Pin = GPIO_PIN_11|GPIO_PIN_10; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_NOPULL; + GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; + GPIO_InitStruct.Alternate = GPIO_AF7_USART3; + HAL_GPIO_Init(GPIOC, &GPIO_InitStruct); + + /* USART3 DMA Init */ + /* USART3_RX Init */ + hdma_usart3_rx.Instance = DMA1_Stream1; + hdma_usart3_rx.Init.Channel = DMA_CHANNEL_4; + hdma_usart3_rx.Init.Direction = DMA_PERIPH_TO_MEMORY; + hdma_usart3_rx.Init.PeriphInc = DMA_PINC_DISABLE; + hdma_usart3_rx.Init.MemInc = DMA_MINC_ENABLE; + hdma_usart3_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE; + hdma_usart3_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE; + hdma_usart3_rx.Init.Mode = DMA_NORMAL; + hdma_usart3_rx.Init.Priority = DMA_PRIORITY_LOW; + hdma_usart3_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; + if (HAL_DMA_Init(&hdma_usart3_rx) != HAL_OK) + { + Error_Handler(); + } + + __HAL_LINKDMA(uartHandle,hdmarx,hdma_usart3_rx); + + /* USER CODE BEGIN USART3_MspInit 1 */ + + /* USER CODE END USART3_MspInit 1 */ + } +} + +void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle) +{ + + if(uartHandle->Instance==USART3) + { + /* USER CODE BEGIN USART3_MspDeInit 0 */ + + /* USER CODE END USART3_MspDeInit 0 */ + /* Peripheral clock disable */ + __HAL_RCC_USART3_CLK_DISABLE(); + + /**USART3 GPIO Configuration + PC11 ------> USART3_RX + PC10 ------> USART3_TX + */ + HAL_GPIO_DeInit(GPIOC, GPIO_PIN_11|GPIO_PIN_10); + + /* USART3 DMA DeInit */ + HAL_DMA_DeInit(uartHandle->hdmarx); + /* USER CODE BEGIN USART3_MspDeInit 1 */ + + /* USER CODE END USART3_MspDeInit 1 */ + } +} + +/* USER CODE BEGIN 1 */ + +/* USER CODE END 1 */ diff --git a/ITM_TROUBLESHOOTING.md b/ITM_TROUBLESHOOTING.md new file mode 100644 index 0000000..e69de29 diff --git a/MDK-ARM/DebugConfig/arm_STM32F407IGHx.dbgconf b/MDK-ARM/DebugConfig/arm_STM32F407IGHx.dbgconf new file mode 100644 index 0000000..1df0a1b --- /dev/null +++ b/MDK-ARM/DebugConfig/arm_STM32F407IGHx.dbgconf @@ -0,0 +1,48 @@ +// File: STM32F405_415_407_417_427_437_429_439.dbgconf +// Version: 1.0.0 +// Note: refer to STM32F405/415 STM32F407/417 STM32F427/437 STM32F429/439 reference manual (RM0090) +// refer to STM32F40x STM32F41x datasheets +// refer to STM32F42x STM32F43x datasheets + +// <<< Use Configuration Wizard in Context Menu >>> + +// Debug MCU configuration register (DBGMCU_CR) +// DBG_STANDBY Debug Standby Mode +// DBG_STOP Debug Stop Mode +// DBG_SLEEP Debug Sleep Mode +// +DbgMCU_CR = 0x00000007; + +// Debug MCU APB1 freeze register (DBGMCU_APB1_FZ) +// Reserved bits must be kept at reset value +// DBG_CAN2_STOP CAN2 stopped when core is halted +// DBG_CAN1_STOP CAN2 stopped when core is halted +// DBG_I2C3_SMBUS_TIMEOUT I2C3 SMBUS timeout mode stopped when core is halted +// DBG_I2C2_SMBUS_TIMEOUT I2C2 SMBUS timeout mode stopped when core is halted +// DBG_I2C1_SMBUS_TIMEOUT I2C1 SMBUS timeout mode stopped when core is halted +// DBG_IWDG_STOP Independent watchdog stopped when core is halted +// DBG_WWDG_STOP Window watchdog stopped when core is halted +// DBG_RTC_STOP RTC stopped when core is halted +// DBG_TIM14_STOP TIM14 counter stopped when core is halted +// DBG_TIM13_STOP TIM13 counter stopped when core is halted +// DBG_TIM12_STOP TIM12 counter stopped when core is halted +// DBG_TIM7_STOP TIM7 counter stopped when core is halted +// DBG_TIM6_STOP TIM6 counter stopped when core is halted +// DBG_TIM5_STOP TIM5 counter stopped when core is halted +// DBG_TIM4_STOP TIM4 counter stopped when core is halted +// DBG_TIM3_STOP TIM3 counter stopped when core is halted +// DBG_TIM2_STOP TIM2 counter stopped when core is halted +// +DbgMCU_APB1_Fz = 0x00000000; + +// Debug MCU APB2 freeze register (DBGMCU_APB2_FZ) +// Reserved bits must be kept at reset value +// DBG_TIM11_STOP TIM11 counter stopped when core is halted +// DBG_TIM10_STOP TIM10 counter stopped when core is halted +// DBG_TIM9_STOP TIM9 counter stopped when core is halted +// DBG_TIM8_STOP TIM8 counter stopped when core is halted +// DBG_TIM1_STOP TIM1 counter stopped when core is halted +// +DbgMCU_APB2_Fz = 0x00000000; + +// <<< end of configuration section >>> \ No newline at end of file diff --git a/MDK-ARM/DebugConfig/arm_STM32F407IGHx_1.0.1.dbgconf b/MDK-ARM/DebugConfig/arm_STM32F407IGHx_1.0.1.dbgconf new file mode 100644 index 0000000..1c568eb --- /dev/null +++ b/MDK-ARM/DebugConfig/arm_STM32F407IGHx_1.0.1.dbgconf @@ -0,0 +1,48 @@ +// File: STM32F405_415_407_417_427_437_429_439.dbgconf +// Version: 1.0.1 +// Note: refer to STM32F405/415 STM32F407/417 STM32F427/437 STM32F429/439 reference manual (RM0090) +// refer to STM32F40x STM32F41x datasheets +// refer to STM32F42x STM32F43x datasheets + +// <<< Use Configuration Wizard in Context Menu >>> + +// Debug MCU configuration register (DBGMCU_CR) +// DBG_STANDBY Debug Standby Mode +// DBG_STOP Debug Stop Mode +// DBG_SLEEP Debug Sleep Mode +// +DbgMCU_CR = 0x00000007; + +// Debug MCU APB1 freeze register (DBGMCU_APB1_FZ) +// Reserved bits must be kept at reset value +// DBG_CAN2_STOP CAN2 stopped when core is halted +// DBG_CAN1_STOP CAN2 stopped when core is halted +// DBG_I2C3_SMBUS_TIMEOUT I2C3 SMBUS timeout mode stopped when core is halted +// DBG_I2C2_SMBUS_TIMEOUT I2C2 SMBUS timeout mode stopped when core is halted +// DBG_I2C1_SMBUS_TIMEOUT I2C1 SMBUS timeout mode stopped when core is halted +// DBG_IWDG_STOP Independent watchdog stopped when core is halted +// DBG_WWDG_STOP Window watchdog stopped when core is halted +// DBG_RTC_STOP RTC stopped when core is halted +// DBG_TIM14_STOP TIM14 counter stopped when core is halted +// DBG_TIM13_STOP TIM13 counter stopped when core is halted +// DBG_TIM12_STOP TIM12 counter stopped when core is halted +// DBG_TIM7_STOP TIM7 counter stopped when core is halted +// DBG_TIM6_STOP TIM6 counter stopped when core is halted +// DBG_TIM5_STOP TIM5 counter stopped when core is halted +// DBG_TIM4_STOP TIM4 counter stopped when core is halted +// DBG_TIM3_STOP TIM3 counter stopped when core is halted +// DBG_TIM2_STOP TIM2 counter stopped when core is halted +// +DbgMCU_APB1_Fz = 0x06E019FF; + +// Debug MCU APB2 freeze register (DBGMCU_APB2_FZ) +// Reserved bits must be kept at reset value +// DBG_TIM11_STOP TIM11 counter stopped when core is halted +// DBG_TIM10_STOP TIM10 counter stopped when core is halted +// DBG_TIM9_STOP TIM9 counter stopped when core is halted +// DBG_TIM8_STOP TIM8 counter stopped when core is halted +// DBG_TIM1_STOP TIM1 counter stopped when core is halted +// +DbgMCU_APB2_Fz = 0x00070003; + +// <<< end of configuration section >>> \ No newline at end of file diff --git a/MDK-ARM/RTE/_arm/RTE_Components.h b/MDK-ARM/RTE/_arm/RTE_Components.h new file mode 100644 index 0000000..dcca127 --- /dev/null +++ b/MDK-ARM/RTE/_arm/RTE_Components.h @@ -0,0 +1,14 @@ +/* + * UVISION generated file: DO NOT EDIT! + * Generated by: uVision version 5.40.0.0 + * + * Project: 'arm' + * Target: 'arm' + */ + +#ifndef RTE_COMPONENTS_H +#define RTE_COMPONENTS_H + + + +#endif /* RTE_COMPONENTS_H */ diff --git a/MDK-ARM/arm.uvguix.Lenovo b/MDK-ARM/arm.uvguix.Lenovo new file mode 100644 index 0000000..9dd8460 --- /dev/null +++ b/MDK-ARM/arm.uvguix.Lenovo @@ -0,0 +1,1860 @@ + + + + -6.1 + +
### uVision Project, (C) Keil Software
+ + + + + + + + + + 38003 + Registers + 124 79 + + + 346 + Code Coverage + 877 160 + + + 204 + Performance Analyzer + 1037 + + + + + + 35141 + Event Statistics + + 200 50 700 + + + 1506 + Symbols + + 71 71 71 + + + 1936 + Watch 1 + + 200 133 133 + + + 1937 + Watch 2 + + 200 133 133 + + + 1935 + Call Stack + Locals + + 200 133 133 + + + 2506 + Trace Data + + 75 135 130 95 70 230 200 150 + + + 466 + Source Browser + 500 + 300 + + + + + + + + 1 + 1 + 0 + 0 + -1 + + + + + + + 44 + 0 + 1 + + -1 + -1 + + + -1 + -1 + + + 142 + 121 + 1401 + 913 + + + + 0 + + 60 + 010000000400000001000000010000000100000001000000000000000200000000000000010000000100000000000000280000002800000000000000 + + + + 0 + Build + + -1 + -1 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + D90000004F000000F0040000E1000000 + + + 16 + D8000000EF0000005003000081010000 + + + + 1005 + 1005 + 1 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 0300000066000000D20000001A020000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 109 + 109 + 1 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 0300000066000000D20000001A020000 + + + 16 + D8000000EF000000D401000039030000 + + + + 1465 + 1465 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 0000000037020000F0040000C9020000 + + + 16 + D8000000EF0000005003000081010000 + + + + 1466 + 1466 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + D8000000EF0000005003000081010000 + + + + 1467 + 1467 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + D8000000EF0000005003000081010000 + + + + 1468 + 1468 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + D8000000EF0000005003000081010000 + + + + 1506 + 1506 + 0 + 0 + 0 + 0 + 32767 + 0 + 16384 + 0 + + 16 + 1E04000066000000ED04000006010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 1913 + 1913 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + DC00000066000000ED040000C8000000 + + + 16 + D8000000EF0000005003000081010000 + + + + 1935 + 1935 + 0 + 0 + 0 + 0 + 32767 + 0 + 32768 + 0 + + 16 + 030000003A020000ED040000B0020000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 1936 + 1936 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 1937 + 1937 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 1939 + 1939 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + D8000000EF0000005003000081010000 + + + + 1940 + 1940 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + D8000000EF0000005003000081010000 + + + + 1941 + 1941 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + D8000000EF0000005003000081010000 + + + + 1942 + 1942 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + D8000000EF0000005003000081010000 + + + + 195 + 195 + 1 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 0300000066000000D20000001A020000 + + + 16 + D8000000EF000000D401000039030000 + + + + 196 + 196 + 1 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 0300000066000000D20000001A020000 + + + 16 + D8000000EF000000D401000039030000 + + + + 197 + 197 + 1 + 0 + 0 + 0 + 32767 + 0 + 32768 + 0 + + 16 + 000000004B020000F0040000C9020000 + + + 16 + D8000000EF0000005003000081010000 + + + + 198 + 198 + 0 + 0 + 0 + 0 + 32767 + 0 + 32768 + 0 + + 16 + 0000000023020000F0040000C9020000 + + + 16 + D8000000EF0000005003000081010000 + + + + 199 + 199 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000004E020000ED040000B0020000 + + + 16 + D8000000EF0000005003000081010000 + + + + 203 + 203 + 0 + 0 + 0 + 0 + 32767 + 0 + 8192 + 0 + + 16 + D900000063000000F0040000E1000000 + + + 16 + D8000000EF0000005003000081010000 + + + + 204 + 204 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + DC00000066000000ED040000C8000000 + + + 16 + D8000000EF0000005003000081010000 + + + + 221 + 221 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 00000000000000000000000000000000 + + + 16 + 0A0000000A0000006E0000006E000000 + + + + 2506 + 2506 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1B04000063000000F004000033020000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 2507 + 2507 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 0000000037020000F0040000B5020000 + + + 16 + D8000000EF0000005003000081010000 + + + + 343 + 343 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + DC00000066000000ED040000C8000000 + + + 16 + D8000000EF0000005003000081010000 + + + + 346 + 346 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + DC00000066000000ED040000C8000000 + + + 16 + D8000000EF0000005003000081010000 + + + + 35141 + 35141 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + D900000063000000F0040000E1000000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35824 + 35824 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + DC00000066000000ED040000C8000000 + + + 16 + D8000000EF0000005003000081010000 + + + + 35885 + 35885 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35886 + 35886 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35887 + 35887 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35888 + 35888 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35889 + 35889 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35890 + 35890 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35891 + 35891 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35892 + 35892 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35893 + 35893 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35894 + 35894 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35895 + 35895 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35896 + 35896 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35897 + 35897 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35898 + 35898 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35899 + 35899 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35900 + 35900 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35901 + 35901 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35902 + 35902 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35903 + 35903 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35904 + 35904 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 35905 + 35905 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 38003 + 38003 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 0300000066000000D2000000B0020000 + + + 16 + D8000000EF000000D401000039030000 + + + + 38007 + 38007 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 000000004B020000F0040000C9020000 + + + 16 + D8000000EF0000005003000081010000 + + + + 436 + 436 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000004E020000ED040000B0020000 + + + 16 + D8000000EF000000D401000039030000 + + + + 437 + 437 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 440 + 440 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 463 + 463 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000004E020000ED040000B0020000 + + + 16 + D8000000EF000000D401000039030000 + + + + 466 + 466 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000004E020000ED040000B0020000 + + + 16 + D8000000EF000000D401000039030000 + + + + 470 + 470 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + DC00000066000000ED040000C8000000 + + + 16 + D8000000EF0000005003000081010000 + + + + 50000 + 50000 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 50001 + 50001 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 50002 + 50002 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 50003 + 50003 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 50004 + 50004 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 50005 + 50005 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 50006 + 50006 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 50007 + 50007 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 50008 + 50008 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 50009 + 50009 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 50010 + 50010 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 50011 + 50011 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 50012 + 50012 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 50013 + 50013 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 50014 + 50014 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 50015 + 50015 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 50016 + 50016 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 50017 + 50017 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 50018 + 50018 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 50019 + 50019 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 59392 + 59392 + 1 + 0 + 0 + 0 + 32767 + 0 + 8192 + 0 + + 16 + 0000000000000000D10300001C000000 + + + 16 + 0A0000000A0000006E0000006E000000 + + + + 59393 + 0 + 1 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 00000000C9020000F0040000DC020000 + + + 16 + 0A0000000A0000006E0000006E000000 + + + + 59399 + 59399 + 1 + 0 + 0 + 0 + 32767 + 0 + 8192 + 1 + + 16 + 000000001C000000E701000038000000 + + + 16 + 0A0000000A0000006E0000006E000000 + + + + 59400 + 59400 + 0 + 0 + 0 + 0 + 32767 + 0 + 8192 + 2 + + 16 + 00000000380000006F02000054000000 + + + 16 + 0A0000000A0000006E0000006E000000 + + + + 824 + 824 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + D8000000EF000000AD010000BF010000 + + + + 3312 + 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 + + + 59392 + File + + 2537 + 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 + + + 1423 + 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 + + + 1423 + 2800FFFF01001100434D4643546F6F6C426172427574746F6E00E100000000000000000000000000000000000000000000000100000001000000018001E100000000000001000000000000000000000000000000000100000001000000018003E1000000000000020000000000000000000000000000000001000000010000000180CD7F0000000000000300000000000000000000000000000000010000000100000001800000000001000000FFFFFFFF000000000000000000000000000100000001000000018023E100000000000004000000000000000000000000000000000100000001000000018022E100000000000005000000000000000000000000000000000100000001000000018025E10000000000000600000000000000000000000000000000010000000100000001800000000001000000FFFFFFFF00000000000000000000000000010000000100000001802BE10000000000000700000000000000000000000000000000010000000100000001802CE10000000000000800000000000000000000000000000000010000000100000001800000000001000000FFFFFFFF00000000000000000000000000010000000100000001807A8A0000000000000900000000000000000000000000000000010000000100000001807B8A0000000000000A00000000000000000000000000000000010000000100000001800000000001000000FFFFFFFF0000000000000000000000000001000000010000000180D3B00000000000000B000000000000000000000000000000000100000001000000018015B10000000000000C0000000000000000000000000000000001000000010000000180F4B00000000000000D000000000000000000000000000000000100000001000000018036B10000000000000E00000000000000000000000000000000010000000100000001800000000001000000FFFFFFFF0000000000000000000000000001000000010000000180FF880000000000000F0000000000000000000000000000000001000000010000000180FE880000000000001000000000000000000000000000000000010000000100000001800B810000000000001100000000000000000000000000000000010000000100000001800C810000000000001200000000000000000000000000000000010000000100000001800000000001000000FFFFFFFF0000000000000000000000000001000000010000000180F088000000000000130000000000000000000000000000000001000000010000000180EE7F00000000000014000000000000000000000000000000000100000001000000018024E10000000000001500000000000000000000000000000000010000000100000001800A810000000000001600000000000000000000000000000000010000000100000001800000000001000000FFFFFFFF000000000000000000000000000100000001000000018022800000000000001700000000000000000000000000000000010000000100000001800000000001000000FFFFFFFF0000000000000000000000000001000000010000000180C488000000000000180000000000000000000000000000000001000000010000000180C988000000000000190000000000000000000000000000000001000000010000000180C7880000000000001A0000000000000000000000000000000001000000010000000180C8880000000000001B00000000000000000000000000000000010000000100000001800000000001000000FFFFFFFF0000000000000000000000000001000000010000000180DD880000000000001C00000000000000000000000000000000010000000100000001800000000001000000FFFFFFFF0000000000000000000000000001000000010000000180FB7F0000000000001D000000000000000000000000000000000100000001000000 + + + + 59399 + Build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ebug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diff --git a/MDK-ARM/arm.uvguix.yxm23 b/MDK-ARM/arm.uvguix.yxm23 new file mode 100644 index 0000000..d4da26f --- /dev/null +++ b/MDK-ARM/arm.uvguix.yxm23 @@ -0,0 +1,1860 @@ + + + + -6.1 + +
### uVision Project, (C) Keil Software
+ + + + + + + + + + 38003 + Registers + 124 79 + + + 346 + Code Coverage + 877 160 + + + 204 + Performance Analyzer + 1037 + + + + + + 35141 + Event Statistics + + 200 50 700 + + + 1506 + Symbols + + 71 71 71 + + + 1936 + Watch 1 + + 200 133 133 + + + 1937 + Watch 2 + + 200 133 133 + + + 1935 + Call Stack + Locals + + 200 133 133 + + + 2506 + Trace Data + + 75 135 130 95 70 230 200 150 + + + 466 + Source Browser + 500 + 300 + + + + + + + + 1 + 1 + 0 + 0 + -1 + + + + + + + 44 + 0 + 1 + + -1 + -1 + + + -1 + -1 + + + 282 + 227 + 1368 + 1040 + + + + 0 + + 60 + 010000000400000001000000010000000100000001000000000000000200000000000000010000000100000000000000280000002800000000000000 + + + + 0 + Build + + -1 + -1 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + D90000004F000000F0040000E1000000 + + + 16 + 8A000000A10000000203000033010000 + + + + 1005 + 1005 + 1 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 0300000066000000D20000000D020000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 109 + 109 + 1 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 0300000066000000D20000000D020000 + + + 16 + 8A000000A100000086010000EB020000 + + + + 1465 + 1465 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 0000000037020000F0040000C9020000 + + + 16 + 8A000000A10000000203000033010000 + + + + 1466 + 1466 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + 8A000000A10000000203000033010000 + + + + 1467 + 1467 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + 8A000000A10000000203000033010000 + + + + 1468 + 1468 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + 8A000000A10000000203000033010000 + + + + 1506 + 1506 + 0 + 0 + 0 + 0 + 32767 + 0 + 16384 + 0 + + 16 + 1E04000066000000ED04000006010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 1913 + 1913 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + DC00000066000000ED040000C8000000 + + + 16 + 8A000000A10000000203000033010000 + + + + 1935 + 1935 + 0 + 0 + 0 + 0 + 32767 + 0 + 32768 + 0 + + 16 + 030000003A020000ED040000B0020000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 1936 + 1936 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 1937 + 1937 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 1939 + 1939 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + 8A000000A10000000203000033010000 + + + + 1940 + 1940 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + 8A000000A10000000203000033010000 + + + + 1941 + 1941 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + 8A000000A10000000203000033010000 + + + + 1942 + 1942 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + 8A000000A10000000203000033010000 + + + + 195 + 195 + 1 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 0300000066000000D20000000D020000 + + + 16 + 8A000000A100000086010000EB020000 + + + + 196 + 196 + 1 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 0300000066000000D20000000D020000 + + + 16 + 8A000000A100000086010000EB020000 + + + + 197 + 197 + 1 + 0 + 0 + 0 + 32767 + 0 + 32768 + 0 + + 16 + 000000003E02000065040000BC020000 + + + 16 + 8A000000A10000000203000033010000 + + + + 198 + 198 + 0 + 0 + 0 + 0 + 32767 + 0 + 32768 + 0 + + 16 + 0000000023020000F0040000C9020000 + + + 16 + 8A000000A10000000203000033010000 + + + + 199 + 199 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 0300000041020000ED040000A3020000 + + + 16 + 8A000000A10000000203000033010000 + + + + 203 + 203 + 0 + 0 + 0 + 0 + 32767 + 0 + 8192 + 0 + + 16 + D900000063000000F0040000E1000000 + + + 16 + 8A000000A10000000203000033010000 + + + + 204 + 204 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + DC00000066000000ED040000C8000000 + + + 16 + 8A000000A10000000203000033010000 + + + + 221 + 221 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 00000000000000000000000000000000 + + + 16 + 0A0000000A0000006E0000006E000000 + + + + 2506 + 2506 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1B04000063000000F004000033020000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 2507 + 2507 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 0000000037020000F0040000B5020000 + + + 16 + 8A000000A10000000203000033010000 + + + + 343 + 343 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + DC00000066000000ED040000C8000000 + + + 16 + 8A000000A10000000203000033010000 + + + + 346 + 346 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + DC00000066000000ED040000C8000000 + + + 16 + 8A000000A10000000203000033010000 + + + + 35141 + 35141 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + D900000063000000F0040000E1000000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35824 + 35824 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + DC00000066000000ED040000C8000000 + + + 16 + 8A000000A10000000203000033010000 + + + + 35885 + 35885 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35886 + 35886 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35887 + 35887 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35888 + 35888 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35889 + 35889 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35890 + 35890 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35891 + 35891 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35892 + 35892 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35893 + 35893 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35894 + 35894 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35895 + 35895 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35896 + 35896 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35897 + 35897 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35898 + 35898 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35899 + 35899 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35900 + 35900 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35901 + 35901 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35902 + 35902 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35903 + 35903 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35904 + 35904 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 35905 + 35905 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 38003 + 38003 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 0300000066000000D2000000B0020000 + + + 16 + 8A000000A100000086010000EB020000 + + + + 38007 + 38007 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 000000003E020000F0040000BC020000 + + + 16 + 8A000000A10000000203000033010000 + + + + 436 + 436 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 0300000041020000ED040000A3020000 + + + 16 + 8A000000A100000086010000EB020000 + + + + 437 + 437 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 440 + 440 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 463 + 463 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 0300000041020000ED040000A3020000 + + + 16 + 8A000000A100000086010000EB020000 + + + + 466 + 466 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 0300000041020000ED040000A3020000 + + + 16 + 8A000000A100000086010000EB020000 + + + + 470 + 470 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + DC00000066000000ED040000C8000000 + + + 16 + 8A000000A10000000203000033010000 + + + + 50000 + 50000 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 50001 + 50001 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 50002 + 50002 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 50003 + 50003 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 50004 + 50004 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 50005 + 50005 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 50006 + 50006 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 50007 + 50007 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 50008 + 50008 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 50009 + 50009 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 50010 + 50010 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 50011 + 50011 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 50012 + 50012 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 50013 + 50013 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 50014 + 50014 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 50015 + 50015 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 50016 + 50016 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 50017 + 50017 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 50018 + 50018 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 50019 + 50019 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 1E04000066000000ED04000084010000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 59392 + 59392 + 1 + 0 + 0 + 0 + 32767 + 0 + 8192 + 0 + + 16 + 0000000000000000D10300001C000000 + + + 16 + 0A0000000A0000006E0000006E000000 + + + + 59393 + 0 + 1 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 00000000BC02000065040000CF020000 + + + 16 + 0A0000000A0000006E0000006E000000 + + + + 59399 + 59399 + 1 + 0 + 0 + 0 + 32767 + 0 + 8192 + 1 + + 16 + 000000001C000000E701000038000000 + + + 16 + 0A0000000A0000006E0000006E000000 + + + + 59400 + 59400 + 0 + 0 + 0 + 0 + 32767 + 0 + 8192 + 2 + + 16 + 00000000380000006F02000054000000 + + + 16 + 0A0000000A0000006E0000006E000000 + + + + 824 + 824 + 0 + 0 + 0 + 0 + 32767 + 0 + 4096 + 0 + + 16 + 030000003A020000ED0400009C020000 + + + 16 + 8A000000A10000005F01000071010000 + + + + 3312 + 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 + + + 59392 + File + + 3003 + 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 + + + 1423 + 2800FFFF01001100434D4643546F6F6C426172427574746F6E00E1000000000000FFFFFFFF000100000000000000010000000000000001000000018001E1000000000000FFFFFFFF000100000000000000010000000000000001000000018003E1000000000000FFFFFFFF0001000000000000000100000000000000010000000180CD7F000000000000FFFFFFFF00010000000000000001000000000000000100000001800000000000000000FFFFFFFF000000000000000000010000000000000001000000018023E1000000000000FFFFFFFF000100000000000000010000000000000001000000018022E1000000000000FFFFFFFF000100000000000000010000000000000001000000018025E1000000000000FFFFFFFF00010000000000000001000000000000000100000001800000000000000000FFFFFFFF00000000000000000001000000000000000100000001802BE1000000000000FFFFFFFF00010000000000000001000000000000000100000001802CE1000000000000FFFFFFFF00010000000000000001000000000000000100000001800000000000000000FFFFFFFF00000000000000000001000000000000000100000001807A8A000000000000FFFFFFFF00010000000000000001000000000000000100000001807B8A000000000000FFFFFFFF00010000000000000001000000000000000100000001800000000000000000FFFFFFFF0000000000000000000100000000000000010000000180D3B0000000000000FFFFFFFF000100000000000000010000000000000001000000018015B1000000000000FFFFFFFF0001000000000000000100000000000000010000000180F4B0000000000000FFFFFFFF000100000000000000010000000000000001000000018036B1000000000000FFFFFFFF00010000000000000001000000000000000100000001800000000000000000FFFFFFFF0000000000000000000100000000000000010000000180FF88000000000000FFFFFFFF0001000000000000000100000000000000010000000180FE88000000000000FFFFFFFF00010000000000000001000000000000000100000001800B81000000000000FFFFFFFF00010000000000000001000000000000000100000001800C81000000000000FFFFFFFF00010000000000000001000000000000000100000001800000000000000000FFFFFFFF0000000000000000000100000000000000010000000180F088000000000000FFFFFFFF0001000000000000000100000000000000010000000180EE7F000000000000FFFFFFFF000100000000000000010000000000000001000000018024E1000000000000FFFFFFFF00010000000000000001000000000000000100000001800A81000000000000FFFFFFFF00010000000000000001000000000000000100000001800000000000000000FFFFFFFF00000000000000000001000000000000000100000001802280000000000000FFFFFFFF00010000000000000001000000000000000100000001800000000000000000FFFFFFFF0000000000000000000100000000000000010000000180C488000000000000FFFFFFFF0001000000000000000100000000000000010000000180C988000000000000FFFFFFFF0001000000000000000100000000000000010000000180C788000000000000FFFFFFFF0001000000000000000100000000000000010000000180C888000000000000FFFFFFFF00010000000000000001000000000000000100000001800000000000000000FFFFFFFF0000000000000000000100000000000000010000000180DD88000000000000FFFFFFFF00010000000000000001000000000000000100000001800000000000000000FFFFFFFF0000000000000000000100000000000000010000000180FB7F000000000000FFFFFFFF000100000000000000010000000000000001000000 + + + 1423 + 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 + + + + 59399 + Build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ebug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diff --git a/MDK-ARM/arm.uvoptx b/MDK-ARM/arm.uvoptx new file mode 100644 index 0000000..3214765 --- /dev/null +++ b/MDK-ARM/arm.uvoptx @@ -0,0 +1,672 @@ + + + + 1.0 + +
### uVision Project, (C) Keil Software
+ + + *.c + *.s*; *.src; *.a* + *.obj; *.o + *.lib + *.txt; *.h; *.inc; *.md + *.plm + *.cpp + 0 + + + + 0 + 0 + + + + arm + 0x4 + ARM-ADS + + 12000000 + + 1 + 1 + 0 + 1 + 0 + + + 1 + 65535 + 0 + 0 + 0 + + + 79 + 66 + 8 + + + + 1 + 1 + 1 + 0 + 1 + 1 + 0 + 1 + 0 + 0 + 0 + 0 + + + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 0 + + + 1 + 0 + 1 + + 18 + + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 0 + 1 + 0 + 0 + 6 + + + + + + + + + + + STLink\ST-LINKIII-KEIL_SWO.dll + + + + 0 + UL2CM3 + UL2CM3(-S0 -C0 -P0 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024 -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)) + + + 0 + ST-LINKIII-KEIL_SWO + -U-O142 -O2254 -S0 -C0 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM) + + + + + 0 + + + 0 + 1 + 1 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + + + + 0 + 0 + 0 + + + + + + + + + + 1 + 1 + 0 + 2 + 10000000 + + + + + + Application/MDK-ARM + 0 + 0 + 0 + 0 + + 1 + 1 + 2 + 0 + 0 + 0 + startup_stm32f407xx.s + startup_stm32f407xx.s + 0 + 0 + + + + + Application/User/Core + 0 + 0 + 0 + 0 + + 2 + 2 + 1 + 0 + 0 + 0 + ../Core/Src/main.c + main.c + 0 + 0 + + + 2 + 3 + 1 + 0 + 0 + 0 + ../Core/Src/gpio.c + gpio.c + 0 + 0 + + + 2 + 4 + 1 + 0 + 0 + 0 + ../Core/Src/freertos.c + freertos.c + 0 + 0 + + + 2 + 5 + 1 + 0 + 0 + 0 + ../Core/Src/can.c + can.c + 0 + 0 + + + 2 + 6 + 1 + 0 + 0 + 0 + ../Core/Src/tim.c + tim.c + 0 + 0 + + + 2 + 7 + 1 + 0 + 0 + 0 + ../Core/Src/stm32f4xx_it.c + stm32f4xx_it.c + 0 + 0 + + + 2 + 8 + 1 + 0 + 0 + 0 + ../Core/Src/stm32f4xx_hal_msp.c + stm32f4xx_hal_msp.c + 0 + 0 + + + + + Drivers/STM32F4xx_HAL_Driver + 1 + 0 + 0 + 0 + + 3 + 9 + 1 + 0 + 0 + 0 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c + stm32f4xx_hal_can.c + 0 + 0 + + + 3 + 10 + 1 + 0 + 0 + 0 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c + stm32f4xx_hal_rcc.c + 0 + 0 + + + 3 + 11 + 1 + 0 + 0 + 0 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c + stm32f4xx_hal_rcc_ex.c + 0 + 0 + + + 3 + 12 + 1 + 0 + 0 + 0 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c + stm32f4xx_hal_flash.c + 0 + 0 + + + 3 + 13 + 1 + 0 + 0 + 0 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c + stm32f4xx_hal_flash_ex.c + 0 + 0 + + + 3 + 14 + 1 + 0 + 0 + 0 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c + stm32f4xx_hal_flash_ramfunc.c + 0 + 0 + + + 3 + 15 + 1 + 0 + 0 + 0 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c + stm32f4xx_hal_gpio.c + 0 + 0 + + + 3 + 16 + 1 + 0 + 0 + 0 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c + stm32f4xx_hal_dma_ex.c + 0 + 0 + + + 3 + 17 + 1 + 0 + 0 + 0 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c + stm32f4xx_hal_dma.c + 0 + 0 + + + 3 + 18 + 1 + 0 + 0 + 0 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c + stm32f4xx_hal_pwr.c + 0 + 0 + + + 3 + 19 + 1 + 0 + 0 + 0 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c + stm32f4xx_hal_pwr_ex.c + 0 + 0 + + + 3 + 20 + 1 + 0 + 0 + 0 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c + stm32f4xx_hal_cortex.c + 0 + 0 + + + 3 + 21 + 1 + 0 + 0 + 0 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c + stm32f4xx_hal.c + 0 + 0 + + + 3 + 22 + 1 + 0 + 0 + 0 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c + stm32f4xx_hal_exti.c + 0 + 0 + + + 3 + 23 + 1 + 0 + 0 + 0 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c + stm32f4xx_hal_tim.c + 0 + 0 + + + 3 + 24 + 1 + 0 + 0 + 0 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c + stm32f4xx_hal_tim_ex.c + 0 + 0 + + + + + Drivers/CMSIS + 1 + 0 + 0 + 0 + + 4 + 25 + 1 + 0 + 0 + 0 + ../Core/Src/system_stm32f4xx.c + system_stm32f4xx.c + 0 + 0 + + + + + Middlewares/FreeRTOS + 0 + 0 + 0 + 0 + + 5 + 26 + 1 + 0 + 0 + 0 + ../Middlewares/Third_Party/FreeRTOS/Source/croutine.c + croutine.c + 0 + 0 + + + 5 + 27 + 1 + 0 + 0 + 0 + ../Middlewares/Third_Party/FreeRTOS/Source/event_groups.c + event_groups.c + 0 + 0 + + + 5 + 28 + 1 + 0 + 0 + 0 + ../Middlewares/Third_Party/FreeRTOS/Source/list.c + list.c + 0 + 0 + + + 5 + 29 + 1 + 0 + 0 + 0 + ../Middlewares/Third_Party/FreeRTOS/Source/queue.c + queue.c + 0 + 0 + + + 5 + 30 + 1 + 0 + 0 + 0 + ../Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c + stream_buffer.c + 0 + 0 + + + 5 + 31 + 1 + 0 + 0 + 0 + ../Middlewares/Third_Party/FreeRTOS/Source/tasks.c + tasks.c + 0 + 0 + + + 5 + 32 + 1 + 0 + 0 + 0 + ../Middlewares/Third_Party/FreeRTOS/Source/timers.c + timers.c + 0 + 0 + + + 5 + 33 + 1 + 0 + 0 + 0 + ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c + cmsis_os2.c + 0 + 0 + + + 5 + 34 + 1 + 0 + 0 + 0 + ../Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c + heap_4.c + 0 + 0 + + + 5 + 35 + 1 + 0 + 0 + 0 + ../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F/port.c + port.c + 0 + 0 + + + + + Middlewares/Library/DSP Library/DSP Library + 0 + 0 + 0 + 0 + + 6 + 36 + 4 + 0 + 0 + 0 + ../Middlewares/ST/ARM/DSP/Lib/arm_cortexM4lf_math.lib + arm_cortexM4lf_math.lib + 0 + 0 + + + + + ::CMSIS + 0 + 0 + 0 + 1 + + + diff --git a/MDK-ARM/arm.uvprojx b/MDK-ARM/arm.uvprojx new file mode 100644 index 0000000..8ceb73b --- /dev/null +++ b/MDK-ARM/arm.uvprojx @@ -0,0 +1,710 @@ + + + + 2.1 + +
### uVision Project, (C) Keil Software
+ + + + arm + 0x4 + ARM-ADS + 0 + + + STM32F407IGHx + STMicroelectronics + Keil.STM32F4xx_DFP.3.1.1 + https://www.keil.com/pack/ + IRAM(0x20000000-0x2001BFFF) IRAM2(0x2001C000-0x2001FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) FPU2 CPUTYPE("Cortex-M4") TZ + + + + 0 + + + + + + + + + + + $$Device:STM32F407IGHx$CMSIS\SVD\STM32F40x.svd + 0 + 0 + + + + + + + 0 + 0 + 0 + 0 + 1 + + arm\ + arm + 1 + 0 + 1 + 1 + 1 + + 1 + 0 + 0 + + 0 + 0 + + + 0 + 0 + 0 + 0 + + + 0 + 0 + + + 0 + 0 + 0 + 0 + + + 0 + 1 + + + 0 + 0 + 0 + 0 + + 1 + + + + 0 + 0 + 0 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 3 + + + 0 + + + SARMCM3.DLL + -REMAP -MPU + DCM.DLL + -pCM4 + SARMCM3.DLL + -MPU + TCM.DLL + -pCM4 + + + + 1 + 0 + 0 + 0 + 16 + + + + + 1 + 0 + 0 + 1 + 1 + 4101 + + 1 + BIN\UL2V8M.DLL + + + + + + 0 + + + + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 1 + 1 + 0 + 1 + 1 + 0 + 0 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 1 + 0 + 0 + "Cortex-M4" + + 0 + 0 + 0 + 1 + 1 + 0 + 0 + 2 + 0 + 0 + 0 + 1 + 0 + 8 + 0 + 0 + 0 + 0 + 3 + 4 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 1 + 1 + 0 + 0 + 0 + 1 + 1 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x20000000 + 0x1c000 + + + 1 + 0x8000000 + 0x100000 + + + 0 + 0x0 + 0x0 + + + 1 + 0x0 + 0x0 + + + 1 + 0x0 + 0x0 + + + 1 + 0x0 + 0x0 + + + 1 + 0x8000000 + 0x100000 + + + 1 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x0 + 0x0 + + + 0 + 0x20000000 + 0x1c000 + + + 0 + 0x2001c000 + 0x4000 + + + + + + 1 + 4 + 0 + 0 + 1 + 0 + 0 + 0 + 0 + 0 + 2 + 0 + 0 + 1 + 0 + 0 + 5 + 3 + 1 + 1 + 0 + 0 + 0 + + + USE_HAL_DRIVER,STM32F407xx + + ../Core/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include;../Middlewares/ST/ARM/DSP/Inc + + + + 1 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 0 + 1 + + + + + ../Core/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include;../Middlewares/ST/ARM/DSP/Inc + + + + 1 + 0 + 0 + 0 + 1 + 0 + + + + + + + + + + + + + + + Application/MDK-ARM + + + startup_stm32f407xx.s + 2 + startup_stm32f407xx.s + + + + + Application/User/Core + + + main.c + 1 + ../Core/Src/main.c + + + gpio.c + 1 + ../Core/Src/gpio.c + + + freertos.c + 1 + ../Core/Src/freertos.c + + + can.c + 1 + ../Core/Src/can.c + + + tim.c + 1 + ../Core/Src/tim.c + + + stm32f4xx_it.c + 1 + ../Core/Src/stm32f4xx_it.c + + + stm32f4xx_hal_msp.c + 1 + ../Core/Src/stm32f4xx_hal_msp.c + + + + + Drivers/STM32F4xx_HAL_Driver + + + stm32f4xx_hal_can.c + 1 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c + + + stm32f4xx_hal_rcc.c + 1 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c + + + stm32f4xx_hal_rcc_ex.c + 1 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c + + + stm32f4xx_hal_flash.c + 1 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c + + + stm32f4xx_hal_flash_ex.c + 1 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c + + + stm32f4xx_hal_flash_ramfunc.c + 1 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c + + + stm32f4xx_hal_gpio.c + 1 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c + + + stm32f4xx_hal_dma_ex.c + 1 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c + + + stm32f4xx_hal_dma.c + 1 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c + + + stm32f4xx_hal_pwr.c + 1 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c + + + stm32f4xx_hal_pwr_ex.c + 1 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c + + + stm32f4xx_hal_cortex.c + 1 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c + + + stm32f4xx_hal.c + 1 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c + + + stm32f4xx_hal_exti.c + 1 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c + + + stm32f4xx_hal_tim.c + 1 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c + + + stm32f4xx_hal_tim_ex.c + 1 + ../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c + + + + + Drivers/CMSIS + + + system_stm32f4xx.c + 1 + ../Core/Src/system_stm32f4xx.c + + + + + Middlewares/FreeRTOS + + + croutine.c + 1 + ../Middlewares/Third_Party/FreeRTOS/Source/croutine.c + + + event_groups.c + 1 + ../Middlewares/Third_Party/FreeRTOS/Source/event_groups.c + + + list.c + 1 + ../Middlewares/Third_Party/FreeRTOS/Source/list.c + + + queue.c + 1 + ../Middlewares/Third_Party/FreeRTOS/Source/queue.c + + + stream_buffer.c + 1 + ../Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c + + + tasks.c + 1 + ../Middlewares/Third_Party/FreeRTOS/Source/tasks.c + + + timers.c + 1 + ../Middlewares/Third_Party/FreeRTOS/Source/timers.c + + + cmsis_os2.c + 1 + ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c + + + heap_4.c + 1 + ../Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c + + + port.c + 1 + ../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F/port.c + + + + + Middlewares/Library/DSP Library/DSP Library + + + 0 + 0 + 0 + 0 + 0 + 2 + 2 + 2 + 2 + 2 + 11 + + + 1 + + + + 2 + 0 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 4 + 2 + 2 + 2 + 2 + 2 + 0 + 0 + 2 + 2 + 2 + 2 + 2 + + + + + + + + + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 2 + 1 + + + + + + + + + + + + arm_cortexM4lf_math.lib + 4 + ../Middlewares/ST/ARM/DSP/Lib/arm_cortexM4lf_math.lib + + + 2 + 0 + 0 + 0 + 0 + 1 + 2 + 2 + 2 + 2 + 11 + + + 1 + + + + + + + + ::CMSIS + + + + + + + + + + + + + + + + + + + + + + arm + 1 + + + + + diff --git a/MDK-ARM/startup_stm32f407xx.s b/MDK-ARM/startup_stm32f407xx.s new file mode 100644 index 0000000..d382ef3 --- /dev/null +++ b/MDK-ARM/startup_stm32f407xx.s @@ -0,0 +1,422 @@ +;******************************************************************************* +;* File Name : startup_stm32f407xx.s +;* Author : MCD Application Team +;* Description : STM32F407xx devices vector table for MDK-ARM toolchain. +;* This module performs: +;* - Set the initial SP +;* - Set the initial PC == Reset_Handler +;* - Set the vector table entries with the exceptions ISR address +;* - Branches to __main in the C library (which eventually +;* calls main()). +;* After Reset the CortexM4 processor is in Thread mode, +;* priority is Privileged, and the Stack is set to Main. +;******************************************************************************* +;* @attention +;* +;* Copyright (c) 2017 STMicroelectronics. +;* All rights reserved. +;* +;* This software is licensed under terms that can be found in the LICENSE file +;* in the root directory of this software component. +;* If no LICENSE file comes with this software, it is provided AS-IS. +;* +;******************************************************************************* +;* <<< Use Configuration Wizard in Context Menu >>> +; +; Amount of memory (in bytes) allocated for Stack +; Tailor this value to your application needs +; Stack Configuration +; Stack Size (in Bytes) <0x0-0xFFFFFFFF:8> +; + +Stack_Size EQU 0x2000 + + AREA STACK, NOINIT, READWRITE, ALIGN=3 +Stack_Mem SPACE Stack_Size +__initial_sp + + +; Heap Configuration +; Heap Size (in Bytes) <0x0-0xFFFFFFFF:8> +; + +Heap_Size EQU 0x2000 + + AREA HEAP, NOINIT, READWRITE, ALIGN=3 +__heap_base +Heap_Mem SPACE Heap_Size +__heap_limit + + PRESERVE8 + THUMB + + +; Vector Table Mapped to Address 0 at Reset + AREA RESET, DATA, READONLY + EXPORT __Vectors + EXPORT __Vectors_End + EXPORT __Vectors_Size + +__Vectors DCD __initial_sp ; Top of Stack + DCD Reset_Handler ; Reset Handler + DCD NMI_Handler ; NMI Handler + DCD HardFault_Handler ; Hard Fault Handler + DCD MemManage_Handler ; MPU Fault Handler + DCD BusFault_Handler ; Bus Fault Handler + DCD UsageFault_Handler ; Usage Fault Handler + DCD 0 ; Reserved + DCD 0 ; Reserved + DCD 0 ; Reserved + DCD 0 ; Reserved + DCD SVC_Handler ; SVCall Handler + DCD DebugMon_Handler ; Debug Monitor Handler + DCD 0 ; Reserved + DCD PendSV_Handler ; PendSV Handler + DCD SysTick_Handler ; SysTick Handler + + ; External Interrupts + DCD WWDG_IRQHandler ; Window WatchDog + DCD PVD_IRQHandler ; PVD through EXTI Line detection + DCD TAMP_STAMP_IRQHandler ; Tamper and TimeStamps through the EXTI line + DCD RTC_WKUP_IRQHandler ; RTC Wakeup through the EXTI line + DCD FLASH_IRQHandler ; FLASH + DCD RCC_IRQHandler ; RCC + DCD EXTI0_IRQHandler ; EXTI Line0 + DCD EXTI1_IRQHandler ; EXTI Line1 + DCD EXTI2_IRQHandler ; EXTI Line2 + DCD EXTI3_IRQHandler ; EXTI Line3 + DCD EXTI4_IRQHandler ; EXTI Line4 + DCD DMA1_Stream0_IRQHandler ; DMA1 Stream 0 + DCD DMA1_Stream1_IRQHandler ; DMA1 Stream 1 + DCD DMA1_Stream2_IRQHandler ; DMA1 Stream 2 + DCD DMA1_Stream3_IRQHandler ; DMA1 Stream 3 + DCD DMA1_Stream4_IRQHandler ; DMA1 Stream 4 + DCD DMA1_Stream5_IRQHandler ; DMA1 Stream 5 + DCD DMA1_Stream6_IRQHandler ; DMA1 Stream 6 + DCD ADC_IRQHandler ; ADC1, ADC2 and ADC3s + DCD CAN1_TX_IRQHandler ; CAN1 TX + DCD CAN1_RX0_IRQHandler ; CAN1 RX0 + DCD CAN1_RX1_IRQHandler ; CAN1 RX1 + DCD CAN1_SCE_IRQHandler ; CAN1 SCE + DCD EXTI9_5_IRQHandler ; External Line[9:5]s + DCD TIM1_BRK_TIM9_IRQHandler ; TIM1 Break and TIM9 + DCD TIM1_UP_TIM10_IRQHandler ; TIM1 Update and TIM10 + DCD TIM1_TRG_COM_TIM11_IRQHandler ; TIM1 Trigger and Commutation and TIM11 + DCD TIM1_CC_IRQHandler ; TIM1 Capture Compare + DCD TIM2_IRQHandler ; TIM2 + DCD TIM3_IRQHandler ; TIM3 + DCD TIM4_IRQHandler ; TIM4 + DCD I2C1_EV_IRQHandler ; I2C1 Event + DCD I2C1_ER_IRQHandler ; I2C1 Error + DCD I2C2_EV_IRQHandler ; I2C2 Event + DCD I2C2_ER_IRQHandler ; I2C2 Error + DCD SPI1_IRQHandler ; SPI1 + DCD SPI2_IRQHandler ; SPI2 + DCD USART1_IRQHandler ; USART1 + DCD USART2_IRQHandler ; USART2 + DCD USART3_IRQHandler ; USART3 + DCD EXTI15_10_IRQHandler ; External Line[15:10]s + DCD RTC_Alarm_IRQHandler ; RTC Alarm (A and B) through EXTI Line + DCD OTG_FS_WKUP_IRQHandler ; USB OTG FS Wakeup through EXTI line + DCD TIM8_BRK_TIM12_IRQHandler ; TIM8 Break and TIM12 + DCD TIM8_UP_TIM13_IRQHandler ; TIM8 Update and TIM13 + DCD TIM8_TRG_COM_TIM14_IRQHandler ; TIM8 Trigger and Commutation and TIM14 + DCD TIM8_CC_IRQHandler ; TIM8 Capture Compare + DCD DMA1_Stream7_IRQHandler ; DMA1 Stream7 + DCD FMC_IRQHandler ; FMC + DCD SDIO_IRQHandler ; SDIO + DCD TIM5_IRQHandler ; TIM5 + DCD SPI3_IRQHandler ; SPI3 + DCD UART4_IRQHandler ; UART4 + DCD UART5_IRQHandler ; UART5 + DCD TIM6_DAC_IRQHandler ; TIM6 and DAC1&2 underrun errors + DCD TIM7_IRQHandler ; TIM7 + DCD DMA2_Stream0_IRQHandler ; DMA2 Stream 0 + DCD DMA2_Stream1_IRQHandler ; DMA2 Stream 1 + DCD DMA2_Stream2_IRQHandler ; DMA2 Stream 2 + DCD DMA2_Stream3_IRQHandler ; DMA2 Stream 3 + DCD DMA2_Stream4_IRQHandler ; DMA2 Stream 4 + DCD ETH_IRQHandler ; Ethernet + DCD ETH_WKUP_IRQHandler ; Ethernet Wakeup through EXTI line + DCD CAN2_TX_IRQHandler ; CAN2 TX + DCD CAN2_RX0_IRQHandler ; CAN2 RX0 + DCD CAN2_RX1_IRQHandler ; CAN2 RX1 + DCD CAN2_SCE_IRQHandler ; CAN2 SCE + DCD OTG_FS_IRQHandler ; USB OTG FS + DCD DMA2_Stream5_IRQHandler ; DMA2 Stream 5 + DCD DMA2_Stream6_IRQHandler ; DMA2 Stream 6 + DCD DMA2_Stream7_IRQHandler ; DMA2 Stream 7 + DCD USART6_IRQHandler ; USART6 + DCD I2C3_EV_IRQHandler ; I2C3 event + DCD I2C3_ER_IRQHandler ; I2C3 error + DCD OTG_HS_EP1_OUT_IRQHandler ; USB OTG HS End Point 1 Out + DCD OTG_HS_EP1_IN_IRQHandler ; USB OTG HS End Point 1 In + DCD OTG_HS_WKUP_IRQHandler ; USB OTG HS Wakeup through EXTI + DCD OTG_HS_IRQHandler ; USB OTG HS + DCD DCMI_IRQHandler ; DCMI + DCD 0 ; Reserved + DCD HASH_RNG_IRQHandler ; Hash and Rng + DCD FPU_IRQHandler ; FPU + + +__Vectors_End + +__Vectors_Size EQU __Vectors_End - __Vectors + + AREA |.text|, CODE, READONLY + +; Reset handler +Reset_Handler PROC + EXPORT Reset_Handler [WEAK] + IMPORT SystemInit + IMPORT __main + + LDR R0, =SystemInit + BLX R0 + LDR R0, =__main + BX R0 + ENDP + +; Dummy Exception Handlers (infinite loops which can be modified) + +NMI_Handler PROC + EXPORT NMI_Handler [WEAK] + B . + ENDP +HardFault_Handler\ + PROC + EXPORT HardFault_Handler [WEAK] + B . + ENDP +MemManage_Handler\ + PROC + EXPORT MemManage_Handler [WEAK] + B . + ENDP +BusFault_Handler\ + PROC + EXPORT BusFault_Handler [WEAK] + B . + ENDP +UsageFault_Handler\ + PROC + EXPORT UsageFault_Handler [WEAK] + B . + ENDP +SVC_Handler PROC + EXPORT SVC_Handler [WEAK] + B . + ENDP +DebugMon_Handler\ + PROC + EXPORT DebugMon_Handler [WEAK] + B . + ENDP +PendSV_Handler PROC + EXPORT PendSV_Handler [WEAK] + B . + ENDP +SysTick_Handler PROC + EXPORT SysTick_Handler [WEAK] + B . + ENDP + +Default_Handler PROC + + EXPORT WWDG_IRQHandler [WEAK] + EXPORT PVD_IRQHandler [WEAK] + EXPORT TAMP_STAMP_IRQHandler [WEAK] + EXPORT RTC_WKUP_IRQHandler [WEAK] + EXPORT FLASH_IRQHandler [WEAK] + EXPORT RCC_IRQHandler [WEAK] + EXPORT EXTI0_IRQHandler [WEAK] + EXPORT EXTI1_IRQHandler [WEAK] + EXPORT EXTI2_IRQHandler [WEAK] + EXPORT EXTI3_IRQHandler [WEAK] + EXPORT EXTI4_IRQHandler [WEAK] + EXPORT DMA1_Stream0_IRQHandler [WEAK] + EXPORT DMA1_Stream1_IRQHandler [WEAK] + EXPORT DMA1_Stream2_IRQHandler [WEAK] + EXPORT DMA1_Stream3_IRQHandler [WEAK] + EXPORT DMA1_Stream4_IRQHandler [WEAK] + EXPORT DMA1_Stream5_IRQHandler [WEAK] + EXPORT DMA1_Stream6_IRQHandler [WEAK] + EXPORT ADC_IRQHandler [WEAK] + EXPORT CAN1_TX_IRQHandler [WEAK] + EXPORT CAN1_RX0_IRQHandler [WEAK] + EXPORT CAN1_RX1_IRQHandler [WEAK] + EXPORT CAN1_SCE_IRQHandler [WEAK] + EXPORT EXTI9_5_IRQHandler [WEAK] + EXPORT TIM1_BRK_TIM9_IRQHandler [WEAK] + EXPORT TIM1_UP_TIM10_IRQHandler [WEAK] + EXPORT TIM1_TRG_COM_TIM11_IRQHandler [WEAK] + EXPORT TIM1_CC_IRQHandler [WEAK] + EXPORT TIM2_IRQHandler [WEAK] + EXPORT TIM3_IRQHandler [WEAK] + EXPORT TIM4_IRQHandler [WEAK] + EXPORT I2C1_EV_IRQHandler [WEAK] + EXPORT I2C1_ER_IRQHandler [WEAK] + EXPORT I2C2_EV_IRQHandler [WEAK] + EXPORT I2C2_ER_IRQHandler [WEAK] + EXPORT SPI1_IRQHandler [WEAK] + EXPORT SPI2_IRQHandler [WEAK] + EXPORT USART1_IRQHandler [WEAK] + EXPORT USART2_IRQHandler [WEAK] + EXPORT USART3_IRQHandler [WEAK] + EXPORT EXTI15_10_IRQHandler [WEAK] + EXPORT RTC_Alarm_IRQHandler [WEAK] + EXPORT OTG_FS_WKUP_IRQHandler [WEAK] + EXPORT TIM8_BRK_TIM12_IRQHandler [WEAK] + EXPORT TIM8_UP_TIM13_IRQHandler [WEAK] + EXPORT TIM8_TRG_COM_TIM14_IRQHandler [WEAK] + EXPORT TIM8_CC_IRQHandler [WEAK] + EXPORT DMA1_Stream7_IRQHandler [WEAK] + EXPORT FMC_IRQHandler [WEAK] + EXPORT SDIO_IRQHandler [WEAK] + EXPORT TIM5_IRQHandler [WEAK] + EXPORT SPI3_IRQHandler [WEAK] + EXPORT UART4_IRQHandler [WEAK] + EXPORT UART5_IRQHandler [WEAK] + EXPORT TIM6_DAC_IRQHandler [WEAK] + EXPORT TIM7_IRQHandler [WEAK] + EXPORT DMA2_Stream0_IRQHandler [WEAK] + EXPORT DMA2_Stream1_IRQHandler [WEAK] + EXPORT DMA2_Stream2_IRQHandler [WEAK] + EXPORT DMA2_Stream3_IRQHandler [WEAK] + EXPORT DMA2_Stream4_IRQHandler [WEAK] + EXPORT ETH_IRQHandler [WEAK] + EXPORT ETH_WKUP_IRQHandler [WEAK] + EXPORT CAN2_TX_IRQHandler [WEAK] + EXPORT CAN2_RX0_IRQHandler [WEAK] + EXPORT CAN2_RX1_IRQHandler [WEAK] + EXPORT CAN2_SCE_IRQHandler [WEAK] + EXPORT OTG_FS_IRQHandler [WEAK] + EXPORT DMA2_Stream5_IRQHandler [WEAK] + EXPORT DMA2_Stream6_IRQHandler [WEAK] + EXPORT DMA2_Stream7_IRQHandler [WEAK] + EXPORT USART6_IRQHandler [WEAK] + EXPORT I2C3_EV_IRQHandler [WEAK] + EXPORT I2C3_ER_IRQHandler [WEAK] + EXPORT OTG_HS_EP1_OUT_IRQHandler [WEAK] + EXPORT OTG_HS_EP1_IN_IRQHandler [WEAK] + EXPORT OTG_HS_WKUP_IRQHandler [WEAK] + EXPORT OTG_HS_IRQHandler [WEAK] + EXPORT DCMI_IRQHandler [WEAK] + EXPORT HASH_RNG_IRQHandler [WEAK] + EXPORT FPU_IRQHandler [WEAK] + +WWDG_IRQHandler +PVD_IRQHandler +TAMP_STAMP_IRQHandler +RTC_WKUP_IRQHandler +FLASH_IRQHandler +RCC_IRQHandler +EXTI0_IRQHandler +EXTI1_IRQHandler +EXTI2_IRQHandler +EXTI3_IRQHandler +EXTI4_IRQHandler +DMA1_Stream0_IRQHandler +DMA1_Stream1_IRQHandler +DMA1_Stream2_IRQHandler +DMA1_Stream3_IRQHandler +DMA1_Stream4_IRQHandler +DMA1_Stream5_IRQHandler +DMA1_Stream6_IRQHandler +ADC_IRQHandler +CAN1_TX_IRQHandler +CAN1_RX0_IRQHandler +CAN1_RX1_IRQHandler +CAN1_SCE_IRQHandler +EXTI9_5_IRQHandler +TIM1_BRK_TIM9_IRQHandler +TIM1_UP_TIM10_IRQHandler +TIM1_TRG_COM_TIM11_IRQHandler +TIM1_CC_IRQHandler +TIM2_IRQHandler +TIM3_IRQHandler +TIM4_IRQHandler +I2C1_EV_IRQHandler +I2C1_ER_IRQHandler +I2C2_EV_IRQHandler +I2C2_ER_IRQHandler +SPI1_IRQHandler +SPI2_IRQHandler +USART1_IRQHandler +USART2_IRQHandler +USART3_IRQHandler +EXTI15_10_IRQHandler +RTC_Alarm_IRQHandler +OTG_FS_WKUP_IRQHandler +TIM8_BRK_TIM12_IRQHandler +TIM8_UP_TIM13_IRQHandler +TIM8_TRG_COM_TIM14_IRQHandler +TIM8_CC_IRQHandler +DMA1_Stream7_IRQHandler +FMC_IRQHandler +SDIO_IRQHandler +TIM5_IRQHandler +SPI3_IRQHandler +UART4_IRQHandler +UART5_IRQHandler +TIM6_DAC_IRQHandler +TIM7_IRQHandler +DMA2_Stream0_IRQHandler +DMA2_Stream1_IRQHandler +DMA2_Stream2_IRQHandler +DMA2_Stream3_IRQHandler +DMA2_Stream4_IRQHandler +ETH_IRQHandler +ETH_WKUP_IRQHandler +CAN2_TX_IRQHandler +CAN2_RX0_IRQHandler +CAN2_RX1_IRQHandler +CAN2_SCE_IRQHandler +OTG_FS_IRQHandler +DMA2_Stream5_IRQHandler +DMA2_Stream6_IRQHandler +DMA2_Stream7_IRQHandler +USART6_IRQHandler +I2C3_EV_IRQHandler +I2C3_ER_IRQHandler +OTG_HS_EP1_OUT_IRQHandler +OTG_HS_EP1_IN_IRQHandler +OTG_HS_WKUP_IRQHandler +OTG_HS_IRQHandler +DCMI_IRQHandler +HASH_RNG_IRQHandler +FPU_IRQHandler + + B . + + ENDP + + ALIGN + +;******************************************************************************* +; User Stack and Heap initialization +;******************************************************************************* + IF :DEF:__MICROLIB + + EXPORT __initial_sp + EXPORT __heap_base + EXPORT __heap_limit + + ELSE + + IMPORT __use_two_region_memory + EXPORT __user_initial_stackheap + +__user_initial_stackheap + + LDR R0, = Heap_Mem + LDR R1, =(Stack_Mem + Stack_Size) + LDR R2, = (Heap_Mem + Heap_Size) + LDR R3, = Stack_Mem + BX LR + + ALIGN + + ENDIF + + END diff --git a/PRINTF_OUTPUT_GUIDE.md b/PRINTF_OUTPUT_GUIDE.md new file mode 100644 index 0000000..95587c3 --- /dev/null +++ b/PRINTF_OUTPUT_GUIDE.md @@ -0,0 +1,176 @@ +# Printf 输出配置指南（Ozone 调试） + +## 📌 当前状态 + +你的项目 `printf` 通过 `__io_putchar()` 输出，但该函数是弱定义（未实现），导致： +- ❌ **Ozone 调试时看不到任何 printf 输出** +- ❌ 验证测试的结果无法查看 + +--- + +## ✅ 解决方案（3种方式） + +### 🔹 **方案1: SWO/ITM 输出（推荐）** + +**优点：** +- ✅ Ozone 原生支持，无需额外硬件 +- ✅ 不占用任何外设（UART等） +- ✅ 实时性好，不影响程序执行 + +**配置步骤：** + +#### 1. 编译设置 +已创建 `Core/Src/retarget.c` 文件，确保： +```c +#define USE_ITM_SWO 1 // 启用 ITM +#define USE_UART 0 +#define USE_SEMIHOSTING 0 +``` + +#### 2. Ozone 配置 +在 Ozone 中： +1. **Tools** → **Terminal** +2. 勾选 **"Enable SWO"** +3. 设置 **SWO Speed**（通常自动检测） +4. 选择 **ITM Port 0** 用于 printf + +#### 3. 硬件连接 +确保调试器的 **SWO** 引脚已连接： +- ST-Link V2: SWO 自动支持 +- J-Link: 需要连接 SWO 线（PB3） + +#### 4. 验证 +运行程序后，在 Ozone 的 **Terminal** 窗口应该能看到： +``` +======================================== + 正逆运动学验证测试 +======================================== +... +``` + +--- + +### 🔹 **方案2: UART 串口输出** + +**优点：** +- ✅ 可以同时用串口助手查看 +- ✅ 脱离调试器也能输出 + +**缺点：** +- ❌ 占用一个 UART 外设 +- ❌ 需要额外的 USB-TTL 模块 + +**配置步骤：** + +#### 1. CubeMX 配置 UART +在 `arm.ioc` 中： +- 启用 USART1（或其他） +- 波特率：115200 +- 生成代码 + +#### 2. 修改 retarget.c +```c +#define USE_ITM_SWO 0 +#define USE_UART 1 // 启用 UART +#define USE_SEMIHOSTING 0 +``` + +并确保包含正确的头文件： +```c +#include "usart.h" // 根据你的 UART 外设调整 +``` + +#### 3. 查看输出 +- 使用串口助手（如 PuTTY、Tera Term） +- 波特率：115200 +- 连接对应的 COM 口 + +--- + +### 🔹 **方案3: 半主机模式（不推荐）** + +**缺点：** +- ❌ 严重降低程序运行速度 +- ❌ 每个 printf 都会暂停 CPU + +仅适用于简单调试，**不推荐**在实时控制系统中使用。 + +--- + +## 🎯 推荐配置（默认已配置） + +**使用 ITM/SWO 方式**，因为： +1. 不占用外设资源 +2. Ozone 原生支持，配置简单 +3. 实时性好，适合机器人控制 + +--- + +## 🔧 Ozone 具体操作步骤 + +### 1. 打开 SWO 配置 +``` +Tools → Terminal +``` + +### 2. 配置 SWO +``` +☑ Enable SWO + SWO Speed: [Auto] (或手动设置为系统时钟/2) + Prescaler: 1 +``` + +### 3. 选择 ITM 通道 +``` +☑ ITM Port 0 (用于 printf) +``` + +### 4. 运行程序 +点击 **Go** 或 **F5** 运行程序 + +### 5. 查看输出 +在 Ozone 底部的 **Terminal** 窗口查看 printf 输出 + +--- + +## 🐛 故障排查 + +| 问题 | 原因 | 解决方法 | +|------|------|---------| +| Terminal 窗口无输出 | SWO 未启用 | 检查 Tools→Terminal 配置 | +| 乱码 | SWO 速度设置错误 | 调整 SWO Speed | +| 仍然无输出 | retarget.c 未编译 | 检查 CMakeLists.txt 是否包含 | +| 硬件错误 | SWO 引脚未连接 | 检查调试器连接 | + +--- + +## 📝 CMakeLists.txt 配置 + +确保 `retarget.c` 被编译： + +```cmake +# 在 CMakeLists.txt 中添加 +set(SOURCES + # ... 其他文件 ... + Core/Src/retarget.c # 添加这一行 +) +``` + +--- + +## 💡 验证是否工作 + +重新编译并运行后，应该在 Ozone Terminal 看到： + +``` +======================================== + 正逆运动学验证测试 +======================================== + +测试1: 零位验证 +原始关节角度: [0.000, 0.000, 0.000, 0.000, 0.000, 0.000] rad +正运动学位姿: x=0.00, y=0.00, z=530.00 mm +... +``` + +如果看不到输出，按照上述步骤逐一检查配置。 diff --git a/STM32F407XX_FLASH.ld b/STM32F407XX_FLASH.ld new file mode 100644 index 0000000..69b01f5 --- /dev/null +++ b/STM32F407XX_FLASH.ld @@ -0,0 +1,269 @@ +/* +****************************************************************************** +** + +** File : LinkerScript.ld +** +** Author : STM32CubeMX +** +** Abstract : Linker script for STM32F407IGHx series +** 1024Kbytes FLASH and 192Kbytes RAM +** +** Set heap size, stack size and stack location according +** to application requirements. +** +** Set memory bank area and size if external memory is used. +** +** Target : STMicroelectronics STM32 +** +** Distribution: The file is distributed “as is,” without any warranty +** of any kind. +** +***************************************************************************** +** @attention +** +**

© COPYRIGHT(c) 2025 STMicroelectronics

+** +** Redistribution and use in source and binary forms, with or without modification, +** are permitted provided that the following conditions are met: +** 1. Redistributions of source code must retain the above copyright notice, +** this list of conditions and the following disclaimer. +** 2. Redistributions in binary form must reproduce the above copyright notice, +** this list of conditions and the following disclaimer in the documentation +** and/or other materials provided with the distribution. +** 3. Neither the name of STMicroelectronics nor the names of its contributors +** may be used to endorse or promote products derived from this software +** without specific prior written permission. +** +** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +** AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +** IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +** DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +** FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +** DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +** SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +** CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +** OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +** +***************************************************************************** +*/ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* Specify the memory areas */ +MEMORY +{ +RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K +CCMRAM (xrw) : ORIGIN = 0x10000000, LENGTH = 64K +FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 1024K +} + +/* Highest address of the user mode stack */ +_estack = ORIGIN(RAM) + LENGTH(RAM); /* end of RAM */ +/* Generate a link error if heap and stack don't fit into RAM */ +_Min_Heap_Size = 0x1000; /* required amount of heap */ +_Min_Stack_Size = 0x1000; /* required amount of stack */ + +/* Define output sections */ +SECTIONS +{ + /* The startup code goes first into FLASH */ + .isr_vector : + { + . = ALIGN(4); + KEEP(*(.isr_vector)) /* Startup code */ + . = ALIGN(4); + } >FLASH + + /* The program code and other data goes into FLASH */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + } >FLASH + + /* Constant data goes into FLASH */ + .rodata : + { + . = ALIGN(4); + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + . = ALIGN(4); + } >FLASH + + .ARM.extab (READONLY) : /* The "READONLY" keyword is only supported in GCC11 and later, remove it if using GCC10 or earlier. */ + { + . = ALIGN(4); + *(.ARM.extab* .gnu.linkonce.armextab.*) + . = ALIGN(4); + } >FLASH + + .ARM (READONLY) : /* The "READONLY" keyword is only supported in GCC11 and later, remove it if using GCC10 or earlier. */ + { + . = ALIGN(4); + __exidx_start = .; + *(.ARM.exidx*) + __exidx_end = .; + . = ALIGN(4); + } >FLASH + + .preinit_array (READONLY) : /* The "READONLY" keyword is only supported in GCC11 and later, remove it if using GCC10 or earlier. */ + { + . = ALIGN(4); + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array*)) + PROVIDE_HIDDEN (__preinit_array_end = .); + . = ALIGN(4); + } >FLASH + + .init_array (READONLY) : /* The "READONLY" keyword is only supported in GCC11 and later, remove it if using GCC10 or earlier. */ + { + . = ALIGN(4); + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array*)) + PROVIDE_HIDDEN (__init_array_end = .); + . = ALIGN(4); + } >FLASH + + .fini_array (READONLY) : /* The "READONLY" keyword is only supported in GCC11 and later, remove it if using GCC10 or earlier. */ + { + . = ALIGN(4); + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(SORT(.fini_array.*))) + KEEP (*(.fini_array*)) + PROVIDE_HIDDEN (__fini_array_end = .); + . = ALIGN(4); + } >FLASH + + _siccmram = LOADADDR(.ccmram); + + /* CCM-RAM section + * + * IMPORTANT NOTE! + * If initialized variables will be placed in this section, + * the startup code needs to be modified to copy the init-values. + */ + .ccmram : + { + . = ALIGN(4); + _sccmram = .; /* create a global symbol at ccmram start */ + *(.ccmram) + *(.ccmram*) + + . = ALIGN(4); + _eccmram = .; /* create a global symbol at ccmram end */ + } >CCMRAM AT> FLASH + + /* used by the startup to initialize data */ + _sidata = LOADADDR(.data); + + /* Initialized data sections goes into RAM, load LMA copy after code */ + .data : + { + . = ALIGN(4); + _sdata = .; /* create a global symbol at data start */ + *(.data) /* .data sections */ + *(.data*) /* .data* sections */ + *(.RamFunc) /* .RamFunc sections */ + *(.RamFunc*) /* .RamFunc* sections */ + + . = ALIGN(4); + } >RAM AT> FLASH + + /* Initialized TLS data section */ + .tdata : ALIGN(4) + { + *(.tdata .tdata.* .gnu.linkonce.td.*) + . = ALIGN(4); + _edata = .; /* define a global symbol at data end */ + PROVIDE(__data_end = .); + PROVIDE(__tdata_end = .); + } >RAM AT> FLASH + + PROVIDE( __tdata_start = ADDR(.tdata) ); + PROVIDE( __tdata_size = __tdata_end - __tdata_start ); + + PROVIDE( __data_start = ADDR(.data) ); + PROVIDE( __data_size = __data_end - __data_start ); + + PROVIDE( __tdata_source = LOADADDR(.tdata) ); + PROVIDE( __tdata_source_end = LOADADDR(.tdata) + SIZEOF(.tdata) ); + PROVIDE( __tdata_source_size = __tdata_source_end - __tdata_source ); + + PROVIDE( __data_source = LOADADDR(.data) ); + PROVIDE( __data_source_end = __tdata_source_end ); + PROVIDE( __data_source_size = __data_source_end - __data_source ); + /* Uninitialized data section */ + .tbss (NOLOAD) : ALIGN(4) + { + /* This is used by the startup in order to initialize the .bss secion */ + _sbss = .; /* define a global symbol at bss start */ + __bss_start__ = _sbss; + *(.tbss .tbss.*) + . = ALIGN(4); + PROVIDE( __tbss_end = . ); + } >RAM + + PROVIDE( __tbss_start = ADDR(.tbss) ); + PROVIDE( __tbss_size = __tbss_end - __tbss_start ); + PROVIDE( __tbss_offset = ADDR(.tbss) - ADDR(.tdata) ); + + PROVIDE( __tls_base = __tdata_start ); + PROVIDE( __tls_end = __tbss_end ); + PROVIDE( __tls_size = __tls_end - __tls_base ); + PROVIDE( __tls_align = MAX(ALIGNOF(.tdata), ALIGNOF(.tbss)) ); + PROVIDE( __tls_size_align = (__tls_size + __tls_align - 1) & ~(__tls_align - 1) ); + PROVIDE( __arm32_tls_tcb_offset = MAX(8, __tls_align) ); + PROVIDE( __arm64_tls_tcb_offset = MAX(16, __tls_align) ); + + .bss (NOLOAD) : ALIGN(4) + { + *(.bss) + *(.bss*) + *(COMMON) + + . = ALIGN(4); + _ebss = .; /* define a global symbol at bss end */ + __bss_end__ = _ebss; + PROVIDE( __bss_end = .); + } >RAM + PROVIDE( __non_tls_bss_start = ADDR(.bss) ); + + PROVIDE( __bss_start = __tbss_start ); + PROVIDE( __bss_size = __bss_end - __bss_start ); + + /* User_heap_stack section, used to check that there is enough RAM left */ + ._user_heap_stack (NOLOAD) : + { + . = ALIGN(8); + PROVIDE ( end = . ); + PROVIDE ( _end = . ); + . = . + _Min_Heap_Size; + . = . + _Min_Stack_Size; + . = ALIGN(8); + } >RAM + + + + /* Remove information from the standard libraries */ + /DISCARD/ : + { + libc.a:* ( * ) + libm.a:* ( * ) + libgcc.a:* ( * ) + } + +} diff --git a/User/bsp/bsp.h b/User/bsp/bsp.h new file mode 100644 index 0000000..1f8cbfd --- /dev/null +++ b/User/bsp/bsp.h @@ -0,0 +1,28 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +#define BSP_OK (0) +#define BSP_ERR (-1) +#define BSP_ERR_NULL (-2) +#define BSP_ERR_INITED (-3) +#define BSP_ERR_NO_DEV (-4) +#define BSP_ERR_TIMEOUT (-5) + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/bsp/bsp_config.yaml b/User/bsp/bsp_config.yaml new file mode 100644 index 0000000..561a788 --- /dev/null +++ b/User/bsp/bsp_config.yaml @@ -0,0 +1,26 @@ +can: + devices: + - instance: CAN1 + name: '1' + - instance: CAN2 + name: '2' + enabled: true +mm: + enabled: true +pwm: + configs: + - channel: TIM_CHANNEL_1 + custom_name: TIM5_CH1 + label: TIM5_CH1 + timer: TIM5 + - channel: TIM_CHANNEL_2 + custom_name: TIM5_CH2 + label: TIM5_CH2 + timer: TIM5 + - channel: TIM_CHANNEL_3 + custom_name: TIM5_CH3 + label: TIM5_CH3 + timer: TIM5 + enabled: true +time: + enabled: true diff --git a/User/bsp/can.c b/User/bsp/can.c new file mode 100644 index 0000000..e401cdd --- /dev/null +++ b/User/bsp/can.c @@ -0,0 +1,708 @@ +/* Includes ----------------------------------------------------------------- */ +#include "bsp/can.h" +#include "bsp/bsp.h" +#include +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +#define CAN_QUEUE_MUTEX_TIMEOUT 100 /* 队列互斥锁超时时间(ms) */ +#define CAN_TX_MAILBOX_NUM 3 /* CAN发送邮箱数量 */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +typedef struct BSP_CAN_QueueNode { + BSP_CAN_t can; /* CAN通道 */ + uint32_t can_id; /* 解析后的CAN ID */ + osMessageQueueId_t queue; /* 消息队列ID */ + uint8_t queue_size; /* 队列大小 */ + struct BSP_CAN_QueueNode *next; /* 指向下一个节点的指针 */ +} BSP_CAN_QueueNode_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static BSP_CAN_QueueNode_t *queue_list = NULL; +static osMutexId_t queue_mutex = NULL; +static void (*CAN_Callback[BSP_CAN_NUM][BSP_CAN_CB_NUM])(void); +static bool inited = false; +static BSP_CAN_IdParser_t id_parser = NULL; /* ID解析器 */ +static BSP_CAN_TxQueue_t tx_queues[BSP_CAN_NUM]; /* 每个CAN的发送队列 */ + +/* Private function prototypes ---------------------------------------------- */ +static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan); +static osMessageQueueId_t BSP_CAN_FindQueue(BSP_CAN_t can, uint32_t can_id); +static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size); +static void BSP_CAN_RxFifo0Callback(void); +static void BSP_CAN_RxFifo1Callback(void); +static void BSP_CAN_TxCompleteCallback(void); +static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header); +static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type); +static void BSP_CAN_TxQueueInit(BSP_CAN_t can); +static bool BSP_CAN_TxQueuePush(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg); +static bool BSP_CAN_TxQueuePop(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg); +static bool BSP_CAN_TxQueueIsEmpty(BSP_CAN_t can); + +/* Private functions -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +/** + * @brief 根据CAN句柄获取BSP_CAN实例 + */ +static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan) { + if (hcan == NULL) return BSP_CAN_ERR; + + if (hcan->Instance == CAN1) + return BSP_CAN_1; + else if (hcan->Instance == CAN2) + return BSP_CAN_2; + else + return BSP_CAN_ERR; +} + +/** + * @brief 查找指定CAN ID的消息队列 + * @note 调用前需要获取互斥锁 + */ +static osMessageQueueId_t BSP_CAN_FindQueue(BSP_CAN_t can, uint32_t can_id) { + BSP_CAN_QueueNode_t *node = queue_list; + while (node != NULL) { + if (node->can == can && node->can_id == can_id) { + return node->queue; + } + node = node->next; + } + return NULL; +} + +/** + * @brief 创建指定CAN ID的消息队列 + * @note 内部函数，已包含互斥锁保护 + */ +static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size) { + if (queue_size == 0) { + queue_size = BSP_CAN_DEFAULT_QUEUE_SIZE; + } + if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) { + return BSP_ERR_TIMEOUT; + } + BSP_CAN_QueueNode_t *node = queue_list; + while (node != NULL) { + if (node->can == can && node->can_id == can_id) { + osMutexRelease(queue_mutex); + return BSP_ERR; // 已存在 + } + node = node->next; + } + BSP_CAN_QueueNode_t *new_node = (BSP_CAN_QueueNode_t *)BSP_Malloc(sizeof(BSP_CAN_QueueNode_t)); + if (new_node == NULL) { + osMutexRelease(queue_mutex); + return BSP_ERR_NULL; + } + new_node->queue = osMessageQueueNew(queue_size, sizeof(BSP_CAN_Message_t), NULL); + if (new_node->queue == NULL) { + BSP_Free(new_node); + osMutexRelease(queue_mutex); + return BSP_ERR; + } + new_node->can = can; + new_node->can_id = can_id; + new_node->queue_size = queue_size; + new_node->next = queue_list; + queue_list = new_node; + osMutexRelease(queue_mutex); + return BSP_OK; +} + + +/** + * @brief 获取帧类型 + */ +static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header) { + if (header->RTR == CAN_RTR_REMOTE) { + return (header->IDE == CAN_ID_EXT) ? BSP_CAN_FRAME_EXT_REMOTE : BSP_CAN_FRAME_STD_REMOTE; + } else { + return (header->IDE == CAN_ID_EXT) ? BSP_CAN_FRAME_EXT_DATA : BSP_CAN_FRAME_STD_DATA; + } +} + +/** + * @brief 默认ID解析器（直接返回原始ID） + */ +static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type) { + (void)frame_type; // 避免未使用参数警告 + return original_id; +} + +/** + * @brief 初始化发送队列 + */ +static void BSP_CAN_TxQueueInit(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return; + + tx_queues[can].head = 0; + tx_queues[can].tail = 0; +} + +/** + * @brief 向发送队列添加消息（无锁） + */ +static bool BSP_CAN_TxQueuePush(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg) { + if (can >= BSP_CAN_NUM || msg == NULL) return false; + + BSP_CAN_TxQueue_t *queue = &tx_queues[can]; + uint32_t next_head = (queue->head + 1) % BSP_CAN_TX_QUEUE_SIZE; + + // 队列满 + if (next_head == queue->tail) { + return false; + } + + // 复制消息 + queue->buffer[queue->head] = *msg; + + // 更新头指针（原子操作） + queue->head = next_head; + + return true; +} + + +/** + * @brief 从发送队列取出消息（无锁） + */ +static bool BSP_CAN_TxQueuePop(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg) { + if (can >= BSP_CAN_NUM || msg == NULL) return false; + + BSP_CAN_TxQueue_t *queue = &tx_queues[can]; + + // 队列空 + if (queue->head == queue->tail) { + return false; + } + + // 复制消息 + *msg = queue->buffer[queue->tail]; + + // 更新尾指针（原子操作） + queue->tail = (queue->tail + 1) % BSP_CAN_TX_QUEUE_SIZE; + + return true; +} + +/** + * @brief 检查发送队列是否为空 + */ +static bool BSP_CAN_TxQueueIsEmpty(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return true; + + return tx_queues[can].head == tx_queues[can].tail; +} + +/** + * @brief 处理所有CAN实例的发送队列 + */ +static void BSP_CAN_TxCompleteCallback(void) { + // 处理所有CAN实例的发送队列 + for (int i = 0; i < BSP_CAN_NUM; i++) { + BSP_CAN_t can = (BSP_CAN_t)i; + CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can); + if (hcan == NULL) continue; + + BSP_CAN_TxMessage_t msg; + uint32_t mailbox; + + // 尝试发送队列中的消息 + while (!BSP_CAN_TxQueueIsEmpty(can)) { + // 检查是否有空闲邮箱 + if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) == 0) { + break; // 没有空闲邮箱，等待下次中断 + } + + // 从队列中取出消息 + if (!BSP_CAN_TxQueuePop(can, &msg)) { + break; + } + + // 发送消息 + if (HAL_CAN_AddTxMessage(hcan, &msg.header, msg.data, &mailbox) != HAL_OK) { + // 发送失败，消息已经从队列中移除，直接丢弃 + break; + } + } + } +} + + +/** + * @brief FIFO0接收处理函数 + */ +static void BSP_CAN_RxFifo0Callback(void) { + CAN_RxHeaderTypeDef rx_header; + uint8_t rx_data[BSP_CAN_MAX_DLC]; + for (int can_idx = 0; can_idx < BSP_CAN_NUM; can_idx++) { + CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle((BSP_CAN_t)can_idx); + if (hcan == NULL) continue; + while (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO0) > 0) { + if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data) == HAL_OK) { + uint32_t original_id = (rx_header.IDE == CAN_ID_STD) ? rx_header.StdId : rx_header.ExtId; + BSP_CAN_FrameType_t frame_type = BSP_CAN_GetFrameType(&rx_header); + uint32_t parsed_id = BSP_CAN_ParseId(original_id, frame_type); + osMessageQueueId_t queue = BSP_CAN_FindQueue((BSP_CAN_t)can_idx, parsed_id); + if (queue != NULL) { + BSP_CAN_Message_t msg = {0}; + msg.frame_type = frame_type; + msg.original_id = original_id; + msg.parsed_id = parsed_id; + msg.dlc = rx_header.DLC; + if (rx_header.RTR == CAN_RTR_DATA) { + memcpy(msg.data, rx_data, rx_header.DLC); + } + msg.timestamp = HAL_GetTick(); + osMessageQueuePut(queue, &msg, 0, BSP_CAN_TIMEOUT_IMMEDIATE); + } + } + } + } +} + +/** + * @brief FIFO1接收处理函数 + */ +static void BSP_CAN_RxFifo1Callback(void) { + CAN_RxHeaderTypeDef rx_header; + uint8_t rx_data[BSP_CAN_MAX_DLC]; + for (int can_idx = 0; can_idx < BSP_CAN_NUM; can_idx++) { + CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle((BSP_CAN_t)can_idx); + if (hcan == NULL) continue; + while (HAL_CAN_GetRxFifoFillLevel(hcan, CAN_RX_FIFO1) > 0) { + if (HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO1, &rx_header, rx_data) == HAL_OK) { + uint32_t original_id = (rx_header.IDE == CAN_ID_STD) ? rx_header.StdId : rx_header.ExtId; + BSP_CAN_FrameType_t frame_type = BSP_CAN_GetFrameType(&rx_header); + uint32_t parsed_id = BSP_CAN_ParseId(original_id, frame_type); + osMessageQueueId_t queue = BSP_CAN_FindQueue((BSP_CAN_t)can_idx, parsed_id); + if (queue != NULL) { + BSP_CAN_Message_t msg = {0}; + msg.frame_type = frame_type; + msg.original_id = original_id; + msg.parsed_id = parsed_id; + msg.dlc = rx_header.DLC; + if (rx_header.RTR == CAN_RTR_DATA) { + memcpy(msg.data, rx_data, rx_header.DLC); + } + msg.timestamp = HAL_GetTick(); + osMessageQueuePut(queue, &msg, 0, BSP_CAN_TIMEOUT_IMMEDIATE); + } + } + } + } +} + +/* HAL Callback Functions --------------------------------------------------- */ +void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + // 调用用户回调 + if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_CPLT_CB]) + CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_CPLT_CB](); + } +} + +void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + // 调用用户回调 + if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_CPLT_CB]) + CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_CPLT_CB](); + } +} + +void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + // 调用用户回调 + if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_CPLT_CB]) + CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_CPLT_CB](); + } +} + +void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + // 调用用户回调 + if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_ABORT_CB]) + CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX0_ABORT_CB](); + } +} + +void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + // 调用用户回调 + if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_ABORT_CB]) + CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX1_ABORT_CB](); + } +} + +void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + // 调用用户回调 + if (CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_ABORT_CB]) + CAN_Callback[bsp_can][HAL_CAN_TX_MAILBOX2_ABORT_CB](); + } +} + +void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_MSG_PENDING_CB]) + CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_MSG_PENDING_CB](); + } +} + +void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_FULL_CB]) + CAN_Callback[bsp_can][HAL_CAN_RX_FIFO0_FULL_CB](); + } +} + +void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_MSG_PENDING_CB]) + CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_MSG_PENDING_CB](); + } +} + +void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + if (CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_FULL_CB]) + CAN_Callback[bsp_can][HAL_CAN_RX_FIFO1_FULL_CB](); + } +} + +void HAL_CAN_SleepCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + if (CAN_Callback[bsp_can][HAL_CAN_SLEEP_CB]) + CAN_Callback[bsp_can][HAL_CAN_SLEEP_CB](); + } +} + +void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + if (CAN_Callback[bsp_can][HAL_CAN_WAKEUP_FROM_RX_MSG_CB]) + CAN_Callback[bsp_can][HAL_CAN_WAKEUP_FROM_RX_MSG_CB](); + } +} + +void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan) { + BSP_CAN_t bsp_can = CAN_Get(hcan); + if (bsp_can != BSP_CAN_ERR) { + if (CAN_Callback[bsp_can][HAL_CAN_ERROR_CB]) + CAN_Callback[bsp_can][HAL_CAN_ERROR_CB](); + } +} + +/* Exported functions ------------------------------------------------------- */ + +int8_t BSP_CAN_Init(void) { + if (inited) { + return BSP_ERR_INITED; + } + + // 清零回调函数数组 + memset(CAN_Callback, 0, sizeof(CAN_Callback)); + + // 初始化发送队列 + for (int i = 0; i < BSP_CAN_NUM; i++) { + BSP_CAN_TxQueueInit((BSP_CAN_t)i); + } + + // 初始化ID解析器为默认解析器 + id_parser = BSP_CAN_DefaultIdParser; + + // 创建互斥锁 + queue_mutex = osMutexNew(NULL); + if (queue_mutex == NULL) { + return BSP_ERR; + } + + // 先设置初始化标志，以便后续回调注册能通过检查 + inited = true; + + // 初始化 CAN1 - 使用 FIFO0 + CAN_FilterTypeDef can1_filter = {0}; + can1_filter.FilterBank = 0; + can1_filter.FilterIdHigh = 0; + can1_filter.FilterIdLow = 0; + can1_filter.FilterMode = CAN_FILTERMODE_IDMASK; + can1_filter.FilterScale = CAN_FILTERSCALE_32BIT; + can1_filter.FilterMaskIdHigh = 0; + can1_filter.FilterMaskIdLow = 0; + can1_filter.FilterActivation = ENABLE; + can1_filter.SlaveStartFilterBank = 14; + can1_filter.FilterFIFOAssignment = CAN_RX_FIFO0; + HAL_CAN_ConfigFilter(&hcan1, &can1_filter); + HAL_CAN_Start(&hcan1); + + // 自动注册CAN1接收回调函数 + BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback); + BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX0_CPLT_CB, BSP_CAN_TxCompleteCallback); + BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX1_CPLT_CB, BSP_CAN_TxCompleteCallback); + BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_TxCompleteCallback); + + // 激活CAN1中断 + HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING | + CAN_IT_TX_MAILBOX_EMPTY); // 激活发送邮箱空中断 + + // 初始化 CAN2 - 使用 FIFO1 + can1_filter.FilterBank = 14; + can1_filter.FilterFIFOAssignment = CAN_RX_FIFO1; + HAL_CAN_ConfigFilter(&hcan2, &can1_filter); // 通过 CAN1 配置 + HAL_CAN_Start(&hcan2); + + // 自动注册CAN2接收回调函数 + BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, BSP_CAN_RxFifo1Callback); + BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX0_CPLT_CB, BSP_CAN_TxCompleteCallback); + BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX1_CPLT_CB, BSP_CAN_TxCompleteCallback); + BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_TxCompleteCallback); + + // 激活CAN2中断 + HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING | + CAN_IT_TX_MAILBOX_EMPTY); // 激活发送邮箱空中断 + + + inited = true; + return BSP_OK; +} + + +CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) { + return NULL; + } + + switch (can) { + case BSP_CAN_1: + return &hcan1; + case BSP_CAN_2: + return &hcan2; + default: + return NULL; + } +} + +int8_t BSP_CAN_RegisterCallback(BSP_CAN_t can, BSP_CAN_Callback_t type, + void (*callback)(void)) { + if (!inited) { + return BSP_ERR_INITED; + } + if (callback == NULL) { + return BSP_ERR_NULL; + } + if (can >= BSP_CAN_NUM) { + return BSP_ERR; + } + if (type >= BSP_CAN_CB_NUM) { + return BSP_ERR; + } + + CAN_Callback[can][type] = callback; + return BSP_OK; +} + +int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format, + uint32_t id, uint8_t *data, uint8_t dlc) { + if (!inited) { + return BSP_ERR_INITED; + } + if (can >= BSP_CAN_NUM) { + return BSP_ERR; + } + if (data == NULL && format != BSP_CAN_FORMAT_STD_REMOTE && format != BSP_CAN_FORMAT_EXT_REMOTE) { + return BSP_ERR_NULL; + } + if (dlc > BSP_CAN_MAX_DLC) { + return BSP_ERR; + } + + CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can); + if (hcan == NULL) { + return BSP_ERR_NULL; + } + + // 准备发送消息 + BSP_CAN_TxMessage_t tx_msg = {0}; + + switch (format) { + case BSP_CAN_FORMAT_STD_DATA: + tx_msg.header.StdId = id; + tx_msg.header.IDE = CAN_ID_STD; + tx_msg.header.RTR = CAN_RTR_DATA; + break; + case BSP_CAN_FORMAT_EXT_DATA: + tx_msg.header.ExtId = id; + tx_msg.header.IDE = CAN_ID_EXT; + tx_msg.header.RTR = CAN_RTR_DATA; + break; + case BSP_CAN_FORMAT_STD_REMOTE: + tx_msg.header.StdId = id; + tx_msg.header.IDE = CAN_ID_STD; + tx_msg.header.RTR = CAN_RTR_REMOTE; + break; + case BSP_CAN_FORMAT_EXT_REMOTE: + tx_msg.header.ExtId = id; + tx_msg.header.IDE = CAN_ID_EXT; + tx_msg.header.RTR = CAN_RTR_REMOTE; + break; + default: + return BSP_ERR; + } + + tx_msg.header.DLC = dlc; + tx_msg.header.TransmitGlobalTime = DISABLE; + + // 复制数据 + if (data != NULL && dlc > 0) { + memcpy(tx_msg.data, data, dlc); + } + + // 尝试直接发送到邮箱 + uint32_t mailbox; + if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) > 0) { + HAL_StatusTypeDef result = HAL_CAN_AddTxMessage(hcan, &tx_msg.header, tx_msg.data, &mailbox); + if (result == HAL_OK) { + return BSP_OK; // 发送成功 + } + } + + // 邮箱满，尝试放入队列 + if (BSP_CAN_TxQueuePush(can, &tx_msg)) { + return BSP_OK; // 成功放入队列 + } + + // 队列也满，丢弃数据 + return BSP_ERR; // 数据丢弃 +} + +int8_t BSP_CAN_TransmitStdDataFrame(BSP_CAN_t can, BSP_CAN_StdDataFrame_t *frame) { + if (frame == NULL) { + return BSP_ERR_NULL; + } + return BSP_CAN_Transmit(can, BSP_CAN_FORMAT_STD_DATA, frame->id, frame->data, frame->dlc); +} + +int8_t BSP_CAN_TransmitExtDataFrame(BSP_CAN_t can, BSP_CAN_ExtDataFrame_t *frame) { + if (frame == NULL) { + return BSP_ERR_NULL; + } + return BSP_CAN_Transmit(can, BSP_CAN_FORMAT_EXT_DATA, frame->id, frame->data, frame->dlc); +} + +int8_t BSP_CAN_TransmitRemoteFrame(BSP_CAN_t can, BSP_CAN_RemoteFrame_t *frame) { + if (frame == NULL) { + return BSP_ERR_NULL; + } + BSP_CAN_Format_t format = frame->is_extended ? BSP_CAN_FORMAT_EXT_REMOTE : BSP_CAN_FORMAT_STD_REMOTE; + return BSP_CAN_Transmit(can, format, frame->id, NULL, frame->dlc); +} + +int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size) { + if (!inited) { + return BSP_ERR_INITED; + } + return BSP_CAN_CreateIdQueue(can, can_id, queue_size); +} + + +int8_t BSP_CAN_GetMessage(BSP_CAN_t can, uint32_t can_id, BSP_CAN_Message_t *msg, uint32_t timeout) { + if (!inited) { + return BSP_ERR_INITED; + } + if (msg == NULL) { + return BSP_ERR_NULL; + } + if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) { + return BSP_ERR_TIMEOUT; + } + osMessageQueueId_t queue = BSP_CAN_FindQueue(can, can_id); + osMutexRelease(queue_mutex); + if (queue == NULL) { + return BSP_ERR_NO_DEV; + } + osStatus_t result = osMessageQueueGet(queue, msg, NULL, timeout); + return (result == osOK) ? BSP_OK : BSP_ERR; +} + +int32_t BSP_CAN_GetQueueCount(BSP_CAN_t can, uint32_t can_id) { + if (!inited) { + return -1; + } + if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) { + return -1; + } + osMessageQueueId_t queue = BSP_CAN_FindQueue(can, can_id); + osMutexRelease(queue_mutex); + if (queue == NULL) { + return -1; + } + return (int32_t)osMessageQueueGetCount(queue); +} + +int8_t BSP_CAN_FlushQueue(BSP_CAN_t can, uint32_t can_id) { + if (!inited) { + return BSP_ERR_INITED; + } + if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) { + return BSP_ERR_TIMEOUT; + } + osMessageQueueId_t queue = BSP_CAN_FindQueue(can, can_id); + osMutexRelease(queue_mutex); + if (queue == NULL) { + return BSP_ERR_NO_DEV; + } + BSP_CAN_Message_t temp_msg; + while (osMessageQueueGet(queue, &temp_msg, NULL, BSP_CAN_TIMEOUT_IMMEDIATE) == osOK) { + // 清空 + } + return BSP_OK; +} + +int8_t BSP_CAN_RegisterIdParser(BSP_CAN_IdParser_t parser) { + if (!inited) { + return BSP_ERR_INITED; + } + if (parser == NULL) { + return BSP_ERR_NULL; + } + + id_parser = parser; + return BSP_OK; +} + +uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type) { + if (id_parser != NULL) { + return id_parser(original_id, frame_type); + } + return BSP_CAN_DefaultIdParser(original_id, frame_type); +} + + diff --git a/User/bsp/can.h b/User/bsp/can.h new file mode 100644 index 0000000..e6b5f71 --- /dev/null +++ b/User/bsp/can.h @@ -0,0 +1,259 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include "bsp/bsp.h" +#include "bsp/mm.h" +#include +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +#define BSP_CAN_MAX_DLC 8 +#define BSP_CAN_DEFAULT_QUEUE_SIZE 10 +#define BSP_CAN_TIMEOUT_IMMEDIATE 0 +#define BSP_CAN_TIMEOUT_FOREVER osWaitForever +#define BSP_CAN_TX_QUEUE_SIZE 32 /* 发送队列大小 */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + BSP_CAN_1, + BSP_CAN_2, + BSP_CAN_NUM, + BSP_CAN_ERR, +} BSP_CAN_t; + +typedef enum { + HAL_CAN_TX_MAILBOX0_CPLT_CB, + HAL_CAN_TX_MAILBOX1_CPLT_CB, + HAL_CAN_TX_MAILBOX2_CPLT_CB, + HAL_CAN_TX_MAILBOX0_ABORT_CB, + HAL_CAN_TX_MAILBOX1_ABORT_CB, + HAL_CAN_TX_MAILBOX2_ABORT_CB, + HAL_CAN_RX_FIFO0_MSG_PENDING_CB, + HAL_CAN_RX_FIFO0_FULL_CB, + HAL_CAN_RX_FIFO1_MSG_PENDING_CB, + HAL_CAN_RX_FIFO1_FULL_CB, + HAL_CAN_SLEEP_CB, + HAL_CAN_WAKEUP_FROM_RX_MSG_CB, + HAL_CAN_ERROR_CB, + BSP_CAN_CB_NUM, +} BSP_CAN_Callback_t; + +/* CAN消息格式枚举 - 用于发送和接收消息时指定格式 */ +typedef enum { + BSP_CAN_FORMAT_STD_DATA, /* 标准数据帧 */ + BSP_CAN_FORMAT_EXT_DATA, /* 扩展数据帧 */ + BSP_CAN_FORMAT_STD_REMOTE, /* 标准远程帧 */ + BSP_CAN_FORMAT_EXT_REMOTE, /* 扩展远程帧 */ +} BSP_CAN_Format_t; + +/* CAN帧类型枚举 - 用于区分不同类型的CAN帧 */ +typedef enum { + BSP_CAN_FRAME_STD_DATA, /* 标准数据帧 */ + BSP_CAN_FRAME_EXT_DATA, /* 扩展数据帧 */ + BSP_CAN_FRAME_STD_REMOTE, /* 标准远程帧 */ + BSP_CAN_FRAME_EXT_REMOTE, /* 扩展远程帧 */ +} BSP_CAN_FrameType_t; + +/* CAN消息结构体 - 支持不同类型帧 */ +typedef struct { + BSP_CAN_FrameType_t frame_type; /* 帧类型 */ + uint32_t original_id; /* 原始ID（未解析） */ + uint32_t parsed_id; /* 解析后的实际ID */ + uint8_t dlc; /* 数据长度 */ + uint8_t data[BSP_CAN_MAX_DLC]; /* 数据 */ + uint32_t timestamp; /* 时间戳（可选） */ +} BSP_CAN_Message_t; + +/* 标准数据帧结构 */ +typedef struct { + uint32_t id; /* CAN ID */ + uint8_t dlc; /* 数据长度 */ + uint8_t data[BSP_CAN_MAX_DLC]; /* 数据 */ +} BSP_CAN_StdDataFrame_t; + +/* 扩展数据帧结构 */ +typedef struct { + uint32_t id; /* 扩展CAN ID */ + uint8_t dlc; /* 数据长度 */ + uint8_t data[BSP_CAN_MAX_DLC]; /* 数据 */ +} BSP_CAN_ExtDataFrame_t; + +/* 远程帧结构 */ +typedef struct { + uint32_t id; /* CAN ID */ + uint8_t dlc; /* 请求的数据长度 */ + bool is_extended; /* 是否为扩展帧 */ +} BSP_CAN_RemoteFrame_t; + +/* ID解析回调函数类型 */ +typedef uint32_t (*BSP_CAN_IdParser_t)(uint32_t original_id, BSP_CAN_FrameType_t frame_type); + +/* CAN发送消息结构体 */ +typedef struct { + CAN_TxHeaderTypeDef header; /* 发送头 */ + uint8_t data[BSP_CAN_MAX_DLC]; /* 数据 */ +} BSP_CAN_TxMessage_t; + +/* 无锁环形队列结构体 */ +typedef struct { + BSP_CAN_TxMessage_t buffer[BSP_CAN_TX_QUEUE_SIZE]; /* 缓冲区 */ + volatile uint32_t head; /* 队列头 */ + volatile uint32_t tail; /* 队列尾 */ +} BSP_CAN_TxQueue_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 初始化 CAN 模块 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_Init(void); + +/** + * @brief 获取 CAN 句柄 + * @param can CAN 枚举 + * @return CAN_HandleTypeDef 指针，失败返回 NULL + */ +CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can); + +/** + * @brief 注册 CAN 回调函数 + * @param can CAN 枚举 + * @param type 回调类型 + * @param callback 回调函数指针 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_RegisterCallback(BSP_CAN_t can, BSP_CAN_Callback_t type, + void (*callback)(void)); + +/** + * @brief 发送 CAN 消息 + * @param can CAN 枚举 + * @param format 消息格式 + * @param id CAN ID + * @param data 数据指针 + * @param dlc 数据长度 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format, + uint32_t id, uint8_t *data, uint8_t dlc); + +/** + * @brief 发送标准数据帧 + * @param can CAN 枚举 + * @param frame 标准数据帧指针 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_TransmitStdDataFrame(BSP_CAN_t can, BSP_CAN_StdDataFrame_t *frame); + +/** + * @brief 发送扩展数据帧 + * @param can CAN 枚举 + * @param frame 扩展数据帧指针 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_TransmitExtDataFrame(BSP_CAN_t can, BSP_CAN_ExtDataFrame_t *frame); + +/** + * @brief 发送远程帧 + * @param can CAN 枚举 + * @param frame 远程帧指针 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_TransmitRemoteFrame(BSP_CAN_t can, BSP_CAN_RemoteFrame_t *frame); + + +/** + * @brief 获取发送队列中待发送消息数量 + * @param can CAN 枚举 + * @return 队列中消息数量，-1表示错误 + */ +int32_t BSP_CAN_GetTxQueueCount(BSP_CAN_t can); + +/** + * @brief 清空发送队列 + * @param can CAN 枚举 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_FlushTxQueue(BSP_CAN_t can); + +/** + * @brief 注册 CAN ID 接收队列 + * @param can CAN 枚举 + * @param can_id 解析后的CAN ID + * @param queue_size 队列大小，0使用默认值 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size); + + + +/** + * @brief 获取 CAN 消息 + * @param can CAN 枚举 + * @param can_id 解析后的CAN ID + * @param msg 存储消息的结构体指针 + * @param timeout 超时时间（毫秒），0为立即返回，osWaitForever为永久等待 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_GetMessage(BSP_CAN_t can, uint32_t can_id, BSP_CAN_Message_t *msg, uint32_t timeout); + +/** + * @brief 获取指定ID队列中的消息数量 + * @param can CAN 枚举 + * @param can_id 解析后的CAN ID + * @return 消息数量，-1表示队列不存在 + */ +int32_t BSP_CAN_GetQueueCount(BSP_CAN_t can, uint32_t can_id); + +/** + * @brief 清空指定ID队列中的所有消息 + * @param can CAN 枚举 + * @param can_id 解析后的CAN ID + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_FlushQueue(BSP_CAN_t can, uint32_t can_id); + +/** + * @brief 注册ID解析器 + * @param parser ID解析回调函数 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_RegisterIdParser(BSP_CAN_IdParser_t parser); + + +/** + * @brief 解析CAN ID + * @param original_id 原始ID + * @param frame_type 帧类型 + * @return 解析后的ID + */ +uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/User/bsp/mm.c b/User/bsp/mm.c new file mode 100644 index 0000000..13d20c0 --- /dev/null +++ b/User/bsp/mm.c @@ -0,0 +1,30 @@ +/* Includes ----------------------------------------------------------------- */ +#include "bsp/mm.h" + +#include "FreeRTOS.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private function -------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +inline void *BSP_Malloc(size_t size) { return pvPortMalloc(size); } + +inline void BSP_Free(void *pv) { vPortFree(pv); } + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/User/bsp/mm.h b/User/bsp/mm.h new file mode 100644 index 0000000..d24634e --- /dev/null +++ b/User/bsp/mm.h @@ -0,0 +1,32 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ +/* Exported functions prototypes -------------------------------------------- */ +void *BSP_Malloc(size_t size); +void BSP_Free(void *pv); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/bsp/pwm.c b/User/bsp/pwm.c new file mode 100644 index 0000000..97df3c8 --- /dev/null +++ b/User/bsp/pwm.c @@ -0,0 +1,112 @@ +/* Includes ----------------------------------------------------------------- */ +#include "tim.h" +#include "bsp/pwm.h" +#include "bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +typedef struct { + TIM_HandleTypeDef *tim; + uint16_t channel; +} BSP_PWM_Config_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static const BSP_PWM_Config_t PWM_Map[BSP_PWM_NUM] = { + {&htim5, TIM_CHANNEL_1}, + {&htim5, TIM_CHANNEL_2}, + {&htim5, TIM_CHANNEL_3}, +}; + +/* Private function -------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ + +int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Start(PWM_Map[ch].tim, PWM_Map[ch].channel); + return BSP_OK; +} + +int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + if (duty_cycle > 1.0f) { + duty_cycle = 1.0f; + } + if (duty_cycle < 0.0f) { + duty_cycle = 0.0f; + } + // 获取ARR值（周期值） + uint32_t arr = __HAL_TIM_GET_AUTORELOAD(PWM_Map[ch].tim); + + // 计算比较值：CCR = duty_cycle * (ARR + 1) + uint32_t ccr = (uint32_t)(duty_cycle * (arr + 1)); + + __HAL_TIM_SET_COMPARE(PWM_Map[ch].tim, PWM_Map[ch].channel, ccr); + + return BSP_OK; +} + +int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + uint32_t timer_clock = HAL_RCC_GetPCLK1Freq(); // Get the timer clock frequency + uint32_t prescaler = PWM_Map[ch].tim->Init.Prescaler; + uint32_t period = (timer_clock / (prescaler + 1)) / freq - 1; + + if (period > UINT16_MAX) { + return BSP_ERR; // Frequency too low + } + __HAL_TIM_SET_AUTORELOAD(PWM_Map[ch].tim, period); + + return BSP_OK; +} + +int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Stop(PWM_Map[ch].tim, PWM_Map[ch].channel); + return BSP_OK; +} + +uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + return PWM_Map[ch].tim->Init.AutoReloadPreload; +} + +TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch) { + return PWM_Map[ch].tim; +} + + +uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + return PWM_Map[ch].channel; +} + +int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Start_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel, pData, Length); + return BSP_OK; +} + +int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Stop_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel); + return BSP_OK; +} \ No newline at end of file diff --git a/User/bsp/pwm.h b/User/bsp/pwm.h new file mode 100644 index 0000000..abc1f24 --- /dev/null +++ b/User/bsp/pwm.h @@ -0,0 +1,50 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include "tim.h" +#include "bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ +/* PWM通道 */ +typedef enum { + BSP_PWM_TIM5_CH1, + BSP_PWM_TIM5_CH2, + BSP_PWM_TIM5_CH3, + BSP_PWM_NUM, + BSP_PWM_ERR, +} BSP_PWM_Channel_t; + +/* Exported functions prototypes -------------------------------------------- */ +int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch); +int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle); +int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq); +int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch); +uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch); +uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch); +TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch); +int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length); +int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/User/bsp/time.c b/User/bsp/time.c new file mode 100644 index 0000000..21918ed --- /dev/null +++ b/User/bsp/time.c @@ -0,0 +1,81 @@ +/* Includes ----------------------------------------------------------------- */ +#include "bsp/time.h" +#include "bsp.h" + +#include +#include "FreeRTOS.h" +#include "main.h" +#include "task.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +/* Private function -------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ + +uint32_t BSP_TIME_Get_ms() { return xTaskGetTickCount(); } + +uint64_t BSP_TIME_Get_us() { + uint32_t tick_freq = osKernelGetTickFreq(); + uint32_t ticks_old = xTaskGetTickCount()*(1000/tick_freq); + uint32_t tick_value_old = SysTick->VAL; + uint32_t ticks_new = xTaskGetTickCount()*(1000/tick_freq); + uint32_t tick_value_new = SysTick->VAL; + if (ticks_old == ticks_new) { + return ticks_new * 1000 + 1000 - tick_value_old * 1000 / (SysTick->LOAD + 1); + } else { + return ticks_new * 1000 + 1000 - tick_value_new * 1000 / (SysTick->LOAD + 1); + } +} + +uint64_t BSP_TIME_Get() __attribute__((alias("BSP_TIME_Get_us"))); + +int8_t BSP_TIME_Delay_ms(uint32_t ms) { + uint32_t tick_period = 1000u / osKernelGetTickFreq(); + uint32_t ticks = ms / tick_period; + + switch (osKernelGetState()) { + case osKernelError: + case osKernelReserved: + case osKernelLocked: + case osKernelSuspended: + return BSP_ERR; + + case osKernelRunning: + osDelay(ticks ? ticks : 1); + break; + + case osKernelInactive: + case osKernelReady: + HAL_Delay(ms); + break; + } + return BSP_OK; +} + +/*阻塞us延迟*/ +int8_t BSP_TIME_Delay_us(uint32_t us) { + uint64_t start = BSP_TIME_Get_us(); + while (BSP_TIME_Get_us() - start < us) { + // 等待us时间 + } + return BSP_OK; +} + +int8_t BSP_TIME_Delay(uint32_t ms) __attribute__((alias("BSP_TIME_Delay_ms"))); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ \ No newline at end of file diff --git a/User/bsp/time.h b/User/bsp/time.h new file mode 100644 index 0000000..c69085b --- /dev/null +++ b/User/bsp/time.h @@ -0,0 +1,43 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include + +#include "bsp/bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ +/* Exported functions prototypes -------------------------------------------- */ +uint32_t BSP_TIME_Get_ms(); + +uint64_t BSP_TIME_Get_us(); + +uint64_t BSP_TIME_Get(); + +int8_t BSP_TIME_Delay_ms(uint32_t ms); + +/*微秒阻塞延时，一般别用*/ +int8_t BSP_TIME_Delay_us(uint32_t us); + +int8_t BSP_TIME_Delay(uint32_t ms); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/bsp/uart.c b/User/bsp/uart.c new file mode 100644 index 0000000..2bfaff4 --- /dev/null +++ b/User/bsp/uart.c @@ -0,0 +1,155 @@ +/* Includes ----------------------------------------------------------------- */ +#include + +#include "bsp/uart.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static void (*UART_Callback[BSP_UART_NUM][BSP_UART_CB_NUM])(void); + +/* Private function -------------------------------------------------------- */ +static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) { + if (huart->Instance == UART5) + return BSP_UART_RC; + else + return BSP_UART_ERR; +} + +void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_TX_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_TX_CPLT_CB](); + } + } +} + +void HAL_UART_TxHalfCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_TX_HALF_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_TX_HALF_CPLT_CB](); + } + } +} + +void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_RX_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_RX_CPLT_CB](); + } + } +} + +void HAL_UART_RxHalfCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_RX_HALF_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_RX_HALF_CPLT_CB](); + } + } +} + +void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_ERROR_CB]) { + UART_Callback[bsp_uart][BSP_UART_ERROR_CB](); + } + } +} + +void HAL_UART_AbortCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_ABORT_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_ABORT_CPLT_CB](); + } + } +} + +void HAL_UART_AbortTransmitCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_ABORT_TX_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_ABORT_TX_CPLT_CB](); + } + } +} + +void HAL_UART_AbortReceiveCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_ABORT_RX_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_ABORT_RX_CPLT_CB](); + } + } +} + +/* Exported functions ------------------------------------------------------- */ +void BSP_UART_IRQHandler(UART_HandleTypeDef *huart) { + if (__HAL_UART_GET_FLAG(huart, UART_FLAG_IDLE)) { + __HAL_UART_CLEAR_IDLEFLAG(huart); + if (UART_Callback[UART_Get(huart)][BSP_UART_IDLE_LINE_CB]) { + UART_Callback[UART_Get(huart)][BSP_UART_IDLE_LINE_CB](); + } + } +} + +UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) { + switch (uart) { + case BSP_UART_RC: + return &huart3; + default: + return NULL; + } +} + +int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type, + void (*callback)(void)) { + if (callback == NULL) return BSP_ERR_NULL; + if (uart >= BSP_UART_NUM || type >= BSP_UART_CB_NUM) return BSP_ERR; + UART_Callback[uart][type] = callback; + return BSP_OK; +} + +int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma) { + if (uart >= BSP_UART_NUM) return BSP_ERR; + if (data == NULL || size == 0) return BSP_ERR_NULL; + + if (dma) { + return HAL_UART_Transmit_DMA(BSP_UART_GetHandle(uart), data, size); + } else { + return HAL_UART_Transmit_IT(BSP_UART_GetHandle(uart), data, size); + } +} + +int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma) { + if (uart >= BSP_UART_NUM) return BSP_ERR; + if (data == NULL || size == 0) return BSP_ERR_NULL; + + if (dma) { + return HAL_UART_Receive_DMA(BSP_UART_GetHandle(uart), data, size); + } else { + return HAL_UART_Receive_IT(BSP_UART_GetHandle(uart), data, size); + } +} + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ \ No newline at end of file diff --git a/User/bsp/uart.h b/User/bsp/uart.h new file mode 100644 index 0000000..e24194e --- /dev/null +++ b/User/bsp/uart.h @@ -0,0 +1,68 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include +#include + +#include "bsp/bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ + +/* 要添加使用UART的新设备，需要先在此添加对应的枚举值 */ + +/* UART实体枚举，与设备对应 */ +typedef enum { + BSP_UART_RC, + BSP_UART_NUM, + BSP_UART_ERR, +} BSP_UART_t; + +/* UART支持的中断回调函数类型，具体参考HAL中定义 */ +typedef enum { + BSP_UART_TX_HALF_CPLT_CB, + BSP_UART_TX_CPLT_CB, + BSP_UART_RX_HALF_CPLT_CB, + BSP_UART_RX_CPLT_CB, + BSP_UART_ERROR_CB, + BSP_UART_ABORT_CPLT_CB, + BSP_UART_ABORT_TX_CPLT_CB, + BSP_UART_ABORT_RX_CPLT_CB, + + BSP_UART_IDLE_LINE_CB, + BSP_UART_CB_NUM, +} BSP_UART_Callback_t; + +/* Exported functions prototypes -------------------------------------------- */ + +UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart); + +void BSP_UART_IRQHandler(UART_HandleTypeDef *huart); + +int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type, + void (*callback)(void)); + +int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma); +int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/component/ARM_USAGE.md b/User/component/ARM_USAGE.md new file mode 100644 index 0000000..9eff76f --- /dev/null +++ b/User/component/ARM_USAGE.md @@ -0,0 +1,184 @@ +# 6自由度机械臂正运动学使用说明 + +## 📋 文件说明 + +- **arm.h** - 机械臂接口头文件 +- **arm.cpp** - 机械臂实现文件（基于toolbox/robotics库） +- **arm_main.c** - 使用示例 + +## 🚀 快速开始 + +### 1. 初始化机械臂 + +```c +#include "component/arm.h" + +// 定义机器人尺寸参数 (TODO: 替换为实际测量值) +ArmParams_t arm_params = { + .h0 = 100.0f, // 基座高度 (mm) + .L1 = 200.0f, // 大臂长度 (mm) + .L2 = 150.0f, // 小臂长度 (mm) + .L3 = 80.0f // 夹爪长度 (mm) +}; + +// 初始化 +Arm_Init(&arm_params); +``` + +### 2. 正运动学计算 + +```c +// 输入：6个关节角度 +JointAngles_t joint_angles; +joint_angles.q[0] = 0.0f; // J1: 基座Yaw (rad) +joint_angles.q[1] = M_PI/4; // J2: 大臂Pitch (rad) +joint_angles.q[2] = -M_PI/4; // J3: 大臂末端Pitch (rad) +joint_angles.q[3] = 0.0f; // J4: 小臂Roll (rad) +joint_angles.q[4] = M_PI/6; // J5: 小臂末端Pitch (rad) +joint_angles.q[5] = 0.0f; // J6: 夹爪Roll (rad) + +// 输出：末端位姿 +Pose_t end_pose; + +// 执行正运动学 +if (Arm_ForwardKinematics(&joint_angles, &end_pose) == 0) { + // 成功！ + printf("末端位置: x=%.2f, y=%.2f, z=%.2f (mm)\n", + end_pose.x, end_pose.y, end_pose.z); + printf("末端姿态: roll=%.2f, pitch=%.2f, yaw=%.2f (rad)\n", + end_pose.roll, end_pose.pitch, end_pose.yaw); +} +``` + +### 3. 获取中间关节位姿（可选） + +```c +Pose_t joint3_pose; + +// 获取第3个关节的位姿 +if (Arm_GetJointPose(&joint_angles, 3, &joint3_pose) == 0) { + printf("J3位置: x=%.2f, y=%.2f, z=%.2f\n", + joint3_pose.x, joint3_pose.y, joint3_pose.z); +} +``` + +## 📐 关节配置说明 + +| 关节 | 名称 | 类型 | 旋转轴 | 说明 | +|-----|------|------|--------|------| +| J1 | 基座 | Yaw | Z轴 | 基座旋转 | +| J2 | 大臂起点 | Pitch | Y轴 | 大臂俯仰 | +| J3 | 大臂末端 | Pitch | Y轴 | 控制小臂 | +| J4 | 小臂起点 | Roll | X轴 | 小臂旋转 | +| J5 | 小臂末端 | Pitch | Y轴 | 控制夹爪段 | +| J6 | 夹爪 | Roll | X轴 | 夹爪旋转 | + +## ⚙️ DH参数配置 + +当前使用的是**标准DH参数**，定义在 `arm.cpp` 的 `CreateDHLinks()` 函数中： + +```cpp +// J1: 基座 Yaw +s_links[0] = robotics::Link(0, params->h0, 0, 0, robotics::R); + +// J2: 大臂 Pitch +s_links[1] = robotics::Link(0, 0, 0, M_PI_2, robotics::R); + +// J3: 大臂末端 Pitch +s_links[2] = robotics::Link(0, 0, params->L1, 0, robotics::R); + +// J4: 小臂起点 Roll +s_links[3] = robotics::Link(0, 0, 0, M_PI_2, robotics::R); + +// J5: 小臂末端 Pitch +s_links[4] = robotics::Link(0, 0, params->L2, M_PI_2, robotics::R); + +// J6: 夹爪 Roll +s_links[5] = robotics::Link(0, params->L3, 0, M_PI_2, robotics::R); +``` + +⚠️ **需要根据实际机器人测量结果调整！** + +## 📊 数据结构 + +### ArmParams_t - 机器人尺寸参数 +```c +typedef struct { + float h0; // 基座高度 (基座到J2的高度, mm) + float L1; // 大臂长度 (J2到J3的距离, mm) + float L2; // 小臂长度 (J4到J5的距离, mm) + float L3; // 夹爪长度 (J6到末端的距离, mm) +} ArmParams_t; +``` + +### JointAngles_t - 关节角度 +```c +typedef struct { + float q[6]; // 6个关节角度 (rad) +} JointAngles_t; +``` + +### Pose_t - 位姿 +```c +typedef struct { + float x, y, z; // 位置 (mm) + float roll, pitch, yaw; // 姿态 (rad) - RPY欧拉角 +} Pose_t; +``` + +## 🔧 待办事项 + +- [ ] **测量实际机器人尺寸**：h0, L1, L2, L3 +- [ ] **拍摄零位照片**：确认零位时各关节的姿态 +- [ ] **验证DH参数**：根据实际机器人调整 `CreateDHLinks()` 函数 +- [ ] **集成电机反馈**：将实际电机角度输入到正运动学 +- [ ] **添加逆运动学**：实现末端位姿到关节角度的计算 + +## 📚 依赖库 + +- **toolbox/robotics.h** - 机器人学工具箱 +- **toolbox/matrix.h** - 矩阵运算库 +- **ARM CMSIS DSP** - 数学计算库 + +## 🎯 使用示例（完整） + +参考 `User/task/arm_main.c` 中的实现： + +```c +void Task_arm_main(void *argument) { + // 1. 初始化 + ArmParams_t params = {100.0f, 200.0f, 150.0f, 80.0f}; + Arm_Init(¶ms); + + // 2. 设置关节角度 + JointAngles_t q = {0}; + + while (1) { + // 3. 读取电机角度（示例） + q.q[0] = get_motor_angle(0); + q.q[1] = get_motor_angle(1); + // ... + + // 4. 计算正运动学 + Pose_t pose; + Arm_ForwardKinematics(&q, &pose); + + // 5. 使用末端位姿 + // ... + + osDelay(10); + } +} +``` + +## 🐛 调试提示 + +如果计算结果不符合预期： +1. 检查DH参数是否正确 +2. 确认角度单位是弧度（不是角度） +3. 验证关节零位的定义 +4. 检查关节旋转方向（正负） + +--- + +**下一步**：提供实际机器人的尺寸数据，我将帮您完善DH参数！ diff --git a/User/component/PowerControl.c b/User/component/PowerControl.c new file mode 100644 index 0000000..d8d07bb --- /dev/null +++ b/User/component/PowerControl.c @@ -0,0 +1,88 @@ +#include "PowerControl.h" +#include "stdint.h" +#include "math.h" + + +void power_calu(power_model_t* param,float* in_array,float* rpm_array) +{ + float InitialGivePower[param->motor_num]; + float totalpower=0; + float in[4]; + float ve[4]; + for (uint8_t i = 0; i < param->motor_num; i++) // first get all the initial motor power and total motor power + { + in[i] = in_array[i]; + ve[i] = rpm_array[i]; + InitialGivePower[i] = in[i] * param->toque_coefficient * ve[i] + + param->k2 * ve[i] * ve[i]+ param->k1 * in[i]*in[i] + param->constant; + + param->power[i] = InitialGivePower[i]; + if (InitialGivePower[i] < 0) // negative power not included (transitory) + continue; + totalpower += InitialGivePower[i]; + } + param->total_power = totalpower; +} +float power_scale_calu(power_model_t** param_array,uint8_t num,float max_power) +{ + float total_power=0; + for(uint8_t i=0;itotal_power; + } + if(total_power>max_power) + {return max_power/total_power;} + else + {return 2;} +} + +void power_out_calu(power_model_t* param,float scale,float* in_array,float* rpm_array,float* out_array) +{ + float in[4]; + float ve[4]; + if(scale<1) + { + + float ScaledGivePower[4]; + for (uint8_t i = 0; i < param->motor_num; i++) + { + in[i] = in_array[i]; + ve[i] = rpm_array[i]; + ScaledGivePower[i] = param->power[i] * scale; // get scaled power + if (ScaledGivePower[i] < 0) + { + continue; + } + float b = param->toque_coefficient * ve[i]; + float c = param->k2 * ve[i]*ve[i] - ScaledGivePower[i] + param->constant; + float inside = b * b - 4 * param->k1 * c; + if (inside < 0) + { + continue; + } + else if (in[i] > 0) // Selection of the calculation formula according to the direction of the original moment + { + float temp = (-b + sqrt(inside)) / (2 * param->k1); + if (temp > 16000) + { + out_array[i] = 16000; + } + else + out_array[i] = temp; + } + else + { + float temp = (-b - sqrt(inside)) / (2 * param->k1); + if (temp < -16000) + { + out_array[i] = -16000; + } + else + out_array[i] = temp; + } + } + } +} + + + diff --git a/User/component/PowerControl.h b/User/component/PowerControl.h new file mode 100644 index 0000000..2ece9f9 --- /dev/null +++ b/User/component/PowerControl.h @@ -0,0 +1,29 @@ +#ifndef CHASSIS_POWER_CONTROL_WITH_SUPERCAP_H +#define CHASSIS_POWER_CONTROL_WITH_SUPERCAP_H + + +#ifdef __cplusplus +extern "C" { +#endif + +#include "main.h" + +typedef struct power_model_t +{ + uint8_t motor_num; + float total_power; + float* power; + float toque_coefficient; + float k1; + float k2 ; + float constant; +}power_model_t; + +void power_calu(power_model_t* param,float* in_array,float* rpm_array); +float power_scale_calu(power_model_t** param_array,uint8_t num,float max_power); +void power_out_calu(power_model_t* param,float scale,float* in_array,float* rpm_array,float* out_array); +#ifdef __cplusplus +} +#endif + +#endif diff --git a/User/component/ahrs.c b/User/component/ahrs.c new file mode 100644 index 0000000..4749736 --- /dev/null +++ b/User/component/ahrs.c @@ -0,0 +1,417 @@ +/* + 开源的AHRS算法。 + MadgwickAHRS +*/ + +#include "ahrs.h" + +#include + +#include "component/user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +#define BETA_IMU (0.033f) +#define BETA_AHRS (0.041f) + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* 2 * proportional gain (Kp) */ +static float beta = BETA_IMU; + +/** + * @brief 不使用磁力计计算姿态 + * + * @param ahrs 姿态解算主结构体 + * @param accl 加速度计数据 + * @param gyro 陀螺仪数据 + * @return int8_t 0对应没有错误 + */ +static int8_t AHRS_UpdateIMU(AHRS_t *ahrs, const AHRS_Accl_t *accl, + const AHRS_Gyro_t *gyro) { + if (ahrs == NULL) return -1; + if (accl == NULL) return -1; + if (gyro == NULL) return -1; + + beta = BETA_IMU; + + float ax = accl->x; + float ay = accl->y; + float az = accl->z; + + float gx = gyro->x; + float gy = gyro->y; + float gz = gyro->z; + + float recip_norm; + float s0, s1, s2, s3; + float q_dot1, q_dot2, q_dot3, q_dot4; + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, + q3q3; + + /* Rate of change of quaternion from gyroscope */ + q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy - + ahrs->quat.q3 * gz); + q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz - + ahrs->quat.q3 * gy); + q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz + + ahrs->quat.q3 * gx); + q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy - + ahrs->quat.q2 * gx); + + /* Compute feedback only if accelerometer measurement valid (avoids NaN in + * accelerometer normalisation) */ + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + /* Normalise accelerometer measurement */ + recip_norm = InvSqrt(ax * ax + ay * ay + az * az); + ax *= recip_norm; + ay *= recip_norm; + az *= recip_norm; + + /* Auxiliary variables to avoid repeated arithmetic */ + _2q0 = 2.0f * ahrs->quat.q0; + _2q1 = 2.0f * ahrs->quat.q1; + _2q2 = 2.0f * ahrs->quat.q2; + _2q3 = 2.0f * ahrs->quat.q3; + _4q0 = 4.0f * ahrs->quat.q0; + _4q1 = 4.0f * ahrs->quat.q1; + _4q2 = 4.0f * ahrs->quat.q2; + _8q1 = 8.0f * ahrs->quat.q1; + _8q2 = 8.0f * ahrs->quat.q2; + q0q0 = ahrs->quat.q0 * ahrs->quat.q0; + q1q1 = ahrs->quat.q1 * ahrs->quat.q1; + q2q2 = ahrs->quat.q2 * ahrs->quat.q2; + q3q3 = ahrs->quat.q3 * ahrs->quat.q3; + + /* Gradient decent algorithm corrective step */ + s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; + s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * ahrs->quat.q1 - + _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; + s2 = 4.0f * q0q0 * ahrs->quat.q2 + _2q0 * ax + _4q2 * q3q3 - + _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; + s3 = 4.0f * q1q1 * ahrs->quat.q3 - _2q1 * ax + + 4.0f * q2q2 * ahrs->quat.q3 - _2q2 * ay; + + /* normalise step magnitude */ + recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); + + s0 *= recip_norm; + s1 *= recip_norm; + s2 *= recip_norm; + s3 *= recip_norm; + + /* Apply feedback step */ + q_dot1 -= beta * s0; + q_dot2 -= beta * s1; + q_dot3 -= beta * s2; + q_dot4 -= beta * s3; + } + + /* Integrate rate of change of quaternion to yield quaternion */ + ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq; + ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq; + ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq; + ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq; + + /* Normalise quaternion */ + recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 + + ahrs->quat.q1 * ahrs->quat.q1 + + ahrs->quat.q2 * ahrs->quat.q2 + + ahrs->quat.q3 * ahrs->quat.q3); + ahrs->quat.q0 *= recip_norm; + ahrs->quat.q1 *= recip_norm; + ahrs->quat.q2 *= recip_norm; + ahrs->quat.q3 *= recip_norm; + + return 0; +} + +/** + * @brief 初始化姿态解算 + * + * @param ahrs 姿态解算主结构体 + * @param magn 磁力计数据 + * @param sample_freq 采样频率 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq) { + if (ahrs == NULL) return -1; + + ahrs->inv_sample_freq = 1.0f / sample_freq; + + ahrs->quat.q0 = 1.0f; + ahrs->quat.q1 = 0.0f; + ahrs->quat.q2 = 0.0f; + ahrs->quat.q3 = 0.0f; + + if (magn) { + float yaw = -atan2(magn->y, magn->x); + + if ((magn->x == 0.0f) && (magn->y == 0.0f) && (magn->z == 0.0f)) { + ahrs->quat.q0 = 0.800884545f; + ahrs->quat.q1 = 0.00862364192f; + ahrs->quat.q2 = -0.00283267116f; + ahrs->quat.q3 = 0.598749936f; + + } else if ((yaw < (M_PI / 2.0f)) || (yaw > 0.0f)) { + ahrs->quat.q0 = 0.997458339f; + ahrs->quat.q1 = 0.000336312107f; + ahrs->quat.q2 = -0.0057230792f; + ahrs->quat.q3 = 0.0740156546; + + } else if ((yaw < M_PI) || (yaw > (M_PI / 2.0f))) { + ahrs->quat.q0 = 0.800884545f; + ahrs->quat.q1 = 0.00862364192f; + ahrs->quat.q2 = -0.00283267116f; + ahrs->quat.q3 = 0.598749936f; + + } else if ((yaw < 90.0f) || (yaw > M_PI)) { + ahrs->quat.q0 = 0.800884545f; + ahrs->quat.q1 = 0.00862364192f; + ahrs->quat.q2 = -0.00283267116f; + ahrs->quat.q3 = 0.598749936f; + + } else if ((yaw < 90.0f) || (yaw > 0.0f)) { + ahrs->quat.q0 = 0.800884545f; + ahrs->quat.q1 = 0.00862364192f; + ahrs->quat.q2 = -0.00283267116f; + ahrs->quat.q3 = 0.598749936f; + } + } + return 0; +} + +/** + * @brief 姿态运算更新一次 + * @note 输入数据必须是NED(North East Down) 参考坐标系 + * + * @param ahrs 姿态解算主结构体 + * @param accl 加速度计数据 + * @param gyro 陀螺仪数据 + * @param magn 磁力计数据 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl, + const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn) { + if (ahrs == NULL) return -1; + if (accl == NULL) return -1; + if (gyro == NULL) return -1; + + beta = BETA_AHRS; + + float recip_norm; + float s0, s1, s2, s3; + float q_dot1, q_dot2, q_dot3, q_dot4; + float hx, hy; + float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, + _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, + q2q2, q2q3, q3q3; + + if (magn == NULL) return AHRS_UpdateIMU(ahrs, accl, gyro); + + float mx = magn->x; + float my = magn->y; + float mz = magn->z; + + /* Use IMU algorithm if magnetometer measurement invalid (avoids NaN in */ + /* magnetometer normalisation) */ + if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { + return AHRS_UpdateIMU(ahrs, accl, gyro); + } + + float ax = accl->x; + float ay = accl->y; + float az = accl->z; + + float gx = gyro->x; + float gy = gyro->y; + float gz = gyro->z; + + /* Rate of change of quaternion from gyroscope */ + q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy - + ahrs->quat.q3 * gz); + q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz - + ahrs->quat.q3 * gy); + q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz + + ahrs->quat.q3 * gx); + q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy - + ahrs->quat.q2 * gx); + + /* Compute feedback only if accelerometer measurement valid (avoids NaN in + * accelerometer normalisation) */ + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + /* Normalise accelerometer measurement */ + recip_norm = InvSqrt(ax * ax + ay * ay + az * az); + ax *= recip_norm; + ay *= recip_norm; + az *= recip_norm; + + /* Normalise magnetometer measurement */ + recip_norm = InvSqrt(mx * mx + my * my + mz * mz); + mx *= recip_norm; + my *= recip_norm; + mz *= recip_norm; + + /* Auxiliary variables to avoid repeated arithmetic */ + _2q0mx = 2.0f * ahrs->quat.q0 * mx; + _2q0my = 2.0f * ahrs->quat.q0 * my; + _2q0mz = 2.0f * ahrs->quat.q0 * mz; + _2q1mx = 2.0f * ahrs->quat.q1 * mx; + _2q0 = 2.0f * ahrs->quat.q0; + _2q1 = 2.0f * ahrs->quat.q1; + _2q2 = 2.0f * ahrs->quat.q2; + _2q3 = 2.0f * ahrs->quat.q3; + _2q0q2 = 2.0f * ahrs->quat.q0 * ahrs->quat.q2; + _2q2q3 = 2.0f * ahrs->quat.q2 * ahrs->quat.q3; + q0q0 = ahrs->quat.q0 * ahrs->quat.q0; + q0q1 = ahrs->quat.q0 * ahrs->quat.q1; + q0q2 = ahrs->quat.q0 * ahrs->quat.q2; + q0q3 = ahrs->quat.q0 * ahrs->quat.q3; + q1q1 = ahrs->quat.q1 * ahrs->quat.q1; + q1q2 = ahrs->quat.q1 * ahrs->quat.q2; + q1q3 = ahrs->quat.q1 * ahrs->quat.q3; + q2q2 = ahrs->quat.q2 * ahrs->quat.q2; + q2q3 = ahrs->quat.q2 * ahrs->quat.q3; + q3q3 = ahrs->quat.q3 * ahrs->quat.q3; + + /* Reference direction of Earth's magnetic field */ + hx = mx * q0q0 - _2q0my * ahrs->quat.q3 + + _2q0mz * ahrs->quat.q2 + mx * q1q1 + + _2q1 * my * ahrs->quat.q2 + _2q1 * mz * ahrs->quat.q3 - + mx * q2q2 - mx * q3q3; + hy = _2q0mx * ahrs->quat.q3 + my * q0q0 - + _2q0mz * ahrs->quat.q1 + _2q1mx * ahrs->quat.q2 - + my * q1q1 + my * q2q2 + _2q2 * mz * ahrs->quat.q3 - my * q3q3; + // _2bx = sqrtf(hx * hx + hy * hy); + // 改为invsqrt + _2bx = 1.f / InvSqrt(hx * hx + hy * hy); + _2bz = -_2q0mx * ahrs->quat.q2 + _2q0my * ahrs->quat.q1 + + mz * q0q0 + _2q1mx * ahrs->quat.q3 - mz * q1q1 + + _2q2 * my * ahrs->quat.q3 - mz * q2q2 + mz * q3q3; + _4bx = 2.0f * _2bx; + _4bz = 2.0f * _2bz; + + /* Gradient decent algorithm corrective step */ + s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - + _2bz * ahrs->quat.q2 * + (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (-_2bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) * + (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + _2bx * ahrs->quat.q2 * + (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - + 4.0f * ahrs->quat.q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + + _2bz * ahrs->quat.q3 * + (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (_2bx * ahrs->quat.q2 + _2bz * ahrs->quat.q0) * + (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + (_2bx * ahrs->quat.q3 - _4bz * ahrs->quat.q1) * + (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - + 4.0f * ahrs->quat.q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + + (-_4bx * ahrs->quat.q2 - _2bz * ahrs->quat.q0) * + (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (_2bx * ahrs->quat.q1 + _2bz * ahrs->quat.q3) * + (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + (_2bx * ahrs->quat.q0 - _4bz * ahrs->quat.q2) * + (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + + (-_4bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) * + (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (-_2bx * ahrs->quat.q0 + _2bz * ahrs->quat.q2) * + (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + _2bx * ahrs->quat.q1 * + (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + /* normalise step magnitude */ + recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); + s0 *= recip_norm; + s1 *= recip_norm; + s2 *= recip_norm; + s3 *= recip_norm; + + /* Apply feedback step */ + q_dot1 -= beta * s0; + q_dot2 -= beta * s1; + q_dot3 -= beta * s2; + q_dot4 -= beta * s3; + } + + /* Integrate rate of change of quaternion to yield quaternion */ + ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq; + ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq; + ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq; + ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq; + + /* Normalise quaternion */ + recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 + + ahrs->quat.q1 * ahrs->quat.q1 + + ahrs->quat.q2 * ahrs->quat.q2 + + ahrs->quat.q3 * ahrs->quat.q3); + ahrs->quat.q0 *= recip_norm; + ahrs->quat.q1 *= recip_norm; + ahrs->quat.q2 *= recip_norm; + ahrs->quat.q3 *= recip_norm; + + return 0; +} + +/** + * @brief 通过姿态解算主结构体中的四元数计算欧拉角 + * + * @param eulr 欧拉角 + * @param ahrs 姿态解算主结构体 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs) { + if (eulr == NULL) return -1; + if (ahrs == NULL) return -1; + + const float sinr_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q1 + + ahrs->quat.q2 * ahrs->quat.q3); + const float cosr_cosp = + 1.0f - 2.0f * (ahrs->quat.q1 * ahrs->quat.q1 + + ahrs->quat.q2 * ahrs->quat.q2); + eulr->pit = atan2f(sinr_cosp, cosr_cosp); + + const float sinp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q2 - + ahrs->quat.q3 * ahrs->quat.q1); + + if (fabsf(sinp) >= 1.0f) + eulr->rol = copysignf(M_PI / 2.0f, sinp); + else + eulr->rol = asinf(sinp); + + const float siny_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q3 + + ahrs->quat.q1 * ahrs->quat.q2); + const float cosy_cosp = + 1.0f - 2.0f * (ahrs->quat.q2 * ahrs->quat.q2 + + ahrs->quat.q3 * ahrs->quat.q3); + eulr->yaw = atan2f(siny_cosp, cosy_cosp); + +#if 0 + eulr->yaw *= M_RAD2DEG_MULT; + eulr->rol *= M_RAD2DEG_MULT; + eulr->pit *= M_RAD2DEG_MULT; +#endif + + return 0; +} + +/** + * \brief 将对应数据置零 + * + * \param eulr 被操作的数据 + */ +void AHRS_ResetEulr(AHRS_Eulr_t *eulr) { memset(eulr, 0, sizeof(*eulr)); } + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/User/component/ahrs.h b/User/component/ahrs.h new file mode 100644 index 0000000..4c30510 --- /dev/null +++ b/User/component/ahrs.h @@ -0,0 +1,114 @@ +/* + 开源的AHRS算法。 + MadgwickAHRS +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "component/user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* 欧拉角（Euler angle） */ +typedef struct { + float yaw; /* 偏航角（Yaw angle） */ + float pit; /* 俯仰角（Pitch angle） */ + float rol; /* 翻滚角（Roll angle） */ +} AHRS_Eulr_t; + +/* 加速度计 Accelerometer */ +typedef struct { + float x; + float y; + float z; +} AHRS_Accl_t; + +/* 陀螺仪 Gyroscope */ +typedef struct { + float x; + float y; + float z; +} AHRS_Gyro_t; + +/* 磁力计 Magnetometer */ +typedef struct { + float x; + float y; + float z; +} AHRS_Magn_t; + +/* 四元数 */ +typedef struct { + float q0; + float q1; + float q2; + float q3; +} AHRS_Quaternion_t; + +/* 姿态解算算法主结构体 */ +typedef struct { + /* 四元数 */ + AHRS_Quaternion_t quat; + + float inv_sample_freq; /* 采样频率的的倒数 */ +} AHRS_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/** + * @brief 初始化姿态解算 + * + * @param ahrs 姿态解算主结构体 + * @param magn 磁力计数据 + * @param sample_freq 采样频率 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq); + +/** + * @brief 姿态运算更新一次 + * + * @param ahrs 姿态解算主结构体 + * @param accl 加速度计数据 + * @param gyro 陀螺仪数据 + * @param magn 磁力计数据 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl, + const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn); + +/** + * @brief 通过姿态解算主结构体中的四元数计算欧拉角 + * + * @param eulr 欧拉角 + * @param ahrs 姿态解算主结构体 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs); + +/** + * \brief 将对应数据置零 + * + * \param eulr 被操作的数据 + */ +void AHRS_ResetEulr(AHRS_Eulr_t *eulr); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/component/arm_kinematics/arm6dof.cpp b/User/component/arm_kinematics/arm6dof.cpp new file mode 100644 index 0000000..4f016e3 --- /dev/null +++ b/User/component/arm_kinematics/arm6dof.cpp @@ -0,0 +1,250 @@ +/** + ****************************************************************************** + * @file arm.cpp + * @brief 6-DOF robotic arm kinematics control implementation + ****************************************************************************** + */ + +#include "arm6dof.h" +#include "toolbox/robotics.h" +#include "toolbox/matrix.h" +#include "user_math.h" +#include + +/* 静态变量 */ +static robotics::Link* s_links = nullptr; +static robotics::Serial_Link<6>* s_robot = nullptr; +static bool s_initialized = false; + +/** + * dh_params = [ + {"a": 0, "alpha": pi / 2, "d": 0, "theta_offset": 0}, # Joint 1 + {"a": 0.404, "alpha": 0.0, "d": 0, "theta_offset": 0}, # Joint 2 + {"a": 0.091579, "alpha": -pi / 2, "d": 0, "theta_offset": pi / 2}, # Joint 3 + {"a": 0, "alpha": pi / 2, "d": 0.2411, "theta_offset": 0}, + {"a": 0, "alpha": -pi / 2, "d": 0, "theta_offset": 0}, + {"a": 0, "alpha": 0, "d": 0.1094, "theta_offset": 0}, + {"a": 0.0577, "alpha": pi / 2, "d": 0.033001, "theta_offset": 0}, + {"a": 0.05, "alpha": 0, "d": 0, "theta_offset": 0}, +] + * @brief 根据参数创建DH参数链表 + * @note 标准DH参数，参考 DH_PARAMETERS.md 文档 + * + * DH参数表（标准DH）： + * 关节 | θᵢ(变量) | dᵢ | aᵢ | αᵢ | 说明 + * -----|----------|-------|------|--------|------------------ + * 1 | q1 | h0 | 0 | π/2 | 底座旋转（Yaw） + * 2 | q2 | 0 | L1 | 0 | 肩部俯仰（Pitch） + * 3 | q3 | 0 | L2 | 0 | 肘部俯仰（Pitch） + * 4 | q4 | d4 | 0 | π/2 | 腕部滚转（Roll） + * 5 | q5 | 0 | 0 | -π/2 | 腕部俯仰（Pitch） + * 6 | q6 | d6 | 0 | 0 | 末端旋转（Roll） + +/** + * @brief 初始化机械臂运动学模型 + */ +void Arm6dof_Init(const Arm6dof_DHParams_t dh_params[6]) { + if (dh_params == nullptr) { + return; + } + + // 创建DH链表 + if (s_links != nullptr) { + delete[] s_links; + } + s_links = new robotics::Link[6]; + + for (int i = 0; i < 6; i++) { + // 构建质心向量（DH连杆坐标系下，由URDF经Rz(-theta_offset)旋转得到） + Matrixf<3, 1> rc_vec; + rc_vec[0][0] = dh_params[i].rc[0]; + rc_vec[1][0] = dh_params[i].rc[1]; + rc_vec[2][0] = dh_params[i].rc[2]; + + s_links[i] = robotics::Link( + dh_params[i].theta, // 初始theta值（通常为0） + dh_params[i].d, // 连杆偏移 (m) + dh_params[i].a, // 连杆长度 (m) + dh_params[i].alpha, // 扭转角 (rad) + robotics::R, // 旋转关节 + dh_params[i].theta_offset, // theta偏移 (rad) + dh_params[i].qmin, // 最小角度 (rad) + dh_params[i].qmax, // 最大角度 (rad) + dh_params[i].m, // 质量 (kg) + rc_vec // 质心坐标（DH连杆坐标系，m） + ); + } + + // 创建机器人模型 + if (s_robot != nullptr) { + delete s_robot; + } + s_robot = new robotics::Serial_Link<6>(s_links); + + s_initialized = true; +} + +/** + * @brief 正运动学 + */ +int Arm6dof_ForwardKinematics(const Arm6dof_JointAngles_t* q, Arm6dof_Pose_t* pose) { + if (!s_initialized || q == nullptr || pose == nullptr) { + return -1; + } + + // 将关节角度转换为矩阵格式 + Matrixf<6, 1> q_mat; + for (int i = 0; i < 6; i++) { + q_mat[i][0] = q->q[i]; + } + + // 计算正运动学 + Matrixf<4, 4> T = s_robot->fkine(q_mat); + + // 提取位置 (x, y, z) + Matrixf<3, 1> pos = robotics::t2p(T); + pose->x = pos[0][0]; + pose->y = pos[1][0]; + pose->z = pos[2][0]; + + // 提取姿态 (RPY欧拉角) + Matrixf<3, 1> rpy = robotics::t2rpy(T); + pose->yaw = rpy[0][0]; + pose->pitch = rpy[1][0]; + pose->roll = rpy[2][0]; + + return 0; +} + +/** + * @brief 获取指定关节的位姿 + */ +int Arm6dof_GetJointPose(const Arm6dof_JointAngles_t* q, uint8_t joint_num, Arm6dof_Pose_t* pose) { + if (!s_initialized || q == nullptr || pose == nullptr) { + return -1; + } + + if (joint_num < 1 || joint_num > 6) { + return -1; + } + + // 将关节角度转换为矩阵格式 + Matrixf<6, 1> q_mat; + for (int i = 0; i < 6; i++) { + q_mat[i][0] = q->q[i]; + } + + // 计算到第k个关节的变换矩阵 + Matrixf<4, 4> T = s_robot->fkine(q_mat, joint_num); + + // 提取位置和姿态 + Matrixf<3, 1> pos = robotics::t2p(T); + pose->x = pos[0][0]; + pose->y = pos[1][0]; + pose->z = pos[2][0]; + + Matrixf<3, 1> rpy = robotics::t2rpy(T); + pose->yaw = rpy[0][0]; + pose->pitch = rpy[1][0]; + pose->roll = rpy[2][0]; + + return 0; +} + +/** + * @brief 逆运动学 + */ +int Arm6dof_InverseKinematics(const Arm6dof_Pose_t* pose, const Arm6dof_JointAngles_t* q_init, + Arm6dof_JointAngles_t* q_result, float tol, uint16_t max_iter) { + if (!s_initialized || pose == nullptr || q_result == nullptr) { + return -1; + } + + // 构造目标变换矩阵 + Matrixf<3, 1> rpy_vec; + rpy_vec[0][0] = pose->yaw; + rpy_vec[1][0] = pose->pitch; + rpy_vec[2][0] = pose->roll; + Matrixf<3, 3> R_target = robotics::rpy2r(rpy_vec); + + Matrixf<3, 1> p_target; + p_target[0][0] = pose->x; + p_target[1][0] = pose->y; + p_target[2][0] = pose->z; + + Matrixf<4, 4> T_target = robotics::rp2t(R_target, p_target); + + // 初始猜测值 + Matrixf<6, 1> q_guess = matrixf::zeros<6, 1>(); + if (q_init != nullptr) { + for (int i = 0; i < 6; i++) { + q_guess[i][0] = q_init->q[i]; + } + } + + // 调用toolbox的逆运动学求解器（牛顿法） + Matrixf<6, 1> q_solved = s_robot->ikine(T_target, q_guess, tol, max_iter); + + // 将结果复制到输出 + for (int i = 0; i < 6; i++) { + q_result->q[i] = q_solved[i][0]; + } + + // 验证解的精度：位置误差 + 姿态误差双y重校验 + Matrixf<4, 4> T_verify = s_robot->fkine(q_solved); + Matrixf<3, 1> p_verify = robotics::t2p(T_verify); + + float pos_error = (p_verify - p_target).norm(); + + if (pos_error > tol * 10.0f) { // 位置误差过大，未收敛 + return -1; + } + + // // 姿态误差：计算两旋转矩阵的测地距离 theta = arccos((trace(R_v * R_t^T) - 1) / 2) + // // 使用旋转矩阵方法避免 RPY 奇异点和角度折叠问题 + // Matrixf<3, 3> R_verify = robotics::t2r(T_verify); + // Matrixf<3, 3> R_diff = R_verify * R_target.trans(); + // float trace_val = R_diff[0][0] + R_diff[1][1] + R_diff[2][2]; + // float cos_theta = (trace_val - 1.0f) * 0.5f; + // // 数值钳位防止 acosf 域溢出 + // if (cos_theta > 1.0f) cos_theta = 1.0f; + // if (cos_theta < -1.0f) cos_theta = -1.0f; + // float ang_error = acosf(cos_theta); // [0, π] + + // const float ang_tol = 0.1f; // 姿态收敛阈值 (rad ≈ 5.7°) + // if (ang_error > ang_tol) { // 姿态未收敛（收敛到错误分支） + // return -1; + // } + + return 0; +} + +/** + * @brief 计算重力补偿力矩 + * @details 利用牛顿-欧拉逆动力学(rne)，设速度qv=0、加速度qa=0， + * 计算的关节力矩即为平衡重力所需的力矩。 + */ +int Arm6dof_GravityCompensation(const Arm6dof_JointAngles_t* q, float gravity_torques[6]) { + if (!s_initialized || q == nullptr || gravity_torques == nullptr) { + return -1; + } + + // 关节角度 + Matrixf<6, 1> q_mat; + for (int i = 0; i < 6; i++) { + q_mat[i][0] = q->q[i]; + } + + // 速度和加速度均为零 —— 只计算重力项 + Matrixf<6, 1> qv = matrixf::zeros<6, 1>(); + Matrixf<6, 1> qa = matrixf::zeros<6, 1>(); + + // 调用牛顿-欧拉逆动力学 + Matrixf<6, 1> torques = s_robot->rne(q_mat, qv, qa); + + for (int i = 0; i < 6; i++) { + gravity_torques[i] = torques[i][0]; + } + + return 0; +} diff --git a/User/component/arm_kinematics/arm6dof.h b/User/component/arm_kinematics/arm6dof.h new file mode 100644 index 0000000..b2e9386 --- /dev/null +++ b/User/component/arm_kinematics/arm6dof.h @@ -0,0 +1,107 @@ +/** + ****************************************************************************** + * @file arm.h + * @brief 6-DOF robotic arm kinematics control + ****************************************************************************** + */ + +#ifndef ARM_H +#define ARM_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +/* 机器人尺寸参数结构体 */ +typedef struct { + // 6个运动关节的DH参数 + float theta; + float d; + float a; + float alpha; + float theta_offset; + float qmin; + float qmax; + float m; // 连杆质量 (kg) + float rc[3]; // 质心在 DH 连杆坐标系下的坐标 (m): {x, y, z} + // 由 URDF inertial/origin 经 Rz(-theta_offset) 旋转得到 + + // MIT控制参数（可选，如不设置则使用默认值） + float kp; // 位置刚度 (N·m/rad)，默认值见Joint类 + float kd; // 阻尼系数 (N·m·s/rad)，默认值见Joint类 +} Arm6dof_DHParams_t; +typedef struct { + Arm6dof_DHParams_t DH_params[6]; + struct { + float x, y, z; // 工具中心偏移 (m) + float roll, pitch, yaw; // 工具姿态偏移 (rad) + } tool_offset; +} Arm6dof_Params_t; +/* 末端位姿结构体 */ +typedef struct { + float x, y, z; // 位置 (m) + float roll, pitch, yaw; // 姿态 (rad) - RPY欧拉角 +} Arm6dof_Pose_t; + +/* 关节角度结构体 */ +typedef struct { + float q[6]; // 6个关节角度 (rad) +} Arm6dof_JointAngles_t; + +/** + * @brief 初始化机械臂运动学模型 + * @param dh_params 6个关节的DH参数数组 + * @note 必须先调用此函数再使用正/逆运动学 + */ +void Arm6dof_Init(const Arm6dof_DHParams_t dh_params[6]); + +/** + * @brief 正运动学：根据关节角度计算末端位姿 + * @param q 输入的6个关节角度 (rad) + * @param pose 输出的末端位姿 (位置m, 姿态rad) + * @retval 0: 成功, -1: 失败(未初始化) + */ +int Arm6dof_ForwardKinematics(const Arm6dof_JointAngles_t* q, Arm6dof_Pose_t* pose); + +/** + * @brief 逆运动学：根据末端位姿计算关节角度 + * @param pose 输入的目标末端位姿 (位置m, 姿态rad) + * @param q_init 初始关节角度猜测值 (用于数值解迭代) + * @param q_result 输出的关节角度解 (rad) + * @param tol 收敛误差容限，位置分量单位为(m)，建议0.001f(即1mm精度) + * @param max_iter 最大迭代次数，默认10 + * @retval 0: 成功, -1: 失败(未初始化或无解) + * @note 使用牛顿法数值求解，需要提供合理的初始猜测值 + */ +int Arm6dof_InverseKinematics(const Arm6dof_Pose_t* pose, const Arm6dof_JointAngles_t* q_init, + Arm6dof_JointAngles_t* q_result, float tol, uint16_t max_iter); + +/** + * @brief 获取指定关节的位姿 (用于可视化或调试) + * @param q 输入的6个关节角度 (rad) + * @param joint_num 关节编号 (1-6) + * @param pose 输出的该关节位姿 + * @retval 0: 成功, -1: 失败 + */ +int Arm6dof_GetJointPose(const Arm6dof_JointAngles_t* q, uint8_t joint_num, Arm6dof_Pose_t* pose); +/** + * @brief 设置机器人参数 (如果需要动态修改) + * @param params 新的机器人尺寸参数 + */ +void Arm6dof_SetParams(const Arm6dof_Params_t* params); + +/** + * @brief 计算重力补偿力矩（基于牛顿-欧拉逆动力学，速度和加速度为零） + * @param q 当前6个关节角度 (rad) + * @param gravity_torques 输出的6个关节重力补偿力矩 (N·m，DH参数单位为m时) + * @retval 0: 成功, -1: 失败(未初始化) + */ +int Arm6dof_GravityCompensation(const Arm6dof_JointAngles_t* q, float gravity_torques[6]); + +#ifdef __cplusplus +} +#endif + +#endif // ARM_H diff --git a/User/component/component_config.yaml b/User/component/component_config.yaml new file mode 100644 index 0000000..21b303d --- /dev/null +++ b/User/component/component_config.yaml @@ -0,0 +1,7 @@ +pid: + dependencies: + - component/filter + enabled: true +user_math: + dependencies: [] + enabled: true diff --git a/User/component/filter.c b/User/component/filter.c new file mode 100644 index 0000000..5375b8e --- /dev/null +++ b/User/component/filter.c @@ -0,0 +1,185 @@ +/* + 各类滤波器。 +*/ + +#include "filter.h" + +#include "user_math.h" + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param cutoff_freq 截止频率 + */ +void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq, + float cutoff_freq) { + if (f == NULL) return; + + f->cutoff_freq = cutoff_freq; + + f->delay_element_1 = 0.0f; + f->delay_element_2 = 0.0f; + + if (f->cutoff_freq <= 0.0f) { + /* no filtering */ + f->b0 = 1.0f; + f->b1 = 0.0f; + f->b2 = 0.0f; + + f->a1 = 0.0f; + f->a2 = 0.0f; + + return; + } + const float fr = sample_freq / f->cutoff_freq; + const float ohm = tanf(M_PI / fr); + const float c = 1.0f + 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm; + + f->b0 = ohm * ohm / c; + f->b1 = 2.0f * f->b0; + f->b2 = f->b0; + + f->a1 = 2.0f * (ohm * ohm - 1.0f) / c; + f->a2 = (1.0f - 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm) / c; +} + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample) { + if (f == NULL) return 0.0f; + + /* do the filtering */ + float delay_element_0 = + sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2; + + if (isinf(delay_element_0)) { + /* don't allow bad values to propagate via the filter */ + delay_element_0 = sample; + } + + const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 + + f->delay_element_2 * f->b2; + + f->delay_element_2 = f->delay_element_1; + f->delay_element_1 = delay_element_0; + + /* return the value. Should be no need to check limits */ + return output; +} + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample) { + if (f == NULL) return 0.0f; + + const float dval = sample / (f->b0 + f->b1 + f->b2); + + if (isfinite(dval)) { + f->delay_element_1 = dval; + f->delay_element_2 = dval; + + } else { + f->delay_element_1 = sample; + f->delay_element_2 = sample; + } + + return LowPassFilter2p_Apply(f, sample); +} + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param notch_freq 中心频率 + * @param bandwidth 带宽 + */ +void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq, + float bandwidth) { + if (f == NULL) return; + + f->notch_freq = notch_freq; + f->bandwidth = bandwidth; + + f->delay_element_1 = 0.0f; + f->delay_element_2 = 0.0f; + + if (notch_freq <= 0.0f) { + /* no filtering */ + f->b0 = 1.0f; + f->b1 = 0.0f; + f->b2 = 0.0f; + + f->a1 = 0.0f; + f->a2 = 0.0f; + + return; + } + + const float alpha = tanf(M_PI * bandwidth / sample_freq); + const float beta = -cosf(M_2PI * notch_freq / sample_freq); + const float a0_inv = 1.0f / (alpha + 1.0f); + + f->b0 = a0_inv; + f->b1 = 2.0f * beta * a0_inv; + f->b2 = a0_inv; + + f->a1 = f->b1; + f->a2 = (1.0f - alpha) * a0_inv; +} + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +inline float NotchFilter_Apply(NotchFilter_t *f, float sample) { + if (f == NULL) return 0.0f; + + /* Direct Form II implementation */ + const float delay_element_0 = + sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2; + const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 + + f->delay_element_2 * f->b2; + + f->delay_element_2 = f->delay_element_1; + f->delay_element_1 = delay_element_0; + + return output; +} + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float NotchFilter_Reset(NotchFilter_t *f, float sample) { + if (f == NULL) return 0.0f; + + float dval = sample; + + if (fabsf(f->b0 + f->b1 + f->b2) > FLT_EPSILON) { + dval = dval / (f->b0 + f->b1 + f->b2); + } + + f->delay_element_1 = dval; + f->delay_element_2 = dval; + + return NotchFilter_Apply(f, sample); +} diff --git a/User/component/filter.h b/User/component/filter.h new file mode 100644 index 0000000..ae2b072 --- /dev/null +++ b/User/component/filter.h @@ -0,0 +1,120 @@ +/* + 各类滤波器。 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* 二阶低通滤波器 */ +typedef struct { + float cutoff_freq; /* 截止频率 */ + + float a1; + float a2; + + float b0; + float b1; + float b2; + + float delay_element_1; + float delay_element_2; + +} LowPassFilter2p_t; + +/* 带阻滤波器 */ +typedef struct { + float notch_freq; /* 阻止频率 */ + float bandwidth; /* 带宽 */ + + float a1; + float a2; + + float b0; + float b1; + float b2; + float delay_element_1; + float delay_element_2; + +} NotchFilter_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param cutoff_freq 截止频率 + */ +void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq, + float cutoff_freq); + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample); + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample); + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param notch_freq 中心频率 + * @param bandwidth 带宽 + */ +void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq, + float bandwidth); + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float NotchFilter_Apply(NotchFilter_t *f, float sample); + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float NotchFilter_Reset(NotchFilter_t *f, float sample); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/component/mixer.c b/User/component/mixer.c new file mode 100644 index 0000000..554b92e --- /dev/null +++ b/User/component/mixer.c @@ -0,0 +1,94 @@ +/* + 混合器 +*/ + +#include "mixer.h" + +#include "math.h" + +/** + * @brief 初始化混合器 + * + * @param mixer 混合器 + * @param mode 混合器模式 + * @return int8_t 0对应没有错误 + */ +int8_t Mixer_Init(Mixer_t *mixer, Mixer_Mode_t mode) { + if (mixer == NULL) return -1; + + mixer->mode = mode; + return 0; +} + +/** + * @brief 计算输出 + * + * @param mixer 混合器 + * @param move_vec 运动向量 + * @param out 输出数组 + * @param len 输出数组长短 + * @param scale 输出放大因子 + * @return int8_t 0对应没有错误 + */ +int8_t Mixer_Apply(Mixer_t *mixer, MoveVector_t *move_vec, float *out, + int8_t len, float scale) { + if (mixer == NULL) return -1; + + switch (mixer->mode) { + case MIXER_MECANUM: + if (len == 4) { + out[0] = move_vec->vx - move_vec->vy + move_vec->wz; + out[1] = move_vec->vx + move_vec->vy + move_vec->wz; + out[2] = -move_vec->vx + move_vec->vy + move_vec->wz; + out[3] = -move_vec->vx - move_vec->vy + move_vec->wz; + } else { + goto error; + } + break; + + case MIXER_PARLFIX4: + if (len == 4) { + out[0] = -move_vec->vx; + out[1] = move_vec->vx; + out[2] = move_vec->vx; + out[3] = -move_vec->vx; + } else { + goto error; + } + case MIXER_PARLFIX2: + if (len == 2) { + out[0] = -move_vec->vx; + out[1] = move_vec->vx; + } else { + goto error; + } + case MIXER_SINGLE: + if (len == 1) { + out[0] = move_vec->vx; + } else { + goto error; + } + case MIXER_OMNICROSS: + case MIXER_OMNIPLUS: + goto error; + } + + float abs_max = 0.f; + for (int8_t i = 0; i < len; i++) { + const float abs_val = fabsf(out[i]); + abs_max = (abs_val > abs_max) ? abs_val : abs_max; + } + if (abs_max > 1.f) { + for (int8_t i = 0; i < len; i++) { + out[i] /= abs_max; + } + } + for (int8_t i = 0; i < len; i++) { + out[i] *= scale; + } + return 0; + +error: + for (uint8_t i = 0; i < len; i++) out[i] = 0; + return -1; +} diff --git a/User/component/mixer.h b/User/component/mixer.h new file mode 100644 index 0000000..b8e4401 --- /dev/null +++ b/User/component/mixer.h @@ -0,0 +1,76 @@ +/* + 混合器 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/** 四轮布局 */ +/* 前 */ +/* 2 1 */ +/* 3 4 */ + +/* 两轮布局 */ +/* 前 */ +/* 2 1 */ + +/* 混合器模式 */ +typedef enum { + MIXER_MECANUM, /* 麦克纳姆轮 */ + MIXER_PARLFIX4, /* 平行四驱动轮 */ + MIXER_PARLFIX2, /* 平行对侧两驱动轮 */ + MIXER_OMNICROSS, /* 叉形全向轮 */ + MIXER_OMNIPLUS, /* 十字全向轮 */ + MIXER_SINGLE, /* 单个摩擦轮 */ +} Mixer_Mode_t; + +typedef struct { + Mixer_Mode_t mode; +} Mixer_t; /* 混合器主结构体 */ + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/** + * @brief 初始化混合器 + * + * @param mixer 混合器 + * @param mode 混合器模式 + * @return int8_t 0对应没有错误 + */ +int8_t Mixer_Init(Mixer_t *mixer, Mixer_Mode_t mode); + +/** + * @brief 计算输出 + * + * @param mixer 混合器 + * @param move_vec 运动向量 + * @param out 输出数组 + * @param len 输出数组长短 + * @param scale 输出放大因子 + * @return int8_t 0对应没有错误 + */ +int8_t Mixer_Apply(Mixer_t *mixer, MoveVector_t *move_vec, float *out, + int8_t len, float scale); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/component/pid.c b/User/component/pid.c new file mode 100644 index 0000000..0a3c7d4 --- /dev/null +++ b/User/component/pid.c @@ -0,0 +1,158 @@ +/* + Modified from + https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.cpp + + 参考资料： + https://github.com/PX4/Firmware/issues/12362 + https://dev.px4.io/master/en/flight_stack/controller_diagrams.html + https://docs.px4.io/master/en/config_mc/pid_tuning_guide_multicopter.html#standard_form + https://www.controleng.com/articles/not-all-pid-controllers-are-the-same/ + https://en.wikipedia.org/wiki/PID_controller + http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/ +*/ + +#include "pid.h" + +#define SIGMA 0.000001f + +/** + * @brief 初始化PID + * + * @param pid PID结构体 + * @param mode PID模式 + * @param sample_freq 采样频率 + * @param param PID参数 + * @return int8_t 0对应没有错误 + */ +int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq, + const KPID_Params_t *param) { + if (pid == NULL) return -1; + + if (!isfinite(param->p)) return -1; + if (!isfinite(param->i)) return -1; + if (!isfinite(param->d)) return -1; + if (!isfinite(param->i_limit)) return -1; + if (!isfinite(param->out_limit)) return -1; + pid->param = param; + + float dt_min = 1.0f / sample_freq; + if (isfinite(dt_min)) + pid->dt_min = dt_min; + else + return -1; + + LowPassFilter2p_Init(&(pid->dfilter), sample_freq, pid->param->d_cutoff_freq); + + pid->mode = mode; + PID_Reset(pid); + return 0; +} + +/** + * @brief PID计算 + * + * @param pid PID结构体 + * @param sp 设定值 + * @param fb 反馈值 + * @param fb_dot 反馈值微分 + * @param dt 间隔时间 + * @return float 计算的输出 + */ +float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt) { + if (!isfinite(sp) || !isfinite(fb) || !isfinite(fb_dot) || !isfinite(dt)) { + return pid->last.out; + } + + /* 计算误差值 */ + const float err = CircleError(sp, fb, pid->param->range); + + /* 计算P项 */ + const float k_err = err * pid->param->k; + + /* 计算D项 */ + const float k_fb = pid->param->k * fb; + const float filtered_k_fb = LowPassFilter2p_Apply(&(pid->dfilter), k_fb); + + float d; + switch (pid->mode) { + case KPID_MODE_CALC_D: + /* 通过fb计算D，避免了由于sp变化导致err突变的问题 */ + /* 当sp不变时，err的微分等于负的fb的微分 */ + d = (filtered_k_fb - pid->last.k_fb) / fmaxf(dt, pid->dt_min); + break; + + case KPID_MODE_SET_D: + d = fb_dot; + break; + + case KPID_MODE_NO_D: + d = 0.0f; + break; + } + pid->last.err = err; + pid->last.k_fb = filtered_k_fb; + + if (!isfinite(d)) d = 0.0f; + + /* 计算PD输出 */ + float output = (k_err * pid->param->p) - (d * pid->param->d); + + /* 计算I项 */ + const float i = pid->i + (k_err * dt); + const float i_out = i * pid->param->i; + + if (pid->param->i > SIGMA) { + /* 检查是否饱和 */ + if (isfinite(i)) { + if ((fabsf(output + i_out) <= pid->param->out_limit) && + (fabsf(i) <= pid->param->i_limit)) { + /* 未饱和，使用新积分 */ + pid->i = i; + } + } + } + + /* 计算PID输出 */ + output += i_out; + + /* 限制输出 */ + if (isfinite(output)) { + if (pid->param->out_limit > SIGMA) { + output = AbsClip(output, pid->param->out_limit); + } + pid->last.out = output; + } + return pid->last.out; +} + +/** + * @brief 重置微分项 + * + * @param pid PID结构体 + * @return int8_t 0对应没有错误 + */ +int8_t PID_ResetIntegral(KPID_t *pid) { + if (pid == NULL) return -1; + + pid->i = 0.0f; + + return 0; +} + +/** + * @brief 重置PID + * + * @param pid PID结构体 + * @return int8_t 0对应没有错误 + */ +int8_t PID_Reset(KPID_t *pid) { + if (pid == NULL) return -1; + + pid->i = 0.0f; + pid->last.err = 0.0f; + pid->last.k_fb = 0.0f; + pid->last.out = 0.0f; + LowPassFilter2p_Reset(&(pid->dfilter), 0.0f); + + return 0; +} diff --git a/User/component/pid.h b/User/component/pid.h new file mode 100644 index 0000000..4b451eb --- /dev/null +++ b/User/component/pid.h @@ -0,0 +1,107 @@ +/* + Modified from + https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.h +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#include "filter.h" +#include "user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* PID模式 */ +typedef enum { + KPID_MODE_NO_D = 0, /* 不使用微分项，PI控制器 */ + KPID_MODE_CALC_D, /* 根据反馈的值计算离散微分，忽略PID_Calc中的fb_dot */ + KPID_MODE_SET_D /* 直接提供微分值，PID_Calc中的fb_dot将被使用，(Gyros) */ +} KPID_Mode_t; + +/* PID参数 */ +typedef struct { + float k; /* 控制器增益，设置为1用于并行模式 */ + float p; /* 比例项增益，设置为1用于标准形式 */ + float i; /* 积分项增益 */ + float d; /* 微分项增益 */ + float i_limit; /* 积分项上限 */ + float out_limit; /* 输出绝对值限制 */ + float d_cutoff_freq; /* D项低通截止频率 */ + float range; /* 计算循环误差时使用，大于0时启用 */ +} KPID_Params_t; + +/* PID主结构体 */ +typedef struct { + KPID_Mode_t mode; + const KPID_Params_t *param; + + float dt_min; /* 最小PID_Calc调用间隔 */ + float i; /* 积分 */ + + struct { + float err; /* 上次误差 */ + float k_fb; /* 上次反馈值 */ + float out; /* 上次输出 */ + } last; + + LowPassFilter2p_t dfilter; /* D项低通滤波器 */ +} KPID_t; + +/** + * @brief 初始化PID + * + * @param pid PID结构体 + * @param mode PID模式 + * @param sample_freq 采样频率 + * @param param PID参数 + * @return int8_t 0对应没有错误 + */ +int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq, + const KPID_Params_t *param); + +/** + * @brief PID计算 + * + * @param pid PID结构体 + * @param sp 设定值 + * @param fb 反馈值 + * @param fb_dot 反馈值微分 + * @param dt 间隔时间 + * @return float 计算的输出 + */ +float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt); + +/** + * @brief 重置微分项 + * + * @param pid PID结构体 + * @return int8_t 0对应没有错误 + */ +int8_t PID_ResetIntegral(KPID_t *pid); + +/** + * @brief 重置PID + * + * @param pid PID结构体 + * @return int8_t 0对应没有错误 + */ +int8_t PID_Reset(KPID_t *pid); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/component/toolbox/matrix.cpp b/User/component/toolbox/matrix.cpp new file mode 100644 index 0000000..51fc95a --- /dev/null +++ b/User/component/toolbox/matrix.cpp @@ -0,0 +1,24 @@ +/** + ****************************************************************************** + * @file matrix.cpp/h + * @brief Matrix/vector calculation. 矩阵/向量运算 + * @author Spoon Guan + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#include "matrix.h" + +// hat of vector +Matrixf<3, 3> vector3f::hat(Matrixf<3, 1> vec) { + float hat[9] = {0, -vec[2][0], vec[1][0], vec[2][0], 0, + -vec[0][0], -vec[1][0], vec[0][0], 0}; + return Matrixf<3, 3>(hat); +} + +// cross product +Matrixf<3, 1> vector3f::cross(Matrixf<3, 1> vec1, Matrixf<3, 1> vec2) { + return vector3f::hat(vec1) * vec2; +} diff --git a/User/component/toolbox/matrix.h b/User/component/toolbox/matrix.h new file mode 100644 index 0000000..70f4fcf --- /dev/null +++ b/User/component/toolbox/matrix.h @@ -0,0 +1,262 @@ +/** + ****************************************************************************** + * @file matrix.cpp/h + * @brief Matrix/vector calculation. 矩阵/向量运算 + * @author Spoon Guan + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#ifndef MATRIX_H +#define MATRIX_H + +#include "arm_math.h" + +// Matrix class +template +class + + +Matrixf { + public: + // Constructor without input data + Matrixf(void) : rows_(_rows), cols_(_cols) { + arm_mat_init_f32(&arm_mat_, _rows, _cols, this->data_); + } + // Constructor with input data + Matrixf(float data[_rows * _cols]) : Matrixf() { + memcpy(this->data_, data, _rows * _cols * sizeof(float)); + } + // Copy constructor + Matrixf(const Matrixf<_rows, _cols>& mat) : Matrixf() { + memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float)); + } + // Destructor + ~Matrixf(void) {} + + // Row size + int rows(void) { return _rows; } + // Column size + int cols(void) { return _cols; } + + // Element + float* operator[](const int& row) { return &this->data_[row * _cols]; } + + // Operators + Matrixf<_rows, _cols>& operator=(const Matrixf<_rows, _cols> mat) { + memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float)); + return *this; + } + Matrixf<_rows, _cols>& operator+=(const Matrixf<_rows, _cols> mat) { + arm_status s; + s = arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_); + return *this; + } + Matrixf<_rows, _cols>& operator-=(const Matrixf<_rows, _cols> mat) { + arm_status s; + s = arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_); + return *this; + } + Matrixf<_rows, _cols>& operator*=(const float& val) { + arm_status s; + s = arm_mat_scale_f32(&this->arm_mat_, val, &this->arm_mat_); + return *this; + } + Matrixf<_rows, _cols>& operator/=(const float& val) { + arm_status s; + s = arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &this->arm_mat_); + return *this; + } + Matrixf<_rows, _cols> operator+(const Matrixf<_rows, _cols>& mat) { + arm_status s; + Matrixf<_rows, _cols> res; + s = arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_); + return res; + } + Matrixf<_rows, _cols> operator-(const Matrixf<_rows, _cols>& mat) { + arm_status s; + Matrixf<_rows, _cols> res; + s = arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_); + return res; + } + Matrixf<_rows, _cols> operator*(const float& val) { + arm_status s; + Matrixf<_rows, _cols> res; + s = arm_mat_scale_f32(&this->arm_mat_, val, &res.arm_mat_); + return res; + } + friend Matrixf<_rows, _cols> operator*(const float& val, + const Matrixf<_rows, _cols>& mat) { + arm_status s; + Matrixf<_rows, _cols> res; + s = arm_mat_scale_f32(&mat.arm_mat_, val, &res.arm_mat_); + return res; + } + Matrixf<_rows, _cols> operator/(const float& val) { + arm_status s; + Matrixf<_rows, _cols> res; + s = arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &res.arm_mat_); + return res; + } + // Matrix multiplication + template + friend Matrixf<_rows, cols> operator*(const Matrixf<_rows, _cols>& mat1, + const Matrixf<_cols, cols>& mat2) { + arm_status s; + Matrixf<_rows, cols> res; + s = arm_mat_mult_f32(&mat1.arm_mat_, &mat2.arm_mat_, &res.arm_mat_); + return res; + } + + // Submatrix + template + Matrixf block(const int& start_row, const int& start_col) { + Matrixf res; + for (int row = start_row; row < start_row + rows; row++) { + memcpy((float*)res[0] + (row - start_row) * cols, + (float*)this->data_ + row * _cols + start_col, + cols * sizeof(float)); + } + return res; + } + // Specific row + Matrixf<1, _cols> row(const int& row) { return block<1, _cols>(row, 0); } + // Specific column + Matrixf<_rows, 1> col(const int& col) { return block<_rows, 1>(0, col); } + + // Transpose + Matrixf<_cols, _rows> trans(void) { + Matrixf<_cols, _rows> res; + arm_mat_trans_f32(&arm_mat_, &res.arm_mat_); + return res; + } + // Trace + float trace(void) { + float res = 0; + for (int i = 0; i < fmin(_rows, _cols); i++) { + res += (*this)[i][i]; + } + return res; + } + // Norm + float norm(void) { return sqrtf((this->trans() * *this)[0][0]); } + + public: + // arm matrix instance + arm_matrix_instance_f32 arm_mat_; + + protected: + // size + int rows_, cols_; + // data + float data_[_rows * _cols]; +}; + +// Matrix funtions +namespace matrixf { + +// Special Matrices +// Zero matrix +template +Matrixf<_rows, _cols> zeros(void) { + float data[_rows * _cols] = {0}; + return Matrixf<_rows, _cols>(data); +} +// Ones matrix +template +Matrixf<_rows, _cols> ones(void) { + float data[_rows * _cols] = {0}; + for (int i = 0; i < _rows * _cols; i++) { + data[i] = 1; + } + return Matrixf<_rows, _cols>(data); +} +// Identity matrix +template +Matrixf<_rows, _cols> eye(void) { + float data[_rows * _cols] = {0}; + for (int i = 0; i < fmin(_rows, _cols); i++) { + data[i * _cols + i] = 1; + } + return Matrixf<_rows, _cols>(data); +} +// Diagonal matrix +template +Matrixf<_rows, _cols> diag(Matrixf<_rows, 1> vec) { + Matrixf<_rows, _cols> res = matrixf::zeros<_rows, _cols>(); + for (int i = 0; i < fmin(_rows, _cols); i++) { + res[i][i] = vec[i][0]; + } + return res; +} + +// Inverse +template +Matrixf<_dim, _dim> inv(Matrixf<_dim, _dim> mat) { + arm_status s; + // extended matrix [A|I] + Matrixf<_dim, 2 * _dim> ext_mat = matrixf::zeros<_dim, 2 * _dim>(); + for (int i = 0; i < _dim; i++) { + memcpy(ext_mat[i], mat[i], _dim * sizeof(float)); + ext_mat[i][_dim + i] = 1; + } + // elimination + for (int i = 0; i < _dim; i++) { + // find maximum absolute value in the first column in lower right block + float abs_max = fabs(ext_mat[i][i]); + int abs_max_row = i; + for (int row = i; row < _dim; row++) { + if (abs_max < fabs(ext_mat[row][i])) { + abs_max = fabs(ext_mat[row][i]); + abs_max_row = row; + } + } + if (abs_max < 1e-12f) { // singular + return matrixf::zeros<_dim, _dim>(); + s = ARM_MATH_SINGULAR; + } + if (abs_max_row != i) { // row exchange + float tmp; + Matrixf<1, 2 * _dim> row_i = ext_mat.row(i); + Matrixf<1, 2 * _dim> row_abs_max = ext_mat.row(abs_max_row); + memcpy(ext_mat[i], row_abs_max[0], 2 * _dim * sizeof(float)); + memcpy(ext_mat[abs_max_row], row_i[0], 2 * _dim * sizeof(float)); + } + float k = 1.f / ext_mat[i][i]; + for (int col = i; col < 2 * _dim; col++) { + ext_mat[i][col] *= k; + } + for (int row = 0; row < _dim; row++) { + if (row == i) { + continue; + } + k = ext_mat[row][i]; + for (int j = i; j < 2 * _dim; j++) { + ext_mat[row][j] -= k * ext_mat[i][j]; + } + } + } + // inv = ext_mat(:,n+1:2n) + s = ARM_MATH_SUCCESS; + Matrixf<_dim, _dim> res; + for (int i = 0; i < _dim; i++) { + memcpy(res[i], &ext_mat[i][_dim], _dim * sizeof(float)); + } + return res; +} + +} // namespace matrixf + +namespace vector3f { + +// hat of vector +Matrixf<3, 3> hat(Matrixf<3, 1> vec); + +// cross product +Matrixf<3, 1> cross(Matrixf<3, 1> vec1, Matrixf<3, 1> vec2); + +} // namespace vector3f + +#endif // MATRIX_H diff --git a/User/component/toolbox/robotics.cpp b/User/component/toolbox/robotics.cpp new file mode 100644 index 0000000..03ca320 --- /dev/null +++ b/User/component/toolbox/robotics.cpp @@ -0,0 +1,342 @@ +/** + ****************************************************************************** + * @file robotics.cpp/h + * @brief Robotic toolbox on STM32. STM32机器人学库 + * @author Spoon Guan + * @ref [1] SJTU ME385-2, Robotics, Y.Ding + * [2] Bruno Siciliano, et al., Robotics: Modelling, Planning and + * Control, Springer, 2010. + * [3] R.Murry, Z.X.Li, and S.Sastry, A Mathematical Introduction + * to Robotic Manipulation, CRC Press, 1994. + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#include "robotics.h" + +Matrixf<3, 1> robotics::r2rpy(Matrixf<3, 3> R) { + float rpy[3] = { + atan2f(R[1][0], R[0][0]), // yaw + atan2f(-R[2][0], sqrtf(R[2][1] * R[2][1] + R[2][2] * R[2][2])), // pitch + atan2f(R[2][1], R[2][2]) // roll + }; + return Matrixf<3, 1>(rpy); +} + +Matrixf<3, 3> robotics::rpy2r(Matrixf<3, 1> rpy) { + float c[3] = {cosf(rpy[0][0]), cosf(rpy[1][0]), cosf(rpy[2][0])}; + float s[3] = {sinf(rpy[0][0]), sinf(rpy[1][0]), sinf(rpy[2][0])}; + float R[9] = { + c[0] * c[1], // R11 + c[0] * s[1] * s[2] - s[0] * c[2], // R12 + c[0] * s[1] * c[2] + s[0] * s[2], // R13 + s[0] * c[1], // R21 + s[0] * s[1] * s[2] + c[0] * c[2], // R22 + s[0] * s[1] * c[2] - c[0] * s[2], // R23 + -s[1], // R31 + c[1] * s[2], // R32 + c[1] * c[2] // R33 + }; + return Matrixf<3, 3>(R); +} + +Matrixf<4, 1> robotics::r2angvec(Matrixf<3, 3> R) { + float theta = acosf(math::limit(0.5f * (R.trace() - 1), -1, 1)); + if (theta == 0 || theta == PI) { + float angvec[4] = { + (1 + R[0][0] - R[1][1] - R[2][2]) / 4.0f, // rx=(1+R11-R22-R33)/4 + (1 - R[0][0] + R[1][1] - R[2][2]) / 4.0f, // ry=(1-R11+R22-R33)/4 + (1 - R[0][0] - R[1][1] + R[2][2]) / 4.0f, // rz=(1-R11-R22+R33)/4 + theta // theta + }; + return Matrixf<4, 1>(angvec); + } + float angvec[4] = { + (R[2][1] - R[1][2]) / (2.0f * sinf(theta)), // rx=(R32-R23)/2sinθ + (R[0][2] - R[2][0]) / (2.0f * sinf(theta)), // ry=(R13-R31)/2sinθ + (R[1][0] - R[0][1]) / (2.0f * sinf(theta)), // rz=(R21-R12)/2sinθ + theta // theta + }; + return Matrixf<4, 1>(angvec); +} + +Matrixf<3, 3> robotics::angvec2r(Matrixf<4, 1> angvec) { + float theta = angvec[3][0]; + Matrixf<3, 1> r = angvec.block<3, 1>(0, 0); + Matrixf<3, 3> R; + // Rodrigues formula: R=I+ω^sinθ+ω^^2(1-cosθ) + R = matrixf::eye<3, 3>() + vector3f::hat(r) * sinf(theta) + + vector3f::hat(r) * vector3f::hat(r) * (1 - cosf(theta)); + return R; +} + +Matrixf<4, 1> robotics::r2quat(Matrixf<3, 3> R) { + float q[4] = { + 0.5f * sqrtf(math::limit(R.trace(), -1, 1) + 1), // q0=cos(θ/2) + math::sign(R[2][1] - R[1][2]) * 0.5f * + sqrtf(math::limit(R[0][0] - R[1][1] - R[2][2], -1, 1) + + 1), // q1=rx*sin(θ/2) + math::sign(R[0][2] - R[2][0]) * 0.5f * + sqrtf(math::limit(-R[0][0] + R[1][1] - R[2][2], -1, 1) + + 1), // q2=ry*sin(θ/2) + math::sign(R[1][0] - R[0][1]) * 0.5f * + sqrtf(math::limit(-R[0][0] - R[1][1] + R[2][2], -1, 1) + + 1), // q3=rz*sin(θ/2) + }; + return (Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm()); +} + +Matrixf<3, 3> robotics::quat2r(Matrixf<4, 1> q) { + float R[9] = { + 1 - 2.0f * (q[2][0] * q[2][0] + q[3][0] * q[3][0]), // R11 + 2.0f * (q[1][0] * q[2][0] - q[0][0] * q[3][0]), // R12 + 2.0f * (q[1][0] * q[3][0] + q[0][0] * q[2][0]), // R13 + 2.0f * (q[1][0] * q[2][0] + q[0][0] * q[3][0]), // R21 + 1 - 2.0f * (q[1][0] * q[1][0] + q[3][0] * q[3][0]), // R22 + 2.0f * (q[2][0] * q[3][0] - q[0][0] * q[1][0]), // R23 + 2.0f * (q[1][0] * q[3][0] - q[0][0] * q[2][0]), // R31 + 2.0f * (q[2][0] * q[3][0] + q[0][0] * q[1][0]), // R32 + 1 - 2.0f * (q[1][0] * q[1][0] + q[2][0] * q[2][0]) // R33 + }; + return Matrixf<3, 3>(R); +} + +Matrixf<3, 1> robotics::quat2rpy(Matrixf<4, 1> q) { + float rpy[3] = { + atan2f(2.0f * (q[1][0] * q[2][0] + q[0][0] * q[3][0]), + 1 - 2.0f * (q[2][0] * q[2][0] + q[3][0] * q[3][0])), // yaw + asinf(2.0f * (q[0][0] * q[2][0] - q[1][0] * q[3][0])), // pitch + atan2f(2.0f * (q[2][0] * q[3][0] + q[0][0] * q[1][0]), + 1 - 2.0f * (q[1][0] * q[1][0] + q[2][0] * q[2][0])) // rol + }; + return Matrixf<3, 1>(rpy); +} + +Matrixf<4, 1> robotics::rpy2quat(Matrixf<3, 1> rpy) { + float c[3] = {cosf(0.5f * rpy[0][0]), cosf(0.5f * rpy[1][0]), + cosf(0.5f * rpy[2][0])}; // cos(*/2) + float s[3] = {sinf(0.5f * rpy[0][0]), sinf(0.5f * rpy[1][0]), + sinf(0.5f * rpy[2][0])}; // sin(*/2) + float q[4] = { + c[0] * c[1] * c[2] + s[0] * s[1] * s[2], // q0=cos(θ/2) + c[0] * c[1] * s[2] - s[0] * s[1] * c[2], // q1=rx*sin(θ/2) + c[0] * s[1] * c[2] + s[0] * c[1] * s[2], // q2=ry*sin(θ/2) + s[0] * c[1] * c[2] - c[0] * s[1] * s[2] // q3=rz*sin(θ/2) + }; + return (Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm()); +} + +Matrixf<4, 1> robotics::quat2angvec(Matrixf<4, 1> q) { + float cosq0; + float theta = 2.0f * acosf(math::limit(q[0][0], -1, 1)); + if (theta == 0 || theta == PI) { + float angvec[4] = {0, 0, 0, theta}; // θ=0||PI, return[0;θ] + return Matrixf<4, 1>(angvec); + } + Matrixf<3, 1> vec = q.block<3, 1>(1, 0); + float angvec[4] = { + vec[0][0] / vec.norm(), // rx + vec[1][0] / vec.norm(), // ry + vec[2][0] / vec.norm(), // rz + theta // theta + }; + return Matrixf<4, 1>(angvec); +} + +Matrixf<4, 1> robotics::angvec2quat(Matrixf<4, 1> angvec) { + float c = cosf(0.5f * angvec[3][0]), s = sinf(0.5f * angvec[3][0]); + float q[4] = { + c, // q0=cos(θ/2) + s * angvec[0][0], // q1=rx*sin(θ/2) + s * angvec[1][0], // q2=ry*sin(θ/2) + s * angvec[2][0] // q3=rz*sin(θ/2) + }; + return Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm(); +} + +Matrixf<3, 3> robotics::t2r(Matrixf<4, 4> T) { + return T.block<3, 3>(0, 0); // R=T(1:3,1:3) +} + +Matrixf<4, 4> robotics::r2t(Matrixf<3, 3> R) { + // T=[R,0;0,1] + float T[16] = {R[0][0], R[0][1], R[0][2], 0, R[1][0], R[1][1], R[1][2], 0, + R[2][0], R[2][1], R[2][2], 0, 0, 0, 0, 1}; + return Matrixf<4, 4>(T); +} + +Matrixf<3, 1> robotics::t2p(Matrixf<4, 4> T) { + return T.block<3, 1>(0, 3); // p=T(1:3,4) +} + +Matrixf<4, 4> robotics::p2t(Matrixf<3, 1> p) { + // T=[I,P;0,1] + float T[16] = {1, 0, 0, p[0][0], 0, 1, 0, p[1][0], + 0, 0, 1, p[2][0], 0, 0, 0, 1}; + return Matrixf<4, 4>(T); +} + +Matrixf<4, 4> robotics::rp2t(Matrixf<3, 3> R, Matrixf<3, 1> p) { + // T=[R,P;0,1] + float T[16] = {R[0][0], R[0][1], R[0][2], p[0][0], R[1][0], R[1][1], + R[1][2], p[1][0], R[2][0], R[2][1], R[2][2], p[2][0], + 0, 0, 0, 1}; + return Matrixf<4, 4>(T); +} + +Matrixf<4, 4> robotics::invT(Matrixf<4, 4> T) { + Matrixf<3, 3> RT = t2r(T).trans(); + Matrixf<3, 1> p_ = -1.0f * RT * t2p(T); + float invT[16] = {RT[0][0], RT[0][1], RT[0][2], p_[0][0], RT[1][0], RT[1][1], + RT[1][2], p_[1][0], RT[2][0], RT[2][1], RT[2][2], p_[2][0], + 0, 0, 0, 1}; + return Matrixf<4, 4>(invT); +} + +Matrixf<3, 1> robotics::t2rpy(Matrixf<4, 4> T) { + return r2rpy(t2r(T)); +} + +Matrixf<4, 4> robotics::rpy2t(Matrixf<3, 1> rpy) { + return r2t(rpy2r(rpy)); +} + +Matrixf<4, 1> robotics::t2angvec(Matrixf<4, 4> T) { + return r2angvec(t2r(T)); +} + +Matrixf<4, 4> robotics::angvec2t(Matrixf<4, 1> angvec) { + return r2t(angvec2r(angvec)); +} + +Matrixf<4, 1> robotics::t2quat(Matrixf<4, 4> T) { + return r2quat(t2r(T)); +} + +Matrixf<4, 4> robotics::quat2t(Matrixf<4, 1> quat) { + return r2t(quat2r(quat)); +} + +Matrixf<6, 1> robotics::t2twist(Matrixf<4, 4> T) { + Matrixf<3, 1> p = t2p(T); + Matrixf<4, 1> angvec = t2angvec(T); + Matrixf<3, 1> phi = angvec.block<3, 1>(0, 0) * angvec[3][0]; + float twist[6] = {p[0][0], p[1][0], p[2][0], phi[0][0], phi[1][0], phi[2][0]}; + return Matrixf<6, 1>(twist); +} + +Matrixf<4, 4> robotics::twist2t(Matrixf<6, 1> twist) { + Matrixf<3, 1> p = twist.block<3, 1>(0, 0); + float theta = twist.block<3, 1>(3, 0).norm(); + float angvec[4] = {0, 0, 0, theta}; + if (theta != 0) { + angvec[0] = twist[3][0] / theta; + angvec[1] = twist[4][0] / theta; + angvec[2] = twist[5][0] / theta; + } + return rp2t(angvec2r(angvec), p); +} + +Matrixf<4, 4> robotics::DH_t::fkine() { + float ct = cosf(theta), st = sinf(theta); // cosθ, sinθ + float ca = cosf(alpha), sa = sinf(alpha); // cosα, sinα + + // T = + // | cθ -sθcα sθsα acθ | + // | sθ cθcα -cθsα asθ | + // | 0 sα cα d | + // | 0 0 0 1 | + T[0][0] = ct; + T[0][1] = -st * ca; + T[0][2] = st * sa; + T[0][3] = a * ct; + + T[1][0] = st; + T[1][1] = ct * ca; + T[1][2] = -ct * sa; + T[1][3] = a * st; + + T[2][0] = 0; + T[2][1] = sa; + T[2][2] = ca; + T[2][3] = d; + + T[3][0] = 0; + T[3][1] = 0; + T[3][2] = 0; + T[3][3] = 1; + + return T; +} + +robotics::Link::Link(float theta, + float d, + float a, + float alpha, + robotics::Joint_Type_e type, + float offset, + float qmin, + float qmax, + float m, + Matrixf<3, 1> rc, + Matrixf<3, 3> I) { + dh_.theta = theta; + dh_.d = d; + dh_.alpha = alpha; + dh_.a = a; + type_ = type; + offset_ = offset; + qmin_ = qmin; + qmax_ = qmax; + m_ = m; + rc_ = rc; + I_ = I; +} + +robotics::Link::Link(const Link& link) { + dh_.theta = link.dh_.theta; + dh_.d = link.dh_.d; + dh_.alpha = link.dh_.alpha; + dh_.a = link.dh_.a; + type_ = link.type_; + offset_ = link.offset_; + qmin_ = link.qmin_; + qmax_ = link.qmax_; + m_ = link.m_; + rc_ = link.rc_; + I_ = link.I_; +} + +robotics::Link& robotics::Link::operator=(Link link) { + dh_.theta = link.dh_.theta; + dh_.d = link.dh_.d; + dh_.alpha = link.dh_.alpha; + dh_.a = link.dh_.a; + type_ = link.type_; + offset_ = link.offset_; + qmin_ = link.qmin_; + qmax_ = link.qmax_; + m_ = link.m_; + rc_ = link.rc_; + I_ = link.I_; + return *this; +} + +Matrixf<4, 4> robotics::Link::T(float q) { + // 先对关节变量q进行限幅 + float q_limited = q; + if (qmin_ < qmax_) { // 有效的限幅范围 + q_limited = math::limit(q, qmin_, qmax_); + } + + // 再计算实际的DH参数 + if (type_ == R) { + dh_.theta = q_limited + offset_; + } else { + dh_.d = q_limited + offset_; + } + + return dh_.fkine(); +} diff --git a/User/component/toolbox/robotics.h b/User/component/toolbox/robotics.h new file mode 100644 index 0000000..d5f8839 --- /dev/null +++ b/User/component/toolbox/robotics.h @@ -0,0 +1,407 @@ +/** + ****************************************************************************** + * @file robotics.cpp/h + * @brief Robotic toolbox on STM32. STM32机器人学库 + * @author Spoon Guan + * @ref [1] SJTU ME385-2, Robotics, Y.Ding + * [2] Bruno Siciliano, et al., Robotics: Modelling, Planning and + * Control, Springer, 2010. + * [3] R.Murry, Z.X.Li, and S.Sastry, A Mathematical Introduction + * to Robotic Manipulation, CRC Press, 1994. + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#ifndef ROBOTICS_H +#define ROBOTICS_H + +#include "utils.h" +#include "matrix.h" + +namespace robotics { +// rotation matrix(R) -> RPY([yaw;pitch;roll]) +Matrixf<3, 1> r2rpy(Matrixf<3, 3> R); +// RPY([yaw;pitch;roll]) -> rotation matrix(R) +Matrixf<3, 3> rpy2r(Matrixf<3, 1> rpy); +// rotation matrix(R) -> angle vector([r;θ]) +Matrixf<4, 1> r2angvec(Matrixf<3, 3> R); +// angle vector([r;θ]) -> rotation matrix(R) +Matrixf<3, 3> angvec2r(Matrixf<4, 1> angvec); +// rotation matrix(R) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] +Matrixf<4, 1> r2quat(Matrixf<3, 3> R); +// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> rotation matrix(R) +Matrixf<3, 3> quat2r(Matrixf<4, 1> quat); +// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> RPY([yaw;pitch;roll]) +Matrixf<3, 1> quat2rpy(Matrixf<4, 1> q); +// RPY([yaw;pitch;roll]) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] +Matrixf<4, 1> rpy2quat(Matrixf<3, 1> rpy); +// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> angle vector([r;θ]) +Matrixf<4, 1> quat2angvec(Matrixf<4, 1> q); +// angle vector([r;θ]) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] +Matrixf<4, 1> angvec2quat(Matrixf<4, 1> angvec); +// homogeneous transformation matrix(T) -> rotation matrix(R) +Matrixf<3, 3> t2r(Matrixf<4, 4> T); +// rotation matrix(R) -> homogeneous transformation matrix(T) +Matrixf<4, 4> r2t(Matrixf<3, 3> R); +// homogeneous transformation matrix(T) -> translation vector(p) +Matrixf<3, 1> t2p(Matrixf<4, 4> T); +// translation vector(p) -> homogeneous transformation matrix(T) +Matrixf<4, 4> p2t(Matrixf<3, 1> p); +// rotation matrix(R) & translation vector(p) -> homogeneous transformation +// matrix(T) +Matrixf<4, 4> rp2t(Matrixf<3, 3> R, Matrixf<3, 1> p); +// homogeneous transformation matrix(T) -> RPY([yaw;pitch;roll]) +Matrixf<3, 1> t2rpy(Matrixf<4, 4> T); +// inverse of homogeneous transformation matrix(T^-1=[R',-R'P;0,1]) +Matrixf<4, 4> invT(Matrixf<4, 4> T); +// RPY([yaw;pitch;roll]) -> homogeneous transformation matrix(T) +Matrixf<4, 4> rpy2t(Matrixf<3, 1> rpy); +// homogeneous transformation matrix(T) -> angle vector([r;θ]) +Matrixf<4, 1> t2angvec(Matrixf<4, 4> T); +// angle vector([r;θ]) -> homogeneous transformation matrix(T) +Matrixf<4, 4> angvec2t(Matrixf<4, 1> angvec); +// homogeneous transformation matrix(T) -> quaternion, +// [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] +Matrixf<4, 1> t2quat(Matrixf<4, 4> T); +// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> homogeneous transformation +// matrix(T) +Matrixf<4, 4> quat2t(Matrixf<4, 1> quat); +// homogeneous transformation matrix(T) -> twist coordinates vector ([p;rθ]) +Matrixf<6, 1> t2twist(Matrixf<4, 4> T); +// twist coordinates vector ([p;rθ]) -> homogeneous transformation matrix(T) +Matrixf<4, 4> twist2t(Matrixf<6, 1> twist); + +// joint type: R-revolute joint, P-prismatic joint +typedef enum joint_type { + R = 0, + P = 1, +} Joint_Type_e; + +// Denavit–Hartenberg(DH) method +struct DH_t { + // forward kinematic + Matrixf<4, 4> fkine(); + // DH parameter + float theta; + float d; + float a; + float alpha; + Matrixf<4, 4> T; +}; + +class Link { + public: + Link(){}; + Link(float theta, float d, float a, float alpha, Joint_Type_e type = R, + float offset = 0, float qmin = 0, float qmax = 0, float m = 1, + Matrixf<3, 1> rc = matrixf::zeros<3, 1>(), + Matrixf<3, 3> I = matrixf::zeros<3, 3>()); + Link(const Link& link); + + Link& operator=(Link link); + + float qmin() { return qmin_; } + float qmax() { return qmax_; } + Joint_Type_e type() { return type_; } + float m() { return m_; } + Matrixf<3, 1> rc() { return rc_; } + Matrixf<3, 3> I() { return I_; } + + Matrixf<4, 4> T(float q); // forward kinematic + + public: + // kinematic parameter + DH_t dh_; + float offset_; + // limit(qmin,qmax), no limit if qmin<=qmax + float qmin_; + float qmax_; + // joint type + Joint_Type_e type_; + // dynamic parameter + float m_; // mass + Matrixf<3, 1> rc_; // centroid(link coordinate) + Matrixf<3, 3> I_; // inertia tensor(3*3) +}; + +template +class Serial_Link { + public: + Serial_Link(Link links[_n]) { + for (int i = 0; i < _n; i++) + links_[i] = links[i]; + gravity_ = matrixf::zeros<3, 1>(); + gravity_[2][0] = -9.81f; + } + + Serial_Link(Link links[_n], Matrixf<3, 1> gravity) { + for (int i = 0; i < _n; i++) + links_[i] = links[i]; + gravity_ = gravity; + } + + // forward kinematic: T_n^0 + // param[in] q: joint variable vector + // param[out] T_n^0 + Matrixf<4, 4> fkine(Matrixf<_n, 1> q) { + T_ = matrixf::eye<4, 4>(); + for (int iminus1 = 0; iminus1 < _n; iminus1++) + T_ = T_ * links_[iminus1].T(q[iminus1][0]); + return T_; + } + + // forward kinematic: T_k^0 + // param[in] q: joint variable vector + // param[in] k: joint number + // param[out] T_k^0 + Matrixf<4, 4> fkine(Matrixf<_n, 1> q, uint16_t k) { + if (k > _n) + k = _n; + Matrixf<4, 4> T = matrixf::eye<4, 4>(); + for (int iminus1 = 0; iminus1 < k; iminus1++) + T = T * links_[iminus1].T(q[iminus1][0]); + return T; + } + + // T_k^k-1: homogeneous transformation matrix of link k + // param[in] q: joint variable vector + // param[in] kminus: joint number k, input k-1 + // param[out] T_k^k-1 + Matrixf<4, 4> T(Matrixf<_n, 1> q, uint16_t kminus1) { + if (kminus1 >= _n) + kminus1 = _n - 1; + return links_[kminus1].T(q[kminus1][0]); + } + + // jacobian matrix, J_i = [J_pi;j_oi] + // param[in] q: joint variable vector + // param[out] jacobian matix J_6*n + Matrixf<6, _n> jacob(Matrixf<_n, 1> q) { + Matrixf<3, 1> p_e = t2p(fkine(q)); // p_e + Matrixf<4, 4> T_iminus1 = matrixf::eye<4, 4>(); // T_i-1^0 + Matrixf<3, 1> z_iminus1; // z_i-1^0 + Matrixf<3, 1> p_iminus1; // p_i-1^0 + Matrixf<3, 1> J_pi; + Matrixf<3, 1> J_oi; + for (int iminus1 = 0; iminus1 < _n; iminus1++) { + // revolute joint: J_pi = z_i-1x(p_e-p_i-1), J_oi = z_i-1 + if (links_[iminus1].type() == R) { + z_iminus1 = T_iminus1.block<3, 1>(0, 2); + p_iminus1 = t2p(T_iminus1); + T_iminus1 = T_iminus1 * links_[iminus1].T(q[iminus1][0]); + J_pi = vector3f::cross(z_iminus1, p_e - p_iminus1); + J_oi = z_iminus1; + } + // prismatic joint: J_pi = z_i-1, J_oi = 0 + else { + z_iminus1 = T_iminus1.block<3, 1>(0, 2); + T_iminus1 = T_iminus1 * links_[iminus1].T(q[iminus1][0]); + J_pi = z_iminus1; + J_oi = matrixf::zeros<3, 1>(); + } + J_[0][iminus1] = J_pi[0][0]; + J_[1][iminus1] = J_pi[1][0]; + J_[2][iminus1] = J_pi[2][0]; + J_[3][iminus1] = J_oi[0][0]; + J_[4][iminus1] = J_oi[1][0]; + J_[5][iminus1] = J_oi[2][0]; + } + return J_; + } + + // inverse kinematic, numerical solution(Newton method) + // param[in] T: homogeneous transformation matrix of end effector + // param[in] q: initial joint variable vector(q0) for Newton method's + // iteration + // param[in] tol: tolerance of error (norm(error of twist vector)) + // param[in] max_iter: maximum iterations, default 30 + // param[out] q: joint variable vector + Matrixf<_n, 1> ikine(Matrixf<4, 4> Td, + Matrixf<_n, 1> q = matrixf::zeros<_n, 1>(), + float tol = 1e-4f, uint16_t max_iter = 50) { + Matrixf<4, 4> T; + Matrixf<3, 1> pe, we; + Matrixf<6, 1> err, new_err; + Matrixf<_n, 1> dq; + float step = 1; + for (int i = 0; i < max_iter; i++) { + T = fkine(q); + pe = t2p(Td) - t2p(T); + // angvec(Td*T^-1), transform angular vector(T->Td) in world coordinate + we = t2twist(Td * invT(T)).block<3, 1>(3, 0); + for (int i = 0; i < 3; i++) { + err[i][0] = pe[i][0]; + err[i + 3][0] = we[i][0]; + } + if (err.norm() < tol) + return q; + // adjust iteration step + Matrixf<6, _n> J = jacob(q); + for (int j = 0; j < 5; j++) { + dq = matrixf::inv(J.trans() * J) * (J.trans() * err) * step; + if (dq[0][0] == INFINITY) // J'*J singular + { + dq = matrixf::inv(J.trans() * J + 0.1f * matrixf::eye<_n, _n>()) * + J.trans() * err * step; + // SVD<6, _n> JTJ_svd(J.trans() * J); + // dq = JTJ_svd.solve(err) * step * 5e-2f; + q += dq; + for (int i = 0; i < _n; i++) { + if (links_[i].type() == R) + q[i][0] = math::loopLimit(q[i][0], -PI, PI); + } + break; + } + T = fkine(q + dq); + pe = t2p(Td) - t2p(T); + we = t2twist(Td * invT(T)).block<3, 1>(3, 0); + for (int i = 0; i < 3; i++) { + new_err[i][0] = pe[i][0]; + new_err[i + 3][0] = we[i][0]; + } + if (new_err.norm() < err.norm()) { + q += dq; + for (int i = 0; i < _n; i++) { + if (links_[i].type() == robotics::Joint_Type_e::R) { + q[i][0] = math::loopLimit(q[i][0], -PI, PI); + } + } + break; + } else { + step /= 2.0f; + } + } + if (step < 1e-3f) + return q; + } + return q; + } + + // (Reserved function) inverse kinematic, analytic solution(geometric method) + Matrixf<_n, 1> (*ikine_analytic)(Matrixf<4, 4> T); + + // inverse dynamic, Newton-Euler method + // param[in] q: joint variable vector + // param[in] qv: dq/dt + // param[in] qa: d^2q/dt^2 + // param[in] he: load on end effector [f;μ], default 0 + Matrixf<_n, 1> rne(Matrixf<_n, 1> q, + Matrixf<_n, 1> qv = matrixf::zeros<_n, 1>(), + Matrixf<_n, 1> qa = matrixf::zeros<_n, 1>(), + Matrixf<6, 1> he = matrixf::zeros<6, 1>()) { + // forward propagation + // record each links' motion state in matrices + // [ωi] angular velocity + Matrixf<3, _n + 1> w = matrixf::zeros<3, _n + 1>(); + // [βi] angular acceleration + Matrixf<3, _n + 1> b = matrixf::zeros<3, _n + 1>(); + // [pi] position of joint + Matrixf<3, _n + 1> p = matrixf::zeros<3, _n + 1>(); + // [vi] velocity of joint + Matrixf<3, _n + 1> v = matrixf::zeros<3, _n + 1>(); + // [ai] acceleration of joint + Matrixf<3, _n + 1> a = matrixf::zeros<3, _n + 1>(); + // [aci] acceleration of mass center + Matrixf<3, _n + 1> ac = matrixf::zeros<3, _n + 1>(); + // temperary vectors + Matrixf<3, 1> w_i, b_i, p_i, v_i, ai, ac_i; + // i & i-1 coordinate convert to 0 coordinate + Matrixf<4, 4> T_0i = matrixf::eye<4, 4>(); + Matrixf<4, 4> T_0iminus1 = matrixf::eye<4, 4>(); + Matrixf<3, 3> R_0i = matrixf::eye<3, 3>(); + Matrixf<3, 3> R_0iminus1 = matrixf::eye<3, 3>(); + // unit vector of z-axis + Matrixf<3, 1> ez = matrixf::zeros<3, 1>(); + ez[2][0] = 1; + + for (int i = 1; i <= _n; i++) { + T_0i = T_0i * T(q, i - 1); // T_i^0 + R_0i = t2r(T_0i); // R_i^0 + R_0iminus1 = t2r(T_0iminus1); // R_i-1^0 + // ω_i = ω_i-1+qv_i*R_i-1^0*ez + w_i = w.col(i - 1) + qv[i - 1][0] * R_0iminus1 * ez; + // β_i = β_i-1+ω_i-1x(qv_i*R_i-1^0*ez)+qa_i*R_i-1^0*ez + b_i = b.col(i - 1) + + vector3f::cross(w.col(i - 1), qv[i - 1][0] * R_0iminus1 * ez) + + qa[i - 1][0] * R_0iminus1 * ez; + p_i = t2p(T_0i); // p_i = T_i^0(1:3,4) + // v_i = v_i-1+ω_ix(p_i-p_i-1) + v_i = v.col(i - 1) + vector3f::cross(w_i, p_i - p.col(i - 1)); + // a_i = a_i-1+β_ix(p_i-p_i-1)+ω_ix(ω_ix(p_i-p_i-1)) + ai = a.col(i - 1) + vector3f::cross(b_i, p_i - p.col(i - 1)) + + vector3f::cross(w_i, vector3f::cross(w_i, p_i - p.col(i - 1))); + // ac_i = a_i+β_ix(R_0^i*rc_i^i)+ω_ix(ω_ix(R_0^i*rc_i^i)) + ac_i = + ai + vector3f::cross(b_i, R_0i * links_[i - 1].rc()) + + vector3f::cross(w_i, vector3f::cross(w_i, R_0i * links_[i - 1].rc())); + for (int row = 0; row < 3; row++) { + w[row][i] = w_i[row][0]; + b[row][i] = b_i[row][0]; + p[row][i] = p_i[row][0]; + v[row][i] = v_i[row][0]; + a[row][i] = ai[row][0]; + ac[row][i] = ac_i[row][0]; + } + T_0iminus1 = T_0i; // T_i-1^0 + } + + // backward propagation + // record each links' force + Matrixf<3, _n + 1> f = matrixf::zeros<3, _n + 1>(); // joint force + Matrixf<3, _n + 1> mu = matrixf::zeros<3, _n + 1>(); // joint moment + // temperary vector + Matrixf<3, 1> f_iminus1, mu_iminus1; + // {T,R',P}_i^i-1 + Matrixf<4, 4> T_iminus1i; + Matrixf<3, 3> RT_iminus1i; + Matrixf<3, 1> P_iminus1i; + // I_i-1(in 0 coordinate) + Matrixf<3, 3> I_i; + // joint torque + Matrixf<_n, 1> torq; + + // load on end effector + for (int row = 0; row < 3; row++) { + f[row][_n] = he.block<3, 1>(0, 0)[row][0]; + mu[row][_n] = he.block<3, 1>(3, 0)[row][0]; + } + for (int i = _n; i > 0; i--) { + T_iminus1i = T(q, i - 1); // T_i^i-1 + P_iminus1i = t2p(T_iminus1i); // P_i^i-1 + RT_iminus1i = t2r(T_iminus1i).trans(); // R_i^i-1' + R_0iminus1 = R_0i * RT_iminus1i; // R_i-1^0 + // I_i^0 = R_i^0*I_i^i*(R_i^0)' + I_i = R_0i * links_[i - 1].I() * R_0i.trans(); + // f_i-1 = f_i+m_i*ac_i-m_i*g + f_iminus1 = f.col(i) + links_[i - 1].m() * ac.col(i) - + links_[i - 1].m() * gravity_; + // μ_i-1 = μ_i+f_ixrc_i-f_i-1xrc_i-1->ci+I_i*b_i+ω_ix(I_i*ω_i) + mu_iminus1 = mu.col(i) + + vector3f::cross(f.col(i), R_0i * links_[i - 1].rc()) - + vector3f::cross(f_iminus1, R_0i * (RT_iminus1i * P_iminus1i + + links_[i - 1].rc())) + + I_i * b.col(i) + vector3f::cross(w.col(i), I_i * w.col(i)); + // τ_i = μ_i-1'*(R_i-1^0*ez) + torq[i - 1][0] = (mu_iminus1.trans() * R_0iminus1 * ez)[0][0]; + for (int row = 0; row < 3; row++) { + f[row][i - 1] = f_iminus1[row][0]; + mu[row][i - 1] = mu_iminus1[row][0]; + } + R_0i = R_0iminus1; + } + + return torq; + } + + private: + Link links_[_n]; + Matrixf<3, 1> gravity_; + + Matrixf<4, 4> T_; + Matrixf<6, _n> J_; +}; +}; // namespace robotics + +#endif // ROBOTICS_H diff --git a/User/component/toolbox/utils.cpp b/User/component/toolbox/utils.cpp new file mode 100644 index 0000000..1c4b42b --- /dev/null +++ b/User/component/toolbox/utils.cpp @@ -0,0 +1,56 @@ +/** + ****************************************************************************** + * @file utils.cpp/h + * @brief General math utils. 常用数学工具函数 + * @author Spoon Guan + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#include "utils.h" + +float math::limit(float val, const float& min, const float& max) { + if (min > max) + return val; + else if (val < min) + val = min; + else if (val > max) + val = max; + return val; +} + +float math::limitMin(float val, const float& min) { + if (val < min) + val = min; + return val; +} + +float math::limitMax(float val, const float& max) { + if (val > max) + val = max; + return val; +} + +float math::loopLimit(float val, const float& min, const float& max) { + if (min >= max) + return val; + if (val > max) { + while (val > max) + val -= (max - min); + } else if (val < min) { + while (val < min) + val += (max - min); + } + return val; +} + +float math::sign(const float& val) { + if (val > 0) + return 1; + else if (val < 0) + return -1; + return 0; +} + diff --git a/User/component/toolbox/utils.h b/User/component/toolbox/utils.h new file mode 100644 index 0000000..1515eb6 --- /dev/null +++ b/User/component/toolbox/utils.h @@ -0,0 +1,27 @@ +/** + ****************************************************************************** + * @file utils.cpp/h + * @brief General math utils. 常用数学工具函数 + * @author Spoon Guan + ****************************************************************************** + * Copyright (c) 2023 Team JiaoLong-SJTU + * All rights reserved. + ****************************************************************************** + */ + +#ifndef UTILS_H +#define UTILS_H + +#include + +namespace math { + +float limit(float val, const float& min, const float& max); +float limitMin(float val, const float& min); +float limitMax(float val, const float& max); +float loopLimit(float val, const float& min, const float& max); +float sign(const float& val); + +} // namespace math + +#endif // UTILS_H diff --git a/User/component/user_math.c b/User/component/user_math.c new file mode 100644 index 0000000..5e0b0c4 --- /dev/null +++ b/User/component/user_math.c @@ -0,0 +1,134 @@ +/* + 自定义的数学运算。 +*/ + +#include "user_math.h" +#include +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +inline float InvSqrt(float x) { +//#if 0 + /* Fast inverse square-root */ + /* See: http://en.wikipedia.org/wiki/Fast_inverse_square_root */ + float halfx = 0.5f * x; + float y = x; + long i = *(long*)&y; + i = 0x5f3759df - (i>>1); + y = *(float*)&i; + y = y * (1.5f - (halfx * y * y)); + y = y * (1.5f - (halfx * y * y)); + return y; +//#else +// return 1.0f / sqrtf(x); +//#endif +} + +inline float AbsClip(float in, float limit) { + return (in < -limit) ? -limit : ((in > limit) ? limit : in); +} + +float fAbs(float in){ + return (in > 0) ? in : -in; +} + +inline void Clip(float *origin, float min, float max) { + if (*origin > max) *origin = max; + if (*origin < min) *origin = min; +} + +inline float Sign(float in) { return (in > 0) ? 1.0f : 0.0f; } + +/** + * \brief 将运动向量置零 + * + * \param mv 被操作的值 + */ +inline void ResetMoveVector(MoveVector_t *mv) { memset(mv, 0, sizeof(*mv)); } + +/** + * \brief 计算循环值的误差，适用于设定值与反馈值均在（x,y）范围内循环的情况，range应设定为y-x + * 例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b; + * \param sp 设定值 + * \param fb 反馈值 + * \param range 被操作的值变化范围，正数时起效 + * \return 函数运行结果 + */ +inline float CircleError(float sp, float fb, float range) { + float error = sp - fb; + if (range > 0.0f) { + float half_range = range / 2.0f; + + if (error > half_range) + error -= range; + else if (error < -half_range) + error += range; + } + return error; +} + +/** + * \brief 循环加法，适用于被操作的值在（0,range）范围内循环的情况 + * \param origin 被操作的值 + * \param delta 变化量 + * \param range 被操作的值变化范围，正数时起效 + */ +inline void CircleAdd(float *origin, float delta, float range) { + float out = *origin + delta; + if (range > 0.0f) { + if (out >= range) + out -= range; + else if (out < 0.0f) + out += range; + } + *origin = out; +} + +/** + * @brief 循环值取反 + * + * @param origin 被操作的值 + */ +inline void CircleReverse(float *origin) { *origin = -(*origin) + M_2PI; } + +/** + * @brief 根据目标弹丸速度计算摩擦轮转速 + * + * @param bullet_speed 弹丸速度 + * @param fric_radius 摩擦轮半径 + * @param is17mm 是否为17mm + * @return 摩擦轮转速 + */ +inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) { + if (bullet_speed == 0.0f) return 0.f; + if (is17mm) { + if (bullet_speed == 15.0f) return 4670.f; + if (bullet_speed == 18.0f) return 5200.f; + if (bullet_speed == 30.0f) return 7350.f; + } else { + if (bullet_speed == 10.0f) return 4450.f; + if (bullet_speed == 16.0f) return 5800.f; + } + + /* 不为裁判系统设定值时,计算转速 */ + return 60.0f * (float)bullet_speed / (M_2PI * fric_radius); +} + +// /** +// * @brief 断言失败处理 +// * +// * @param file 文件名 +// * @param line 行号 +// */ +// void VerifyFailed(const char *file, uint32_t line) { +// UNUSED(file); +// UNUSED(line); +// while (1) { +// __NOP(); +// } +// } + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ \ No newline at end of file diff --git a/User/component/user_math.h b/User/component/user_math.h new file mode 100644 index 0000000..6cbb21e --- /dev/null +++ b/User/component/user_math.h @@ -0,0 +1,183 @@ +/* + 自定义的数学运算。 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +#define M_DEG2RAD_MULT (0.01745329251f) +#define M_RAD2DEG_MULT (57.2957795131f) + +// 角度与弧度转换宏 +#define DEG_TO_RAD (0.01745329251f) // π/180 +#define RAD_TO_DEG (57.2957795131f) // 180/π + +#ifndef M_PI_2 +#define M_PI_2 1.57079632679f +#endif + +#ifndef M_PI +#define M_PI 3.14159265358979323846f +#endif + +#ifndef M_2PI +#define M_2PI 6.28318530717958647692f +#endif + +#ifndef __packed + #define __packed __attribute__((__packed__)) +#endif /* __packed */ + +#define max(a, b) \ + ({ \ + __typeof__(a) _a = (a); \ + __typeof__(b) _b = (b); \ + _a > _b ? _a : _b; \ + }) + +#define min(a, b) \ + ({ \ + __typeof__(a) _a = (a); \ + __typeof__(b) _b = (b); \ + _a < _b ? _a : _b; \ + }) + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + + + +/* 移动向量 */ +typedef struct { + float vx; /* 前后平移 */ + float vy; /* 左右平移 */ + float wz; /* 转动 */ +} MoveVector_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +float InvSqrt(float x); + +float AbsClip(float in, float limit); + +float fAbs(float in); + +void Clip(float *origin, float min, float max); + +float Sign(float in); + +/** + * \brief 将运动向量置零 + * + * \param mv 被操作的值 + */ +void ResetMoveVector(MoveVector_t *mv); + +/** + * \brief 计算循环值的误差，适用于设定值与反馈值均在（x,y）范围内循环的情况，range应设定为y-x + * 例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b; + * \param sp 设定值 + * \param fb 反馈值 + * \param range 被操作的值变化范围，正数时起效 + * \return 函数运行结果 + */ +float CircleError(float sp, float fb, float range); + +/** + * \brief 循环加法，适用于被操作的值在（0,range）范围内循环的情况 + * \param origin 被操作的值 + * \param delta 变化量 + * \param range 被操作的值变化范围，正数时起效 + */ +void CircleAdd(float *origin, float delta, float range); + +/** + * @brief 循环值取反 + * + * @param origin 被操作的值 + */ +void CircleReverse(float *origin); + +/** + * @brief 根据目标弹丸速度计算摩擦轮转速 + * + * @param bullet_speed 弹丸速度 + * @param fric_radius 摩擦轮半径 + * @param is17mm 是否为17mm + * @return 摩擦轮转速 + */ +float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm); + +#ifdef __cplusplus +} +#endif + +#ifdef DEBUG + +/** + * @brief 如果表达式的值为假则运行处理函数 + * + */ +#define ASSERT(expr) \ + do { \ + if (!(expr)) { \ + VerifyFailed(__FILE__, __LINE__); \ + } \ + } while (0) +#else + +/** + * @brief 未定DEBUG，表达式不会运行，断言被忽略 + * + */ +#define ASSERT(expr) ((void)(0)) +#endif + +#ifdef DEBUG + +/** + * @brief 如果表达式的值为假则运行处理函数 + * + */ +#define VERIFY(expr) \ + do { \ + if (!(expr)) { \ + VerifyFailed(__FILE__, __LINE__); \ + } \ + } while (0) +#else + +/** + * @brief 表达式会运行，忽略表达式结果 + * + */ +#define VERIFY(expr) ((void)(expr)) +#endif + +// /** +// * @brief 断言失败处理 +// * +// * @param file 文件名 +// * @param line 行号 +// */ +// void VerifyFailed(const char *file, uint32_t line); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ \ No newline at end of file diff --git a/User/device/at9s.c b/User/device/at9s.c new file mode 100644 index 0000000..7ae0a96 --- /dev/null +++ b/User/device/at9s.c @@ -0,0 +1,121 @@ +/* Includes ----------------------------------------------------------------- */ +#include "at9s.h" + +#include +#include "bsp/uart.h" +#include "device/device.h" + +/* Private define ----------------------------------------------------------- */ +#define DEAD_AREA 0.025f +/* Private variables -------------------------------------------------------- */ +static osThreadId_t thread_alert; +static bool inited = false; + +/* Private function --------------------------------------------------------- */ +static void AT9S_RxCpltCallback(void) +{ + osThreadFlagsSet(thread_alert, SIGNAL_AT9S_RAW_REDY); +} + +/* Exported functions ------------------------------------------------------- */ +int8_t AT9S_Init(AT9S_t *at9s) +{ + if (at9s == NULL) return DEVICE_ERR_NULL; + if (inited) return DEVICE_ERR_INITED; + if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL; + + BSP_UART_RegisterCallback(BSP_UART_RC, BSP_UART_RX_CPLT_CB, + AT9S_RxCpltCallback); + + inited = true; + return DEVICE_OK; +} + +int8_t AT9S_Restart(void) +{ + __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_RC)); + __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_RC)); + return DEVICE_OK; +} + +int8_t AT9S_StartDmaRecv(uint8_t *cmd_buffer) +{ + if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_RC), + cmd_buffer, + AT9S_FRAME_LEN) == HAL_OK) + return DEVICE_OK; + return DEVICE_ERR; +} + +bool AT9S_WaitDmaCplt(uint32_t timeout) +{ + return (osThreadFlagsWait(SIGNAL_AT9S_RAW_REDY, + osFlagsWaitAll, timeout) == SIGNAL_AT9S_RAW_REDY); +} +AT9S_Raw_t r; +/* 解析 25 字节原始帧并填充结构体 */ +void AT9S_ParseRaw(const uint8_t raw[AT9S_FRAME_LEN], AT9S_t *out) +{ +// AT9S_Raw_t r; + + /* 摇杆 */ + r.ch[0] = ((raw[1] | raw[2] << 8) & 0x07FF); + r.ch[1] = ((raw[2] >> 3 | raw[3] << 5) & 0x07FF); + r.ch[2] = ((raw[3] >> 6 | raw[4] << 2 | raw[5] << 10) & 0x07FF); + r.ch[3] = ((raw[5] >> 1 | raw[6] << 7) & 0x07FF); + + /* 开关/旋钮 */ + r.sw[0] = ((raw[6] >> 4 | raw[7] << 4) & 0x07FF); + r.sw[1] = ((raw[7] >> 7 | raw[8] << 1 | raw[9] << 9) & 0x07FF); + r.sw[2] = ((raw[9] >> 2 | raw[10]<< 6) & 0x07FF); + r.sw[3] = ((raw[10]>> 5 | raw[11]<< 3) & 0x07FF); + r.sw[4] = ((raw[12] | raw[13]<< 8) & 0x07FF); + r.sw[5] = ((raw[13]>> 3 | raw[14]<< 5) & 0x07FF); + r.sw[6] = ((raw[14]>> 6 | raw[15]<< 2 | raw[16]<< 10) & 0x07FF); + r.sw[7] = ((raw[16]>> 1 | raw[17]<< 7) & 0x07FF); + + /* 开关映射 */ + #define MAP_SWITCH(v) \ + ((v) > 300 && (v) < 500) ? AT9S_CMD_SW_DOWN : \ + ((v) >= 500 && (v) < 1500) ? AT9S_CMD_SW_MID : \ + ((v) >= 1500 && (v) < 1700) ? AT9S_CMD_SW_UP : AT9S_CMD_SW_ERR + + #define MAP_SWITCH_RESERVE(v) \ + ((v) > 300 && (v) < 500) ? AT9S_CMD_SW_UP : \ + ((v) >= 500 && (v) < 1500) ? AT9S_CMD_SW_MID : \ + ((v) >= 1500 && (v) < 1700) ? AT9S_CMD_SW_DOWN : AT9S_CMD_SW_ERR + + out->data.rc.ch_l_x = 2.0f*(r.ch[3]-209.0f)/(1596.0f-209.0f)-1.0f; + out->data.rc.ch_l_y = 2.0f*(r.ch[2]-178.0f)/(1575.0f-198.0f)-1.0f; + out->data.rc.ch_r_x = 2.0f*(r.ch[0]-210.0f)/(1596.0f-210.0f)-1.0f; + out->data.rc.ch_r_y = 2.0f*(r.ch[1]-221.0f)/(1604.0f-217.0f)-1.0f; + + if(fabs(out->data.rc.ch_l_x)<=DEAD_AREA)out->data.rc.ch_l_x = 0; + if(fabs(out->data.rc.ch_l_y)<=4*DEAD_AREA)out->data.rc.ch_l_y = 0; + if(fabs(out->data.rc.ch_r_x)<=DEAD_AREA)out->data.rc.ch_r_x = 0; + if(fabs(out->data.rc.ch_r_y)<=DEAD_AREA)out->data.rc.ch_r_y = 0; + + if(fabs(out->data.rc.ch_l_x)>=1.0f)out->data.rc.ch_l_x= out->data.rc.ch_l_x>0?1.0f:-1.0f; + if(fabs(out->data.rc.ch_l_y)>=1.0f)out->data.rc.ch_l_y= out->data.rc.ch_l_y>0?1.0f:-1.0f; + if(fabs(out->data.rc.ch_r_x)>=1.0f)out->data.rc.ch_r_x= out->data.rc.ch_r_x>0?1.0f:-1.0f; + if(fabs(out->data.rc.ch_r_y)>=1.0f)out->data.rc.ch_r_y= out->data.rc.ch_r_y>0?1.0f:-1.0f; +// out->data.ch_r_x = r.ch[0]; +// out->data.ch_r_y = r.ch[1]; +// out->data.ch_l_y = r.ch[2]; +// out->data.ch_l_x = r.ch[3]; + + out->data.rc.key_A = MAP_SWITCH_RESERVE(r.sw[0]); + out->data.rc.key_B = MAP_SWITCH_RESERVE(r.sw[1]); + out->data.rc.key_C = MAP_SWITCH_RESERVE(r.sw[2]); + out->data.rc.key_D = MAP_SWITCH(r.sw[3]); + out->data.rc.key_E = MAP_SWITCH(r.sw[4]); + out->data.rc.key_F = MAP_SWITCH(r.sw[5]); + out->data.rc.key_G = MAP_SWITCH_RESERVE(r.sw[6]); + out->data.rc.key_H = MAP_SWITCH(r.sw[7]); +// out->knob_left = MAP_SWITCH(r.sw[6]); +// out->knob_right = MAP_SWITCH(r.sw[7]); + + #undef MAP_SWITCH + //check online. + out->header.online = (r.sw[7] != 300&&r.ch[0] != 1000); +} diff --git a/User/device/at9s.h b/User/device/at9s.h new file mode 100644 index 0000000..dc48c5f --- /dev/null +++ b/User/device/at9s.h @@ -0,0 +1,70 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include + +#include "component/user_math.h" +#include "device.h" + +/* Exported constants ------------------------------------------------------- */ +#define AT9S_FRAME_LEN 25u + +/* Exported types ----------------------------------------------------------- */ +typedef struct __attribute__((packed)) +{ + int16_t ch[4]; /* 摇杆原始值 */ + int16_t sw[8]; /* 开关/旋钮原始值 */ +} AT9S_Raw_t; + +/* 拨杆位置 */ +typedef enum { + AT9S_CMD_SW_ERR = 0, + AT9S_CMD_SW_UP = 1, + AT9S_CMD_SW_MID = 3, + AT9S_CMD_SW_DOWN = 2, +} AT9S_CMD_SwitchPos_t; +typedef struct +{ + float ch_l_x; /* 左摇杆 X */ + float ch_l_y; /* 左摇杆 Y（油门） */ + float ch_r_x; /* 右摇杆 X */ + float ch_r_y; /* 右摇杆 Y */ + + /* 开关/旋钮离散化后状态 */ + AT9S_CMD_SwitchPos_t key_A; + AT9S_CMD_SwitchPos_t key_B; + AT9S_CMD_SwitchPos_t key_C; + AT9S_CMD_SwitchPos_t key_D; + AT9S_CMD_SwitchPos_t key_E; + AT9S_CMD_SwitchPos_t key_F; + AT9S_CMD_SwitchPos_t key_G; + AT9S_CMD_SwitchPos_t key_H; + float knob_left; + float knob_right; + float back_left; + float back_right; +} AT9S_DataRC_t; +typedef struct +{ + AT9S_DataRC_t rc; +} AT9S_Data_t; + +typedef struct{ + DEVICE_Header_t header; + AT9S_Data_t data; +} AT9S_t; + +/* Exported functions prototypes -------------------------------------------- */ +int8_t AT9S_Init(AT9S_t *at9s); +int8_t AT9S_Restart(void); +int8_t AT9S_StartDmaRecv(uint8_t *cmd_buffer); +bool AT9S_WaitDmaCplt(uint32_t timeout); +void AT9S_ParseRaw(const uint8_t raw[AT9S_FRAME_LEN], AT9S_t *out); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/User/device/device.h b/User/device/device.h new file mode 100644 index 0000000..5682a96 --- /dev/null +++ b/User/device/device.h @@ -0,0 +1,49 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +#define DEVICE_OK (0) +#define DEVICE_ERR (-1) +#define DEVICE_ERR_NULL (-2) +#define DEVICE_ERR_INITED (-3) +#define DEVICE_ERR_NO_DEV (-4) + +/* AUTO GENERATED SIGNALS BEGIN */ +#define SIGNAL_DR16_RAW_REDY (1u << 0) +#define SIGNAL_AT9S_RAW_REDY (1u << 7) +#define SIGNAL_VT13_RAW_REDY (1u << 8) +/* AUTO GENERATED SIGNALS END */ + +/* USER SIGNALS BEGIN */ + +/* USER SIGNALS END */ +/*设备层通用Header*/ +typedef struct { + bool online; + uint64_t last_online_time; +} DEVICE_Header_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/device/device_config.yaml b/User/device/device_config.yaml new file mode 100644 index 0000000..2fe8b69 --- /dev/null +++ b/User/device/device_config.yaml @@ -0,0 +1,6 @@ +motor: + bsp_config: {} + enabled: true +motor_dm: + bsp_config: {} + enabled: true diff --git a/User/device/dr16.c b/User/device/dr16.c new file mode 100644 index 0000000..85f2484 --- /dev/null +++ b/User/device/dr16.c @@ -0,0 +1,221 @@ +/* + DR16接收机 + Example： + + DR16_Init(&dr16); + + while (1) { + DR16_StartDmaRecv(&dr16); + if (DR16_WaitDmaCplt(20)) { + DR16_ParseData(&dr16); + } else { + DR16_Offline(&dr16); + } +} +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "dr16.h" +#include "bsp/uart.h" +#include "bsp/time.h" +#include "device.h" + +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ +/* Private define ----------------------------------------------------------- */ +#define DR16_CH_VALUE_MIN (364u) +#define DR16_CH_VALUE_MID (1024u) +#define DR16_CH_VALUE_MAX (1684u) +//#define DR16_FRAME_SIZE (18u) // DR16数据帧固定18字节 + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ + +static osThreadId_t thread_alert; +static bool inited = false; + +/* Private function -------------------------------------------------------- */ +static void DR16_RxCpltCallback(void) { + osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY); +} + +//static void DR16_ErrorCallback(void) { +// UART_HandleTypeDef *huart = BSP_UART_GetHandle(BSP_UART_RC); +// +// // 清除所有错误标志（包括FIFO错误） +// __HAL_UART_CLEAR_OREFLAG(huart); +// __HAL_UART_CLEAR_NEFLAG(huart); +// __HAL_UART_CLEAR_FEFLAG(huart); +// __HAL_UART_CLEAR_PEFLAG(huart); +// +// // 清除FIFO超时标志（STM32H7特有） +// if (__HAL_UART_GET_FLAG(huart, UART_FLAG_RTOF)) { +// __HAL_UART_CLEAR_FLAG(huart, UART_CLEAR_RTOF); +// } +// +// // 中止并清除当前DMA传输 +// HAL_UART_AbortReceive(huart); +//} + +static bool DR16_DataCorrupted(const DR16_t *dr16) { + if (dr16 == NULL) return DEVICE_ERR_NULL; + + if ((dr16->raw_data.ch_r_x < DR16_CH_VALUE_MIN) || + (dr16->raw_data.ch_r_x > DR16_CH_VALUE_MAX)) + return DEVICE_ERR; + + if ((dr16->raw_data.ch_r_y < DR16_CH_VALUE_MIN) || + (dr16->raw_data.ch_r_y > DR16_CH_VALUE_MAX)) + return DEVICE_ERR; + + if ((dr16->raw_data.ch_l_x < DR16_CH_VALUE_MIN) || + (dr16->raw_data.ch_l_x > DR16_CH_VALUE_MAX)) + return DEVICE_ERR; + + if ((dr16->raw_data.ch_l_y < DR16_CH_VALUE_MIN) || + (dr16->raw_data.ch_l_y > DR16_CH_VALUE_MAX)) + return DEVICE_ERR; + + if (dr16->raw_data.sw_l == 0) return DEVICE_ERR; + + if (dr16->raw_data.sw_r == 0) return DEVICE_ERR; + + return DEVICE_OK; +} + +/* Exported functions ------------------------------------------------------- */ +int8_t DR16_Init(DR16_t *dr16) { + if (dr16 == NULL) return DEVICE_ERR_NULL; + if (inited) return DEVICE_ERR_INITED; + if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL; + +// // 清零结构体 +// memset(dr16, 0, sizeof(DR16_t)); + + BSP_UART_RegisterCallback(BSP_UART_RC, BSP_UART_RX_CPLT_CB, + DR16_RxCpltCallback); +// BSP_UART_RegisterCallback(BSP_UART_RC, BSP_UART_ERROR_CB, +// DR16_ErrorCallback); + + inited = true; + return DEVICE_OK; +} + +int8_t DR16_Restart(void) { + __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_RC)); + __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_RC)); + return DEVICE_OK; +} + +int8_t DR16_StartDmaRecv(DR16_t *dr16) { + UART_HandleTypeDef *huart = BSP_UART_GetHandle(BSP_UART_RC); + +// // 确保之前的DMA接收已停止 +// if (huart->RxState != HAL_UART_STATE_READY) { +// HAL_UART_AbortReceive(huart); +// } +// +// // 清除所有可能的错误标志和IDLE标志 +// __HAL_UART_CLEAR_OREFLAG(huart); +// __HAL_UART_CLEAR_NEFLAG(huart); +// __HAL_UART_CLEAR_FEFLAG(huart); +// __HAL_UART_CLEAR_PEFLAG(huart); +// __HAL_UART_CLEAR_IDLEFLAG(huart); +// +// // 清除FIFO超时标志 +// if (__HAL_UART_GET_FLAG(huart, UART_FLAG_RTOF)) { +// __HAL_UART_CLEAR_FLAG(huart, UART_CLEAR_RTOF); +// } + + if (HAL_UART_Receive_DMA(huart, (uint8_t *)&(dr16->raw_data), + sizeof(dr16->raw_data)) == HAL_OK) + return DEVICE_OK; + return DEVICE_ERR; +} + +bool DR16_WaitDmaCplt(uint32_t timeout) { + return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) == + SIGNAL_DR16_RAW_REDY); +} + +int8_t DR16_ParseData(DR16_t *dr16){ + if (dr16 == NULL) return DEVICE_ERR_NULL; + +// // STM32H7 D-Cache一致性处理：使DMA接收的数据对CPU可见 +// // D-Cache按32字节对齐，需要对齐处理 +// uint32_t addr = (uint32_t)&(dr16->raw_data); +// uint32_t size = sizeof(dr16->raw_data); +// // 向下对齐到32字节边界 +// uint32_t aligned_addr = addr & ~0x1FU; +// // 计算对齐后的大小 +// uint32_t aligned_size = ((size + (addr - aligned_addr) + 31) & ~0x1FU); +// SCB_InvalidateDCache_by_Addr((uint32_t *)aligned_addr, aligned_size); + + if (DR16_DataCorrupted(dr16)) { + return DEVICE_ERR; + } + + dr16->header.online = true; + dr16->header.last_online_time = BSP_TIME_Get_us(); + + memset(&(dr16->data), 0, sizeof(dr16->data)); + + float full_range = (float)(DR16_CH_VALUE_MAX - DR16_CH_VALUE_MIN); + + // 解析摇杆数据 + dr16->data.ch_r_x = 2.0f * ((float)dr16->raw_data.ch_r_x - DR16_CH_VALUE_MID) / full_range; + dr16->data.ch_r_y = 2.0f * ((float)dr16->raw_data.ch_r_y - DR16_CH_VALUE_MID) / full_range; + dr16->data.ch_l_x = 2.0f * ((float)dr16->raw_data.ch_l_x - DR16_CH_VALUE_MID) / full_range; + dr16->data.ch_l_y = 2.0f * ((float)dr16->raw_data.ch_l_y - DR16_CH_VALUE_MID) / full_range; + + // 解析拨杆位置 + dr16->data.sw_l = (DR16_SwitchPos_t)dr16->raw_data.sw_l; + dr16->data.sw_r = (DR16_SwitchPos_t)dr16->raw_data.sw_r; + + // 解析鼠标数据 + dr16->data.mouse.x = dr16->raw_data.x; + dr16->data.mouse.y = dr16->raw_data.y; + dr16->data.mouse.z = dr16->raw_data.z; + + dr16->data.mouse.l_click = dr16->raw_data.press_l; + dr16->data.mouse.r_click = dr16->raw_data.press_r; + + // 解析键盘按键 - 使用union简化代码 + uint16_t key_value = dr16->raw_data.key; + + // 解析键盘位映射（W-B键，位0-15） + for (int i = DR16_KEY_W; i <= DR16_KEY_B; i++) { + dr16->data.keyboard.key[i] = (key_value & (1 << i)) != 0; + } + + // 解析鼠标点击 + dr16->data.keyboard.key[DR16_L_CLICK] = dr16->data.mouse.l_click; + dr16->data.keyboard.key[DR16_R_CLICK] = dr16->data.mouse.r_click; + + // 解析第五通道 + dr16->data.ch_res = 2.0f * ((float)dr16->raw_data.res - DR16_CH_VALUE_MID) / full_range; + + return DEVICE_OK; +} + +int8_t DR16_Offline(DR16_t *dr16){ + if (dr16 == NULL) return DEVICE_ERR_NULL; + + dr16->header.online = false; + memset(&(dr16->data), 0, sizeof(dr16->data)); + + return DEVICE_OK; +} + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/User/device/dr16.h b/User/device/dr16.h new file mode 100644 index 0000000..1b138e3 --- /dev/null +++ b/User/device/dr16.h @@ -0,0 +1,119 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include + +#include "component/user_math.h" +#include "device.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef struct __packed { + uint16_t ch_r_x : 11; + uint16_t ch_r_y : 11; + uint16_t ch_l_x : 11; + uint16_t ch_l_y : 11; + uint8_t sw_r : 2; + uint8_t sw_l : 2; + int16_t x; + int16_t y; + int16_t z; + uint8_t press_l; + uint8_t press_r; + uint16_t key; + uint16_t res; +} DR16_RawData_t; +// typedef struct __packed { +// uint8_t buff[18]; // 原始接收缓冲区 +// } DR16_RawData_t; + +typedef enum { + DR16_SW_ERR = 0, + DR16_SW_UP = 1, + DR16_SW_MID = 3, + DR16_SW_DOWN = 2, +} DR16_SwitchPos_t; + +/* 键盘按键值 */ +typedef enum { + DR16_KEY_W = 0, + DR16_KEY_S, + DR16_KEY_A, + DR16_KEY_D, + DR16_KEY_SHIFT, + DR16_KEY_CTRL, + DR16_KEY_Q, + DR16_KEY_E, + DR16_KEY_R, + DR16_KEY_F, + DR16_KEY_G, + DR16_KEY_Z, + DR16_KEY_X, + DR16_KEY_C, + DR16_KEY_V, + DR16_KEY_B, + DR16_L_CLICK, + DR16_R_CLICK, + DR16_KEY_NUM, +} DR16_Key_t; + +typedef struct { + float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */ + float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */ + float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */ + float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */ + + float ch_res; /* 第五通道值 */ + + DR16_SwitchPos_t sw_r; /* 右侧拨杆位置 */ + DR16_SwitchPos_t sw_l; /* 左侧拨杆位置 */ + + struct { + int16_t x; + int16_t y; + int16_t z; + bool l_click; /* 左键 */ + bool r_click; /* 右键 */ + } mouse; /* 鼠标值 */ + + struct { + bool key[DR16_KEY_NUM]; /* 键盘按键值 */ + } keyboard; + + uint16_t res; /* 保留，未启用 */ +} DR16_Data_t; + +typedef struct { + DEVICE_Header_t header; + DR16_RawData_t raw_data; + DR16_Data_t data; +} DR16_t; + +/* Exported functions prototypes -------------------------------------------- */ +int8_t DR16_Init(DR16_t *dr16); +int8_t DR16_Restart(void); +int8_t DR16_StartDmaRecv(DR16_t *dr16); +bool DR16_WaitDmaCplt(uint32_t timeout); +int8_t DR16_ParseData(DR16_t *dr16); +int8_t DR16_Offline(DR16_t *dr16); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/device/motor.c b/User/device/motor.c new file mode 100644 index 0000000..1fb059d --- /dev/null +++ b/User/device/motor.c @@ -0,0 +1,52 @@ +/* + 电机通用函数 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "motor.h" + +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + + +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ + +/* Private function -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +/* Exported functions ------------------------------------------------------- */ +float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + return motor->feedback.rotor_abs_angle; +} + +float MOTOR_GetRotorSpeed(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + return motor->feedback.rotor_speed; +} + +float MOTOR_GetTorqueCurrent(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + return motor->feedback.torque_current; +} + +float MOTOR_GetTemp(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + return motor->feedback.temp; +} diff --git a/User/device/motor.h b/User/device/motor.h new file mode 100644 index 0000000..e1f945b --- /dev/null +++ b/User/device/motor.h @@ -0,0 +1,68 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef struct { + float rotor_abs_angle; /* 转子绝对角度 */ + float rotor_speed; /* 实际转子转速 */ + float torque_current; /* 转矩电流 */ + float temp; /* 温度 */ +} MOTOR_Feedback_t; + +/** + * @brief mit电机输出参数结构体 + */ +typedef struct { + float torque; /* 目标力矩 */ + float velocity; /* 目标速度 */ + float angle; /* 目标位置 */ + float kp; /* 位置环增益 */ + float kd; /* 速度环增益 */ +} MOTOR_MIT_Output_t; + +/** + * @brief 转矩电流控制模式参数结构体 + */ +typedef struct { + float current; /* 目标电流 */ +} MOTOR_Current_Output_t; + +typedef struct { + DEVICE_Header_t header; + bool reverse; /* 是否反装 true表示反装 */ + MOTOR_Feedback_t feedback; +} MOTOR_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Exported functions prototypes -------------------------------------------- */ +float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor); +float MOTOR_GetRotorSpeed(const MOTOR_t *motor); +float MOTOR_GetTorqueCurrent(const MOTOR_t *motor); +float MOTOR_GetTemp(const MOTOR_t *motor); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/device/motor_dm.c b/User/device/motor_dm.c new file mode 100644 index 0000000..0ab7520 --- /dev/null +++ b/User/device/motor_dm.c @@ -0,0 +1,522 @@ +#define MOTOR_DM_FLOAT_TO_INT_SIGNED(x, x_min, x_max, bits) \ + ((int32_t)roundf(((x) / ((x_max) - (x_min))) * (1 << (bits)) + (1 << ((bits) - 1)))) + +#define MOTOR_DM_INT_TO_FLOAT_SIGNED(x_int, x_min, x_max, bits) \ + (((float)((int32_t)(x_int) - (1 << ((bits) - 1))) * ((x_max) - (x_min)) / (float)(1 << (bits)))) +/* Includes ----------------------------------------------------------------- */ +#include "device/motor_dm.h" +#include "bsp/mm.h" +#include "bsp/time.h" +#include "component/user_math.h" +#include "string.h" +#include "math.h" + +/* Private constants -------------------------------------------------------- */ +/* DM电机数据映射范围 */ +#define DM_P_MIN (-12.56637f) +#define DM_P_MAX (12.56637f) +#define DM_V_MIN (-30.0f) +#define DM_V_MAX (30.0f) +#define DM_T_MIN (-12.0f) +#define DM_T_MAX (12.0f) +#define DM_KP_MIN (0.0f) +#define DM_KP_MAX (500.0f) +#define DM_KD_MIN (0.0f) +#define DM_KD_MAX (5.0f) + +/* CAN ID偏移量 */ +#define DM_CAN_ID_OFFSET_POS_VEL 0x100 +#define DM_CAN_ID_OFFSET_VEL 0x200 + +/* Private macro ------------------------------------------------------------ */ +#define FLOAT_TO_UINT(x, x_min, x_max, bits) \ + (uint32_t)((x - x_min) * ((1 << bits) - 1) / (x_max - x_min)) + +#define UINT_TO_FLOAT(x_int, x_min, x_max, bits) \ + ((float)(x_int) * (x_max - x_min) / ((1 << bits) - 1) + x_min) + + +/* Private variables -------------------------------------------------------- */ +static MOTOR_DM_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL}; + +static int float_to_uint(float x_float, float x_min, float x_max, int bits) +{ + /* Converts a float to an unsigned int, given range and number of bits */ + float span = x_max - x_min; + float offset = x_min; + return (int) ((x_float-offset)*((float)((1<motor.feedback.rotor_abs_angle = uint_to_float(p_int, DM_P_MIN, DM_P_MAX, 16); // (-12.5,12.5) + uint16_t v_int=(data[3]<<4)|(data[4]>>4); + motor->motor.feedback.rotor_speed = uint_to_float(v_int, DM_V_MIN, DM_V_MAX, 12); // (-30.0,30.0) + uint16_t t_int=((data[4]&0xF)<<8)|data[5]; + motor->motor.feedback.torque_current = uint_to_float(t_int, DM_T_MIN, DM_T_MAX, 12); // (-12.0,12.0) + motor->motor.feedback.temp = (float)(data[6]); + + while (motor->motor.feedback.rotor_abs_angle < 0) { + motor->motor.feedback.rotor_abs_angle += M_2PI; + } + while (motor->motor.feedback.rotor_abs_angle >= M_2PI) { + motor->motor.feedback.rotor_abs_angle -= M_2PI; + } + + if (motor->param.reverse) { + motor->motor.feedback.rotor_abs_angle = M_2PI - motor->motor.feedback.rotor_abs_angle; + motor->motor.feedback.rotor_speed = -motor->motor.feedback.rotor_speed; + motor->motor.feedback.torque_current = -motor->motor.feedback.torque_current; + } + return DEVICE_OK; +} + +/** + * @brief 发送MIT模式控制命令 + * @param motor 电机实例 + * @param output MIT控制参数 + * @return 发送结果 + */ +static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output) { + if (motor == NULL || output == NULL) { + return DEVICE_ERR_NULL; + } + + uint8_t data[8]; + uint16_t pos_tmp,vel_tmp,kp_tmp,kd_tmp,tor_tmp; + uint16_t id = motor->param.can_id; + + pos_tmp = float_to_uint(output->angle, DM_P_MIN , DM_P_MAX, 16); + vel_tmp = float_to_uint(output->velocity, DM_V_MIN , DM_V_MAX, 12); + kp_tmp = float_to_uint(output->kp, DM_KP_MIN, DM_KP_MAX, 12); + kd_tmp = float_to_uint(output->kd, DM_KD_MIN, DM_KD_MAX, 12); + tor_tmp = float_to_uint(output->torque, DM_T_MIN , DM_T_MAX, 12); + + /* 打包数据 */ + data[0] = (pos_tmp >> 8); + data[1] = pos_tmp; + data[2] = (vel_tmp >> 4); + data[3] = ((vel_tmp&0xF)<<4)|(kp_tmp>>8); + data[4] = kp_tmp; + data[5] = (kd_tmp >> 4); + data[6] = ((kd_tmp&0xF)<<4)|(tor_tmp>>8); + data[7] = tor_tmp; + + /* 发送CAN消息 */ + BSP_CAN_StdDataFrame_t frame; + frame.id = motor->param.can_id; + frame.dlc = 8; + memcpy(frame.data, data, 8); + + + return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 发送位置速度模式控制命令 + * @param motor 电机实例 + * @param pos 目标位置 + * @param vel 目标速度 + * @return 发送结果 + */ +static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel) { + if (motor == NULL) { + return DEVICE_ERR_NULL; + } + + uint8_t data[8] = {0}; + + + /* 直接发送浮点数数据 */ + memcpy(&data[0], &pos, 4); // 位置，低位在前 + memcpy(&data[4], &vel, 4); // 速度，低位在前 + + /* 发送CAN消息，ID为原ID+0x100 */ + uint32_t can_id = DM_CAN_ID_OFFSET_POS_VEL + motor->param.can_id; + BSP_CAN_StdDataFrame_t frame; + frame.id = can_id; + frame.dlc = 8; + memcpy(frame.data, data, 8); + + return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 发送速度模式控制命令 + * @param motor 电机实例 + * @param vel 目标速度 + * @return 发送结果 + */ +static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel) { + if (motor == NULL) { + return DEVICE_ERR_NULL; + } + + uint8_t data[4] = {0}; + + /* 直接发送浮点数数据 */ + memcpy(&data[0], &vel, 4); // 速度，低位在前 + + /* 发送CAN消息，ID为原ID+0x200 */ + uint32_t can_id = DM_CAN_ID_OFFSET_VEL + motor->param.can_id; + BSP_CAN_StdDataFrame_t frame; + frame.id = can_id; + frame.dlc = 4; + memcpy(frame.data, data, 4); + + return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 获取指定CAN总线的管理器 + * @param can CAN总线 + * @return CAN管理器指针 + */ +static MOTOR_DM_CANManager_t* MOTOR_DM_GetCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) { + return NULL; + } + + return can_managers[can]; +} + +/** + * @brief 创建CAN管理器 + * @param can CAN总线 + * @return 创建结果 + */ +static int8_t MOTOR_DM_CreateCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return DEVICE_ERR; + if (can_managers[can] != NULL) return DEVICE_OK; + + can_managers[can] = (MOTOR_DM_CANManager_t*)BSP_Malloc(sizeof(MOTOR_DM_CANManager_t)); + if (can_managers[can] == NULL) return DEVICE_ERR; + + memset(can_managers[can], 0, sizeof(MOTOR_DM_CANManager_t)); + can_managers[can]->can = can; + return DEVICE_OK; +} + +/* Exported functions ------------------------------------------------------- */ + +/** + * @brief 注册一个DM电机 + * @param param 电机参数 + * @return 注册结果 + */ +int8_t MOTOR_DM_Register(MOTOR_DM_Param_t *param) { + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + /* 创建CAN管理器 */ + if (MOTOR_DM_CreateCANManager(param->can) != DEVICE_OK) { + return DEVICE_ERR; + } + + /* 获取CAN管理器 */ + MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can); + if (manager == NULL) { + return DEVICE_ERR; + } + + /* 检查是否已注册 */ + for (int i = 0; i < manager->motor_count; i++) { + if (manager->motors[i] && manager->motors[i]->param.master_id == param->master_id) { + return DEVICE_ERR_INITED; + } + } + + /* 检查是否已达到最大数量 */ + if (manager->motor_count >= MOTOR_DM_MAX_MOTORS) { + return DEVICE_ERR; + } + + /* 分配内存 */ + MOTOR_DM_t *motor = (MOTOR_DM_t *)BSP_Malloc(sizeof(MOTOR_DM_t)); + if (motor == NULL) { + return DEVICE_ERR; + } + + /* 初始化电机 */ + memset(motor, 0, sizeof(MOTOR_DM_t)); + memcpy(&motor->param, param, sizeof(MOTOR_DM_Param_t)); + motor->motor.header.online = false; + motor->motor.reverse = param->reverse; + + /* 注册CAN接收ID - DM电机使用Master ID接收反馈 */ + uint16_t feedback_id = param->master_id; + if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) { + BSP_Free(motor); + return DEVICE_ERR; + } + + /* 添加到管理器 */ + manager->motors[manager->motor_count] = motor; + manager->motor_count++; + + return DEVICE_OK; +} + +/** + * @brief 更新指定电机数据 + * @param param 电机参数 + * @return 更新结果 + */ +int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param) { + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can); + if (manager == NULL) { + return DEVICE_ERR_NO_DEV; + } + + /* 查找电机 */ + MOTOR_DM_t *motor = NULL; + for (int i = 0; i < manager->motor_count; i++) { + if (manager->motors[i] && manager->motors[i]->param.master_id == param->master_id) { + motor = manager->motors[i]; + break; + } + } + + if (motor == NULL) { + return DEVICE_ERR_NO_DEV; + } + + /* 主动接收CAN消息 */ + uint16_t feedback_id = param->master_id; + BSP_CAN_Message_t rx_msg; + if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) { + uint64_t now_time = BSP_TIME_Get(); + if (now_time - motor->motor.header.last_online_time > 100000) { // 100ms超时，单位微秒 + motor->motor.header.online = false; + } + return DEVICE_ERR; + } + + motor->motor.header.online = true; + motor->motor.header.last_online_time = BSP_TIME_Get(); + MOTOR_DM_ParseFeedbackFrame(motor, rx_msg.data); + + return DEVICE_OK; +} + +/** + * @brief 更新所有电机数据 + * @return 更新结果 + */ +int8_t MOTOR_DM_UpdateAll(void) { + int8_t ret = DEVICE_OK; + for (int can = 0; can < BSP_CAN_NUM; can++) { + MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager((BSP_CAN_t)can); + if (manager == NULL) continue; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_DM_t *motor = manager->motors[i]; + if (motor != NULL) { + if (MOTOR_DM_Update(&motor->param) != DEVICE_OK) { + ret = DEVICE_ERR; + } + } + } + } + return ret; +} + +/** + * @brief MIT模式控制 + * @param param 电机参数 + * @param output MIT控制参数 + * @return 控制结果 + */ +int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output) { + if (param == NULL || output == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param); + if (motor == NULL) { + return DEVICE_ERR_NO_DEV; + } + + return MOTOR_DM_SendMITCmd(motor, output); +} + +/** + * @brief 位置速度模式控制 + * @param param 电机参数 + * @param target_pos 目标位置 + * @param target_vel 目标速度 + * @return 控制结果 + */ +int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel) { + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param); + if (motor == NULL) { + return DEVICE_ERR_NO_DEV; + } + + return MOTOR_DM_SendPosVelCmd(motor, target_pos, target_vel); +} + +/** + * @brief 速度模式控制 + * @param param 电机参数 + * @param target_vel 目标速度 + * @return 控制结果 + */ +int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel) { + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param); + if (motor == NULL) { + return DEVICE_ERR_NO_DEV; + } + + return MOTOR_DM_SendVelCmd(motor, target_vel); +} + +/** + * @brief 获取指定电机的实例指针 + * @param param 电机参数 + * @return 电机实例指针 + */ +MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param) { + if (param == NULL) { + return NULL; + } + + MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can); + if (manager == NULL) { + return NULL; + } + + /* 查找对应的电机 */ + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_DM_t *motor = manager->motors[i]; + if (motor && motor->param.can == param->can && + motor->param.master_id == param->master_id) { + return motor; + } + } + + return NULL; +} + + +int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param){ + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param); + if (motor == NULL) { + return DEVICE_ERR_NO_DEV; + } + + BSP_CAN_StdDataFrame_t frame; + frame.id = motor->param.can_id; + frame.dlc = 8; + frame.data[0] = 0XFF; + frame.data[1] = 0xFF; + frame.data[2] = 0xFF; + frame.data[3] = 0xFF; + frame.data[4] = 0xFF; + frame.data[5] = 0xFF; + frame.data[6] = 0xFF; + frame.data[7] = 0xFC; + + return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +//重新设置角度零点 +int8_t MOTOR_DM_SetZero(MOTOR_DM_Param_t *param){ + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param); + if (motor == NULL) { + return DEVICE_ERR_NO_DEV; + } + + BSP_CAN_StdDataFrame_t frame; + frame.id = motor->param.can_id; + frame.dlc = 8; + frame.data[0] = 0XFF; + frame.data[1] = 0xFF; + frame.data[2] = 0xFF; + frame.data[3] = 0xFF; + frame.data[4] = 0xFF; + frame.data[5] = 0xFF; + frame.data[6] = 0xFF; + frame.data[7] = 0xFE; + + return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 使电机松弛（设置输出为0） + * @param param 电机参数 + * @return 操作结果 + */ +int8_t MOTOR_DM_Relax(MOTOR_DM_Param_t *param) { + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_MIT_Output_t output = {0}; + return MOTOR_DM_MITCtrl(param, &output); +} + +/** + * @brief 使电机离线（设置在线状态为false） + * @param param 电机参数 + * @return 操作结果 + */ +int8_t MOTOR_DM_Offine(MOTOR_DM_Param_t *param) { + if (param == NULL) { + return DEVICE_ERR_NULL; + } + + MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param); + if (motor == NULL) { + return DEVICE_ERR_NO_DEV; + } + + motor->motor.header.online = false; + return DEVICE_OK; +} diff --git a/User/device/motor_dm.h b/User/device/motor_dm.h new file mode 100644 index 0000000..0ea39a3 --- /dev/null +++ b/User/device/motor_dm.h @@ -0,0 +1,105 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "device/motor.h" +#include "bsp/can.h" + +/* Exported constants ------------------------------------------------------- */ +#define MOTOR_DM_MAX_MOTORS 32 + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + MOTOR_DM_J4310, +} MOTOR_DM_Module_t; + +/*每个电机需要的参数*/ +typedef struct { + BSP_CAN_t can; + uint16_t master_id; /* 主站ID，用于发送控制命令 */ + uint16_t can_id; /* 反馈ID，用于接收电机反馈 */ + MOTOR_DM_Module_t module; + bool reverse; +} MOTOR_DM_Param_t; + +/*电机实例*/ +typedef struct{ + MOTOR_DM_Param_t param; + MOTOR_t motor; +} MOTOR_DM_t; + +/*CAN管理器，管理一个CAN总线上所有的电机*/ +typedef struct { + BSP_CAN_t can; + MOTOR_DM_t *motors[MOTOR_DM_MAX_MOTORS]; + uint8_t motor_count; +} MOTOR_DM_CANManager_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 注册一个LK电机 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_DM_Register(MOTOR_DM_Param_t *param); + +/** + * @brief 更新指定电机数据 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param); + +/** + * @brief 更新所有电机数据 + * @return + */ +int8_t MOTOR_DM_UpdateAll(void); + +int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output); + +int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel); + +int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel); + +/** + * @brief 获取指定电机的实例指针 + * @param param 电机参数 + * @return + */ +MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param); + +int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param); + +/** + * @brief 使电机松弛（设置输出为0） + * @param param + * @return + */ +int8_t MOTOR_DM_Relax(MOTOR_DM_Param_t *param); + +/** + * @brief 使电机离线（设置在线状态为false） + * @param param + * @return + */ +int8_t MOTOR_DM_Offine(MOTOR_DM_Param_t *param); + +/** + * @brief 重新设置角度零点（将当前位置设为电机零位） + * @param param 电机参数指针 + * @return DEVICE_OK 成功，DEVICE_ERR 失败 + */ +int8_t MOTOR_DM_SetZero(MOTOR_DM_Param_t *param); + + + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/User/device/motor_lz.c b/User/device/motor_lz.c new file mode 100644 index 0000000..dea454b --- /dev/null +++ b/User/device/motor_lz.c @@ -0,0 +1,440 @@ +/* + 灵足电机驱动 + + 灵足电机通信协议： + - CAN 2.0通信接口，波特率1Mbps + - 采用扩展帧格式(29位ID) + - ID格式：Bit28~24(通信类型) + Bit23~8(数据区2) + Bit7~0(目标地址) +*/ +/* Includes ----------------------------------------------------------------- */ +#include "motor_lz.h" +#include +#include +#include +#include "bsp/can.h" +#include "bsp/mm.h" +#include "bsp/time.h" +#include "component/user_math.h" + +/* Private define ----------------------------------------------------------- */ +// 灵足电机协议参数 +#define LZ_ANGLE_RANGE_RAD (12.57f) /* 角度范围 ±12.57 rad */ +#define LZ_VELOCITY_RANGE_RAD_S (20.0f) /* 角速度范围 ±20 rad/s */ +#define LZ_TORQUE_RANGE_NM (60.0f) /* 力矩范围 ±60 Nm */ +#define LZ_KP_MAX (5000.0f) /* Kp最大值 */ +#define LZ_KD_MAX (100.0f) /* Kd最大值 */ + +#define LZ_RAW_VALUE_MAX (65535) /* 16位原始值最大值 */ +#define LZ_TEMP_SCALE (10.0f) /* 温度缩放因子 */ + +#define LZ_MAX_RECOVER_DIFF_RAD (0.28f) +#define MOTOR_TX_BUF_SIZE (8) +#define MOTOR_RX_BUF_SIZE (8) + +/* Private macro ------------------------------------------------------------ */ + +MOTOR_LZ_MotionParam_t lz_relax_param = { + .target_angle = 0.0f, + .target_velocity = 0.0f, + .kp = 0.0f, + .kd = 0.0f, + .torque = 0.0f, +}; +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ +static MOTOR_LZ_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL}; + +/* Private function prototypes ---------------------------------------------- */ +static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can); +static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can); +static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg); +static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id); +static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value); +static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value); +static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc); +static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type); + +/* Private functions -------------------------------------------------------- */ + +/** + * @brief 获取CAN管理器 + */ +static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return NULL; + return can_managers[can]; +} + +/** + * @brief 创建CAN管理器 + */ +static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return DEVICE_ERR; + if (can_managers[can] != NULL) return DEVICE_OK; + + can_managers[can] = (MOTOR_LZ_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LZ_CANManager_t)); + if (can_managers[can] == NULL) return DEVICE_ERR; + + memset(can_managers[can], 0, sizeof(MOTOR_LZ_CANManager_t)); + can_managers[can]->can = can; + return DEVICE_OK; +} + +/** + * @brief 构建扩展ID + */ +static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id) { + uint32_t ext_id = 0; + ext_id |= ((uint32_t)cmd_type & 0x1F) << 24; // Bit28~24: 通信类型 + ext_id |= ((uint32_t)data2 & 0xFFFF) << 8; // Bit23~8: 数据区2 + ext_id |= ((uint32_t)target_id & 0xFF); // Bit7~0: 目标地址 + return ext_id; +} + +/** + * @brief 浮点值转换为原始值（对称范围：-max_value ~ +max_value） + */ +static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value) { + // 限制范围 + if (value > max_value) value = max_value; + if (value < -max_value) value = -max_value; + + // 转换为0~65535范围，对应-max_value~max_value + return (uint16_t)((value + max_value) / (2.0f * max_value) * (float)LZ_RAW_VALUE_MAX); +} + +/** + * @brief 浮点值转换为原始值（单向范围：0 ~ +max_value） + */ +static uint16_t MOTOR_LZ_FloatToRawPositive(float value, float max_value) { + // 限制范围 + if (value > max_value) value = max_value; + if (value < 0.0f) value = 0.0f; + + // 转换为0~65535范围，对应0~max_value + return (uint16_t)(value / max_value * (float)LZ_RAW_VALUE_MAX); +} + +/** + * @brief 原始值转换为浮点值 + */ +static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value) { + // 将0~65535范围转换为-max_value~max_value + return ((float)raw_value / (float)LZ_RAW_VALUE_MAX) * (2.0f * max_value) - max_value; +} + +/** + * @brief 发送扩展帧 + */ +static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc) { + BSP_CAN_ExtDataFrame_t tx_frame; + tx_frame.id = ext_id; + tx_frame.dlc = dlc; + if (data != NULL) { + memcpy(tx_frame.data, data, dlc); + } else { + memset(tx_frame.data, 0, dlc); + } + return BSP_CAN_TransmitExtDataFrame(can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 灵足电机ID解析器 + * @param original_id 原始CAN ID（29位扩展帧） + * @param frame_type 帧类型 + * @return 解析后的ID（用于队列匹配） + * + * 灵足电机扩展ID格式： + * Bit28~24: 通信类型 (0x1=运控控制, 0x2=反馈数据, 0x3=使能, 0x4=停止, 0x6=设零位) + * Bit23~8: 数据区2 (根据通信类型而定) + * Bit7~0: 目标地址 (目标电机CAN ID) + */ +static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type) { + // 只处理扩展数据帧 + if (frame_type != BSP_CAN_FRAME_EXT_DATA) { + return original_id; // 非扩展帧直接返回原始ID + } + + // 解析扩展ID各个字段 + uint8_t cmd_type = (original_id >> 24) & 0x1F; // Bit28~24: 通信类型 + uint16_t data2 = (original_id >> 8) & 0xFFFF; // Bit23~8: 数据区2 + uint8_t host_id = (uint8_t)(original_id & 0xFF); // Bit7~0: 主机CAN ID + + // 对于反馈数据帧，我们使用特殊的解析规则 + if (cmd_type == MOTOR_LZ_CMD_FEEDBACK) { + // 反馈数据的data2字段包含： + // Bit8~15: 当前电机CAN ID + // Bit16~21: 故障信息 + // Bit22~23: 模式状态 + uint8_t motor_can_id = data2 & 0xFF; // bit8~15: 当前电机CAN ID + + // 返回格式化的ID，便于匹配 + // 格式：0x02HHMMTT (02=反馈命令, HH=主机ID, MM=电机ID, TT=主机ID) + return (0x02000000) | (host_id << 16) | (motor_can_id << 8) | host_id; + } + + // 对于其他命令类型，直接返回原始ID + return original_id; +} + +/** + * @brief 解码灵足电机反馈数据 + */ +static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg) { + if (motor == NULL || msg == NULL) return; + uint8_t cmd_type = (msg->original_id >> 24) & 0x1F; + if (cmd_type != MOTOR_LZ_CMD_FEEDBACK) return; + uint16_t id_data2 = (msg->original_id >> 8) & 0xFFFF; + uint8_t motor_can_id = id_data2 & 0xFF; + uint8_t fault_info = (id_data2 >> 8) & 0x3F; + uint8_t mode_state = (id_data2 >> 14) & 0x03; + motor->lz_feedback.motor_can_id = motor_can_id; + motor->lz_feedback.fault.under_voltage = (fault_info & 0x01) != 0; + motor->lz_feedback.fault.driver_fault = (fault_info & 0x02) != 0; + motor->lz_feedback.fault.over_temp = (fault_info & 0x04) != 0; + motor->lz_feedback.fault.encoder_fault = (fault_info & 0x08) != 0; + motor->lz_feedback.fault.stall_overload = (fault_info & 0x10) != 0; + motor->lz_feedback.fault.uncalibrated = (fault_info & 0x20) != 0; + motor->lz_feedback.state = (MOTOR_LZ_State_t)mode_state; + + // 反馈解码并自动反向 + uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]); + float angle = MOTOR_LZ_RawToFloat(raw_angle, LZ_ANGLE_RANGE_RAD); + uint16_t raw_velocity = (uint16_t)((msg->data[2] << 8) | msg->data[3]); + float velocity = MOTOR_LZ_RawToFloat(raw_velocity, LZ_VELOCITY_RANGE_RAD_S); + uint16_t raw_torque = (uint16_t)((msg->data[4] << 8) | msg->data[5]); + float torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM); + + // while (angle <0){ + // angle += M_2PI; + // } + // while (angle > M_2PI){ + // angle -= M_2PI; + // } + // 自动反向 + if (motor->param.reverse) { + angle = - angle; + velocity = -velocity; + torque = -torque; + } + + motor->lz_feedback.current_angle = angle; + motor->lz_feedback.current_velocity = velocity; + motor->lz_feedback.current_torque = torque; + + uint16_t raw_temp = (uint16_t)((msg->data[6] << 8) | msg->data[7]); + motor->lz_feedback.temperature = (float)raw_temp / LZ_TEMP_SCALE; + + motor->motor.feedback.rotor_abs_angle = angle; + motor->motor.feedback.rotor_speed = velocity; + motor->motor.feedback.torque_current = torque; + motor->motor.feedback.temp = (int8_t)motor->lz_feedback.temperature; + motor->motor.header.online = true; + motor->motor.header.last_online_time = BSP_TIME_Get(); +} + +/* Exported functions ------------------------------------------------------- */ + +/** + * @brief 初始化灵足电机驱动系统 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Init(void) { + // 注册灵足电机专用的ID解析器 + return BSP_CAN_RegisterIdParser(MOTOR_LZ_IdParser) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + + +int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + if (MOTOR_LZ_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR; + MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR; + + // 检查是否已注册 + for (int i = 0; i < manager->motor_count; i++) { + if (manager->motors[i] && manager->motors[i]->param.motor_id == param->motor_id) { + return DEVICE_ERR; // 已注册 + } + } + + // 检查数量 + if (manager->motor_count >= MOTOR_LZ_MAX_MOTORS) return DEVICE_ERR; + + // 创建新电机实例 + MOTOR_LZ_t *new_motor = (MOTOR_LZ_t*)BSP_Malloc(sizeof(MOTOR_LZ_t)); + if (new_motor == NULL) return DEVICE_ERR; + + memcpy(&new_motor->param, param, sizeof(MOTOR_LZ_Param_t)); + memset(&new_motor->motor, 0, sizeof(MOTOR_t)); + memset(&new_motor->lz_feedback, 0, sizeof(MOTOR_LZ_Feedback_t)); + memset(&new_motor->motion_param, 0, sizeof(MOTOR_LZ_MotionParam_t)); + + new_motor->motor.reverse = param->reverse; + + // 注册CAN接收ID - 使用解析后的反馈数据ID + // 构建反馈数据的原始扩展ID + // 反馈数据：data2包含电机ID(bit8~15)，target_id是主机ID + uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id); + // 通过ID解析器得到解析后的ID + uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA); + + if (BSP_CAN_RegisterId(param->can, parsed_feedback_id, 3) != BSP_OK) { + BSP_Free(new_motor); + return DEVICE_ERR; + } + + manager->motors[manager->motor_count] = new_motor; + manager->motor_count++; + return DEVICE_OK; +} + +int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LZ_t *motor = manager->motors[i]; + if (motor && motor->param.motor_id == param->motor_id) { + // 获取反馈数据 - 使用解析后的ID + uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id); + uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA); + BSP_CAN_Message_t msg; + + while (BSP_CAN_GetMessage(param->can, parsed_feedback_id, &msg, 0) == BSP_OK) { + MOTOR_LZ_Decode(motor, &msg); + } + return DEVICE_OK; + } + } + return DEVICE_ERR_NO_DEV; +} + +int8_t MOTOR_LZ_UpdateAll(void) { + int8_t ret = DEVICE_OK; + for (int can = 0; can < BSP_CAN_NUM; can++) { + MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager((BSP_CAN_t)can); + if (manager == NULL) continue; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LZ_t *motor = manager->motors[i]; + if (motor) { + if (MOTOR_LZ_Update(&motor->param) != DEVICE_OK) { + ret = DEVICE_ERR; + } + } + } + } + return ret; +} + +int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param) { + if (param == NULL || motion_param == NULL) return DEVICE_ERR_NULL; + MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param); + if (motor == NULL) return DEVICE_ERR_NO_DEV; + + // 自动反向控制 + MOTOR_LZ_MotionParam_t send_param = *motion_param; + if (param->reverse) { + send_param.target_angle = -send_param.target_angle; + send_param.target_velocity = -send_param.target_velocity; + send_param.torque = -send_param.torque; + } + + memcpy(&motor->motion_param, motion_param, sizeof(MOTOR_LZ_MotionParam_t)); + + uint16_t raw_torque = MOTOR_LZ_FloatToRaw(send_param.torque, LZ_TORQUE_RANGE_NM); + uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_MOTION, raw_torque, param->motor_id); + uint8_t data[8]; + uint16_t raw_angle = MOTOR_LZ_FloatToRaw(send_param.target_angle, LZ_ANGLE_RANGE_RAD); + data[0] = (raw_angle >> 8) & 0xFF; + data[1] = raw_angle & 0xFF; + uint16_t raw_velocity = MOTOR_LZ_FloatToRaw(send_param.target_velocity, LZ_VELOCITY_RANGE_RAD_S); + data[2] = (raw_velocity >> 8) & 0xFF; + data[3] = raw_velocity & 0xFF; + uint16_t raw_kp = MOTOR_LZ_FloatToRawPositive(send_param.kp, LZ_KP_MAX); + data[4] = (raw_kp >> 8) & 0xFF; + data[5] = raw_kp & 0xFF; + uint16_t raw_kd = MOTOR_LZ_FloatToRawPositive(send_param.kd, LZ_KD_MAX); + data[6] = (raw_kd >> 8) & 0xFF; + data[7] = raw_kd & 0xFF; + return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8); +} + + +int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + // 构建扩展ID - 使能命令 + uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_ENABLE, param->host_id, param->motor_id); + + // 数据区清零 + uint8_t data[8] = {0}; + + return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8); +} + +int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault) { + if (param == NULL) return DEVICE_ERR_NULL; + + // 构建扩展ID - 停止命令 + uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_DISABLE, param->host_id, param->motor_id); + + // 数据区 + uint8_t data[8] = {0}; + if (clear_fault) { + data[0] = 1; // Byte[0]=1时清故障 + } + + return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8); +} + +int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + // 构建扩展ID - 设置零位命令 + uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_SET_ZERO, param->host_id, param->motor_id); + + // 数据区 - Byte[0]=1 + uint8_t data[8] = {1, 0, 0, 0, 0, 0, 0, 0}; + + return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8); +} + +MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param) { + if (param == NULL) return NULL; + + MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can); + if (manager == NULL) return NULL; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LZ_t *motor = manager->motors[i]; + if (motor && motor->param.motor_id == param->motor_id) { + return motor; + } + } + return NULL; +} + +int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param) { + return MOTOR_LZ_MotionControl(param, &lz_relax_param); +} + +int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param) { + MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param); + if (motor) { + motor->motor.header.online = false; + return DEVICE_OK; + } + return DEVICE_ERR_NO_DEV; +} + +static MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param) { + MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param); + if (motor && motor->motor.header.online) { + return &motor->lz_feedback; + } + return NULL; +} diff --git a/User/device/motor_lz.h b/User/device/motor_lz.h new file mode 100644 index 0000000..d2a8002 --- /dev/null +++ b/User/device/motor_lz.h @@ -0,0 +1,212 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "device/motor.h" +#include "bsp/can.h" + +/* Exported constants ------------------------------------------------------- */ +#define MOTOR_LZ_MAX_MOTORS 32 + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + MOTOR_LZ_RSO0, + MOTOR_LZ_RSO1, + MOTOR_LZ_RSO2, + MOTOR_LZ_RSO3, + MOTOR_LZ_RSO4, + MOTOR_LZ_RSO5, + MOTOR_LZ_RSO6, +} MOTOR_LZ_Module_t; + +/* 灵足电机控制模式 */ +typedef enum { + MOTOR_LZ_MODE_MOTION = 0x1, /* 运控模式 */ + MOTOR_LZ_MODE_CURRENT = 0x2, /* 电流模式 */ + MOTOR_LZ_MODE_VELOCITY = 0x3, /* 速度模式 */ + MOTOR_LZ_MODE_POSITION = 0x4, /* 位置模式 */ +} MOTOR_LZ_ControlMode_t; + +/* 灵足电机通信类型 */ +typedef enum { + MOTOR_LZ_CMD_MOTION = 0x1, /* 运控模式控制 */ + MOTOR_LZ_CMD_FEEDBACK = 0x2, /* 电机反馈数据 */ + MOTOR_LZ_CMD_ENABLE = 0x3, /* 电机使能运行 */ + MOTOR_LZ_CMD_DISABLE = 0x4, /* 电机停止运行 */ + MOTOR_LZ_CMD_SET_ZERO = 0x6, /* 设置电机机械零位 */ +} MOTOR_LZ_CmdType_t; + +/* 灵足电机运行状态 */ +typedef enum { + MOTOR_LZ_STATE_RESET = 0, /* Reset模式[复位] */ + MOTOR_LZ_STATE_CALI = 1, /* Cali模式[标定] */ + MOTOR_LZ_STATE_MOTOR = 2, /* Motor模式[运行] */ +} MOTOR_LZ_State_t; + +/* 灵足电机故障信息 */ +typedef struct { + bool uncalibrated; /* bit21: 未标定 */ + bool stall_overload; /* bit20: 堵转过载故障 */ + bool encoder_fault; /* bit19: 磁编码故障 */ + bool over_temp; /* bit18: 过温 */ + bool driver_fault; /* bit17: 驱动故障 */ + bool under_voltage; /* bit16: 欠压故障 */ +} MOTOR_LZ_Fault_t; + +/* 灵足电机运控参数 */ +typedef struct { + float target_angle; /* 目标角度 (-12.57f~12.57f rad) */ + float target_velocity; /* 目标角速度 (-20~20 rad/s) */ + float kp; /* 位置增益 (0.0~5000.0) */ + float kd; /* 微分增益 (0.0~100.0) */ + float torque; /* 力矩 (-60~60 Nm) */ +} MOTOR_LZ_MotionParam_t; + +/*每个电机需要的参数*/ +typedef struct { + BSP_CAN_t can; /* CAN总线 */ + uint8_t motor_id; /* 电机CAN ID */ + uint8_t host_id; /* 主机CAN ID */ + MOTOR_LZ_Module_t module; /* 电机型号 */ + bool reverse; /* 是否反向 */ + MOTOR_LZ_ControlMode_t mode; /* 控制模式 */ +} MOTOR_LZ_Param_t; + +/*电机反馈信息扩展*/ +typedef struct { + float current_angle; /* 当前角度 (-12.57f~12.57f rad) */ + float current_velocity; /* 当前角速度 (-20~20 rad/s) */ + float current_torque; /* 当前力矩 (-60~60 Nm) */ + float temperature; /* 当前温度 (摄氏度) */ + MOTOR_LZ_State_t state; /* 运行状态 */ + MOTOR_LZ_Fault_t fault; /* 故障信息 */ + uint8_t motor_can_id; /* 当前电机CAN ID */ +} MOTOR_LZ_Feedback_t; + +/*电机实例*/ +typedef struct { + MOTOR_LZ_Param_t param; + MOTOR_t motor; + MOTOR_LZ_Feedback_t lz_feedback; /* 灵足电机特有反馈信息 */ + MOTOR_LZ_MotionParam_t motion_param; /* 运控模式参数 */ +} MOTOR_LZ_t; + +/*CAN管理器，管理一个CAN总线上所有的电机*/ +typedef struct { + BSP_CAN_t can; + MOTOR_LZ_t *motors[MOTOR_LZ_MAX_MOTORS]; + uint8_t motor_count; +} MOTOR_LZ_CANManager_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 初始化灵足电机驱动系统 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Init(void); + +/** + * @brief 注册一个灵足电机 + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param); + +/** + * @brief 更新指定电机数据 + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param); + +/** + * @brief 更新所有电机数据 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_UpdateAll(void); + +/** + * @brief 运控模式控制电机 + * @param param 电机参数 + * @param motion_param 运控参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param); + +/** + * @brief 电流(力矩)模式控制电机 + * @param param 电机参数 + * @param torque 目标力矩 (-60~60 Nm) + * @return 设备状态码 + */ +int8_t MOTOR_LZ_TorqueControl(MOTOR_LZ_Param_t *param, float torque); + +/** + * @brief 位置模式控制电机 + * @param param 电机参数 + * @param target_angle 目标角度 (-12.57~12.57 rad) + * @param max_velocity 最大速度 (0~20 rad/s) + * @return 设备状态码 + */ +int8_t MOTOR_LZ_PositionControl(MOTOR_LZ_Param_t *param, float target_angle, float max_velocity); + +/** + * @brief 速度模式控制电机 + * @param param 电机参数 + * @param target_velocity 目标速度 (-20~20 rad/s) + * @return 设备状态码 + */ +int8_t MOTOR_LZ_VelocityControl(MOTOR_LZ_Param_t *param, float target_velocity); + +/** + * @brief 电机使能运行 + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param); + +/** + * @brief 电机停止运行 + * @param param 电机参数 + * @param clear_fault 是否清除故障 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault); + +/** + * @brief 设置电机机械零位 + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param); + +/** + * @brief 获取指定电机的实例指针 + * @param param 电机参数 + * @return 电机实例指针，失败返回NULL + */ +MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param); + +/** + * @brief 使电机松弛（发送停止命令） + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param); + +/** + * @brief 使电机离线（设置在线状态为false） + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/User/device/motor_rm.c b/User/device/motor_rm.c new file mode 100644 index 0000000..88c1ef4 --- /dev/null +++ b/User/device/motor_rm.c @@ -0,0 +1,321 @@ +/* + RM电机驱动 +*/ +/* Includes ----------------------------------------------------------------- */ +#include "motor_rm.h" +#include +#include +#include "bsp/can.h" +#include "bsp/mm.h" +#include "bsp/time.h" +#include "component/user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +#define GM6020_FB_ID_BASE (0x205) +#define GM6020_CTRL_ID_BASE (0x1ff) +#define GM6020_CTRL_ID_EXTAND (0x2ff) + +#define M3508_M2006_FB_ID_BASE (0x201) +#define M3508_M2006_CTRL_ID_BASE (0x200) +#define M3508_M2006_CTRL_ID_EXTAND (0x1ff) +#define M3508_M2006_ID_SETTING_ID (0x700) + +#define GM6020_MAX_ABS_LSB (30000) +#define M3508_MAX_ABS_LSB (16384) +#define M2006_MAX_ABS_LSB (10000) + +#define MOTOR_TX_BUF_SIZE (8) +#define MOTOR_RX_BUF_SIZE (8) + +#define MOTOR_ENC_RES (8192) /* 电机编码器分辨率 */ +#define MOTOR_CUR_RES (16384) /* 电机转矩电流分辨率 */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static MOTOR_RM_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL}; + +/* Private function -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module) { + switch (module) { + case MOTOR_M2006: + case MOTOR_M3508: + if (can_id >= M3508_M2006_FB_ID_BASE && can_id <= M3508_M2006_FB_ID_BASE + 7) { + return can_id - M3508_M2006_FB_ID_BASE; + } + break; + case MOTOR_GM6020: + if (can_id >= GM6020_FB_ID_BASE && can_id <= GM6020_FB_ID_BASE + 6) { + return can_id - GM6020_FB_ID_BASE + 4; + } + break; + default: + break; + } + return DEVICE_ERR; +} + +static float MOTOR_RM_GetRatio(MOTOR_RM_Module_t module) { + switch (module) { + case MOTOR_M2006: return 36.0f; + case MOTOR_M3508: return 3591.0f / 187.0f; + case MOTOR_GM6020: return 1.0f; + default: return 1.0f; + } +} + +static int16_t MOTOR_RM_GetLSB(MOTOR_RM_Module_t module) { + switch (module) { + case MOTOR_M2006: return M2006_MAX_ABS_LSB; + case MOTOR_M3508: return M3508_MAX_ABS_LSB; + case MOTOR_GM6020: return GM6020_MAX_ABS_LSB; + default: return DEVICE_ERR; + } +} + +static MOTOR_RM_CANManager_t* MOTOR_RM_GetCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return NULL; + return can_managers[can]; +} + +static int8_t MOTOR_RM_CreateCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return DEVICE_ERR; + if (can_managers[can] != NULL) return DEVICE_OK; + can_managers[can] = (MOTOR_RM_CANManager_t*)BSP_Malloc(sizeof(MOTOR_RM_CANManager_t)); + if (can_managers[can] == NULL) return DEVICE_ERR; + memset(can_managers[can], 0, sizeof(MOTOR_RM_CANManager_t)); + can_managers[can]->can = can; + return DEVICE_OK; +} + +static void Motor_RM_Decode(MOTOR_RM_t *motor, BSP_CAN_Message_t *msg) { + uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]); + int16_t raw_speed = (int16_t)((msg->data[2] << 8) | msg->data[3]); + int16_t raw_current = (int16_t)((msg->data[4] << 8) | msg->data[5]); + int16_t lsb = MOTOR_RM_GetLSB(motor->param.module); + float ratio = MOTOR_RM_GetRatio(motor->param.module); + + float rotor_angle = raw_angle / (float)MOTOR_ENC_RES * M_2PI; + float rotor_speed = raw_speed; + float torque_current = raw_current * lsb / (float)MOTOR_CUR_RES; + + if (motor->param.gear) { + // 多圈累加 + int32_t delta = (int32_t)raw_angle - (int32_t)motor->last_raw_angle; + if (delta > (MOTOR_ENC_RES / 2)) { + motor->gearbox_round_count--; + } else if (delta < -(MOTOR_ENC_RES / 2)) { + motor->gearbox_round_count++; + } + motor->last_raw_angle = raw_angle; + float single_turn = rotor_angle; + motor->gearbox_total_angle = (motor->gearbox_round_count * M_2PI + single_turn) / ratio; + // 输出轴多圈绝对值 + motor->feedback.rotor_abs_angle = motor->gearbox_total_angle; + motor->feedback.rotor_speed = rotor_speed / ratio; + motor->feedback.torque_current = torque_current * ratio; + } else { + // 非gear模式，直接用转子单圈 + motor->gearbox_round_count = 0; + motor->last_raw_angle = raw_angle; + motor->gearbox_total_angle = 0.0f; + motor->feedback.rotor_abs_angle = rotor_angle; + motor->feedback.rotor_speed = rotor_speed; + motor->feedback.torque_current = torque_current; + } + while (motor->feedback.rotor_abs_angle < 0) { + motor->feedback.rotor_abs_angle += M_2PI; + } + while (motor->feedback.rotor_abs_angle >= M_2PI) { + motor->feedback.rotor_abs_angle -= M_2PI; + } + if (motor->motor.reverse) { + motor->feedback.rotor_abs_angle = M_2PI - motor->feedback.rotor_abs_angle; + motor->feedback.rotor_speed = -motor->feedback.rotor_speed; + motor->feedback.torque_current = -motor->feedback.torque_current; + } + motor->feedback.temp = msg->data[6]; +} + +/* Exported functions ------------------------------------------------------- */ + +int8_t MOTOR_RM_Register(MOTOR_RM_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + if (MOTOR_RM_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR; + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR; + // 检查是否已注册 + for (int i = 0; i < manager->motor_count; i++) { + if (manager->motors[i] && manager->motors[i]->param.id == param->id) { + return DEVICE_ERR_INITED; + } + } + // 检查数量 + if (manager->motor_count >= MOTOR_RM_MAX_MOTORS) return DEVICE_ERR; + // 创建新电机实例 + MOTOR_RM_t *new_motor = (MOTOR_RM_t*)BSP_Malloc(sizeof(MOTOR_RM_t)); + if (new_motor == NULL) return DEVICE_ERR; + memcpy(&new_motor->param, param, sizeof(MOTOR_RM_Param_t)); + memset(&new_motor->motor, 0, sizeof(MOTOR_t)); + new_motor->motor.reverse = param->reverse; + // 注册CAN接收ID + if (BSP_CAN_RegisterId(param->can, param->id, 3) != BSP_OK) { + BSP_Free(new_motor); + return DEVICE_ERR; + } + manager->motors[manager->motor_count] = new_motor; + manager->motor_count++; + return DEVICE_OK; +} + +int8_t MOTOR_RM_Update(MOTOR_RM_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_RM_t *motor = manager->motors[i]; + if (motor && motor->param.id == param->id) { + BSP_CAN_Message_t rx_msg; + if (BSP_CAN_GetMessage(param->can, param->id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) { + uint64_t now_time = BSP_TIME_Get(); + if (now_time - motor->motor.header.last_online_time > 1000) { + motor->motor.header.online = false; + return DEVICE_ERR_NO_DEV; + } + return DEVICE_ERR; + } + motor->motor.header.online = true; + motor->motor.header.last_online_time = BSP_TIME_Get(); + Motor_RM_Decode(motor, &rx_msg); + motor->motor.feedback = motor->feedback; + return DEVICE_OK; + } + } + return DEVICE_ERR_NO_DEV; +} + +int8_t MOTOR_RM_UpdateAll(void) { + int8_t ret = DEVICE_OK; + for (int can = 0; can < BSP_CAN_NUM; can++) { + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager((BSP_CAN_t)can); + if (manager == NULL) continue; + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_RM_t *motor = manager->motors[i]; + if (motor != NULL) { + if (MOTOR_RM_Update(&motor->param) != DEVICE_OK) { + ret = DEVICE_ERR; + } + } + } + } + return ret; +} + +int8_t MOTOR_RM_SetOutput(MOTOR_RM_Param_t *param, float value) { + if (param == NULL) return DEVICE_ERR_NULL; + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + if (value > 1.0f) value = 1.0f; + if (value < -1.0f) value = -1.0f; + if (param->reverse){ + value = -value; + } + MOTOR_RM_t *motor = MOTOR_RM_GetMotor(param); + if (motor == NULL) return DEVICE_ERR_NO_DEV; + int8_t logical_index = MOTOR_RM_GetLogicalIndex(param->id, param->module); + if (logical_index < 0) return DEVICE_ERR; + MOTOR_RM_MsgOutput_t *output_msg = &manager->output_msg; + int16_t output_value = (int16_t)(value * (float)MOTOR_RM_GetLSB(param->module)); + output_msg->output[logical_index] = output_value; + return DEVICE_OK; +} + +int8_t MOTOR_RM_Ctrl(MOTOR_RM_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + MOTOR_RM_MsgOutput_t *output_msg = &manager->output_msg; + BSP_CAN_StdDataFrame_t tx_frame; + uint16_t id = param->id; + switch (id) { + case M3508_M2006_FB_ID_BASE: + case M3508_M2006_FB_ID_BASE+1: + case M3508_M2006_FB_ID_BASE+2: + case M3508_M2006_FB_ID_BASE+3: + tx_frame.id = M3508_M2006_CTRL_ID_BASE; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + for (int i = 0; i < 4; i++) { + tx_frame.data[i*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF); + tx_frame.data[i*2+1] = (uint8_t)(output_msg->output[i] & 0xFF); + } + break; + case M3508_M2006_FB_ID_BASE+4: + case M3508_M2006_FB_ID_BASE+5: + case M3508_M2006_FB_ID_BASE+6: + case M3508_M2006_FB_ID_BASE+7: + tx_frame.id = M3508_M2006_CTRL_ID_EXTAND; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + for (int i = 4; i < 8; i++) { + tx_frame.data[(i-4)*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF); + tx_frame.data[(i-4)*2+1] = (uint8_t)(output_msg->output[i] & 0xFF); + } + break; + case GM6020_FB_ID_BASE+4: + case GM6020_FB_ID_BASE+5: + case GM6020_FB_ID_BASE+6: + tx_frame.id = GM6020_CTRL_ID_EXTAND; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + for (int i = 8; i < 11; i++) { + tx_frame.data[(i-8)*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF); + tx_frame.data[(i-8)*2+1] = (uint8_t)(output_msg->output[i] & 0xFF); + } + tx_frame.data[6] = 0; + tx_frame.data[7] = 0; + break; + default: + return DEVICE_ERR; + } + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +MOTOR_RM_t* MOTOR_RM_GetMotor(MOTOR_RM_Param_t *param) { + if (param == NULL) return NULL; + MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can); + if (manager == NULL) return NULL; + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_RM_t *motor = manager->motors[i]; + if (motor && motor->param.id == param->id) { + return motor; + } + } + return NULL; +} + +int8_t MOTOR_RM_Relax(MOTOR_RM_Param_t *param) { + return MOTOR_RM_SetOutput(param, 0.0f); +} + +int8_t MOTOR_RM_Offine(MOTOR_RM_Param_t *param) { + MOTOR_RM_t *motor = MOTOR_RM_GetMotor(param); + if (motor) { + motor->motor.header.online = false; + return DEVICE_OK; + } + return DEVICE_ERR_NO_DEV; +} \ No newline at end of file diff --git a/User/device/motor_rm.h b/User/device/motor_rm.h new file mode 100644 index 0000000..670b427 --- /dev/null +++ b/User/device/motor_rm.h @@ -0,0 +1,132 @@ +#pragma once + +#include "motor.h" +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "device/motor.h" +#include "bsp/can.h" + +/* Exported constants ------------------------------------------------------- */ +#define MOTOR_RM_MAX_MOTORS 11 + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + MOTOR_M2006, + MOTOR_M3508, + MOTOR_GM6020, +} MOTOR_RM_Module_t; + +/*一个can最多控制11个电机*/ +typedef union { + int16_t output[MOTOR_RM_MAX_MOTORS]; + struct { + int16_t m3508_m2006_id201; + int16_t m3508_m2006_id202; + int16_t m3508_m2006_id203; + int16_t m3508_m2006_id204; + int16_t m3508_m2006_gm6020_id205; + int16_t m3508_m2006_gm6020_id206; + int16_t m3508_m2006_gm6020_id207; + int16_t m3508_m2006_gm6020_id208; + int16_t gm6020_id209; + int16_t gm6020_id20A; + int16_t gm6020_id20B; + } named; +} MOTOR_RM_MsgOutput_t; + +/*每个电机需要的参数*/ +typedef struct { + BSP_CAN_t can; + uint16_t id; + MOTOR_RM_Module_t module; + bool reverse; + bool gear; +} MOTOR_RM_Param_t; + +typedef MOTOR_Feedback_t MOTOR_RM_Feedback_t; + +typedef struct { + MOTOR_RM_Param_t param; + MOTOR_RM_Feedback_t feedback; + MOTOR_t motor; + // 多圈相关变量，仅gear模式下有效 + uint16_t last_raw_angle; + int32_t gearbox_round_count; + float gearbox_total_angle; +} MOTOR_RM_t; + +/*CAN管理器，管理一个CAN总线上所有的电机*/ +typedef struct { + BSP_CAN_t can; + MOTOR_RM_MsgOutput_t output_msg; + MOTOR_RM_t *motors[MOTOR_RM_MAX_MOTORS]; + uint8_t motor_count; +} MOTOR_RM_CANManager_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 注册一个RM电机 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_RM_Register(MOTOR_RM_Param_t *param); + +/** + * @brief 更新指定电机数据 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_RM_Update(MOTOR_RM_Param_t *param); + +/** + * @brief 设置一个电机的输出 + * @param param 电机参数 + * @param value 输出值，范围[-1.0, 1.0] + * @return + */ +int8_t MOTOR_RM_SetOutput(MOTOR_RM_Param_t *param, float value); + +/** + * @brief 发送控制命令到电机，注意一个CAN可以控制多个电机，所以只需要发送一次即可 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_RM_Ctrl(MOTOR_RM_Param_t *param); + +/** + * @brief 获取指定电机的实例指针 + * @param param 电机参数 + * @return + */ +MOTOR_RM_t* MOTOR_RM_GetMotor(MOTOR_RM_Param_t *param); + +/** + * @brief 使电机松弛（设置输出为0） + * @param param + * @return + */ +int8_t MOTOR_RM_Relax(MOTOR_RM_Param_t *param); + +/** + * @brief 使电机离线（设置在线状态为false） + * @param param + * @return + */ +int8_t MOTOR_RM_Offine(MOTOR_RM_Param_t *param); + +/** + * @brief + * @param + * @return + */ +int8_t MOTOR_RM_UpdateAll(void); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/User/device/supercap.c b/User/device/supercap.c new file mode 100644 index 0000000..c0fe4e3 --- /dev/null +++ b/User/device/supercap.c @@ -0,0 +1,158 @@ +#include "device/supercap.h" + +/* 全局变量 */ +CAN_SuperCapRXDataTypeDef CAN_SuperCapRXData = {0}; + +uint8_t PowerOffset =0; + +uint32_t LastCapTick = 0; //上一次收到超电信号的时间戳 +uint32_t NowCapTick = 0; //现在收到超电信号的时间戳 + +uint32_t chassis_energy_in_gamming =0; + +/* 静态变量 - 用于CAN接收管理 */ +static bool supercap_inited = false; +static osMessageQueueId_t supercap_rx_queue = NULL; + +/** + * @brief 获取超级电容在线状态，1在线，0离线 + */ +uint8_t get_supercap_online_state(void){ + + NowCapTick = HAL_GetTick(); //更新时间戳 + + uint32_t DeltaCapTick = 0; + DeltaCapTick = NowCapTick - LastCapTick; //计算时间差 + +// if(get_game_progress() == 4){ +// //比赛开始的时候，开始统计消耗的能量 + + chassis_energy_in_gamming += CAN_SuperCapRXData.BatPower * DeltaCapTick *0.001f; + // 因为STM32的系统定时器是1ms的周期，所以*0.001，单位化为秒（S），能量单位才是焦耳（J） +// } + + if(DeltaCapTick > 1000){ + //如果时间差大于1s，说明超电信号丢失，返回超电离线的标志 + return 0; + }else { + //如果时间差小于1s，说明超电信号正常，返回超电在线的标志 + return 1; + } +} + +/** + * @brief 获取根据超级电容功率统计的底盘消耗能量，单位：焦耳（J） + */ +uint32_t get_chassis_energy_from_supercap(void){ + + return chassis_energy_in_gamming; + +} + +/******************超级电容数据从CAN传到结构体******************/ +int8_t SuperCap_Init(void) +{ + if (supercap_inited) { + return DEVICE_OK; // 已经初始化过 + } + + if (BSP_CAN_RegisterId( SUPERCAP_CAN , SUPERCAP_RX_ID, 3) != BSP_OK) { + return DEVICE_ERR; + } + + supercap_inited = true; + return DEVICE_OK; +} + +int8_t SuperCap_Update(void) +{ + if (!supercap_inited) { + return DEVICE_ERR; + } + + BSP_CAN_Message_t rx_msg; + + // 从CAN队列获取数据 + if (BSP_CAN_GetMessage( SUPERCAP_CAN , SUPERCAP_RX_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + // 处理接收到的数据 + transfer_SuperCap_measure(rx_msg.data); + return DEVICE_OK; + } + + return DEVICE_ERR; // 没有新数据 +} + +void transfer_SuperCap_measure(uint8_t *data) +{ + LastCapTick = HAL_GetTick(); + CAN_SuperCapRXData.SuperCapReady = (SuperCap_Status_t)data[0]; + CAN_SuperCapRXData.SuperCapState = (SuperCap_Status_t)data[1]; + CAN_SuperCapRXData.SuperCapEnergy = data[2]; + CAN_SuperCapRXData.ChassisPower = data[3] << 1; //左移一位是为了扩大范围，超电发出来的的时候右移了一位 + CAN_SuperCapRXData.BatVoltage = data[4]; + CAN_SuperCapRXData.BatPower = data[5]; +} + +/** + * @brief 获取超级电容的运行状态，具体查看枚举SuperCapStateEnum + * @retval none + */ +SuperCapStateEnum get_supercap_state(void){ + return (SuperCapStateEnum)CAN_SuperCapRXData.SuperCapState; +} + +/** + * @brief 获取超级电容读到的电池电压，单位伏（V） + * @retval none + */ +float get_battery_voltage_from_supercap(void){ + return (float)CAN_SuperCapRXData.BatVoltage * 0.1f; +} + +/** + * @brief 获取超级电容可用能量，范围：0-100% + * @retval none + */ +uint8_t get_supercap_energy(void){ + return CAN_SuperCapRXData.SuperCapEnergy; +} + +/** + * @brief 设置超级电容功率补偿 + * @retval none + */ +void set_supercap_power_offset(uint8_t offset){ + PowerOffset = offset; +} + +/** + * @brief 发送超级电容数据 + * @param[in] Enable: 超级电容使能 + * @param[in] Charge: 超级电容充电，在PLUS版本中无效 + * @param[in] PowerLimit: 裁判系统功率限制 + * @param[in] Chargepower: 超级电容充电功率，在PLUS版本中无效 + * @retval none + */ +int8_t CAN_TX_SuperCapData(CAN_SuperCapTXDataTypeDef * TX_Temp) +{ + if (TX_Temp == NULL) return DEVICE_ERR_NULL; + + BSP_FDCAN_StdDataFrame_t tx_frame; + + tx_frame.id = SUPERCAP_TX_ID; + tx_frame.dlc = 0x08; + + tx_frame.data[0] = TX_Temp-> Enable; + tx_frame.data[1] = TX_Temp-> Charge;//PLUS disabled + tx_frame.data[2] = TX_Temp-> Powerlimit - PowerOffset; + tx_frame.data[3] = TX_Temp-> ChargePower;//PLUS disabled + + tx_frame.data[4] = 0; + tx_frame.data[5] = 0; + tx_frame.data[6] = 0; + tx_frame.data[7] = 0; + + return BSP_CAN_TransmitStdDataFrame( SUPERCAP_CAN , &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + + diff --git a/User/device/supercap.h b/User/device/supercap.h new file mode 100644 index 0000000..94c1417 --- /dev/null +++ b/User/device/supercap.h @@ -0,0 +1,102 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "bsp/can.h" +#include "device/device.h" +//#include "referee.h" + +#define SUPERCAP_CAN BSP_CAN_1 +//#define SUPERCAP_CAN BSP_CAN_2 + +#define SUPERCAP_TX_ID 0x001 //C板发给超级电容的ID +#define SUPERCAP_RX_ID 0x100 //超级电容发给C板的ID + + +//超级电容的状态标志位，超级电容运行或者保护的具体状态反馈 +typedef enum +{ + DISCHARGE =0 , //放电状态 + CHARGE =1, //充电状态 + WAIT =2, //待机状态 + SOFTSTART_PROTECTION =3,//处于软起动状态 + OVER_LOAD_PROTECTION = 4, //超电过载保护状态 + BAT_OVER_VOLTAGE_PROTECTION =5, //过压保护状态 + BAT_UNDER_VOLTAGE_PROTECTION =6, //电池欠压保护，电池要没电了，换电池 + CAP_UNDER_VOLTAGE_PROTECTION =7, //超级电容欠压保护，超级电容用完电了，要充一会才能用 + OVER_TEMPERATURE_PROTECTION =8, //过温保护，太热了 + BOOM = 9, //超电爆炸了 +}SuperCapStateEnum; + +//超级电容准备状态，用于判断超级电容是否可以使用 +typedef enum +{ + SUPERCAP_STATUS_OFFLINE =0 , + SUPERCAP_STATUS_RUNNING =1, +}SuperCap_Status_t; + +// 发送给超级电容的数据 +typedef struct { + FunctionalState Enable ; //超级电容使能。1使能，0失能 + SuperCapStateEnum Charge ; //V1.1超级电容充电。1充电，0放电，在PLUS版本中，此标志位无效，超电的充放电是自动的 + uint8_t Powerlimit; //裁判系统功率限制 + uint8_t ChargePower; //V1.1超级电容充电功率，在PLUS版本中，此参数，超电的充电功率随着底盘功率变化 +}CAN_SuperCapTXDataTypeDef; + +// 从超级电容接收到的数据 +typedef struct { + uint8_t SuperCapEnergy;//超级电容可用能量：0-100% + uint16_t ChassisPower; //底盘功率，0-512，由于传输的时候为了扩大量程右移了一位，所以接收的时候需要左移还原（丢精度）。 + SuperCap_Status_t SuperCapReady;//超级电容【可用标志】：1为可用，0为不可用 + SuperCapStateEnum SuperCapState;//超级电容【状态标志】：各个状态对应的状态码查看E_SuperCapState枚举。 + uint8_t BatVoltage; //通过超级电容监控电池电压*10， + uint8_t BatPower; +}CAN_SuperCapRXDataTypeDef; + +extern CAN_SuperCapRXDataTypeDef CAN_SuperCapRXData; + +void set_supercap_power_offset(uint8_t offset); + + +// 以下函数是超电控制所需要调用的函数 +void transfer_SuperCap_measure( uint8_t *data); +int8_t CAN_TX_SuperCapData(CAN_SuperCapTXDataTypeDef * TX_Temp); + + +/** + * @brief 获取超级电容在线状态，1在线，0离线 + */ +uint8_t get_supercap_online_state(void); + + +/** + * @brief 获取超级电容的运行状态，具体查看枚举SuperCapStateEnum + */ +SuperCapStateEnum get_supercap_state(void); + + +/** + * @brief 获取超级电容读到的电池电压，单位伏（V） + */ +float get_battery_voltage_from_supercap(void); + + +/** + * @brief 获取超级电容可用能量，范围：0-100% + */ +uint8_t get_supercap_energy(void); + + +/** + * @brief 获取根据超级电容功率统计的底盘消耗能量，单位：焦耳（J） + */ +uint32_t get_chassis_energy_from_supercap(void); + +int8_t SuperCap_Init(void); +int8_t SuperCap_Update(void); + +#ifdef __cplusplus +} +#endif /*SUPERCAP_H*/ diff --git a/User/module/arm.cpp b/User/module/arm.cpp new file mode 100644 index 0000000..5dc229c --- /dev/null +++ b/User/module/arm.cpp @@ -0,0 +1,507 @@ + + +// /* Includes ----------------------------------------------------------------- */ +// #include +// #include +// #include +// #include "arm.h" +// #include "bsp/mm.h" +// #include "bsp/time.h" +// #include "component/user_math.h" +// #include "device/motor_dm.h" +// #include "device/motor_lz.h" +// /* Private typedef ---------------------------------------------------------- */ +// /* Private define ----------------------------------------------------------- */ + +// /* Private macro ------------------------------------------------------------ */ +// /* Private variables -------------------------------------------------------- */ + +// /* Private function -------------------------------------------------------- */ + +// // ============================================================================ +// // Joint（关节）级别操作 - 面向对象接口 +// // ============================================================================ + +// /** +// * @brief 初始化单个关节 +// */ +// int8_t Joint_Init(Joint_t *joint, uint8_t id, Joint_MotorType_t motor_type, +// const Arm6dof_DHParams_t *dh_params, const KPID_Params_t *pid_params, float freq) { +// if (joint == NULL || dh_params == NULL || pid_params == NULL) { +// return -1; +// } + +// joint->id = id; +// joint->motor_type = motor_type; +// memcpy(&joint->dh_params, dh_params, sizeof(Arm6dof_DHParams_t)); +// joint->q_offset = 0.0f; + +// PID_Init(&joint->pid, KPID_MODE_CALC_D, freq, pid_params); + +// joint->state.current_angle = 0.0f; +// joint->state.current_velocity = 0.0f; +// joint->state.current_torque = 0.0f; +// joint->state.target_angle = 0.0f; +// joint->state.target_velocity = 0.0f; +// joint->state.online = false; + +// return 0; +// } + +// /** +// * @brief 更新关节状态 +// */ +// int8_t Joint_Update(Joint_t *joint) { +// if (joint == NULL) return -1; + +// if (joint->motor_type == JOINT_MOTOR_TYPE_LZ) { +// joint->state.current_angle = joint->motor.lz_motor.motor.feedback.rotor_abs_angle - joint->q_offset; +// joint->state.current_velocity = joint->motor.lz_motor.motor.feedback.rotor_speed; +// joint->state.current_torque = joint->motor.lz_motor.motor.feedback.torque_current; +// joint->state.online = joint->motor.lz_motor.motor.header.online; +// } else { +// joint->state.current_angle = joint->motor.dm_motor.motor.feedback.rotor_abs_angle - joint->q_offset; +// joint->state.current_velocity = joint->motor.dm_motor.motor.feedback.rotor_speed; +// joint->state.current_torque = joint->motor.dm_motor.motor.feedback.torque_current; +// joint->state.online = joint->motor.dm_motor.motor.header.online; +// } + +// return 0; +// } + +// /** +// * @brief 关节位置控制 +// */ +// int8_t Joint_PositionControl(Joint_t *joint, float target_angle, float dt) { +// if (joint == NULL) return -1; + +// if (target_angle < joint->dh_params.qmin || target_angle > joint->dh_params.qmax) { +// return -1; +// } + +// joint->state.target_angle = target_angle; +// float pid_output = PID_Calc(&joint->pid, target_angle, joint->state.current_angle, 0, dt); + +// if (joint->motor_type == JOINT_MOTOR_TYPE_LZ) { +// joint->output.lz_output.target_angle = target_angle + joint->q_offset; +// joint->output.lz_output.target_velocity = 0.0f; +// joint->output.lz_output.kp = 10.0f; +// joint->output.lz_output.kd = 0.5f; +// joint->output.lz_output.torque = pid_output; +// MOTOR_LZ_MotionControl(&joint->motor_params.lz_params, &joint->output.lz_output); +// } else { +// joint->output.dm_output.angle = target_angle + joint->q_offset; +// joint->output.dm_output.velocity = 0.0f; +// joint->output.dm_output.kp = 50.0f; +// joint->output.dm_output.kd = 3.0f; +// joint->output.dm_output.torque = pid_output; +// MOTOR_DM_MITCtrl(&joint->motor_params.dm_params, &joint->output.dm_output); +// } + +// return 0; +// } + +// /** +// * @brief 关节松弛 +// */ +// int8_t Joint_Relax(Joint_t *joint) { +// if (joint == NULL) return -1; + +// if (joint->motor_type == JOINT_MOTOR_TYPE_LZ) { +// MOTOR_LZ_Relax(&joint->motor_params.lz_params); +// } else { +// MOTOR_DM_Relax(&joint->motor_params.dm_params); +// } +// return 0; +// } + +// /** +// * @brief 检查关节是否到达目标 +// */ +// bool Joint_IsReached(const Joint_t *joint, float tolerance) { +// if (joint == NULL) return false; +// return fabsf(joint->state.target_angle - joint->state.current_angle) < tolerance; +// } + +// // ============================================================================ +// // Arm（机械臂）级别操作 +// // ============================================================================ + +// /* Exported functions ------------------------------------------------------- */ +// int8_t Arm_Init(Arm_t *arm, Arm_Params_t *arm_param, float freq) { +// if (arm == NULL||arm_param == NULL) { +// return -1; +// } +// arm->param = arm_param; + +// BSP_CAN_Init(); + +// MOTOR_LZ_Init(); + + +// // 初始化控制参数 +// arm->control.mode = ARM_MODE_IDLE; +// arm->control.state = ARM_STATE_STOPPED; +// arm->control.position_tolerance = 0.02f; // 关节角容差：0.02rad ≈ 1.15度 +// arm->control.velocity_tolerance = 0.01f; // 速度容差：0.01rad/s +// arm->control.enable = false; + +// return 0; +// } + +// int8_t Arm_Updata(Arm_t *arm) { +// if (arm == NULL) { +// return -1; +// } + + +// MOTOR_LZ_Update(&arm->param->jointMotor1_params); +// MOTOR_LZ_Update(&arm->param->jointMotor2_params); +// MOTOR_LZ_Update(&arm->param->jointMotor3_params); +// MOTOR_DM_Update(&arm->param->jointMotor4_params); +// MOTOR_DM_Update(&arm->param->jointMotor5_params); +// MOTOR_DM_Update(&arm->param->jointMotor6_params); + +// // 同步电机实例数据到actuator +// MOTOR_LZ_t *motor1 = MOTOR_LZ_GetMotor(&arm->param->jointMotor1_params); +// if (motor1 != NULL) { +// memcpy(&arm->actuator.jointMotor1, motor1, sizeof(MOTOR_LZ_t)); +// } + +// MOTOR_LZ_t *motor2 = MOTOR_LZ_GetMotor(&arm->param->jointMotor2_params); +// if (motor2 != NULL) { +// memcpy(&arm->actuator.jointMotor2, motor2, sizeof(MOTOR_LZ_t)); +// } + +// MOTOR_LZ_t *motor3 = MOTOR_LZ_GetMotor(&arm->param->jointMotor3_params); +// if (motor3 != NULL) { +// memcpy(&arm->actuator.jointMotor3, motor3, sizeof(MOTOR_LZ_t)); +// } + +// MOTOR_DM_t *motor4 = MOTOR_DM_GetMotor(&arm->param->jointMotor4_params); +// if (motor4 != NULL) { +// memcpy(&arm->actuator.jointMotor4, motor4, sizeof(MOTOR_DM_t)); +// } + +// MOTOR_DM_t *motor5 = MOTOR_DM_GetMotor(&arm->param->jointMotor5_params); +// if (motor5 != NULL) { +// memcpy(&arm->actuator.jointMotor5, motor5, sizeof(MOTOR_DM_t)); +// } + +// MOTOR_DM_t *motor6 = MOTOR_DM_GetMotor(&arm->param->jointMotor6_params); +// if (motor6 != NULL) { +// memcpy(&arm->actuator.jointMotor6, motor6, sizeof(MOTOR_DM_t)); +// } + +// // 从电机反馈更新当前关节角度 +// arm->stateVariable.currentJointAngles.q[0] = +// arm->actuator.jointMotor1.motor.feedback.rotor_abs_angle - arm->param->q_offset[0]; +// arm->stateVariable.currentJointAngles.q[1] = +// arm->actuator.jointMotor2.motor.feedback.rotor_abs_angle - arm->param->q_offset[1]; +// arm->stateVariable.currentJointAngles.q[2] = +// arm->actuator.jointMotor3.motor.feedback.rotor_abs_angle - arm->param->q_offset[2]; +// arm->stateVariable.currentJointAngles.q[3] = +// arm->actuator.jointMotor4.motor.feedback.rotor_abs_angle - arm->param->q_offset[3]; +// arm->stateVariable.currentJointAngles.q[4] = +// arm->actuator.jointMotor5.motor.feedback.rotor_abs_angle - arm->param->q_offset[4]; +// arm->stateVariable.currentJointAngles.q[5] = +// arm->actuator.jointMotor6.motor.feedback.rotor_abs_angle - arm->param->q_offset[5]; + +// // 正运动学：计算当前末端位姿 +// Arm6dof_ForwardKinematics(&arm->stateVariable.currentJointAngles, +// &arm->stateVariable.currentEndPose); + +// return 0; +// } +// int8_t Arm_Ctrl(Arm_t *arm) { +// if (arm == NULL) { +// return -1; +// } + +// arm->timer.now = BSP_TIME_Get_us() / 1000000.0f; +// arm->timer.dt = (BSP_TIME_Get_us() - arm->timer.last_wakeup) / 1000000.0f; +// arm->timer.last_wakeup = BSP_TIME_Get_us(); + +// // 如果未使能，松弛所有电机 +// if (!arm->control.enable) { +// MOTOR_LZ_Relax(&arm->param->jointMotor1_params); +// MOTOR_LZ_Relax(&arm->param->jointMotor2_params); +// MOTOR_LZ_Relax(&arm->param->jointMotor3_params); +// MOTOR_DM_Relax(&arm->param->jointMotor4_params); +// MOTOR_DM_Relax(&arm->param->jointMotor5_params); +// MOTOR_DM_Relax(&arm->param->jointMotor6_params); +// arm->control.state = ARM_STATE_STOPPED; +// return 0; +// } + +// // 根据控制模式执行不同的控制策略 +// switch (arm->control.mode) { +// case ARM_MODE_IDLE: +// // 空闲模式：电机松弛 +// MOTOR_LZ_Relax(&arm->param->jointMotor1_params); +// MOTOR_LZ_Relax(&arm->param->jointMotor2_params); +// MOTOR_LZ_Relax(&arm->param->jointMotor3_params); +// MOTOR_DM_Relax(&arm->param->jointMotor4_params); +// MOTOR_DM_Relax(&arm->param->jointMotor5_params); +// MOTOR_DM_Relax(&arm->param->jointMotor6_params); +// arm->control.state = ARM_STATE_STOPPED; +// break; + +// case ARM_MODE_JOINT_POSITION: +// case ARM_MODE_CARTESIAN_POSITION: +// // 关节位置控制（关节空间和笛卡尔空间最终都转为关节角控制） +// { +// // 检查是否到达目标 +// bool reached = true; +// float max_error = 0.0f; + +// for (int i = 0; i < 6; i++) { +// float error = arm->stateVariable.targetJointAngles.q[i] - +// arm->stateVariable.currentJointAngles.q[i]; +// if (fabsf(error) > arm->control.position_tolerance) { +// reached = false; +// } +// if (fabsf(error) > max_error) { +// max_error = fabsf(error); +// } +// } + +// if (reached) { +// arm->control.state = ARM_STATE_REACHED; +// } else { +// arm->control.state = ARM_STATE_MOVING; +// } + +// // PID位置控制（关节1-3使用LZ电机） +// for (int i = 0; i < 3; i++) { +// float pid_output = PID_Calc(&arm->controller.joint_pid[i], +// arm->stateVariable.targetJointAngles.q[i], +// arm->stateVariable.currentJointAngles.q[i], +// 0, +// arm->timer.dt +// ); + +// MOTOR_LZ_MotionParam_t* output = NULL; +// if (i == 0) output = &arm->output.jointMotor1_output; +// else if (i == 1) output = &arm->output.jointMotor2_output; +// else output = &arm->output.jointMotor3_output; + +// output->target_angle = arm->stateVariable.targetJointAngles.q[i] + arm->param->q_offset[i]; +// output->target_velocity = 0.0f; // 位置模式速度由PID控制 +// output->kp = 10.0f; // 可调参数 +// output->kd = 0.5f; +// output->torque = pid_output; +// } + +// // MIT模式控制（关节4-6使用DM电机） +// for (int i = 3; i < 6; i++) { +// float pid_output = PID_Calc(&arm->controller.joint_pid[i], +// arm->stateVariable.targetJointAngles.q[i], +// arm->stateVariable.currentJointAngles.q[i], +// 0, +// arm->timer.dt +// ); + +// MOTOR_MIT_Output_t* output = NULL; +// if (i == 3) output = &arm->output.jointMotor4_output; +// else if (i == 4) output = &arm->output.jointMotor5_output; +// else output = &arm->output.jointMotor6_output; + +// output->angle = arm->stateVariable.targetJointAngles.q[i] + arm->param->q_offset[i]; +// output->velocity = 0.0f; +// output->kp = 50.0f; // 位置刚度（提供阻抗和稳定性） +// output->kd = 3.0f; // 速度阻尼（抑制振荡） +// output->torque = pid_output; // PID输出作为前馈力矩（主要驱动力） +// } + +// // 发送控制命令 +// MOTOR_LZ_MotionControl(&arm->param->jointMotor1_params, &arm->output.jointMotor1_output); +// MOTOR_LZ_MotionControl(&arm->param->jointMotor2_params, &arm->output.jointMotor2_output); +// MOTOR_LZ_MotionControl(&arm->param->jointMotor3_params, &arm->output.jointMotor3_output); +// MOTOR_DM_MITCtrl(&arm->param->jointMotor4_params, &arm->output.jointMotor4_output); +// MOTOR_DM_MITCtrl(&arm->param->jointMotor5_params, &arm->output.jointMotor5_output); +// MOTOR_DM_MITCtrl(&arm->param->jointMotor6_params, &arm->output.jointMotor6_output); +// } +// break; + +// case ARM_MODE_TRAJECTORY: +// // 轨迹跟踪模式（待实现） +// arm->control.state = ARM_STATE_ERROR; +// break; + +// case ARM_MODE_TEACH: +// // 示教模式（待实现） +// arm->control.state = ARM_STATE_ERROR; +// break; + +// default: +// arm->control.state = ARM_STATE_ERROR; +// break; +// } + +// return 0; +// } + +// /** +// * @brief 逆运动学求解：根据目标末端位姿计算关节角度 +// * @note 此函数计算量大，不要在控制循环中频繁调用 +// */ +// int8_t Arm_SolveIK(Arm_t *arm, const Arm6dof_Pose_t* target_pose, Arm6dof_JointAngles_t* result_angles) { +// if (arm == NULL || target_pose == NULL || result_angles == NULL) { +// return -1; +// } + +// // 验证目标位姿的有效性 +// if (isnan(target_pose->x) || isnan(target_pose->y) || isnan(target_pose->z) || +// isnan(target_pose->roll) || isnan(target_pose->pitch) || isnan(target_pose->yaw) || +// isinf(target_pose->x) || isinf(target_pose->y) || isinf(target_pose->z) || +// isinf(target_pose->roll) || isinf(target_pose->pitch) || isinf(target_pose->yaw)) { +// return -1; // 无效的目标位姿 +// } + +// // 使用当前关节角度作为初始猜测值（如果已初始化） +// Arm6dof_JointAngles_t initial_guess; + +// // 检查currentJointAngles是否已初始化（不全为0） +// bool has_init = false; +// for (int i = 0; i < 6; i++) { +// if (arm->stateVariable.currentJointAngles.q[i] != 0.0f) { +// has_init = true; +// break; +// } +// } + +// if (has_init) { +// // 使用当前角度作为初始猜测 +// memcpy(&initial_guess, &arm->stateVariable.currentJointAngles, sizeof(Arm6dof_JointAngles_t)); +// } else { +// // 使用零位作为初始猜测（机械臂竖直向上） +// memset(&initial_guess, 0, sizeof(Arm6dof_JointAngles_t)); +// } + +// // 调用逆运动学求解，容限1mm，最大迭代10次（减少计算量） +// int ret = Arm6dof_InverseKinematics(target_pose, &initial_guess, result_angles, 1.0f, 10); + +// if (ret == 0) { +// // 求解成功，更新到inverseKineJointAngles +// memcpy(&arm->stateVariable.inverseKineJointAngles, result_angles, sizeof(Arm6dof_JointAngles_t)); +// } + +// return ret; +// } + +// // ============================================================================ +// // 控制API函数实现 +// // ============================================================================ + +// /** +// * @brief 设置控制模式 +// */ +// int8_t Arm_SetMode(Arm_t *arm, Arm_ControlMode_t mode) { +// if (arm == NULL) { +// return -1; +// } +// arm->control.mode = mode; +// arm->control.state = ARM_STATE_STOPPED; +// return 0; +// } + +// /** +// * @brief 使能/失能机械臂 +// */ +// int8_t Arm_Enable(Arm_t *arm, bool enable) { +// if (arm == NULL) { +// return -1; +// } +// arm->control.enable = enable; + +// if (!enable) { +// // 失能时切换到空闲模式 +// arm->control.mode = ARM_MODE_IDLE; +// arm->control.state = ARM_STATE_STOPPED; +// } + +// return 0; +// } + +// /** +// * @brief 关节空间位置控制 +// */ +// int8_t Arm_MoveJoint(Arm_t *arm, const Arm6dof_JointAngles_t* target_angles) { +// if (arm == NULL || target_angles == NULL) { +// return -1; +// } + +// // 检查关节角度是否在限制范围内 +// for (int i = 0; i < 6; i++) { +// float qmin = arm->param->arm6dof_params.DH_params[i].qmin; +// float qmax = arm->param->arm6dof_params.DH_params[i].qmax; + +// if (target_angles->q[i] < qmin || target_angles->q[i] > qmax) { +// return -1; // 超出关节限位 +// } +// } + +// // 更新目标角度 +// memcpy(&arm->stateVariable.targetJointAngles, target_angles, sizeof(Arm6dof_JointAngles_t)); + +// // 切换到关节位置控制模式 +// arm->control.mode = ARM_MODE_JOINT_POSITION; +// arm->control.state = ARM_STATE_MOVING; + +// return 0; +// } + +// /** +// * @brief 笛卡尔空间位置控制 +// */ +// int8_t Arm_MoveCartesian(Arm_t *arm, const Arm6dof_Pose_t* target_pose) { +// if (arm == NULL || target_pose == NULL) { +// return -1; +// } + +// // 求解逆运动学 +// Arm6dof_JointAngles_t ik_solution; +// if (Arm_SolveIK(arm, target_pose, &ik_solution) != 0) { +// return -1; // 逆运动学求解失败 +// } + +// // 更新目标 +// memcpy(&arm->stateVariable.targetEndPose, target_pose, sizeof(Arm6dof_Pose_t)); +// memcpy(&arm->stateVariable.targetJointAngles, &ik_solution, sizeof(Arm6dof_JointAngles_t)); + +// // 切换到笛卡尔位置控制模式 +// arm->control.mode = ARM_MODE_CARTESIAN_POSITION; +// arm->control.state = ARM_STATE_MOVING; + +// return 0; +// } + +// /** +// * @brief 急停 +// */ +// int8_t Arm_Stop(Arm_t *arm) { +// if (arm == NULL) { +// return -1; +// } + +// // 将目标设置为当前位置 +// memcpy(&arm->stateVariable.targetJointAngles, +// &arm->stateVariable.currentJointAngles, +// sizeof(Arm6dof_JointAngles_t)); + +// arm->control.state = ARM_STATE_STOPPED; + +// return 0; +// } + +// /** +// * @brief 获取当前运动状态 +// */ +// Arm_MotionState_t Arm_GetState(Arm_t *arm) { +// if (arm == NULL) { +// return ARM_STATE_ERROR; +// } +// return arm->control.state; +// } + diff --git a/User/module/arm.h b/User/module/arm.h new file mode 100644 index 0000000..7acdcb3 --- /dev/null +++ b/User/module/arm.h @@ -0,0 +1,282 @@ +// /* +// * far♂蛇模块 +// */ + +// #pragma once + +// #include +// #include +// #include +// #ifdef __cplusplus +// extern "C" { +// #endif + +// #include "main.h" +// #include "component/pid.h" +// #include "component/arm_kinematics/arm6dof.h" +// #include "device/motor_dm.h" +// #include "device/motor_lz.h" +// /* Exported constants ------------------------------------------------------- */ +// #define ARM_JOINT_NUM 6 // 关节数量 + +// /* Exported macro ----------------------------------------------------------- */ +// /* Exported types ----------------------------------------------------------- */ + +// // 电机类型 +// typedef enum { +// JOINT_MOTOR_TYPE_LZ = 0, // 凌控电机 +// JOINT_MOTOR_TYPE_DM, // 达妙电机 +// } Joint_MotorType_t; + +// // 控制模式 +// typedef enum { +// ARM_MODE_IDLE = 0, // 空闲模式（电机松弛） +// ARM_MODE_JOINT_POSITION, // 关节空间位置控制 +// ARM_MODE_CARTESIAN_POSITION, // 笛卡尔空间位置控制 +// ARM_MODE_TRAJECTORY, // 轨迹跟踪模式 +// ARM_MODE_TEACH, // 示教模式（力控） +// } Arm_ControlMode_t; + +// // 运动状态 +// typedef enum { +// ARM_STATE_STOPPED = 0, // 停止 +// ARM_STATE_MOVING, // 运动中 +// ARM_STATE_REACHED, // 到达目标 +// ARM_STATE_ERROR, // 错误 +// } Arm_MotionState_t; + +// struct { +// union { +// MOTOR_LZ_Param_t lzMotor; +// MOTOR_DM_Param_t dmMotor; +// }; +// KPID_Params_t pid; +// } *Joint_MotorParams_t; +// typedef struct { +// // 关节标识 +// uint8_t id; // 关节编号（0-5） +// Joint_MotorType_t motor_type; // 电机类型 + +// // 运动学参数（DH参数） +// Arm6dof_DHParams_t dh_params; // DH参数（包含限位） +// float q_offset; // 零位偏移 + +// // 参数 +// *params; + +// // 电机实例 +// union { +// MOTOR_LZ_t lz_motor; +// MOTOR_DM_t dm_motor; +// } motor; + +// // 控制器 +// KPID_t pid; // PID控制器 + +// // 状态变量 +// struct { +// float current_angle; // 当前角度（去除零位偏移后） +// float current_velocity; // 当前速度 +// float current_torque; // 当前力矩 +// float target_angle; // 目标角度 +// float target_velocity; // 目标速度 +// bool online; // 在线状态 +// } state; + +// // 控制输出（根据motor_type选择使用） +// union { +// MOTOR_LZ_MotionParam_t lz_output; +// MOTOR_MIT_Output_t dm_output; +// } output; + +// } Joint_t; + +// // ============================================================================ +// // Arm（机械臂）- 由多个关节组成 +// // ============================================================================ +// typedef struct { +// // 关节数组（面向对象：机械臂由6个关节对象组成） +// Joint_t joints[ARM_JOINT_NUM]; + +// // 全局运动学参数 +// struct { +// float x, y, z; // 工具中心偏移 (mm) +// float roll, pitch, yaw; // 工具姿态偏移 (rad) +// } tool_offset; + +// // 时间管理 +// struct { +// float now; // 当前时间（秒） +// uint64_t last_wakeup; // 上次唤醒时间（微秒） +// float dt; // 控制周期（秒） +// } timer; + +// // 末端状态（笛卡尔空间） +// struct { +// Arm6dof_Pose_t current; // 当前末端位姿（正运动学结果） +// Arm6dof_Pose_t target; // 目标末端位姿 +// } end_effector; + +// // 控制状态 +// struct { +// Arm_ControlMode_t mode; // 控制模式 +// Arm_MotionState_t state; // 运动状态 +// float position_tolerance; // 位置到达容差（rad） +// float velocity_tolerance; // 速度到达容差（rad/s） +// bool enable; // 使能标志 +// } control; + +// } Arm_t; + +// // 兼容旧接口的参数结构体 +// typedef struct { +// Arm6dof_Params_t arm6dof_params; +// KPID_Params_t joint_pid_params[6]; +// MOTOR_LZ_Param_t jointMotor1_params; +// MOTOR_LZ_Param_t jointMotor2_params; +// MOTOR_LZ_Param_t jointMotor3_params; +// MOTOR_DM_Param_t jointMotor4_params; +// MOTOR_DM_Param_t jointMotor5_params; +// MOTOR_DM_Param_t jointMotor6_params; +// float q_offset[6]; +// } Arm_Params_t; + +// /* Exported functions prototypes -------------------------------------------- */ + +// /** +// * @brief 初始化机械臂（兼容旧接口） +// */ +// int8_t Arm_Init(Arm_t *arm, Arm_Params_t *arm_param, float freq); + +// /** +// * @brief 更新机械臂状态 +// */ +// int8_t Arm_Updata(Arm_t *arm); + +// /** +// * @brief 机械臂控制 +// */ +// int8_t Arm_Ctrl(Arm_t *arm); + +// // ============================================================================ +// // 关节级别操作（面向对象接口） +// // ============================================================================ + +// /** +// * @brief 初始化单个关节 +// * @param joint 关节指针 +// * @param id 关节编号（0-5） +// * @param motor_type 电机类型 +// * @param dh_params DH参数 +// * @param pid_params PID参数 +// * @param freq 控制频率 +// * @return 0: 成功, -1: 失败 +// */ +// int8_t Joint_Init(Joint_t *joint, uint8_t id, Joint_MotorType_t motor_type, +// const Arm6dof_DHParams_t *dh_params, const KPID_Params_t *pid_params, float freq); + +// /** +// * @brief 更新关节状态（读取电机反馈） +// * @param joint 关节指针 +// * @return 0: 成功, -1: 失败 +// */ +// int8_t Joint_Update(Joint_t *joint); + +// /** +// * @brief 关节位置控制 +// * @param joint 关节指针 +// * @param target_angle 目标角度（rad） +// * @param dt 控制周期（s） +// * @return 0: 成功, -1: 失败 +// */ +// int8_t Joint_PositionControl(Joint_t *joint, float target_angle, float dt); + +// /** +// * @brief 关节松弛 +// * @param joint 关节指针 +// * @return 0: 成功, -1: 失败 +// */ +// int8_t Joint_Relax(Joint_t *joint); + +// /** +// * @brief 检查关节是否到达目标 +// * @param joint 关节指针 +// * @param tolerance 位置容差（rad） +// * @return true: 已到达, false: 未到达 +// */ +// bool Joint_IsReached(const Joint_t *joint, float tolerance); + +// // ============================================================================ +// // 机械臂级别操作（继续保持现有接口） +// // ============================================================================ + +// /** +// * @brief 设置控制模式 +// * @param arm 机械臂实例 +// * @param mode 控制模式 +// * @return 0: 成功, -1: 失败 +// */ +// int8_t Arm_SetMode(Arm_t *arm, Arm_ControlMode_t mode); + +// /** +// * @brief 使能/失能机械臂 +// * @param arm 机械臂实例 +// * @param enable true: 使能, false: 失能 +// * @return 0: 成功, -1: 失败 +// */ +// int8_t Arm_Enable(Arm_t *arm, bool enable); + +// /** +// * @brief 关节空间位置控制：设置目标关节角度 +// * @param arm 机械臂实例 +// * @param target_angles 目标关节角度（rad） +// * @return 0: 成功, -1: 失败 +// */ +// int8_t Arm_MoveJoint(Arm_t *arm, const Arm6dof_JointAngles_t* target_angles); + +// /** +// * @brief 笛卡尔空间位置控制：设置目标末端位姿 +// * @param arm 机械臂实例 +// * @param target_pose 目标末端位姿 +// * @return 0: 成功（含逆解成功）, -1: 失败（含逆解失败） +// */ +// int8_t Arm_MoveCartesian(Arm_t *arm, const Arm6dof_Pose_t* target_pose); + +// /** +// * @brief 急停：立即停止所有运动 +// * @param arm 机械臂实例 +// * @return 0: 成功, -1: 失败 +// */ +// int8_t Arm_Stop(Arm_t *arm); + +// /** +// * @brief 获取当前运动状态 +// * @param arm 机械臂实例 +// * @return 运动状态 +// */ +// Arm_MotionState_t Arm_GetState(Arm_t *arm); + +// /** +// * @brief ; +// }output; +// */ +// int8_t Arm_Init(Arm_t *arm, Arm_Params_t *arm_param, float freq); +// int8_t Arm_Updata(Arm_t *arm); +// int8_t Arm_Ctrl(Arm_t *arm); + +// /** +// * @brief 逆运动学求解：根据目标末端位姿计算关节角度 +// * @param arm 机械臂实例 +// * @param target_pose 目标末端位姿 +// * @param result_angles 输出的关节角度解 +// * @return 0: 成功, -1: 失败 +// * @note 使用当前关节角度作为初始猜测值，迭代50次，误差容限1.0mm +// */ +// int8_t Arm_SolveIK(Arm_t *arm, const Arm6dof_Pose_t* target_pose, Arm6dof_JointAngles_t* result_angles); + +// #ifdef __cplusplus +// } +// #endif + + + diff --git a/User/module/arm_oop.hpp b/User/module/arm_oop.hpp new file mode 100644 index 0000000..83d4949 --- /dev/null +++ b/User/module/arm_oop.hpp @@ -0,0 +1,706 @@ +/** + * @file arm_oop.hpp + * @brief 机械臂类 - C++面向对象实现 + */ + +#pragma once + +#include +#include +#include +#include +#include "joint.hpp" +#include "bsp/time.h" +#include "component/arm_kinematics/arm6dof.h" +#include "component/toolbox/robotics.h" +#define qLimitTolerance 0.5// 关节角度误差容限（rad），用于IK解的有效性验证 +namespace arm { + +// 控制模式 +enum class ControlMode { + IDLE = 0, + JOINT_POSITION, + CARTESIAN_POSITION, + TRAJECTORY, + TEACH, // 示教模式（重力补偿 + 零刚度） + GRAVITY_COMP, // 重力补偿模式（带位置保持） +}; + +// 运动状态 +enum class MotionState { + STOPPED = 0, + MOVING, + REACHED, + ERROR, +}; +// ============================================================================ +// RoboticArm 类 - 机械臂 +// ============================================================================ +class RoboticArm { +private: + static constexpr size_t JOINT_NUM = 6; + + std::array joints_; // 关节数组（多态指针） + + // 末端状态 + struct { + Arm6dof_Pose_t current; // 当前末端位姿（FK计算） + Arm6dof_Pose_t target; // 目标末端位姿 + } end_effector_; + + // 控制状态 + struct { + ControlMode mode; + MotionState state; + float position_tolerance; + float velocity_tolerance; + bool enable; + bool gravity_comp_enable; // 重力补偿使能 + } control_; + + // 逆运动学 + struct { + Arm6dof_JointAngles_t angles; // IK解算得到的关节角度 + bool valid; // IK解是否有效 + } inverseKinematics_; + + // 重力补偿 + struct { + float torques[JOINT_NUM]; // 各关节重力补偿力矩 + float scale; // 补偿比例系数（用于微调） + } gravity_comp_; + + // 笛卡尔空间轨迹规划 + // 原理：将大步运动拆分为每帧一小步的线性插值，IK初始猜测始终是上一帧解， + // 从而保证牛顿迭代永远在正确分支附近收敛，根本消除大跨度跳变问题 + struct { + Arm6dof_Pose_t start; // 轨迹起点（启动时的当前末端位姿） + Arm6dof_Pose_t goal; // 轨迹终点（目标末端位姿） + float t; // 插值进度 [0.0, 1.0]，0=起点，1=终点 + bool active; // 是否正在执行轨迹 + float max_lin_vel; // 末端线速度限制 (m/s) + float max_ang_vel; // 末端角速度限制 (rad/s) + Arm6dof_JointAngles_t angles; // 轨迹模式下当前插值点的IK解算结果 + bool valid; // IK解是否有效 + } traj_; + + // 时间管理 + struct { + float now; + uint64_t last_wakeup; + float dt; + } timer_; + +public: + // 构造函数 + RoboticArm() { + joints_.fill(nullptr); + + control_.mode = ControlMode::IDLE; + control_.state = MotionState::STOPPED; + control_.position_tolerance = 0.02f; // 0.02 rad + control_.velocity_tolerance = 0.01f; + control_.enable = false; + control_.gravity_comp_enable = false; + + for (size_t i = 0; i < JOINT_NUM; ++i) { + gravity_comp_.torques[i] = 0.0f; + } + gravity_comp_.scale = 1.0f; + inverseKinematics_.valid = false; + + memset(&traj_.start, 0, sizeof(traj_.start)); + memset(&traj_.goal, 0, sizeof(traj_.goal)); + traj_.t = 0.0f; + traj_.active = false; + traj_.max_lin_vel = 0.15f; // 150 mm/s，可通过 SetLinVelLimit() 调整 + traj_.max_ang_vel = 1.0f; // ~57°/s，可通过 SetAngVelLimit() 调整 + + timer_.now = 0.0f; + timer_.last_wakeup = 0; + timer_.dt = 0.001f; + } + + // 禁止拷贝 + RoboticArm(const RoboticArm&) = delete; + RoboticArm& operator=(const RoboticArm&) = delete; + + // 添加关节（依赖注入） + void AddJoint(size_t index, Joint* joint) { + if (index < JOINT_NUM) { + joints_[index] = joint; + } + } + + // 访问关节 + Joint* GetJoint(size_t index) { + return (index < JOINT_NUM) ? joints_[index] : nullptr; + } + + const Joint* GetJoint(size_t index) const { + return (index < JOINT_NUM) ? joints_[index] : nullptr; + } + + // 初始化所有关节 + int8_t Init() { + for (Joint* joint : joints_) { // 遍历关节数组 + if (joint) { + joint->Register(); + joint->Enable(); + } + } + memset(&end_effector_.target, 0, sizeof(end_effector_.target)); + return 0; + } + + // 更新所有关节状态 + int8_t Update() { + // 更新每个关节 + for (Joint* joint : joints_) { // 遍历关节数组 + if (joint) { + joint->Update(); + } + } + + // 读取当前关节角度（局部变量，正运动学和重力补偿共用一次读取） + Arm6dof_JointAngles_t q; + for (size_t i = 0; i < JOINT_NUM; ++i) { + q.q[i] = joints_[i] ? joints_[i]->GetCurrentAngle() : 0.0f; + } + + // 正运动学：计算当前末端位姿 + Arm6dof_ForwardKinematics(&q, &end_effector_.current); + + // 计算重力补偿力矩 + if (control_.gravity_comp_enable) { + CalcGravityTorques(q); + } + + return 0; + } + + /** + * @brief 计算当前姿态下各关节的重力补偿力矩 + * @param q 当前关节角度（由Update()统一读取后传入，避免重复读取） + * @details 基于牛顿-欧拉逆动力学，设速度和加速度为零， + * 求解的力矩即为平衡重力所需的各关节力矩 + */ + int8_t CalcGravityTorques(const Arm6dof_JointAngles_t& q) { + float raw_torques[JOINT_NUM]; + int ret = Arm6dof_GravityCompensation(&q, raw_torques); + if (ret != 0) return ret; + + for (size_t i = 0; i < JOINT_NUM; ++i) { + gravity_comp_.torques[i] = raw_torques[i] * gravity_comp_.scale; + } + return 0; + } + + /** + * @brief 轨迹规划核心：每帧插值一小步并对插值点求IK + * + * 每帧执行流程： + * 1. 按速度限制计算本帧 t 步进量 Δt = v_max * dt / 总距离 + * 2. P(t) = start + t*(goal-start) 线性插值得到本帧中间目标 + * 3. IK(P(t), q_init=当前关节角) → 初始猜测与结果相差极小，必然收敛 + * 4. 验证 + 写入 inverseKinematics_.angles + */ + void StepTrajectory() { + if (!traj_.active) return; + constexpr float TWO_PI = 2.0f * (float)M_PI; + + // 计算总距离（用于将速度限制转换为 t 步进量） + float dx = traj_.goal.x - traj_.start.x; + float dy = traj_.goal.y - traj_.start.y; + float dz = traj_.goal.z - traj_.start.z; + float pos_dist = sqrtf(dx*dx + dy*dy + dz*dz); + + float dr = traj_.goal.roll - traj_.start.roll; + float dp = traj_.goal.pitch - traj_.start.pitch; + float dyw = traj_.goal.yaw - traj_.start.yaw; + // 姿态差 wrap 到 (-π, π]：确保插值走最短路径，避免跨越 r2rpy 值域边界时绕大弯 + dr -= roundf(dr / TWO_PI) * TWO_PI; + dp -= roundf(dp / TWO_PI) * TWO_PI; + dyw -= roundf(dyw / TWO_PI) * TWO_PI; + float ang_dist = sqrtf(dr*dr + dp*dp + dyw*dyw); + + // Δt = v_max * dt / 总距离 + // 物理意义：以限定速度匀速运动，谁先到达限制谁决定步进 + // 例：v=0.15m/s，dt=2ms，dist=0.3m → Δt=0.001，需1000帧=2s走完 + float dt_pos = (pos_dist > 1e-6f) ? (traj_.max_lin_vel * timer_.dt / pos_dist) : 1.0f; + float dt_ang = (ang_dist > 1e-6f) ? (traj_.max_ang_vel * timer_.dt / ang_dist) : 1.0f; + float dt_step = (dt_pos < dt_ang) ? dt_pos : dt_ang; // 取小值（慢者为约束） + + traj_.t += dt_step; + if (traj_.t >= 1.0f) { + traj_.t = 1.0f; + traj_.active = false; // 到达终点 + } + + // 线性插值：P(t) = start + t*(goal-start) + Arm6dof_Pose_t interp; + interp.x = traj_.start.x + traj_.t * dx; + interp.y = traj_.start.y + traj_.t * dy; + interp.z = traj_.start.z + traj_.t * dz; + interp.roll = traj_.start.roll + traj_.t * dr; + interp.pitch = traj_.start.pitch + traj_.t * dp; + interp.yaw = traj_.start.yaw + traj_.t * dyw; + + // IK初始猜测 = 当前关节角（与上一帧解相差极小，牛顿法必然收敛到正确分支） + Arm6dof_JointAngles_t q_init; + for (size_t i = 0; i < JOINT_NUM; ++i) { + q_init.q[i] = joints_[i] ? joints_[i]->GetCurrentAngle() : 0.0f; + } + + if (Arm6dof_InverseKinematics(&interp, &q_init, &traj_.angles, 0.001f, 50) != 0) { + traj_.valid = false; + traj_.active = false; + control_.state = MotionState::ERROR; + return; + } + + // 角度连续性修正：将IK解调整到与当前关节角最近的等效角 + // 仅对范围跨度 > 2π 的关节（如 J1: [-15.7, 15.7]）有意义： + // IK 求解器对多圈关节可能返回与当前角差 2π 整数倍的等效解，导致跳变。 + // 通过将差值 wrap 到 (-π, π] 选取最近分支解决此问题。 + // 对范围 ≤ 2π 的关节（如 J4/J6: [0, 2π]）： + // 每个角度值在物理上唯一，不存在 2π 等效分支，直接信任 IK 结果，跳过修正。 + for (size_t i = 0; i < JOINT_NUM; ++i) { + if (!joints_[i]) continue; + const auto& dh = joints_[i]->GetDHParams(); + // 关节范围跨度不足 2π——无等效分支，无需修正 + if ((dh.qmax - dh.qmin) <= TWO_PI + 0.1f) continue; + + float q_ik = traj_.angles.q[i]; + float q_cur = q_init.q[i]; + float diff = q_ik - q_cur; + // roundf(diff / 2π) * 2π = 最近的 2π 整数倍，减去后差值落在 (-π, π] + diff -= roundf(diff / TWO_PI) * TWO_PI; + traj_.angles.q[i] = q_cur + diff; + } + + // 限位检查 + for (size_t i = 0; i < JOINT_NUM; ++i) { + if (joints_[i]) { + const auto& dh = joints_[i]->GetDHParams(); + if (traj_.angles.q[i] < (dh.qmin-qLimitTolerance) || + traj_.angles.q[i] > (dh.qmax+qLimitTolerance)) { + traj_.valid = false; + traj_.active = false; + control_.state = MotionState::ERROR; + return; + } + } + } + + // // 突变检查：轨迹模式下每帧步长极小，若仍触发突变说明严重异常 + // // 阈值比直接IK时更严：max_lin_vel*dt对应的关节角变化通常远小于0.3rad + // static constexpr float TRAJ_MAX_JOINT_DELTA = 0.3f; // rad ≈ 17°/帧 + // for (size_t i = 0; i < JOINT_NUM; ++i) { + // if (joints_[i]) { + // float delta = fabsf(traj_.angles.q[i] - + // joints_[i]->GetCurrentAngle()); + // if (delta > TRAJ_MAX_JOINT_DELTA) { + // traj_.valid = false; + // traj_.active = false; + // control_.state = MotionState::ERROR; + // return; + // } + // } + // } + + traj_.valid = true; + end_effector_.target = interp; + } + + int8_t GetPose(Arm6dof_Pose_t* pose) const { + if (pose == nullptr) { + return -1; + } + std::memcpy(pose, &end_effector_.current, sizeof(Arm6dof_Pose_t)); + return 0; + } + + int8_t SetTargetPose(const Arm6dof_Pose_t pose) { + end_effector_.target = pose; + return 0; + } + + int8_t GetTargetPose(Arm6dof_Pose_t* pose) const { + if (pose == nullptr) { + return -1; + } + std::memcpy(pose, &end_effector_.target, sizeof(Arm6dof_Pose_t)); + return 0; + } + + // 控制循环 + int8_t Control() { + // 更新时间 + uint64_t now_us = BSP_TIME_Get_us(); + timer_.dt = (now_us - timer_.last_wakeup) / 1000000.0f; + timer_.last_wakeup = now_us; + timer_.now = now_us / 1000000.0f; + + // 未使能则松弛 + if (!control_.enable) { + for (Joint* joint : joints_) { // 遍历关节数组 + if (joint) joint->Relax(); + } + control_.state = MotionState::STOPPED; + return 0; + } + + // 根据控制模式执行 + switch (control_.mode) { + case ControlMode::IDLE: + for (Joint* joint : joints_) { // 遍历关节数组 + if (joint) joint->Relax(); + } + control_.state = MotionState::STOPPED; + break; + + case ControlMode::JOINT_POSITION: + case ControlMode::CARTESIAN_POSITION: + case ControlMode::GRAVITY_COMP: { + // 检查错误状态（不可达或超限） + if (control_.state == MotionState::ERROR) { + // 进入错误状态，松弛所有关节（安全停止） + for (Joint* joint : joints_) { + if (joint) joint->Relax(); + } + control_.enable = false; // 自动禁用 + return -1; + } + + // 将重力补偿力矩写入各关节前馈 + if (control_.gravity_comp_enable) { + for (size_t i = 0; i < JOINT_NUM; ++i) { + if (joints_[i]) { + joints_[i]->SetFeedforwardTorque(gravity_comp_.torques[i]); + } + } + } + + // GRAVITY_COMP 模式：保持当前位置 + 重力补偿 + if (control_.mode == ControlMode::GRAVITY_COMP) { + for (size_t i = 0; i < JOINT_NUM; ++i) { + if (joints_[i]) { + joints_[i]->SetTargetAngle(joints_[i]->GetCurrentAngle()); + } + } + } + + // CARTESIAN_POSITION 模式：每帧推进轨迹一小步，再应用IK解到关节 + if (control_.mode == ControlMode::CARTESIAN_POSITION) { + // 轨迹规划核心调用：插值 + IK，结果写入 traj_ + StepTrajectory(); + + if (traj_.valid) { + for (size_t i = 0; i < JOINT_NUM; ++i) { + if (joints_[i]) { + joints_[i]->SetTargetAngle(traj_.angles.q[i]); + } + } + } else { + // IK解无效：松弛所有关节并进入错误状态 + for (Joint* joint : joints_) { + if (joint) joint->Relax(); + } + control_.state = MotionState::ERROR; + return -1; + } + } + + // 位置控制 + bool all_reached = true; + for (Joint* joint : joints_) { // 遍历关节数组 + if (joint) { + joint->PositionControl(joint->GetTargetAngle(), timer_.dt); + if (!joint->IsReached(control_.position_tolerance)) { + all_reached = false; + } + } + } + control_.state = all_reached ? MotionState::REACHED : MotionState::MOVING; + break; + } + + case ControlMode::TEACH: { + // 示教模式：仅施加重力补偿力矩，不施加位置刚度 + // 操作者可以自由拖动机械臂 + if (control_.gravity_comp_enable) { + for (size_t i = 0; i < JOINT_NUM; ++i) { + if (joints_[i]) { + joints_[i]->TorqueControl(gravity_comp_.torques[i]); + } + } + } else { + // 未使能重力补偿时，示教模式下松弛 + for (Joint* joint : joints_) { + if (joint) joint->Relax(); + } + } + control_.state = MotionState::MOVING; + break; + } + + default: + control_.state = MotionState::ERROR; + break; + } + + return 0; + } + + // ======================================================================== + // 高层API + // ======================================================================== + + void Enable(bool enable) { + control_.enable = enable; + if (!enable) { + control_.mode = ControlMode::IDLE; + } + } + + void SetMode(ControlMode mode) { + control_.mode = mode; + } + + MotionState GetState() const { + return control_.state; + } + + /** + * @brief 使能/禁用重力补偿 + * @param enable true=使能, false=禁用 + */ + void EnableGravityCompensation(bool enable) { + control_.gravity_comp_enable = enable; + if (!enable) { + // 清除补偿力矩 + for (size_t i = 0; i < JOINT_NUM; ++i) { + gravity_comp_.torques[i] = 0.0f; + if (joints_[i]) joints_[i]->SetFeedforwardTorque(0.0f); + } + } + } + + /** + * @brief 设置重力补偿比例系数（用于微调） + * @param scale 补偿系数，1.0=完全补偿，<1.0=欠补偿，>1.0=过补偿 + */ + void SetGravityCompScale(float scale) { + gravity_comp_.scale = scale; + } + + /** + * @brief 获取指定关节的重力补偿力矩 + */ + float GetGravityTorque(size_t index) const { + return (index < JOINT_NUM) ? gravity_comp_.torques[index] : 0.0f; + } + + bool IsGravityCompEnabled() const { + return control_.gravity_comp_enable; + } + + // 关节空间控制（直接设置目标关节角度） + int8_t MoveJoint(const float target_angles[JOINT_NUM]) { + // 1. 检查限位 + for (size_t i = 0; i < JOINT_NUM; ++i) { + if (joints_[i]) { + const auto& dh = joints_[i]->GetDHParams(); + if (target_angles[i] < dh.qmin || target_angles[i] > dh.qmax) { + return -1; // 超限 + } + } + } + + // 2. 设置目标角度到各关节 + for (size_t i = 0; i < JOINT_NUM; ++i) { + if (joints_[i]) { + joints_[i]->SetTargetAngle(target_angles[i]); + } + } + + control_.state = MotionState::MOVING; + return 0; + } + + /** + * @brief 设置笛卡尔空间目标位姿，自动启动轨迹规划 + * 目标发生变化时，从当前末端位姿重新规划轨迹。 + * 实际运动由 Control() 每帧调用 StepTrajectory() 推进。 + * + * @param goal 目标末端位姿 + */ + int8_t MoveCartesian(const Arm6dof_Pose_t& goal) { + // 检测目标是否发生有效变化（避免每帧重置轨迹） + constexpr float POS_THRESH = 0.001f; // 1mm + constexpr float ANG_THRESH = 0.01f; // ~0.57° + bool goal_changed = + fabsf(goal.x - traj_.goal.x) > POS_THRESH || + fabsf(goal.y - traj_.goal.y) > POS_THRESH || + fabsf(goal.z - traj_.goal.z) > POS_THRESH || + fabsf(goal.roll - traj_.goal.roll) > ANG_THRESH || + fabsf(goal.pitch - traj_.goal.pitch) > ANG_THRESH || + fabsf(goal.yaw - traj_.goal.yaw) > ANG_THRESH; + + if (goal_changed) { + // 从当前末端位姿出发重新规划轨迹 + traj_.start = end_effector_.current; + traj_.goal = goal; + traj_.t = 0.0f; + traj_.active = true; + traj_.valid = true; // 允许Control()进入，等待第一帧StepTrajectory + control_.state = MotionState::MOVING; + } + return 0; + } + + // 急停 + void Stop() { + control_.state = MotionState::STOPPED; + } + + /** + * @brief 工具坐标系目标位姿控制 + * + * 与 MoveCartesian 的区别： + * - MoveCartesian ：输入世界系绝对位姿 + * - MoveCartesianTool：输入工具坐标系下的目标位姿，内部变换到世界系 + * + * 变换原理： + * 位置： p_world = R_cur * [x_t, y_t, z_t]^T + * 姿态： R_world = R_cur * R_target + * + * @param target_tool 工具坐标系下的目标位姿（绝对值） + * .x/.y/.z ：工具系下的目标位置 (m) + * .roll/.pitch/.yaw ：工具系下的目标姿态 (rad) + */ + void MoveCartesianTool(const Arm6dof_Pose_t& target_tool) { + const Arm6dof_Pose_t& cur = end_effector_.current; + + // ── R_cur = Rz(yaw)*Ry(pitch)*Rx(roll) ── + float rpy_cur_arr[3] = {cur.yaw, cur.pitch, cur.roll}; + Matrixf<3,3> R = robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr)); + + // ── 位置：p_world = R_cur * p_tool（绝对目标，工具系坐标→世界系） ── + Arm6dof_Pose_t goal; + float p_arr[3] = {target_tool.x, target_tool.y, target_tool.z}; + Matrixf<3,1> p_world = R * Matrixf<3,1>(p_arr); + goal.x = p_world[0][0]; + goal.y = p_world[1][0]; + goal.z = p_world[2][0]; + + // ── 姿态：R_world = R_cur * R_target（工具系绝对姿态→世界系） ── + float rpy_target_arr[3] = {target_tool.yaw, target_tool.pitch, target_tool.roll}; + Matrixf<3,3> Rn = R * robotics::rpy2r(Matrixf<3,1>(rpy_target_arr)); + + // ── R_new → RPY，写入目标位姿 ── + Matrixf<3,1> rpy_new = robotics::r2rpy(Rn); // [yaw; pitch; roll] + goal.yaw = rpy_new[0][0]; + goal.pitch = rpy_new[1][0]; + goal.roll = rpy_new[2][0]; + + MoveCartesian(goal); + } + + /** + * @brief 航向参考系目标位姿控制 + * + * 位置（航向系）：世界系绕Z轴旋转当前yaw角度得到，X/Y始终水平，Z始终世界竖直 + * 姿态（航向系）：roll/pitch/yaw 为末端在航向系下的绝对目标姿态，直观地对应末端各轴旋转 + * + * 变换原理： + * 位置 X/Y：p_world = Rz(yaw) * [x, y, 0]^T + * 位置 Z ：p_world.z = z (世界竖直直接赋值) + * 姿态 ：R_world = Rz(yaw) * R_target （航向系下绝对姿态→世界系） + * + * @param target_heading + * .x/.y ：航向系（水平）下的目标位置 (m) + * .z ：世界竖直方向目标高度 (m) + * .roll/.pitch/.yaw ：航向系下的末端绝对姿态 (rad) + * 直观含义：yaw转动就是末端 yaw，pitch/roll 同理 + */ + void MoveCartesianHeading(const Arm6dof_Pose_t& target_heading) { + const Arm6dof_Pose_t& cur = end_effector_.current; + + // ── 位置：航向系→世界系，Rz(yaw) 旋转水平分量，Z 直接为世界Z ── + float cy = cosf(cur.yaw), sy = sinf(cur.yaw); + Arm6dof_Pose_t goal; + goal.x = cy * target_heading.x - sy * target_heading.y; // 世界X + goal.y = sy * target_heading.x + cy * target_heading.y; // 世界Y + goal.z = target_heading.z; // 世界Z 直接赋值 + + // ── 姿态：航向系下绝对姿态→世界系：R_world = Rz(yaw) * R_target ── + float rpy_yaw_arr[3] = {cur.yaw, 0.0f, 0.0f}; + float rpy_target_arr[3] = {target_heading.yaw, target_heading.pitch, target_heading.roll}; + Matrixf<3,3> Rn = robotics::rpy2r(Matrixf<3,1>(rpy_yaw_arr)) + * robotics::rpy2r(Matrixf<3,1>(rpy_target_arr)); + + // ── R_new → RPY，写入目标位姿 ── + Matrixf<3,1> rpy_new = robotics::r2rpy(Rn); // [yaw; pitch; roll] + goal.yaw = rpy_new[0][0]; + goal.pitch = rpy_new[1][0]; + goal.roll = rpy_new[2][0]; + + MoveCartesian(goal); + } + + // 获取末端位姿（FK结果，单位 m/rad） + const Arm6dof_Pose_t& GetEndPose() const { + return end_effector_.current; + } + + // 获取最近一次 IK 解算结果 + const Arm6dof_JointAngles_t& GetIkAngles() const { + return inverseKinematics_.angles; + } + + // 获取最近一次 IK 是否有效 + bool IsIkValid() const { + return inverseKinematics_.valid; + } + + /** 设置末端线速度限制 (m/s)，默认 0.15 m/s */ + void SetLinVelLimit(float vel) { traj_.max_lin_vel = vel; } + + /** 设置末端角速度限制 (rad/s)，默认 1.0 rad/s */ + void SetAngVelLimit(float vel) { traj_.max_ang_vel = vel; } + + /** 获取当前轨迹进度 [0.0, 1.0]，1.0 表示已到达目标 */ + float GetTrajProgress() const { return traj_.t; } + + /** 轨迹是否正在执行（false 表示已到达目标或尚未启动） */ + bool IsTrajActive() const { return traj_.active; } + + /** 清除错误状态并同步目标位姿到当前位姿（错误恢复用） */ + void ResetError() { + control_.state = MotionState::REACHED; + control_.enable = true; + inverseKinematics_.valid = true; + traj_.valid = true; + traj_.active = false; + + Arm6dof_JointAngles_t q; + for (size_t i = 0; i < JOINT_NUM; ++i) { + q.q[i] = joints_[i] ? joints_[i]->GetCurrentAngle() : 0.0f; + } + for (size_t i = 0; i < JOINT_NUM; ++i) { + if (joints_[i]) { + joints_[i]->SetTargetAngle(joints_[i]->GetCurrentAngle()); + } + } + inverseKinematics_.angles = q; + + + // 将轨迹目标同步为当前位姿，防止下次启动时从旧目标出发 + traj_.goal = end_effector_.current; + end_effector_.target = end_effector_.current; + } +}; + +} // namespace arm diff --git a/User/module/chassis.c b/User/module/chassis.c new file mode 100644 index 0000000..5de1edb --- /dev/null +++ b/User/module/chassis.c @@ -0,0 +1,314 @@ +/* +µ×ÅÌÄ£×é +*/ + +#include "cmsis_os2.h" +#include +#include "bsp/mm.h" +#include "bsp/can.h" +#include "component/ahrs.h" +#include "device/motor_rm.h" +#include "device/motor.h" +#include "module/chassis.h" +#include "component/PowerControl.h" +#include "config.h" +#include "math.h" +/** + * @brief µ×ÅÌСÍÓÂÝģʽÏà¹Ø²ÎÊý + */ +#define CHASSIS_ROTOR_WZ_MIN 0.6f //СÍÓÂÝ×îСËÙ¶È +#define CHASSIS_ROTOR_WZ_MAX 0.8f //СÍÓÂÝ×î´óËÙ¶È +#define M_7OVER72PI (M_2PI * 7.0f / 72.0f) //½Ç¶ÈÆ«ÒÆÁ¿£¨ÓÃÔÚ¸úËæÔÆ̨35¡ã£© +#define CHASSIS_ROTOR_OMEGA 0.001f //½ÇËٶȱ仯ƵÂÊ + + + +/** + * @brief ÉèÖõ×ÅÌģʽ + * @param c µ×Å̽ṹÌåÖ¸Õë + * @param mode Ä¿±ê¿ØÖÆģʽ + * @param now µ±Ç°Ê±¼ä´Á(ms) + * @return CHASSIS_OK:³É¹¦ CHASSIS_ERR_NULL:¿Õ + */ +static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode, uint32_t now) { + if (!c) + return CHASSIS_ERR_NULL; + if (mode == c->mode) + return CHASSIS_OK; +//Ëæ»úÖÖ×Ó£¬Ð¡ÍÓÂÝģʽËæ»úÉèÖÃÐýת·½Ïò + if (mode == CHASSIS_MODE_ROTOR && c->mode != CHASSIS_MODE_ROTOR) { + srand(now); + c->wz_multi = (rand() % 2) ? -1 : 1; + } +//ÖØÖÃPIDºÍÂ˲¨ + for (uint8_t i = 0; i < c->num_wheel; i++) { + PID_Reset(&c->pid.motor[i]); + LowPassFilter2p_Reset(&c->filter.in[i], 0.0f); + LowPassFilter2p_Reset(&c->filter.out[i], 0.0f); + } + + c->mode = mode; + return CHASSIS_OK; +} + +/** + * @brief СÍÓÂÝģʽ¶¯Ì¬½ÇËÙ¶È + * @param min ×îСËÙ¶È + * @param max ×î´óËÙ¶È + * @param now µ±Ç°Ê±¼ä´Á(ms) + * @return ½ÇËÙ¶ÈÖµ + */ +static float Chassis_CalcWz(const float min, const float max, uint32_t now) { + float wz_vary = fabsf(0.2f * sinf(CHASSIS_ROTOR_OMEGA * (float)now)) + min; + return (wz_vary > max) ? max : wz_vary; +} + +/** + * @brief µ×ÅÌģʽ³õʼ»¯ + * @param c µ×Å̽ṹÌåÖ¸Õë + * @param param µ×Å̲ÎÊý½á¹¹ÌåÖ¸Õë + * @param mech_zero »úеÁãµãÅ·À­½Ç + * @param target_freq ¿ØÖÆƵÂÊ(Hz) + * @return CHASSIS_OK:³É¹¦ CHASSIS_ERR_NULL:¿Õ CHASSIS_ERR_TYPE:²»Ö§³ÖµÄģʽ + */ +int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param, + float target_freq) { + if (!c) return CHASSIS_ERR_NULL; + + //³õʼ»¯CANͨÐÅ + BSP_CAN_Init(); + c->param = param; + c->mode = CHASSIS_MODE_RELAX; +//¸ù¾Ýµ×Å̲»Í¬ÉèÖÃģʽÂÖ×ÓÓë»ìºÏÆ÷ + Mixer_Mode_t mixer_mode; + switch (param->type) { + case CHASSIS_TYPE_MECANUM://ÂóÂÖ + c->num_wheel = 4; + mixer_mode = MIXER_MECANUM; + break; + case CHASSIS_TYPE_PARLFIX4: + c->num_wheel = 4; + mixer_mode = MIXER_PARLFIX4; + break; + case CHASSIS_TYPE_PARLFIX2: + c->num_wheel = 2; + mixer_mode = MIXER_PARLFIX2; + break; + case CHASSIS_TYPE_OMNI_CROSS: + c->num_wheel = 4; + mixer_mode = MIXER_OMNICROSS; + break; + case CHASSIS_TYPE_OMNI_PLUS: //È«ÏòÂÖ£¨Àϲ½±øÀàÐÍ£© + c->num_wheel = 4; + mixer_mode = MIXER_OMNIPLUS; + break; + case CHASSIS_TYPE_SINGLE: + c->num_wheel = 1; + mixer_mode = MIXER_SINGLE; + break; + default: + return CHASSIS_ERR_TYPE; + } + //³õʼ»¯Ê±¼ä´Á + c->last_wakeup = 0; + c->dt = 0.0f; + //³õʼ»¯PIDºÍÂ˲¨ + for (uint8_t i = 0; i < c->num_wheel; i++) { + PID_Init(&c->pid.motor[i], KPID_MODE_NO_D, target_freq, ¶m->pid.motor_pid_param); + LowPassFilter2p_Init(&c->filter.in[i], target_freq, param->low_pass_cutoff_freq.in); + LowPassFilter2p_Init(&c->filter.out[i], target_freq, param->low_pass_cutoff_freq.out); + //ÇåÁãµç»ú·´À¡ + c->feedback.motor[i].rotor_speed = 0; + c->feedback.motor[i].torque_current = 0; + c->feedback.motor[i].rotor_abs_angle = 0; + c->feedback.motor[i].temp = 0; + } + //³õʼ»¯PIDºÍ»ìºÏÆ÷ + PID_Init(&c->pid.follow, KPID_MODE_CALC_D, target_freq, ¶m->pid.follow_pid_param); + Mixer_Init(&c->mixer, mixer_mode); + //ÇåÁãÔ˶¯ÏòÁ¿ºÍÊä³ö + c->move_vec.vx = c->move_vec.vy = c->move_vec.wz = 0.0f; + for (uint8_t i = 0; i < c->num_wheel; i++) { + c->out.motor[i] = 0.0f; + } + //×¢²á´ó½®µç»ú + for (int i = 0; i < c->num_wheel; i++) { + MOTOR_RM_Register(&(c->param->motor_param[i])); + + } + return CHASSIS_OK; +} + +/** + * @brief ¸üеç»ú·´À¡(IMU+µç»ú״̬) + * @param c µ×Å̽ṹÌåÖ¸Õë + * @param feedback µ×ÅÌ·´À¡Ö¸Õë½á¹¹Ìå + * @return CHASSIS_OK:³É¹¦ CHASSIS_ERR_NULL:¿Õ + */ +int8_t Chassis_UpdateFeedback(Chassis_t *c) { + + + //¸üÐÂËùÓеç»ú·´À¡ + for (uint8_t i = 0; i < c->num_wheel; i++) { + MOTOR_RM_Update(&(c->param->motor_param[i])); + MOTOR_RM_t *rm_motor = MOTOR_RM_GetMotor(&(c->param->motor_param[i])); + c->motors[i] = rm_motor; + MOTOR_RM_t *rm = c->motors[i]; + if (rm_motor != NULL) { + c->feedback.motor[i] = rm_motor->feedback; + }else + { + return CHASSIS_ERR_NULL; + } + } + return CHASSIS_OK; +} + +/** + * @brief µ×Å̵ç»ú¿ØÖÆ + * @param c µ×Å̽ṹÌåÖ¸Õë + * @param c_cmd ¿ØÖÆÃüÁî + * @param now µ±Ç°Ê±¼ä´Á(ms) + * @return CHASSIS_OK:³É¹¦ CHASSIS_ERR_NULL:¿Õ + */ +int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd, uint32_t now) { + if (!c || !c_cmd) return CHASSIS_ERR_NULL; + //¼ÆËã¿ØÖÆÖÜÆÚ + c->dt = (float)(now - c->last_wakeup) / 1000.0f; + c->last_wakeup = now; + if (!isfinite(c->dt) || c->dt <= 0.0f) { + c->dt = 0.001f; + } + if (c->dt < 0.0005f) c->dt = 0.0005f; + if (c->dt > 0.050f) c->dt = 0.050f; + //ÉèÖÃģʽ + Chassis_SetMode(c, c_cmd->mode, now); + //²»Í¬Ä£Ê½Ï¶ÔÓ¦½âË㣨Ô˶¯ÏòÁ¿£© + switch (c->mode) { + case CHASSIS_MODE_BREAK: + c->move_vec.vx = c->move_vec.vy = 0.0f; + break; + case CHASSIS_MODE_INDEPENDENT: + c->move_vec.vx = c_cmd->ctrl_vec.vx; + c->move_vec.vy = c_cmd->ctrl_vec.vy; + break; + default: { //Ò£¿ØÆ÷×ø±ê->»úÌå×ø±êϵ + float beta = c->feedback.encoder_gimbalYawMotor - c->mech_zero; + float cosb = cosf(beta); + float sinb = sinf(beta); + c->move_vec.vx = cosb * c_cmd->ctrl_vec.vx - sinb * c_cmd->ctrl_vec.vy; + c->move_vec.vy = sinb * c_cmd->ctrl_vec.vx + cosb * c_cmd->ctrl_vec.vy; + break; + } + } + //¸ù¾Ýģʽ¼ÆËãµ×Å̽ÇËÙ¶È + switch (c->mode) { + case CHASSIS_MODE_RELAX: + case CHASSIS_MODE_BREAK: + case CHASSIS_MODE_INDEPENDENT: + c->move_vec.wz = 0.0f; + break; + case CHASSIS_MODE_OPEN: + c->move_vec.wz = c_cmd->ctrl_vec.wz; + break; + //ÔÆ̨¸úËæ + case CHASSIS_MODE_FOLLOW_GIMBAL: + c->move_vec.wz = PID_Calc(&c->pid.follow, c->mech_zero, c->feedback.encoder_gimbalYawMotor, 0.0f, c->dt); + break; + //ÔÆ̨¸úË棨ƫÒÆ£© + case CHASSIS_MODE_FOLLOW_GIMBAL_35: + c->move_vec.wz = PID_Calc(&c->pid.follow,c->mech_zero +M_7OVER72PI, c->feedback.encoder_gimbalYawMotor, 0.0f, c->dt); + break; + //СÍÓÂÝ + case CHASSIS_MODE_ROTOR: + c->move_vec.wz = c->wz_multi * Chassis_CalcWz(CHASSIS_ROTOR_WZ_MIN,CHASSIS_ROTOR_WZ_MAX, now); + break; + } + //Ô˶¯Ñ§Äæ½âË㣬Ô˶¯ÏòÁ¿·Ö½âΪµç»úתËÙ + Mixer_Apply(&c->mixer, &c->move_vec, c->setpoint.motor_rpm, c->num_wheel, 500.0f); + + + for (uint8_t i = 0; i < c->num_wheel; i++) { + float rf = c->setpoint.motor_rpm[i];///Ä¿±êתËÙ + float fb = LowPassFilter2p_Apply(&c->filter.in[i], (float)c->feedback.motor[i].rotor_speed); + float out_current; + switch (c->mode) { + case CHASSIS_MODE_BREAK: + case CHASSIS_MODE_FOLLOW_GIMBAL: + case CHASSIS_MODE_FOLLOW_GIMBAL_35: + case CHASSIS_MODE_ROTOR: + case CHASSIS_MODE_INDEPENDENT: + out_current = PID_Calc(&c->pid.motor[i], c->setpoint.motor_rpm[i], fb, 0.0f, c->dt); + break; + case CHASSIS_MODE_OPEN: + out_current = c->setpoint.motor_rpm[i] / 7000.0f; + break; + case CHASSIS_MODE_RELAX: + out_current = 0.0f; + break; + } + + //µÍͨÂ˲¨ºÍÏÞ·ù + c->out.motor[i] = LowPassFilter2p_Apply(&c->filter.out[i], out_current); + Clip(&c->out.motor[i], -c->param->limit.max_current, c->param->limit.max_current); + } + + + return CHASSIS_OK; +} + + + +void Chassis_Power_Control(Chassis_t *c,float max_power) +{ + float* rpm=(float[4]){c->motors[0]->feedback.rotor_speed,c->motors[1]->feedback.rotor_speed,c->motors[2]->feedback.rotor_speed,c->motors[3]->feedback.rotor_speed}; + power_model_t* param = (c->mode == CHASSIS_MODE_ROTOR) ? &cha2 : &cha; + power_calu(param,(float[4]){c->out.motor[0],c->out.motor[1],c->out.motor[2],c->out.motor[3]},rpm); + float scale = power_scale_calu(¶m,1,max_power); + power_out_calu(param,scale,(float[4]){c->out.motor[0],c->out.motor[1],c->out.motor[2],c->out.motor[3]},rpm,c->out.motor); +} + + +/** + * @brief µç»úÊä³ö + * @param c µ×Å̽ṹÌåÖ¸Õë + */ +void Chassis_Output(Chassis_t *c) { + if (!c) + return; + + for (uint8_t i = 0; i < c->num_wheel; i++) { + MOTOR_RM_t *rm = c->motors[i]; + if (!rm) continue; + MOTOR_RM_SetOutput(&rm->param, c->out.motor[i]); + } + + //µ÷ÓÃctrl + // for (uint8_t i = 0; i < c->num_wheel; i++) { + // MOTOR_RM_t *rm = c->motors[0]; + // if (rm) { + // MOTOR_RM_Ctrl(&rm->param); + // } + // } + + MOTOR_RM_t *rm = c->motors[0]; + if (rm) { + MOTOR_RM_Ctrl(&rm->param); + } +} + +/** + * @brief ÖØÖõ×ÅÌÊä³ö + * @param c µ×Å̽ṹÌåÖ¸Õë + */ +void Chassis_ResetOutput(Chassis_t *c) { + if (!c) return; + for (uint8_t i = 0; i < c->num_wheel; i++) { + MOTOR_RM_t *m = c->motors[i]; + if (m) { + MOTOR_RM_Relax(&(m->param)); + } + } +} + + diff --git a/User/module/chassis.h b/User/module/chassis.h new file mode 100644 index 0000000..ef3c5ca --- /dev/null +++ b/User/module/chassis.h @@ -0,0 +1,239 @@ +/* + µ×ÅÌÄ£×é + */ +#pragma once + + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include "bsp/can.h" +#include "component/filter.h" +#include "component/mixer.h" +#include "component/pid.h" +#include "component/ahrs.h" +#include "device/motor_rm.h" +/* Exported constants ------------------------------------------------------- */ +#define CHASSIS_OK (0) /* ÔËÐÐÕý³£ */ +#define CHASSIS_ERR (-1) /* ÔËÐÐʱ³öÏÖÁËһЩС´íÎó */ +#define CHASSIS_ERR_NULL (-2) /* ÔËÐÐʱ·¢ÏÖNULLÖ¸Õë */ +#define CHASSIS_ERR_MODE (-3) /* ÔËÐÐʱ³öÅäÖÃÁË´íÎóµÄChassisMode_t */ +#define CHASSIS_ERR_TYPE (-4) /* ÔËÐÐʱ³öÅäÖÃÁË´íÎóµÄChassis_Type_t */ + +#define MAX_MOTOR_CURRENT 20.0f +/* µ×ÅÌ¿ØÖÆģʽ */ +typedef enum { + CHASSIS_MODE_RELAX, /* ·ÅËÉģʽ,µç»ú²»Êä³ö¡£Ò»°ãÇé¿öµ×Å̳õʼ»¯Ö®ºóµÄģʽ */ + CHASSIS_MODE_BREAK, /* ɲ³µÄ£Ê½£¬µç»ú±Õ»·¿ØÖƾ²Ö¹¡£ÓÃÓÚ»úÆ÷ÈËֹͣ״̬ */ + CHASSIS_MODE_FOLLOW_GIMBAL, /* ͨ¹ý±Õ»·¿ØÖÆʹ³µÍ··½Ïò¸úËæÔÆ̨ */ + CHASSIS_MODE_FOLLOW_GIMBAL_35, /* ͨ¹ý±Õ»·¿ØÖÆʹ³µÍ··½Ïò35¡ã¸úËæÔÆ̨ */ + CHASSIS_MODE_ROTOR, /* СÍÓÂÝģʽ£¬Í¨¹ý±Õ»·¿ØÖÆʹµ×Å̲»Í£Ðýת */ + CHASSIS_MODE_INDEPENDENT, /*¶ÀÁ¢Ä£Ê½¡£µ×ÅÌÔËÐв»ÊÜÔÆ̨ӰÏì */ + CHASSIS_MODE_OPEN, /* ¿ª»·Ä£Ê½¡£µ×ÅÌÔËÐв»ÊÜPID¿ØÖÆ£¬Ö±½ÓÊä³öµ½µç»ú */ +} Chassis_Mode_t; + +/* СÍÓÂÝת¶¯Ä£Ê½ */ +typedef enum { + ROTOR_MODE_CW, /* ˳ʱÕëת¶¯ */ + ROTOR_MODE_CCW, /* ÄæʱÕëת¶¯ */ + ROTOR_MODE_RAND, /* Ëæ»úת¶¯ */ +} Chassis_RotorMode_t; + +/* µ×ÅÌ¿ØÖÆÃüÁî */ +typedef struct { + Chassis_Mode_t mode; /* µ×ÅÌÔËÐÐģʽ */ + Chassis_RotorMode_t mode_rotor; /* СÍÓÂÝת¶¯Ä£Ê½ */ + MoveVector_t ctrl_vec; /* µ×ÅÌ¿ØÖÆÏòÁ¿ */ +} Chassis_CMD_t; + +/* ÏÞλ */ +typedef struct { + float max; + float min; +} Chassis_Limit_t; + + /* µ×ÅÌÀàÐÍ£¨µ×Å̵ÄÎïÀíÉè¼Æ£© */ +typedef enum { + CHASSIS_TYPE_MECANUM, /* Âó¿ËÄÉÄ·ÂÖ */ + CHASSIS_TYPE_PARLFIX4, /* ƽÐаڷŵÄËĸöÇý¶¯ÂÖ */ + CHASSIS_TYPE_PARLFIX2, /* ƽÐаڷŵÄÁ½¸öÇý¶¯ÂÖ */ + CHASSIS_TYPE_OMNI_CROSS, /* ²æÐͰڷŵÄËĸöÈ«ÏòÂÖ */ + CHASSIS_TYPE_OMNI_PLUS, /* Ê®×ÖÐÍ°ÚÉèµÄËĸöÈ«ÏòÂÖ */ + CHASSIS_TYPE_DRONE, /* ÎÞÈË»úµ×ÅÌ */ + CHASSIS_TYPE_SINGLE, /* µ¥¸öĦ²ÁÂÖ */ +} Chassis_Type_t; + + +/* µ×Å̲ÎÊý½á¹¹Ìå,ALL³õʼ»¯²ÎÊý */ +typedef struct { + MOTOR_RM_Param_t motor_param[4]; + struct { + KPID_Params_t motor_pid_param; /* µ×Å̵ç»úPID²ÎÊý */ + KPID_Params_t follow_pid_param; /* ¸úËæÔÆ̨PID²ÎÊý */ + } pid; + Chassis_Type_t type; /* µ×ÅÌÀàÐÍ£¬µ×Å̵ĻúеÉè¼ÆºÍÂÖ×ÓÑ¡ÐÍ */ + + /* µÍͨÂ˲¨Æ÷½ØÖÁƵÂÊ*/ + struct { + float in; /* ÊäÈë */ + float out; /* Êä³ö */ + } low_pass_cutoff_freq; + + /* µç»ú·´×°£¬Ó¦¸ÃºÍÔÆ̨ÉèÖÃÏàͬ*/ + struct { + bool yaw; + } reverse; + struct { + float max_vx, max_vy, max_wz; + float max_current; + } limit; +} Chassis_Params_t; + +typedef struct { + AHRS_Gyro_t gyro; + AHRS_Eulr_t eulr; +} Chassis_IMU_t; + +typedef struct { + MOTOR_Feedback_t motor[4]; // Ëĸö 3508µç»ú ·´À¡ + float encoder_gimbalYawMotor; +} Chassis_Feedback_t; + +/* µ×ÅÌÊä³ö½á¹¹Ìå*/ +typedef struct { + float motor[4]; +} Chassis_Output_t; + +/* + * ÔËÐеÄÖ÷½á¹¹Ìå£þ + * °üº¬³õʼ»¯²ÎÊý,Öмä±äÁ¿,Êä³ö±äÁ¿ + */ +typedef struct { + uint64_t last_wakeup; + float dt; + + Chassis_Params_t *param; /* µ×Å̲ÎÊý,ÓÃChassis_InitÉ趨 */ + + /* Ä£¿éͨÓà */ + Chassis_Mode_t mode; /* µ×ÅÌģʽ */ + + + /* µ×ÅÌÉè¼Æ */ + int8_t num_wheel; /* µ×ÅÌÂÖ×ÓÊýÁ¿ */ + Mixer_t mixer; /* »ìºÏÆ÷,Òƶ¯ÏòÁ¿->µç»úÄ¿±êÖµ */ + MoveVector_t move_vec; /* µ×ÅÌʵ¼ÊµÄÔ˶¯ÏòÁ¿ */ + MOTOR_RM_t *motors[4];/*Ö¸Ïòµ×ÅÌÿ¸öµç»ú²ÎÊý*/ + float mech_zero; + float wz_multi; /* СÍÓÂÝÐýתģʽ */ + + /* PID¼ÆËãÄ¿±êÖµ */ + struct { + float motor_rpm[4]; /* µç»úתËٵĶ¯Ì¬Êý×é,µ¥Î»:RPM */ + } setpoint; + + /* ·´À¡¿ØÖÆÓõÄPID */ + struct { + KPID_t motor[4]; /* ¿ØÖÆÂÖ×Óµç»úÓõÄPIDµÄ¶¯Ì¬Êý×é */ + KPID_t follow; /* ¸úËæÔÆ̨ÓõÄPID */ + } pid; + + struct { + Chassis_Limit_t vx, vy, wz; + } limit; + + /* Â˲¨Æ÷ */ + struct { + LowPassFilter2p_t in[4]; /* ·´À¡ÖµÂ˲¨Æ÷ */ + LowPassFilter2p_t out[4]; /* Êä³öÖµÂ˲¨Æ÷ */ + } filter; + + Chassis_Output_t out; /* µç»úÊä³ö */ + Chassis_Feedback_t feedback; + //float out_motor[4]; +} Chassis_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * \brief µ×Å̳õʼ»¯ + * + * \param c °üº¬µ×ÅÌÊý¾ÝµÄ½á¹¹Ìå + * \param param °üº¬µ×Å̲ÎÊýµÄ½á¹¹ÌåÖ¸Õë + * \param target_freq ÈÎÎñÔ¤ÆÚµÄÔËÐÐƵÂÊ + * + * \return ÔËÐнá¹û + */ +int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param, + float target_freq); + +/** + * \brief ¸üе×ÅÌ·´À¡ÐÅÏ¢ + * + * \param c °üº¬µ×ÅÌÊý¾ÝµÄ½á¹¹Ìå + * \param can CANÉ豸½á¹¹Ìå + * + * \return ÔËÐнá¹û + */ +int8_t Chassis_UpdateFeedback(Chassis_t *c); + +/** + * \brief ÔËÐе×ÅÌ¿ØÖÆÂß¼­ + * + * \param c °üº¬µ×ÅÌÊý¾ÝµÄ½á¹¹Ìå + * \param c_cmd µ×ÅÌ¿ØÖÆÖ¸Áî + * \param dt_sec Á½´Îµ÷ÓõÄʱ¼ä¼ä¸ô + * + * \return ÔËÐнá¹û + */ +int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd, + uint32_t now); + + +/** + * \brief ¸´ÖƵ×ÅÌÊä³öÖµ + * + * \param s °üº¬µ×ÅÌÊý¾ÝµÄ½á¹¹Ìå + * \param out CANÉ豸µ×ÅÌÊä³ö½á¹¹Ìå + */ +void Chassis_Output(Chassis_t *c); + +/** + * \brief Çå¿ÕChassisÊä³öÊý¾Ý + * + * \param out CANÉ豸µ×ÅÌÊä³ö½á¹¹Ìå + */ +void Chassis_ResetOutput(Chassis_t *c); + + +void Chassis_Power_Control(Chassis_t *c,float max_power); +/** + * @brief µ×Å̹¦ÂÊÏÞÖÆ + * + * @param c µ×ÅÌÊý¾Ý + * @param cap µçÈÝÊý¾Ý + * @param ref ²ÃÅÐϵͳÊý¾Ý + * @return º¯ÊýÔËÐнá¹û + */ +//»¹Ã»ÓмÓÈ룬waiting¡£¡£¡£¡£¡£¡£int8_t Chassis_PowerLimit(Chassis_t *c, const CAN_Capacitor_t *cap, +// const Referee_ForChassis_t *ref); + + +/** + * @brief µ¼³öµ×ÅÌÊý¾Ý + * + * @param chassis µ×ÅÌÊý¾Ý½á¹¹Ìå + * @param ui UIÊý¾Ý½á¹¹Ìå + */ +//void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui); + + +#ifdef __cplusplus +} +#endif + + + + diff --git a/User/module/cmd/cmd.c b/User/module/cmd/cmd.c new file mode 100644 index 0000000..7969c94 --- /dev/null +++ b/User/module/cmd/cmd.c @@ -0,0 +1,502 @@ +/* + * CMD 模块 V2 - 主控制模块实现 + */ +#include "cmd.h" +#include "bsp/time.h" +#include +#include + +/* ========================================================================== */ +/* 命令构建函数 */ +/* ========================================================================== */ + +/* 从RC输入生成底盘命令 */ +#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_CHASSIS +static void CMD_RC_BuildChassisCmd(CMD_t *ctx) { + CMD_RCModeMap_t *map = &ctx->config->rc_mode_map; + + /* 根据左拨杆位置选择模式 */ + switch (ctx->input.rc.sw[0]) { + case CMD_SW_UP: + ctx->output.chassis.cmd.mode = map->sw_left_up; + break; + case CMD_SW_MID: + ctx->output.chassis.cmd.mode = map->sw_left_mid; + break; + case CMD_SW_DOWN: + ctx->output.chassis.cmd.mode = map->sw_left_down; + break; + default: + ctx->output.chassis.cmd.mode = CHASSIS_MODE_RELAX; + break; + } + + /* 摇杆控制移动 */ + ctx->output.chassis.cmd.ctrl_vec.vx = ctx->input.rc.joy_right.x; + ctx->output.chassis.cmd.ctrl_vec.vy = ctx->input.rc.joy_right.y; +} +#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_CHASSIS */ + +/* 从 RC 输入生成云台命令 */ +#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_GIMBAL +static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) { + CMD_RCModeMap_t *map = &ctx->config->rc_mode_map; + ctx->output.gimbal.cmd.is_ai = false; + /* 根据拨杆选择云台模式 */ + switch (ctx->input.rc.sw[0]) { + case CMD_SW_UP: + ctx->output.gimbal.cmd.mode = map->gimbal_sw_up; + break; + case CMD_SW_MID: + ctx->output.gimbal.cmd.mode = map->gimbal_sw_mid; + break; + case CMD_SW_DOWN: + ctx->output.gimbal.cmd.mode = map->gimbal_sw_down; + break; + default: + ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX; + break; + } + + /* 左摇杆控制云台 */ + ctx->output.gimbal.cmd.delta_yaw = -ctx->input.rc.joy_left.x * 4.0f; + ctx->output.gimbal.cmd.delta_pit = -ctx->input.rc.joy_left.y * 2.5f; +} +#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_GIMBAL */ + +/* 从 RC 输入生成射击命令 */ +#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_SHOOT +static void CMD_RC_BuildShootCmd(CMD_t *ctx) { + if (ctx->input.online[CMD_SRC_RC]) { + ctx->output.shoot.cmd.mode = SHOOT_MODE_SINGLE; + } else { + ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE; + } + + /* 根据右拨杆控制射击 */ + switch (ctx->input.rc.sw[1]) { + case CMD_SW_DOWN: + ctx->output.shoot.cmd.ready = true; + ctx->output.shoot.cmd.firecmd = true; + break; + case CMD_SW_MID: + ctx->output.shoot.cmd.ready = true; + ctx->output.shoot.cmd.firecmd = false; + break; + case CMD_SW_UP: + default: + ctx->output.shoot.cmd.ready = false; + ctx->output.shoot.cmd.firecmd = false; + break; + } +} +#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_SHOOT */ + +/* 从 RC 输入生成履带命令 */ +#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_TRACK +static void CMD_RC_BuildTrackCmd(CMD_t *ctx) { + CMD_RCModeMap_t *map = &ctx->config->rc_mode_map; + + if (!ctx->input.online[CMD_SRC_RC]) { + ctx->output.track.cmd.enable = false; + ctx->output.track.cmd.vel = 0.0f; + ctx->output.track.cmd.omega = 0.0f; + return; + } + switch (ctx->input.rc.sw[0]) { + case CMD_SW_UP: + ctx->output.track.cmd.enable = map->track_sw_up; + break; + case CMD_SW_MID: + ctx->output.track.cmd.enable = map->track_sw_mid; + break; + case CMD_SW_DOWN: + ctx->output.track.cmd.enable = map->track_sw_down; + break; + default: + ctx->output.track.cmd.enable = false; + break; + } + ctx->output.track.cmd.enable = ctx->input.online[CMD_SRC_RC]; + ctx->output.track.cmd.vel = ctx->input.rc.joy_right.y * 2.0f; + if(fabsf(ctx->input.rc.joy_right.y * 2.0f) > 1.0f) + ctx->output.track.cmd.vel = ctx->output.track.cmd.vel > 0 ? 1.0f + : -1.0f; + + CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_TRACK); +} +#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_TRACK */ + +/* 从PC输入生成底盘命令 */ +#if CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_CHASSIS +static void CMD_PC_BuildChassisCmd(CMD_t *ctx) { + + if (!ctx->input.online[CMD_SRC_PC]) { + ctx->output.chassis.cmd.mode = CHASSIS_MODE_RELAX; + return; + } + + ctx->output.chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL; + + /* WASD控制移动 */ + ctx->output.chassis.cmd.ctrl_vec.vx = 0.0f; + ctx->output.chassis.cmd.ctrl_vec.vy = 0.0f; + CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_CHASSIS); +} +#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_CHASSIS */ + +/* 从PC输入生成云台命令 */ +#if CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_GIMBAL +static void CMD_PC_BuildGimbalCmd(CMD_t *ctx) { + CMD_Sensitivity_t *sens = &ctx->config->sensitivity; + + if (!ctx->input.online[CMD_SRC_PC]) { + ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX; + return; + } + ctx->output.gimbal.cmd.is_ai = false; + ctx->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE; + + /* 鼠标控制云台 */ + ctx->output.gimbal.cmd.delta_yaw = (float)-ctx->input.pc.mouse.x * ctx->timer.dt * sens->mouse_sens; + ctx->output.gimbal.cmd.delta_pit = (float)ctx->input.pc.mouse.y * ctx->timer.dt * sens->mouse_sens * 1.5f; + CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_GIMBAL); +} +#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_GIMBAL */ + +/* 从PC输入生成射击命令 */ +#if CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_SHOOT +static void CMD_PC_BuildShootCmd(CMD_t *ctx) { + if (!ctx->input.online[CMD_SRC_PC]) { + ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE; + return; + } + + ctx->output.shoot.cmd.ready = true; + ctx->output.shoot.cmd.firecmd = ctx->input.pc.mouse.l_click; + + CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_SHOOT); + +} +#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_SHOOT */ + +/* 从PC输入生成履带命令 */ +#if CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_TRACK +static void CMD_PC_BuildTrackCmd(CMD_t *ctx) { + if (!ctx->input.online[CMD_SRC_PC]) { + ctx->output.track.cmd.enable = false; + ctx->output.track.cmd.vel = 0.0f; + ctx->output.track.cmd.omega = 0.0f; + return; + } + + ctx->output.track.cmd.enable = true; + /* 可根据需要添加PC对履带的控制，例如键盘按键 */ + ctx->output.track.cmd.vel = 0.0f; + ctx->output.track.cmd.omega = 0.0f; + + CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_TRACK); +} +#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_TRACK */ + +/* 从NUC/AI输入生成云台命令 */ +#if CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_GIMBAL +static void CMD_NUC_BuildGimbalCmd(CMD_t *ctx) { + if (!ctx->input.online[CMD_SRC_NUC]) { + ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX; + return; + } + + /* 使用AI提供的云台控制数据 */ + + if (ctx->input.nuc.mode!=0) { + ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELATIVE; + ctx->output.gimbal.cmd.is_ai = true; + ctx->output.gimbal.cmd.setpoint_yaw = ctx->input.nuc.gimbal.setpoint.yaw; + ctx->output.gimbal.cmd.setpoint_pit = ctx->input.nuc.gimbal.setpoint.pit; + } + +} +#endif /* CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_GIMBAL */ + +/* 从 NUC/AI 输入生成射击命令 */ +#if CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_SHOOT +static void CMD_NUC_BuildShootCmd(CMD_t *ctx) { + if (!ctx->input.online[CMD_SRC_NUC]) { + ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE; + return; + } + + /* 根据AI模式决定射击行为 */ + switch (ctx->input.nuc.mode) { + case 0: + ctx->output.shoot.cmd.ready = false; + ctx->output.shoot.cmd.firecmd = false; + break; + case 1: + ctx->output.shoot.cmd.ready = true; + ctx->output.shoot.cmd.firecmd = false; + break; + case 2: + if (ctx->input.rc.sw[1]==CMD_SW_DOWN) { + ctx->output.shoot.cmd.ready = true; + ctx->output.shoot.cmd.firecmd = true; + }else { + ctx->output.shoot.cmd.ready = true; + ctx->output.shoot.cmd.firecmd = false; + } + + break; + } +} +#endif /* CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_SHOOT */ + +/* 离线安全模式 */ +static void CMD_SetOfflineMode(CMD_t *ctx) { +#if CMD_ENABLE_MODULE_CHASSIS + ctx->output.chassis.cmd.mode = CHASSIS_MODE_RELAX; +#endif +#if CMD_ENABLE_MODULE_GIMBAL + ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX; +#endif +#if CMD_ENABLE_MODULE_SHOOT + ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE; +#endif +#if CMD_ENABLE_MODULE_TRACK + ctx->output.track.cmd.enable = false; +#endif +} + +/* ========================================================================== */ +/* 公开API实现 */ +/* ========================================================================== */ + +int8_t CMD_Init(CMD_t *ctx, CMD_Config_t *config) { + if (ctx == NULL || config == NULL) { + return CMD_ERR_NULL; + } + + memset(ctx, 0, sizeof(CMD_t)); + ctx->config = config; + + /* 初始化适配器 */ + CMD_Adapter_InitAll(); + + /* 初始化行为处理器 */ + CMD_Behavior_Init(); + + return CMD_OK; +} + +int8_t CMD_UpdateInput(CMD_t *ctx) { + if (ctx == NULL) { + return CMD_ERR_NULL; + } + + /* 保存上一帧输入 */ + memcpy(&ctx->last_input, &ctx->input, sizeof(ctx->input)); + + /* 更新所有输入源 */ + for (int i = 0; i < CMD_SRC_NUM; i++) { + CMD_Adapter_GetInput((CMD_InputSource_t)i, &ctx->input); + } + + return CMD_OK; +} +typedef void (*CMD_BuildCommandFunc)(CMD_t *cmd); +typedef struct { + CMD_InputSource_t source; + CMD_BuildCommandFunc chassisFunc; + CMD_BuildCommandFunc gimbalFunc; + CMD_BuildCommandFunc shootFunc; + CMD_BuildCommandFunc trackFunc; +} CMD_SourceHandler_t; + +/* Build-function conditional references */ +#if CMD_ENABLE_MODULE_CHASSIS && CMD_ENABLE_SRC_RC + #define _FN_RC_CHASSIS CMD_RC_BuildChassisCmd +#else + #define _FN_RC_CHASSIS +#endif +#if CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_SRC_RC + #define _FN_RC_GIMBAL CMD_RC_BuildGimbalCmd +#else + #define _FN_RC_GIMBAL NULL +#endif +#if CMD_ENABLE_MODULE_SHOOT && CMD_ENABLE_SRC_RC + #define _FN_RC_SHOOT CMD_RC_BuildShootCmd +#else + #define _FN_RC_SHOOT NULL +#endif +#if CMD_ENABLE_MODULE_TRACK && CMD_ENABLE_SRC_RC + #define _FN_RC_TRACK CMD_RC_BuildTrackCmd +#else + #define _FN_RC_TRACK NULL +#endif +#if CMD_ENABLE_MODULE_CHASSIS && CMD_ENABLE_SRC_PC + #define _FN_PC_CHASSIS CMD_PC_BuildChassisCmd +#else + #define _FN_PC_CHASSIS NULL +#endif +#if CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_SRC_PC + #define _FN_PC_GIMBAL CMD_PC_BuildGimbalCmd +#else + #define _FN_PC_GIMBAL NULL +#endif +#if CMD_ENABLE_MODULE_SHOOT && CMD_ENABLE_SRC_PC + #define _FN_PC_SHOOT CMD_PC_BuildShootCmd +#else + #define _FN_PC_SHOOT NULL +#endif +#if CMD_ENABLE_MODULE_TRACK && CMD_ENABLE_SRC_PC + #define _FN_PC_TRACK CMD_PC_BuildTrackCmd +#else + #define _FN_PC_TRACK NULL +#endif +#if CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_SRC_NUC + #define _FN_NUC_GIMBAL CMD_NUC_BuildGimbalCmd +#else + #define _FN_NUC_GIMBAL NULL +#endif +#if CMD_ENABLE_MODULE_SHOOT && CMD_ENABLE_SRC_NUC + #define _FN_NUC_SHOOT CMD_NUC_BuildShootCmd +#else + #define _FN_NUC_SHOOT NULL +#endif + +CMD_SourceHandler_t sourceHandlers[CMD_SRC_NUM] = { +#if CMD_ENABLE_SRC_RC + [CMD_SRC_RC] = {CMD_SRC_RC, _FN_RC_CHASSIS, _FN_RC_GIMBAL, _FN_RC_SHOOT, _FN_RC_TRACK}, +#endif +#if CMD_ENABLE_SRC_PC + [CMD_SRC_PC] = {CMD_SRC_PC, _FN_PC_CHASSIS, _FN_PC_GIMBAL, _FN_PC_SHOOT, _FN_PC_TRACK}, +#endif +#if CMD_ENABLE_SRC_NUC + [CMD_SRC_NUC] = {CMD_SRC_NUC, NULL, _FN_NUC_GIMBAL, _FN_NUC_SHOOT, NULL}, +#endif +#if CMD_ENABLE_SRC_REF + [CMD_SRC_REF] = {CMD_SRC_REF, NULL, NULL, NULL, NULL}, +#endif +}; + +int8_t CMD_Arbitrate(CMD_t *ctx) { + if (ctx == NULL) { + return CMD_ERR_NULL; + } + + /* RC > PC priority arbitration */ + CMD_InputSource_t candidates[] = { +#if CMD_ENABLE_SRC_RC + CMD_SRC_RC, +#endif +#if CMD_ENABLE_SRC_PC + CMD_SRC_PC, +#endif + }; + const int num_candidates = sizeof(candidates) / sizeof(candidates[0]); + + /* keep current source if still online */ + if (ctx->active_source < CMD_SRC_NUM && +#if CMD_ENABLE_SRC_REF + ctx->active_source != CMD_SRC_REF && +#endif + ctx->input.online[ctx->active_source]) { + goto seize; + } + + /* 否则选择第一个可用的控制输入源 */ + for (int i = 0; i < num_candidates; i++) { + CMD_InputSource_t src = candidates[i]; + if (ctx->input.online[src]) { + ctx->active_source = src; + break; + }else { + ctx->active_source = CMD_SRC_NUM; + continue; + } + } + + /* 优先级抢占逻辑 */ + seize: + + /* reset output sources */ +#if CMD_ENABLE_MODULE_CHASSIS + ctx->output.chassis.source = ctx->active_source; +#endif +#if CMD_ENABLE_MODULE_GIMBAL + ctx->output.gimbal.source = ctx->active_source; +#endif +#if CMD_ENABLE_MODULE_SHOOT + ctx->output.shoot.source = ctx->active_source; +#endif +#if CMD_ENABLE_MODULE_TRACK + ctx->output.track.source = ctx->active_source; +#endif + + CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_NONE); + +#if CMD_ENABLE_SRC_NUC && CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_MODULE_SHOOT + if (ctx->input.online[CMD_SRC_NUC]) { + if (ctx->active_source==CMD_SRC_RC) { + if (ctx->input.rc.sw[0] == CMD_SW_DOWN) { + ctx->output.gimbal.source = CMD_SRC_NUC; + ctx->output.shoot.source = CMD_SRC_NUC; + } + } + } +#endif + + return CMD_OK; +} + +int8_t CMD_GenerateCommands(CMD_t *ctx) { + if (ctx == NULL) { + return CMD_ERR_NULL; + } + + /* 更新时间 */ + uint64_t now_us = BSP_TIME_Get_us(); + ctx->timer.now = now_us / 1000000.0f; + ctx->timer.dt = (now_us - ctx->timer.last_us) / 1000000.0f; + ctx->timer.last_us = now_us; + + /* 没有有效输入源 */ + if (ctx->active_source >= CMD_SRC_NUM) { + CMD_SetOfflineMode(ctx); + return CMD_ERR_NO_INPUT; + } + +#if CMD_ENABLE_MODULE_GIMBAL + if (sourceHandlers[ctx->output.gimbal.source].gimbalFunc != NULL) { + sourceHandlers[ctx->output.gimbal.source].gimbalFunc(ctx); + } +#endif +#if CMD_ENABLE_MODULE_CHASSIS + if (sourceHandlers[ctx->output.chassis.source].chassisFunc != NULL) { + sourceHandlers[ctx->output.chassis.source].chassisFunc(ctx); + } +#endif +#if CMD_ENABLE_MODULE_SHOOT + if (sourceHandlers[ctx->output.shoot.source].shootFunc != NULL) { + sourceHandlers[ctx->output.shoot.source].shootFunc(ctx); + } +#endif +#if CMD_ENABLE_MODULE_TRACK + if (sourceHandlers[ctx->output.track.source].trackFunc != NULL) { + sourceHandlers[ctx->output.track.source].trackFunc(ctx); + } +#endif + return CMD_OK; +} + +int8_t CMD_Update(CMD_t *ctx) { + int8_t ret; + + ret = CMD_UpdateInput(ctx); + if (ret != CMD_OK) return ret; + + CMD_Arbitrate(ctx); + + ret = CMD_GenerateCommands(ctx); + return ret; +} diff --git a/User/module/cmd/cmd.h b/User/module/cmd/cmd.h new file mode 100644 index 0000000..634bd20 --- /dev/null +++ b/User/module/cmd/cmd.h @@ -0,0 +1,227 @@ +/* + * CMD 模块 V2 - 主控制模块 + * 统一的命令控制接口 + */ +#pragma once + +#include "cmd_types.h" +#include "cmd_adapter.h" +#include "cmd_behavior.h" + +/* 按需引入输出模块的命令类型 */ +#if CMD_ENABLE_MODULE_CHASSIS + #include "module/chassis.h" +#endif +#if CMD_ENABLE_MODULE_GIMBAL + #include "module/gimbal.h" +#endif +#if CMD_ENABLE_MODULE_SHOOT + #include "module/shoot.h" +#endif +#if CMD_ENABLE_MODULE_TRACK + #include "module/track.h" +#endif +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* ========================================================================== */ +/* 输出命令结构 */ +/* ========================================================================== */ + +/* 每个模块的输出包含源信息和命令 */ +#if CMD_ENABLE_MODULE_CHASSIS +typedef struct { + CMD_InputSource_t source; + Chassis_CMD_t cmd; +} CMD_ChassisOutput_t; +#endif + +#if CMD_ENABLE_MODULE_GIMBAL +typedef struct { + CMD_InputSource_t source; + Gimbal_CMD_t cmd; +} CMD_GimbalOutput_t; +#endif + +#if CMD_ENABLE_MODULE_SHOOT +typedef struct { + CMD_InputSource_t source; + Shoot_CMD_t cmd; +} CMD_ShootOutput_t; +#endif + +#if CMD_ENABLE_MODULE_TRACK +typedef struct { + CMD_InputSource_t source; + Track_CMD_t cmd; +} CMD_TrackOutput_t; +#endif + +/* ========================================================================== */ +/* 配置结构 */ +/* ========================================================================== */ + +/* 灵敏度配置 */ +typedef struct { + float mouse_sens; /* 鼠标灵敏度 */ + float move_sens; /* 移动灵敏度 */ + float move_fast_mult; /* 快速移动倍率 */ + float move_slow_mult; /* 慢速移动倍率 */ +} CMD_Sensitivity_t; + +/* RC模式映射配置 - 定义开关位置到模式的映射 */ +typedef struct { +#if CMD_ENABLE_MODULE_CHASSIS + /* 左拨杆映射 - 底盘模式 */ + Chassis_Mode_t sw_left_up; + Chassis_Mode_t sw_left_mid; + Chassis_Mode_t sw_left_down; +#endif + +#if CMD_ENABLE_MODULE_GIMBAL + /* 右拨杆映射 - 云台/射击模式 */ + Gimbal_Mode_t gimbal_sw_up; + Gimbal_Mode_t gimbal_sw_mid; + Gimbal_Mode_t gimbal_sw_down; +#endif + +#if CMD_ENABLE_MODULE_TRACK + /* 左拨杆映射 - 履带使能 */ + bool track_sw_up; + bool track_sw_mid; + bool track_sw_down; +#endif +} CMD_RCModeMap_t; + +/* 整体配置 */ +typedef struct { + /* 输入源优先级，索引越小优先级越高 */ + CMD_InputSource_t source_priority[CMD_SRC_NUM]; + + /* 灵敏度设置 */ + CMD_Sensitivity_t sensitivity; + + /* RC模式映射 */ + CMD_RCModeMap_t rc_mode_map; + +} CMD_Config_t; + +/* ========================================================================== */ +/* 主控制上下文 */ +/* ========================================================================== */ + +typedef struct { + float now; + float dt; + uint32_t last_us; +} CMD_Timer_t; + +typedef struct CMD_Context { + /* 配置 */ + CMD_Config_t *config; + + /* 时间 */ + CMD_Timer_t timer; + + /* 当前帧和上一帧的原始输入 */ + CMD_RawInput_t input; + CMD_RawInput_t last_input; + + /* 仲裁后的活跃输入源 */ + CMD_InputSource_t active_source; + + /* 输出 */ + struct { +#if CMD_ENABLE_MODULE_CHASSIS + CMD_ChassisOutput_t chassis; +#endif +#if CMD_ENABLE_MODULE_GIMBAL + CMD_GimbalOutput_t gimbal; +#endif +#if CMD_ENABLE_MODULE_SHOOT + CMD_ShootOutput_t shoot; +#endif +#if CMD_ENABLE_MODULE_TRACK + CMD_TrackOutput_t track; +#endif + } output; +} CMD_t; + +/* ========================================================================== */ +/* 主API接口 */ +/* ========================================================================== */ + +/** + * @brief 初始化CMD模块 + * @param ctx CMD上下文 + * @param config 配置指针 + * @return CMD_OK成功，其他失败 + */ +int8_t CMD_Init(CMD_t *ctx, CMD_Config_t *config); + +/** + * @brief 更新所有输入源的数据 + * @param ctx CMD上下文 + * @return CMD_OK成功 + */ +int8_t CMD_UpdateInput(CMD_t *ctx); + +/** + * @brief 执行仲裁，决定使用哪个输入源 + * @param ctx CMD上下文 + * @return 选中的输入源 + */ +int8_t CMD_Arbitrate(CMD_t *ctx); + +/** + * @brief 生成所有模块的控制命令 + * @param ctx CMD上下文 + * @return CMD_OK成功 + */ +int8_t CMD_GenerateCommands(CMD_t *ctx); + +/** + * @brief 一键更新（包含UpdateInput + Arbitrate + GenerateCommands） + * @param ctx CMD上下文 + * @return CMD_OK成功 + */ +int8_t CMD_Update(CMD_t *ctx); + +/* ========================================================================== */ +/* 输出获取接口 */ +/* ========================================================================== */ + +/* 获取底盘命令 */ +#if CMD_ENABLE_MODULE_CHASSIS +static inline Chassis_CMD_t* CMD_GetChassisCmd(CMD_t *ctx) { + return &ctx->output.chassis.cmd; + } +#endif + +/* 获取云台命令 */ +#if CMD_ENABLE_MODULE_GIMBAL +static inline Gimbal_CMD_t* CMD_GetGimbalCmd(CMD_t *ctx) { + return &ctx->output.gimbal.cmd; +} +#endif + +/* 获取射击命令 */ +#if CMD_ENABLE_MODULE_SHOOT +static inline Shoot_CMD_t* CMD_GetShootCmd(CMD_t *ctx) { + return &ctx->output.shoot.cmd; +} +#endif + +/* 获取履带命令 */ +#if CMD_ENABLE_MODULE_TRACK +static inline Track_CMD_t* CMD_GetTrackCmd(CMD_t *ctx) { + return &ctx->output.track.cmd; +} +#endif + +#ifdef __cplusplus +} +#endif diff --git a/User/module/cmd/cmd_adapter.c b/User/module/cmd/cmd_adapter.c new file mode 100644 index 0000000..d5b84eb --- /dev/null +++ b/User/module/cmd/cmd_adapter.c @@ -0,0 +1,224 @@ +/* + * CMD 模块 V2 - 输入适配器实现 + */ +#include "cmd_adapter.h" +#include "module/cmd/cmd_adapter.h" +#include +#include + +/* ========================================================================== */ +/* 适配器存储 */ +/* ========================================================================== */ +// static CMD_InputAdapter_t *g_adapters[CMD_SRC_NUM] = {0}; +CMD_InputAdapter_t *g_adapters[CMD_SRC_NUM] = {0}; +/* ========================================================================== */ +/* DR16 适配器实现 */ +/* ========================================================================== */ +#if CMD_RC_DEVICE_TYPE == 0 + +int8_t CMD_DR16_Init(void *data) { + DR16_t *dr16 = (DR16_t *)data; + return DR16_Init(dr16); +} + +int8_t CMD_DR16_RC_GetInput(void *data, CMD_RawInput_t *output) { + DR16_t *dr16 = (DR16_t *)data; + + memset(&output->rc, 0, sizeof(CMD_RawInput_RC_t)); + + output->online[CMD_SRC_RC] = dr16->header.online; + + /* 遥控器摇杆映射 */ + output->rc.joy_left.x = dr16->data.ch_l_x; + output->rc.joy_left.y = dr16->data.ch_l_y; + output->rc.joy_right.x = dr16->data.ch_r_x; + output->rc.joy_right.y = dr16->data.ch_r_y; + + /* 拨杆映射 */ + switch (dr16->data.sw_l) { + case DR16_SW_UP: output->rc.sw[0] = CMD_SW_UP; break; + case DR16_SW_MID: output->rc.sw[0] = CMD_SW_MID; break; + case DR16_SW_DOWN: output->rc.sw[0] = CMD_SW_DOWN; break; + default: output->rc.sw[0] = CMD_SW_ERR; break; + } + switch (dr16->data.sw_r) { + case DR16_SW_UP: output->rc.sw[1] = CMD_SW_UP; break; + case DR16_SW_MID: output->rc.sw[1] = CMD_SW_MID; break; + case DR16_SW_DOWN: output->rc.sw[1] = CMD_SW_DOWN; break; + default: output->rc.sw[1] = CMD_SW_ERR; break; + } + + /* 拨轮映射 */ + output->rc.dial = dr16->data.ch_res; + + return CMD_OK; +} + +int8_t CMD_DR16_PC_GetInput(void *data, CMD_RawInput_t *output) { + DR16_t *dr16 = (DR16_t *)data; + + memset(&output->pc, 0, sizeof(CMD_RawInput_PC_t)); + + output->online[CMD_SRC_PC] = dr16->header.online; + + /* PC端鼠标映射 */ + output->pc.mouse.x = dr16->data.mouse.x; + output->pc.mouse.y = dr16->data.mouse.y; + output->pc.mouse.l_click = dr16->data.mouse.l_click; + output->pc.mouse.r_click = dr16->data.mouse.r_click; + + /* 键盘映射 */ + output->pc.keyboard.bitmap = dr16->raw_data.key; + + return CMD_OK; +} + +bool CMD_DR16_IsOnline(void *data) { + DR16_t *dr16 = (DR16_t *)data; + return dr16->header.online; +} +extern DR16_t cmd_dr16; +/* 定义适配器实例 */ +CMD_DEFINE_ADAPTER(DR16_RC, cmd_dr16, CMD_SRC_RC, CMD_DR16_Init, CMD_DR16_RC_GetInput, CMD_DR16_IsOnline) +CMD_DEFINE_ADAPTER(DR16_PC, cmd_dr16, CMD_SRC_PC, CMD_DR16_Init, CMD_DR16_PC_GetInput, CMD_DR16_IsOnline) + +#endif /* CMD_RC_DEVICE_TYPE == 0 */ + +/* ========================================================================== */ +/* AT9S 适配器实现 (示例框架) */ +/* ========================================================================== */ +#if CMD_RC_DEVICE_TYPE == 1 + +int8_t CMD_AT9S_Init(void *data) { + AT9S_t *at9s = (AT9S_t *)data; + return AT9S_Init(at9s); +} + +int8_t CMD_AT9S_GetInput(void *data, CMD_RawInput_t *output) { + AT9S_t *at9s = (AT9S_t *)data; + + memset(output, 0, sizeof(CMD_RawInput_RC_t)); + + output->online[CMD_SRC_RC] = at9s->header.online; + + /* TODO: 按照AT9S的数据格式进行映射 */ + output->joy_left.x = at9s->data.ch_l_x; + output->joy_left.y = at9s->data.ch_l_y; + output->joy_right.x = at9s->data.ch_r_x; + output->joy_right.y = at9s->data.ch_r_y; + + /* 拨杆映射需要根据AT9S的实际定义 */ + + return CMD_OK; +} + +bool CMD_AT9S_IsOnline(void *data) { + AT9S_t *at9s = (AT9S_t *)data; + return at9s->header.online; +} + +CMD_DEFINE_ADAPTER(AT9S, at9s, CMD_SRC_RC, CMD_AT9S_Init, CMD_AT9S_GetInput, CMD_AT9S_IsOnline) + +#endif /* CMD_RC_DEVICE_TYPE == 1 */ + +/* ========================================================================== */ +/* NUC/AI 适配器实现 */ +/* ========================================================================== */ +#if CMD_ENABLE_SRC_NUC + +int8_t CMD_NUC_AdapterInit(void *data) { + /* NUC适配器不需要特殊初始化 */ + return CMD_OK; +} + +int8_t CMD_NUC_GetInput(void *data, CMD_RawInput_t *output) { + AI_cmd_t *ai_cmd = (AI_cmd_t *)data; + + output->online[CMD_SRC_NUC] = true; + + /* 映射AI数据到NUC输入结构 */ + output->nuc.mode = ai_cmd->mode; + output->nuc.gimbal.setpoint.yaw = ai_cmd->gimbal.setpoint.yaw; + output->nuc.gimbal.setpoint.pit = ai_cmd->gimbal.setpoint.pit; + output->nuc.gimbal.accl.pit = ai_cmd->gimbal.accl.pit; + output->nuc.gimbal.accl.yaw = ai_cmd->gimbal.accl.yaw; + output->nuc.gimbal.vel.pit = ai_cmd->gimbal.vel.pit; + output->nuc.gimbal.vel.yaw = ai_cmd->gimbal.vel.yaw; + + return CMD_OK; +} + +bool CMD_NUC_IsOnline(void *data) { + return true; +} + +/* 定义NUC适配器实例 */ +extern AI_cmd_t ai_cmd; +CMD_DEFINE_ADAPTER(NUC, cmd_ai, CMD_SRC_NUC, CMD_NUC_AdapterInit, CMD_NUC_GetInput, CMD_NUC_IsOnline) + +#endif /* CMD_ENABLE_SRC_NUC */ + +/* ========================================================================== */ +/* 适配器管理实现 */ +/* ========================================================================== */ + +int8_t CMD_Adapter_Register(CMD_InputAdapter_t *adapter) { + if (adapter == NULL || adapter->source >= CMD_SRC_NUM) { + return CMD_ERR_NULL; + } + g_adapters[adapter->source] = adapter; + return CMD_OK; +} + +int8_t CMD_Adapter_InitAll(void) { + /* 注册编译时选择的RC设备适配器 */ +#if CMD_RC_DEVICE_TYPE == 0 + /* DR16 支持 RC 和 PC 输入 */ + CMD_Adapter_Register(&g_adapter_DR16_RC); + CMD_Adapter_Register(&g_adapter_DR16_PC); +#elif CMD_RC_DEVICE_TYPE == 1 + /* AT9S 目前只支持 RC 输入 */ + CMD_Adapter_Register(&g_adapter_AT9S); +#endif + +#if CMD_ENABLE_SRC_NUC + /* 注册NUC适配器 */ + CMD_Adapter_Register(&g_adapter_NUC); +#endif + + /* 初始化所有已注册的适配器 */ + for (int i = 0; i < CMD_SRC_NUM; i++) { + if (g_adapters[i] != NULL && g_adapters[i]->init != NULL) { + g_adapters[i]->init(g_adapters[i]->device_data); + } + } + + return CMD_OK; +} + +int8_t CMD_Adapter_GetInput(CMD_InputSource_t source, CMD_RawInput_t *output) { + if (source >= CMD_SRC_NUM || output == NULL) { + return CMD_ERR_NULL; + } + + CMD_InputAdapter_t *adapter = g_adapters[source]; + if (adapter == NULL || adapter->get_input == NULL) { + output->online[adapter->source] = false; + return CMD_ERR_NO_INPUT; + } + + return adapter->get_input(adapter->device_data, output); +} + +bool CMD_Adapter_IsOnline(CMD_InputSource_t source) { + if (source >= CMD_SRC_NUM) { + return false; + } + + CMD_InputAdapter_t *adapter = g_adapters[source]; + if (adapter == NULL || adapter->is_online == NULL) { + return false; + } + + return adapter->is_online(adapter->device_data); +} diff --git a/User/module/cmd/cmd_adapter.h b/User/module/cmd/cmd_adapter.h new file mode 100644 index 0000000..680c262 --- /dev/null +++ b/User/module/cmd/cmd_adapter.h @@ -0,0 +1,121 @@ +/* + * CMD 模块 V2 - 输入适配器接口 + * 定义设备到统一输入结构的转换接口 + */ +#pragma once + +#include "cmd_types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* ========================================================================== */ +/* 适配器接口定义 */ +/* ========================================================================== */ + +/* 适配器操作函数指针类型 */ +typedef int8_t (*CMD_AdapterInitFunc)(void *device_data); +typedef int8_t (*CMD_AdapterGetInputFunc)(void *device_data, CMD_RawInput_t *output); +typedef bool (*CMD_AdapterIsOnlineFunc)(void *device_data); + +/* 适配器描述结构 */ +typedef struct { + const char *name; /* 适配器名称 */ + CMD_InputSource_t source; /* 对应的输入源 */ + void *device_data; /* 设备数据指针 */ + CMD_AdapterInitFunc init; /* 初始化函数 */ + CMD_AdapterGetInputFunc get_input; /* 获取输入函数 */ + CMD_AdapterIsOnlineFunc is_online; /* 在线检测函数 */ +} CMD_InputAdapter_t; + +/* ========================================================================== */ +/* 适配器注册宏 */ +/* ========================================================================== */ + +/* + * 声明适配器 + * 使用示例: + * CMD_DECLARE_ADAPTER(DR16, dr16, DR16_t) + * + * 会生成: + * - extern DR16_t dr16; // 设备实例声明 + * - int8_t CMD_DR16_Init(void *data); + * - int8_t CMD_DR16_GetInput(void *data, CMD_RawInput_t *output); + * - bool CMD_DR16_IsOnline(void *data); + */ +#define CMD_DECLARE_ADAPTER(NAME, var, TYPE) \ + int8_t CMD_##NAME##_Init(void *data); \ + int8_t CMD_##NAME##_GetInput(void *data, CMD_RawInput_t *output); \ + bool CMD_##NAME##_IsOnline(void *data); + +/* + * 定义适配器实例 + * 使用示例: + * CMD_DEFINE_ADAPTER(DR16_RC, dr16, CMD_SRC_RC, CMD_DR16_Init, CMD_DR16_RC_GetInput, CMD_DR16_RC_IsOnline) + */ +#define CMD_DEFINE_ADAPTER(NAME, var, source_enum, init_func, get_func, online_func) \ + static CMD_InputAdapter_t g_adapter_##NAME = { \ + .name = #NAME, \ + .source = source_enum, \ + .device_data = (void*)&var, \ + .init = init_func, \ + .get_input = get_func, \ + .is_online = online_func, \ + }; + +/* ========================================================================== */ +/* RC设备适配器配置 */ +/* ========================================================================== */ + +/* 选择使用的RC设备 - 只需修改这里 */ +#define CMD_RC_DEVICE_TYPE 0 /* 0:DR16, 1:AT9S, 2:VT13 */ + +#if CMD_RC_DEVICE_TYPE == 0 + #include "device/dr16.h" + CMD_DECLARE_ADAPTER(DR16_RC, dr16, DR16_t) + CMD_DECLARE_ADAPTER(DR16_PC, dr16, DR16_t) + #define CMD_RC_ADAPTER_NAME DR16 + #define CMD_RC_ADAPTER_VAR dr16 +#elif CMD_RC_DEVICE_TYPE == 1 + #include "device/at9s_pro.h" + CMD_DECLARE_ADAPTER(AT9S, at9s, AT9S_t) + #define CMD_RC_ADAPTER_NAME AT9S + #define CMD_RC_ADAPTER_VAR at9s +#elif CMD_RC_DEVICE_TYPE == 2 + #include "device/vt13.h" + CMD_DECLARE_ADAPTER(VT13, vt13, VT13_t) + #define CMD_RC_ADAPTER_NAME VT13 + #define CMD_RC_ADAPTER_VAR vt13 +#endif + +/* ========================================================================== */ +/* NUC/AI适配器配置 */ +/* ========================================================================== */ +#if CMD_ENABLE_SRC_NUC + #include "module/vision_bridge.h" + extern AI_cmd_t cmd_ai; + int8_t CMD_NUC_AdapterInit(void *data); + int8_t CMD_NUC_GetInput(void *data, CMD_RawInput_t *output); + bool CMD_NUC_IsOnline(void *data); +#endif + +/* ========================================================================== */ +/* 适配器管理接口 */ +/* ========================================================================== */ + +/* 初始化所有适配器 */ +int8_t CMD_Adapter_InitAll(void); + +/* 获取指定输入源的原始输入 */ +int8_t CMD_Adapter_GetInput(CMD_InputSource_t source, CMD_RawInput_t *output); + +/* 检查输入源是否在线 */ +bool CMD_Adapter_IsOnline(CMD_InputSource_t source); + +/* 注册适配器 (运行时注册，可选) */ +int8_t CMD_Adapter_Register(CMD_InputAdapter_t *adapter); + +#ifdef __cplusplus +} +#endif diff --git a/User/module/cmd/cmd_behavior.c b/User/module/cmd/cmd_behavior.c new file mode 100644 index 0000000..071240c --- /dev/null +++ b/User/module/cmd/cmd_behavior.c @@ -0,0 +1,214 @@ +/* + * CMD 模块 V2 - 行为处理器实现 + */ +#include "cmd_behavior.h" +#include "cmd.h" +#if CMD_ENABLE_MODULE_GIMBAL + #include "module/gimbal.h" +#endif +#include + +/* ========================================================================== */ +/* 行为回调函数 */ +/* ========================================================================== */ + +/* 行为处理函数实现 */ +int8_t CMD_Behavior_Handle_FORE(CMD_t *ctx) { +#if CMD_ENABLE_MODULE_CHASSIS + ctx->output.chassis.cmd.ctrl_vec.vy += ctx->config->sensitivity.move_sens; +#endif + return CMD_OK; +} + +int8_t CMD_Behavior_Handle_BACK(CMD_t *ctx) { +#if CMD_ENABLE_MODULE_CHASSIS + ctx->output.chassis.cmd.ctrl_vec.vy -= ctx->config->sensitivity.move_sens; +#endif + return CMD_OK; +} + +int8_t CMD_Behavior_Handle_LEFT(CMD_t *ctx) { +#if CMD_ENABLE_MODULE_CHASSIS + ctx->output.chassis.cmd.ctrl_vec.vx -= ctx->config->sensitivity.move_sens; +#endif + return CMD_OK; +} + +int8_t CMD_Behavior_Handle_RIGHT(CMD_t *ctx) { +#if CMD_ENABLE_MODULE_CHASSIS + ctx->output.chassis.cmd.ctrl_vec.vx += ctx->config->sensitivity.move_sens; +#endif + return CMD_OK; +} + +int8_t CMD_Behavior_Handle_ACCELERATE(CMD_t *ctx) { +#if CMD_ENABLE_MODULE_CHASSIS + ctx->output.chassis.cmd.ctrl_vec.vx *= ctx->config->sensitivity.move_fast_mult; + ctx->output.chassis.cmd.ctrl_vec.vy *= ctx->config->sensitivity.move_fast_mult; +#endif + return CMD_OK; +} + +int8_t CMD_Behavior_Handle_DECELERATE(CMD_t *ctx) { +#if CMD_ENABLE_MODULE_CHASSIS + ctx->output.chassis.cmd.ctrl_vec.vx *= ctx->config->sensitivity.move_slow_mult; + ctx->output.chassis.cmd.ctrl_vec.vy *= ctx->config->sensitivity.move_slow_mult; +#endif + return CMD_OK; +} + +int8_t CMD_Behavior_Handle_FIRE(CMD_t *ctx) { +#if CMD_ENABLE_MODULE_SHOOT + ctx->output.shoot.cmd.firecmd = true; +#endif + return CMD_OK; +} + +int8_t CMD_Behavior_Handle_FIRE_MODE(CMD_t *ctx) { +#if CMD_ENABLE_MODULE_SHOOT + ctx->output.shoot.cmd.mode = (ctx->output.shoot.cmd.mode + 1) % SHOOT_MODE_NUM; +#endif + return CMD_OK; +} + +int8_t CMD_Behavior_Handle_ROTOR(CMD_t *ctx) { +#if CMD_ENABLE_MODULE_CHASSIS + ctx->output.chassis.cmd.mode = CHASSIS_MODE_ROTOR; + ctx->output.chassis.cmd.mode_rotor = ROTOR_MODE_RAND; +#endif +#if CMD_ENABLE_MODULE_GIMBAL + ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELATIVE; +#endif + return CMD_OK; +} + +int8_t CMD_Behavior_Handle_AUTOAIM(CMD_t *ctx) { + /* 自瞄模式切换 */ +#if CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_MODULE_SHOOT + if (ctx->input.online[CMD_SRC_NUC]) { + if (ctx->active_source == CMD_SRC_PC){ + ctx->output.gimbal.source = CMD_SRC_NUC; + ctx->output.shoot.source = CMD_SRC_NUC; + } + } +#endif + return CMD_OK; +} + +int8_t CMD_Behavior_Handle_CHECKSOURCERCPC(CMD_t *ctx) { + /* 切换RC和PC输入源 */ + if (ctx->active_source == CMD_SRC_PC) { + ctx->active_source = CMD_SRC_RC; +#if CMD_ENABLE_MODULE_CHASSIS + ctx->output.chassis.source = CMD_SRC_RC; +#endif +#if CMD_ENABLE_MODULE_GIMBAL + ctx->output.gimbal.source = CMD_SRC_RC; +#endif +#if CMD_ENABLE_MODULE_SHOOT + ctx->output.shoot.source = CMD_SRC_RC; +#endif + } else if(ctx->active_source == CMD_SRC_RC) { + ctx->active_source = CMD_SRC_PC; +#if CMD_ENABLE_MODULE_CHASSIS + ctx->output.chassis.source = CMD_SRC_PC; +#endif +#if CMD_ENABLE_MODULE_GIMBAL + ctx->output.gimbal.source = CMD_SRC_PC; +#endif +#if CMD_ENABLE_MODULE_SHOOT + ctx->output.shoot.source = CMD_SRC_PC; +#endif + } + return CMD_OK; +} + +/* 行为配置表 - 由宏生成 */ +static const CMD_BehaviorConfig_t g_behavior_configs[] = { + CMD_BEHAVIOR_TABLE(BUILD_BEHAVIOR_CONFIG) +}; + +/* ========================================================================== */ +/* API实现 */ +/* ========================================================================== */ + +int8_t CMD_Behavior_Init(void) { + /* 当前静态配置，无需初始化 */ + return CMD_OK; +} + +bool CMD_Behavior_IsTriggered(const CMD_RawInput_t *current, + const CMD_RawInput_t *last, + const CMD_BehaviorConfig_t *config) { + if (config == NULL || current == NULL) { + return false; + } + + bool now_pressed = false; + bool last_pressed = false; + + // 鼠标特殊按键处理 + if (config->key == (CMD_KEY_L_CLICK)) { + now_pressed = current->pc.mouse.l_click; + last_pressed = last ? last->pc.mouse.l_click : false; + } else if (config->key == (CMD_KEY_R_CLICK)) { + now_pressed = current->pc.mouse.r_click; + last_pressed = last ? last->pc.mouse.r_click : false; + } else if (config->key == (CMD_KEY_M_CLICK)) { + now_pressed = current->pc.mouse.m_click; + last_pressed = last ? last->pc.mouse.m_click : false; + } else if (config->key == 0) { + return false; + } else { + // 多按键组合检测 + now_pressed = ((current->pc.keyboard.bitmap & config->key) == config->key); + last_pressed = last ? ((last->pc.keyboard.bitmap & config->key) == config->key) : false; + } + + switch (config->trigger) { + case CMD_ACTIVE_PRESSED: + return now_pressed; + case CMD_ACTIVE_RISING_EDGE: + return now_pressed && !last_pressed; + case CMD_ACTIVE_FALLING_EDGE: + return !now_pressed && last_pressed; + default: + return false; + } +} + +int8_t CMD_Behavior_ProcessAll(CMD_t *ctx, + const CMD_RawInput_t *current, + const CMD_RawInput_t *last, + CMD_ModuleMask_t active_modules) { + if (ctx == NULL || current == NULL) { + return CMD_ERR_NULL; + } + + for (size_t i = 0; i < BEHAVIOR_CONFIG_COUNT; i++) { + const CMD_BehaviorConfig_t *config = &g_behavior_configs[i]; + + /* 过滤模块掩码 */ + if ((config->module_mask & active_modules) == 0) { + continue; + } + + /* 检查是否触发 */ + if (CMD_Behavior_IsTriggered(current, last, config)) { + if (config->handler != NULL) { + config->handler(ctx); + } + } + } + + return CMD_OK; +} + +const CMD_BehaviorConfig_t* CMD_Behavior_GetConfig(CMD_Behavior_t behavior) { + for (size_t i = 0; i < BEHAVIOR_CONFIG_COUNT; i++) { + if (g_behavior_configs[i].behavior == behavior) { + return &g_behavior_configs[i]; + } + } + return NULL; +} diff --git a/User/module/cmd/cmd_behavior.h b/User/module/cmd/cmd_behavior.h new file mode 100644 index 0000000..fda9074 --- /dev/null +++ b/User/module/cmd/cmd_behavior.h @@ -0,0 +1,69 @@ +/* + * CMD 模块 V2 - 行为处理器 + * 实现PC端按键到行为的映射和处理 + */ +#pragma once + +#include "cmd_types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* ========================================================================== */ +/* 行为处理器接口 */ +/* ========================================================================== */ + +/* 行为处理函数类型 */ +struct CMD_Context; /* 前向声明 */ +typedef int8_t (*CMD_BehaviorHandler)(struct CMD_Context *ctx); + +/* 行为配置项 */ +typedef struct { + CMD_Behavior_t behavior; /* 行为枚举 */ + uint32_t key; /* 绑定的按键 */ + CMD_TriggerType_t trigger; /* 触发类型 */ + CMD_ModuleMask_t module_mask; /* 影响的模块 */ + CMD_BehaviorHandler handler; /* 处理函数 */ +} CMD_BehaviorConfig_t; + +/* ========================================================================== */ +/* 行为表生成宏 */ +/* ========================================================================== */ + +/* 从宏表生成配置数组 */ +#define BUILD_BEHAVIOR_CONFIG(name, key, trigger, mask) \ + { CMD_BEHAVIOR_##name, key, trigger, mask, CMD_Behavior_Handle_##name }, + +/* 声明所有行为处理函数 */ +#define DECLARE_BEHAVIOR_HANDLER(name, key, trigger, mask) \ + int8_t CMD_Behavior_Handle_##name(struct CMD_Context *ctx); + +/* 展开声明 */ +CMD_BEHAVIOR_TABLE(DECLARE_BEHAVIOR_HANDLER) +#undef DECLARE_BEHAVIOR_HANDLER + +/* ========================================================================== */ +/* 行为处理器API */ +/* ========================================================================== */ + +/* 初始化行为处理器 */ +int8_t CMD_Behavior_Init(void); + +/* 检查行为是否被触发 */ +bool CMD_Behavior_IsTriggered(const CMD_RawInput_t *current, + const CMD_RawInput_t *last, + const CMD_BehaviorConfig_t *config); + +/* 处理所有触发的行为 */ +int8_t CMD_Behavior_ProcessAll(struct CMD_Context *ctx, + const CMD_RawInput_t *current, + const CMD_RawInput_t *last, + CMD_ModuleMask_t active_modules); + +/* 获取行为配置 */ +const CMD_BehaviorConfig_t* CMD_Behavior_GetConfig(CMD_Behavior_t behavior); + +#ifdef __cplusplus +} +#endif diff --git a/User/module/cmd/cmd_example.c b/User/module/cmd/cmd_example.c new file mode 100644 index 0000000..476c32e --- /dev/null +++ b/User/module/cmd/cmd_example.c @@ -0,0 +1,167 @@ +/* + * CMD 模块 V2 - 使用示例和配置模板 + * + * 本文件展示如何配置和使用新的CMD模块 + */ +#include "cmd.h" + +/* ========================================================================== */ +/* config示例 */ +/* ========================================================================== */ + +/* 默认配置 */ +// static CMD_Config_t g_cmd_config = { +// /* 灵敏度设置 */ +// .sensitivity = { +// .mouse_sens = 0.8f, +// .move_sens = 1.0f, +// .move_fast_mult = 1.5f, +// .move_slow_mult = 0.5f, +// }, + +// /* RC拨杆模式映射 */ +// .rc_mode_map = { +// /* 左拨杆控制底盘模式 */ +// .sw_left_up = CHASSIS_MODE_BREAK, +// .sw_left_mid = CHASSIS_MODE_FOLLOW_GIMBAL, +// .sw_left_down = CHASSIS_MODE_ROTOR, + +// /* 用于云台模式 */ +// .gimbal_sw_up = GIMBAL_MODE_ABSOLUTE, +// .gimbal_sw_mid = GIMBAL_MODE_ABSOLUTE, +// .gimbal_sw_down = GIMBAL_MODE_RELATIVE, +// }, + +// }; + +// /* CMD上下文 */ +// static CMD_t g_cmd_ctx; + +/* ========================================================================== */ +/* 队列创建示例 */ +/* ========================================================================== */ +// #if CMD_RCTypeTable_Index == 0 +// task_runtime.msgq.cmd.rc= osMessageQueueNew(3u, sizeof(DR16_t), NULL); +// #elif CMD_RCTypeTable_Index == 1 +// task_runtime.msgq.cmd.rc= osMessageQueueNew(3u, sizeof(AT9S_t), NULL); +// #endif + +/* ========================================================================== */ +/* 任务示例 */ +/* ========================================================================== */ + +// #if CMD_RCTypeTable_Index == 0 +// DR16_t cmd_dr16; +// #elif CMD_RCTypeTable_Index == 1 +// AT9S_t cmd_at9s; +// #endif +// Shoot_CMD_t *cmd_for_shoot; +// Chassis_CMD_t *cmd_for_chassis; +// Gimbal_CMD_t *cmd_for_gimbal; + +// static CMD_t cmd; + +// void Task_cmd() { + +// CMD_Init(&cmd, &Config_GetRobotParam()->cmd_param); + +// while (1) { +// #if CMD_RCTypeTable_Index == 0 +// osMessageQueueGet(task_runtime.msgq.cmd.rc, &cmd_dr16, NULL, 0); +// #elif CMD_RCTypeTable_Index == 1 +// osMessageQueueGet(task_runtime.msgq.cmd.rc, &cmd_at9s, NULL, 0); +// #endif +// CMD_Update(&cmd); + +// /* 获取命令发送到各模块 */ +// cmd_for_chassis = CMD_GetChassisCmd(&cmd); +// cmd_for_gimbal = CMD_GetGimbalCmd(&cmd); +// cmd_for_shoot = CMD_GetShootCmd(&cmd); +// osMessageQueueReset(task_runtime.msgq.gimbal.cmd); +// osMessageQueuePut(task_runtime.msgq.gimbal.cmd, cmd_for_gimbal, 0, 0); +// osMessageQueueReset(task_runtime.msgq.shoot.cmd); +// osMessageQueuePut(task_runtime.msgq.shoot.cmd, cmd_for_shoot, 0, 0); +// osMessageQueueReset(task_runtime.msgq.chassis.cmd); +// osMessageQueuePut(task_runtime.msgq.chassis.cmd, cmd_for_chassis, 0, 0); +// } + +// } + + + +/* ========================================================================== */ +/* 架构说明 */ +/* ========================================================================== */ + +/* + * ## 新架构优势 + * + * ### 1. 统一的输入抽象层 (CMD_RawInput_t) + * - 所有设备（DR16/AT9S/VT13等）都转换成相同格式 + * - 上层代码无需关心具体设备类型 + * - 添加新设备只需实现适配器，不改动主逻辑 + * + * ### 2. 适配器模式 + * - 每个设备一个适配器文件 + * - 实现 Init, GetInput, IsOnline 三个函数 + * - 通过宏选择编译哪个适配器 + * + * ### 3. X-Macro配置表 + * - CMD_INPUT_SOURCE_TABLE: 配置输入源 + * - CMD_OUTPUT_MODULE_TABLE: 配置输出模块 + * - CMD_BEHAVIOR_TABLE: 配置按键行为映射 + * - 编译时生成枚举、配置数组、处理函数 + * + * ### 4. 行为驱动设计 + * - 行为与按键解耦 + * - 运行时可修改映射 + * - 支持边沿触发和持续触发 + * + * ### 5. 清晰的分层 + * + * ┌──────────────────────────────────────┐ + * │ 应用层 (cmd.c) │ + * │ - CMD_Update() │ + * │ - 仲裁、命令生成 │ + * └──────────────┬───────────────────────┘ + * │ + * ┌──────────────▼───────────────────────┐ + * │ 行为处理层 (cmd_behavior.c) │ + * │ - 按键触发检测 │ + * │ - 行为函数调用 │ + * └──────────────┬───────────────────────┘ + * │ + * ┌──────────────▼──────────────────────────┐ + * │ 抽象输入层 (cmd_types.h) │ + * │ - 多输入源操作同一CMD_RawInput_t不同分区 │ + * │ - 统一的摇杆、开关、键鼠结构 │ + * └──────────────┬──────────────────────────┘ + * │ + * ┌──────────────▼───────────────────────┐ + * │ 适配器层 (cmd_adapter.c) │ + * │ - DR16_Adapter │ + * │ - AT9S_Adapter │ + * │ - 设备数据 → CMD_RawInput_t │ + * └──────────────────────────────────────┘ + * + * ## 扩展指南 + * + * ### 添加新遥控器设备 + * 1. 在 cmd_adapter.h 中添加宏定义选项 + * 2. 在 cmd_adapter.c 中实现三个适配器函数 + * 3. 修改 CMD_RC_DEVICE_TYPE 宏选择新设备 + * + * ### 添加新输入源（如自定义协议） + * 1. 在 CMD_INPUT_SOURCE_TABLE 添加条目 + * 2. 实现对应的适配器 + * 3. 在 CMD_GenerateCommands 添加处理分支 + * + * ### 添加新行为 + * 1. 在 CMD_BEHAVIOR_TABLE 添加条目,并修正BEHAVIOR_CONFIG_COUNT + * 2. 实现 CMD_Behavior_Handle_XXX 函数 + * + * ### 添加新输出模块 + * 1. 在 CMD_OUTPUT_MODULE_TABLE 添加条目 + * 2. 在 CMD_t 中添加输出成员 + * 3. 实现对应的 BuildXXXCmd 函数 + */ diff --git a/User/module/cmd/cmd_feature.h b/User/module/cmd/cmd_feature.h new file mode 100644 index 0000000..10b158f --- /dev/null +++ b/User/module/cmd/cmd_feature.h @@ -0,0 +1,53 @@ +/* + * CMD 模块 V2 - 功能特性开关配置 + * + * 修改此文件来快速使能/失能各个模块和输入源。 + * 失能后，对应的代码和头文件依赖将被完全排除在编译之外。 + */ +#pragma once + +/* ========================================================================== */ +/* 输入源使能开关 */ +/* ========================================================================== */ + +/** 遥控器输入 (DR16 / AT9S 等) */ +#define CMD_ENABLE_SRC_RC 1 + +/** PC 端键鼠输入 (通过 DR16 转发) */ +#define CMD_ENABLE_SRC_PC 1 + +/** NUC / AI 输入 (需要 vision_bridge 模块) */ +#define CMD_ENABLE_SRC_NUC 0 + +/** 裁判系统输入 */ +#define CMD_ENABLE_SRC_REF 0 + +/* ========================================================================== */ +/* 输出模块使能开关 */ +/* ========================================================================== */ + +/** 底盘模块 (需要 module/chassis.h) */ +#define CMD_ENABLE_MODULE_CHASSIS 1 + +/** 云台模块 (需要 module/gimbal.h) */ +#define CMD_ENABLE_MODULE_GIMBAL 0 + +/** 射击模块 (需要 module/shoot.h) */ +#define CMD_ENABLE_MODULE_SHOOT 0 + +/** 履带模块 (需要 module/track.h) */ +#define CMD_ENABLE_MODULE_TRACK 0 + +/* ========================================================================== */ +/* 合法性检查 */ +/* ========================================================================== */ + +/* PC输入源依赖RC适配器共同存在（DR16同时提供RC和PC数据） */ +#if CMD_ENABLE_SRC_PC && !CMD_ENABLE_SRC_RC + #error "CMD_ENABLE_SRC_PC requires CMD_ENABLE_SRC_RC (both share the DR16 adapter)" +#endif + +/* NUC依赖vision_bridge模块，确保已包含相关模块 */ +/* #if CMD_ENABLE_SRC_NUC && !defined(VISION_BRIDGE_ENABLED) + #error "CMD_ENABLE_SRC_NUC requires vision_bridge module" +#endif */ diff --git a/User/module/cmd/cmd_types.h b/User/module/cmd/cmd_types.h new file mode 100644 index 0000000..73957af --- /dev/null +++ b/User/module/cmd/cmd_types.h @@ -0,0 +1,273 @@ +/* + * CMD 模块 V2 - 类型定义 + * 统一的输入/输出抽象层 + */ +#pragma once + +#include +#include +#include "cmd_feature.h" /* 功能特性开关 */ + +#ifdef __cplusplus +extern "C" { +#endif + +/* ========================================================================== */ +/* 错误码定义 */ +/* ========================================================================== */ +#define CMD_OK (0) +#define CMD_ERR_NULL (-1) +#define CMD_ERR_MODE (-2) +#define CMD_ERR_SOURCE (-3) +#define CMD_ERR_NO_INPUT (-4) + +/* ========================================================================== */ +/* 输入源配置宏表 */ +/* ========================================================================== */ +/* + * 使用方法：在 cmd_feature.h 中设置各 CMD_ENABLE_SRC_xxx 宏。 + * 格式: X(枚举名, 适配器初始化函数, 获取数据函数) + */ + +/* 各输入源条件展开辅助宏 */ +#if CMD_ENABLE_SRC_RC + #define _X_SRC_RC(X) X(RC, CMD_RC_AdapterInit, CMD_RC_GetInput) +#else + #define _X_SRC_RC(X) +#endif + +#if CMD_ENABLE_SRC_PC + #define _X_SRC_PC(X) X(PC, CMD_PC_AdapterInit, CMD_PC_GetInput) +#else + #define _X_SRC_PC(X) +#endif + +#if CMD_ENABLE_SRC_NUC + #define _X_SRC_NUC(X) X(NUC, CMD_NUC_AdapterInit, CMD_NUC_GetInput) +#else + #define _X_SRC_NUC(X) +#endif + +#if CMD_ENABLE_SRC_REF + #define _X_SRC_REF(X) X(REF, CMD_REF_AdapterInit, CMD_REF_GetInput) +#else + #define _X_SRC_REF(X) +#endif + +#define CMD_INPUT_SOURCE_TABLE(X) \ + _X_SRC_RC(X) \ + _X_SRC_PC(X) \ + _X_SRC_NUC(X) \ + _X_SRC_REF(X) + +/* 各输出模块条件展开辅助宏 */ +#if CMD_ENABLE_MODULE_CHASSIS + #define _X_MOD_CHASSIS(X) X(CHASSIS, Chassis_CMD_t, chassis) +#else + #define _X_MOD_CHASSIS(X) +#endif + +#if CMD_ENABLE_MODULE_GIMBAL + #define _X_MOD_GIMBAL(X) X(GIMBAL, Gimbal_CMD_t, gimbal) +#else + #define _X_MOD_GIMBAL(X) +#endif + +#if CMD_ENABLE_MODULE_SHOOT + #define _X_MOD_SHOOT(X) X(SHOOT, Shoot_CMD_t, shoot) +#else + #define _X_MOD_SHOOT(X) +#endif + +#if CMD_ENABLE_MODULE_TRACK + #define _X_MOD_TRACK(X) X(TRACK, Track_CMD_t, track) +#else + #define _X_MOD_TRACK(X) +#endif + +/* 输出模块配置宏表 */ +#define CMD_OUTPUT_MODULE_TABLE(X) \ + _X_MOD_CHASSIS(X) \ + _X_MOD_GIMBAL(X) \ + _X_MOD_SHOOT(X) \ + _X_MOD_TRACK(X) + + +/* ========================================================================== */ +/* 输入源枚举 */ +/* ========================================================================== */ +#define ENUM_INPUT_SOURCE(name, ...) CMD_SRC_##name, +typedef enum { + CMD_INPUT_SOURCE_TABLE(ENUM_INPUT_SOURCE) + CMD_SRC_NUM +} CMD_InputSource_t; +#undef ENUM_INPUT_SOURCE + +/* ========================================================================== */ +/* 统一输入数据结构 */ +/* ========================================================================== */ + +/* 摇杆数据 - 统一为-1.0 ~ 1.0 */ +typedef struct { + float x; + float y; +} CMD_Joystick_t; + +/* 开关位置 */ +typedef enum { + CMD_SW_ERR = 0, + CMD_SW_UP, + CMD_SW_MID, + CMD_SW_DOWN, +} CMD_SwitchPos_t; + +/* 鼠标数据 */ +typedef struct { + int16_t x; /* 鼠标X轴移动速度 */ + int16_t y; /* 鼠标Y轴移动速度 */ + int16_t z; /* 鼠标滚轮 */ + bool l_click; /* 左键 */ + bool r_click; /* 右键 */ + bool m_click; /* 中键 */ +} CMD_Mouse_t; + +/* 键盘数据 - 最多支持32个按键 */ +typedef struct { + uint32_t bitmap; /* 按键位图 */ +} CMD_Keyboard_t; + +/* 键盘按键索引 */ +typedef enum { + CMD_KEY_W = (1 << 0), CMD_KEY_S = (1 << 1), CMD_KEY_A = (1 << 2), CMD_KEY_D = (1 << 3), + CMD_KEY_SHIFT = (1 << 4), CMD_KEY_CTRL = (1 << 5), CMD_KEY_Q = (1 << 6), CMD_KEY_E = (1 << 7), + CMD_KEY_R = (1 << 8), CMD_KEY_F = (1 << 9), CMD_KEY_G = (1 << 10), CMD_KEY_Z = (1 << 11), + CMD_KEY_X = (1 << 12), CMD_KEY_C = (1 << 13), CMD_KEY_V = (1 << 14), CMD_KEY_B = (1 << 15), + CMD_KEY_NUM +} CMD_KeyIndex_t; + +/* 裁判系统数据 */ +typedef struct { + uint8_t game_status; /* 比赛状态 */ +} CMD_Referee_t; + +typedef struct { + CMD_Joystick_t joy_left; /* 左摇杆 */ + CMD_Joystick_t joy_right; /* 右摇杆 */ + CMD_SwitchPos_t sw[4]; /* 4个拨杆 */ + float dial; /* 拨轮 */ +} CMD_RawInput_RC_t; + +typedef struct { + CMD_Mouse_t mouse; + CMD_Keyboard_t keyboard; +} CMD_RawInput_PC_t; + +/* AI输入数据 */ +typedef struct { + uint8_t mode; + struct { + struct { + float yaw; + float pit; + } setpoint; + struct { + float pit; + float yaw; + } accl; + struct { + float pit; + float yaw; + } vel; + } gimbal; +} CMD_RawInput_NUC_t; + +typedef struct { + CMD_Referee_t referee; +} CMD_RawInput_REF_t; + +/* 统一的原始输入结构 - 所有设备适配后都转换成这个格式 */ +typedef struct { + bool online[CMD_SRC_NUM]; + +#if CMD_ENABLE_SRC_RC + /* 遥控器部分 */ + CMD_RawInput_RC_t rc; +#endif + +#if CMD_ENABLE_SRC_PC + /* PC部分 */ + CMD_RawInput_PC_t pc; +#endif + +#if CMD_ENABLE_SRC_NUC + /* NUC部分 */ + CMD_RawInput_NUC_t nuc; +#endif + +#if CMD_ENABLE_SRC_REF + /* REF部分 - 裁判系统数据 */ + CMD_RawInput_REF_t ref; +#endif +} CMD_RawInput_t; + +/* ========================================================================== */ +/* 模块掩码 */ +/* ========================================================================== */ +typedef enum { + CMD_MODULE_NONE = (1 << 0), + CMD_MODULE_CHASSIS = (1 << 1), + CMD_MODULE_GIMBAL = (1 << 2), + CMD_MODULE_SHOOT = (1 << 3), + CMD_MODULE_TRACK = (1 << 4), + CMD_MODULE_ALL = 0x1E + +} CMD_ModuleMask_t; + +/* ========================================================================== */ +/* 行为定义 */ +/* ========================================================================== */ +/* 行为-按键映射宏表 */ +#define BEHAVIOR_CONFIG_COUNT (11) +#define CMD_BEHAVIOR_TABLE(X) \ + X(FORE, CMD_KEY_W, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \ + X(BACK, CMD_KEY_S, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \ + X(LEFT, CMD_KEY_A, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \ + X(RIGHT, CMD_KEY_D, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \ + X(ACCELERATE, CMD_KEY_SHIFT, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \ + X(DECELERATE, CMD_KEY_CTRL, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \ + X(FIRE, CMD_KEY_L_CLICK, CMD_ACTIVE_PRESSED, CMD_MODULE_SHOOT) \ + X(FIRE_MODE, CMD_KEY_B, CMD_ACTIVE_RISING_EDGE, CMD_MODULE_SHOOT) \ + X(ROTOR, CMD_KEY_E, CMD_ACTIVE_PRESSED, CMD_MODULE_CHASSIS) \ + X(AUTOAIM, CMD_KEY_R_CLICK, CMD_ACTIVE_PRESSED, CMD_MODULE_NONE) \ + X(CHECKSOURCERCPC, CMD_KEY_CTRL|CMD_KEY_SHIFT|CMD_KEY_V, CMD_ACTIVE_RISING_EDGE, CMD_MODULE_NONE) +/* 触发类型 */ +typedef enum { + CMD_ACTIVE_PRESSED, /* 按住时触发 */ + CMD_ACTIVE_RISING_EDGE, /* 按下瞬间触发 */ + CMD_ACTIVE_FALLING_EDGE, /* 松开瞬间触发 */ +} CMD_TriggerType_t; + +/* 特殊按键值 */ +#define CMD_KEY_NONE 0xFF +#define CMD_KEY_L_CLICK (1 << 31) +#define CMD_KEY_R_CLICK (1 << 30) +#define CMD_KEY_M_CLICK (1 << 29) + +/* 行为枚举 - 由宏表自动生成 */ +#define ENUM_BEHAVIOR(name, key, trigger, mask) CMD_BEHAVIOR_##name, +typedef enum { + CMD_BEHAVIOR_TABLE(ENUM_BEHAVIOR) + CMD_BEHAVIOR_NUM +} CMD_Behavior_t; +#undef ENUM_BEHAVIOR + +/* ========================================================================== */ +/* 键盘辅助宏 */ +/* ========================================================================== */ +#define CMD_KEY_PRESSED(kb, key) (((kb)->bitmap >> (key)) & 1) +#define CMD_KEY_SET(kb, key) ((kb)->bitmap |= (1 << (key))) +#define CMD_KEY_CLEAR(kb, key) ((kb)->bitmap &= ~(1 << (key))) + +#ifdef __cplusplus +} +#endif diff --git a/User/module/config.c b/User/module/config.c new file mode 100644 index 0000000..bd39091 --- /dev/null +++ b/User/module/config.c @@ -0,0 +1,145 @@ +/* + * 配置相关 + */ + +/* Includes ----------------------------------------------------------------- */ +#include "module/config.h" +#include "bsp/can.h" +#include "device/motor_dm.h" +#include +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ + +/* Exported variables ------------------------------------------------------- */ + +// 机器人参数配置 +// DH参数来源: URDF模型 (newURDF.urdf, 2026-01-20) +// theta(变量), d(mm), a(mm), alpha(rad), theta_offset(rad) +// 零位姿态: 机械臂竖直向上 +Config_RobotParam_t robot_config = { + .chassis_param = { + /* DJI3508µç»ú*/ + .motor_param = { + { + .can = BSP_CAN_2, + .id = 0x201, + .module = MOTOR_M3508, + .reverse = false, + .gear = true + }, + { + .can = BSP_CAN_2, + .id = 0x202, + .module = MOTOR_M3508, + .reverse = false, + .gear = true + }, + { + .can = BSP_CAN_2, + .id = 0x203, + .module = MOTOR_M3508, + .reverse = false, + .gear = true + }, + { + .can = BSP_CAN_2, + .id = 0x204, + .module = MOTOR_M3508, + .reverse = false, + .gear = true + }, + }, + .type = CHASSIS_TYPE_MECANUM, + /* PID */ + .pid = { + /* µ×Å̵ç»ú PID */ + .motor_pid_param = { + .k = 0.0015f, + .p = 1.0f, + .i = 0.0f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, + .d_cutoff_freq = -1.0f, + .range = -1.0f, + }, + + /* ¸úËæ */ + .follow_pid_param = { + .k = 1.2f, + .p = 1.0f, + .i = 0.5f, + .d = 0.01f, + .i_limit = 1.0f, + .out_limit = 1.0f, + .d_cutoff_freq = -1.0f, + .range = M_2PI, + }, + }, + + .low_pass_cutoff_freq = { + .in = 50.0f, + .out = 50.0f, + }, + + .reverse = { + .yaw = true, + }, + .limit = { + .max_vx = 3.0f, + .max_vy = 3.0f, + .max_wz = 2.0f, + .max_current = 16000.0f + }, + }, + .cmd_param={ + .source_priority = { + CMD_SRC_RC, + CMD_SRC_PC + }, + .sensitivity = { + .mouse_sens = 2.0f, + .move_sens = 1.0f, + .move_fast_mult = 1.5f, + .move_slow_mult = 0.5f, + }, + .rc_mode_map = { + .sw_left_up = CHASSIS_MODE_RELAX, + .sw_left_mid = CHASSIS_MODE_FOLLOW_GIMBAL, + .sw_left_down = CHASSIS_MODE_RELAX, + + }, + }, +}; +power_model_t cha= +{ + .motor_num =4, + .constant= 0.7910660075305308, + .k1 = 98.97509148647588, + .k2= 0.00018014375383727376, + .toque_coefficient =0.4568913750568248, + .power = (float[4]){} + }; + +power_model_t cha2= +{ + .motor_num =4, + .constant= 2.354223704504904, + .k1 = 94.85324024176896, + .k2= 0.00012622263917529537, + .toque_coefficient =0.3680225540649464, + + .power = (float[4]){} + }; +/* Private function prototypes ---------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ + +/** + * @brief 获取机器人配置参数 + * @return 机器人配置参数指针 + */ +Config_RobotParam_t* Config_GetRobotParam(void) { + return &robot_config; +} diff --git a/User/module/config.h b/User/module/config.h new file mode 100644 index 0000000..60b8dae --- /dev/null +++ b/User/module/config.h @@ -0,0 +1,37 @@ +/* + * 配置相关 + */ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "component/pid.h" +#include "device/motor.h" +#include "device/motor_dm.h" +#include "module/arm.h" +#include "module/chassis.h" +#include "module/cmd/cmd.h" +#include "component/PowerControl.h" +typedef struct { + // Arm_Params_t arm_param; + Chassis_Params_t chassis_param; + CMD_Config_t cmd_param; +} Config_RobotParam_t; + +/* Exported variables ------------------------------------------------------- */ +extern power_model_t cha; +extern power_model_t cha2; +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 获取机器人配置参数 + * @return 机器人配置参数指针 + */ +Config_RobotParam_t* Config_GetRobotParam(void); +#ifdef __cplusplus +} +#endif diff --git a/User/module/joint.hpp b/User/module/joint.hpp new file mode 100644 index 0000000..c5e7ad0 --- /dev/null +++ b/User/module/joint.hpp @@ -0,0 +1,159 @@ +/** + * @file joint.hpp + * @brief 关节类 - 封装单个关节的所有功能 + */ + +#pragma once + +#include +#include +#include "motor_base.hpp" +#include "component/arm_kinematics/arm6dof.h" + +namespace arm { + +// ============================================================================ +// Joint 类 - 关节对象 +// ============================================================================ +class Joint { +private: + uint8_t id_; // 关节编号 + IMotor* motor_; // 电机指针（多态） + Arm6dof_DHParams_t dh_params_; // DH参数 + float q_offset_; // 零位偏移 + + // 状态 + struct { + float current_angle; + float current_velocity; + float current_torque; + float target_angle; + float feedforward_torque; // 前馈力矩（如重力补偿） + bool online; + } state_; + +public: + // 构造函数 + Joint(uint8_t id, IMotor* motor, const Arm6dof_DHParams_t& dh_params, + float q_offset, float freq) + : id_(id), motor_(motor), dh_params_(dh_params), q_offset_(q_offset) { + + state_.current_angle = 0.0f; + state_.current_velocity = 0.0f; + state_.current_torque = 0.0f; + state_.target_angle = 0.0f; + state_.feedforward_torque = 0.0f; + state_.online = false; + } + + // 禁止拷贝（电机资源唯一） + Joint(const Joint&) = delete; + Joint& operator=(const Joint&) = delete; + + // Getters + uint8_t GetId() const { return id_; } + float GetCurrentAngle() const { return state_.current_angle; } + float GetCurrentVelocity() const { return state_.current_velocity; } + float GetTargetAngle() const { return state_.target_angle; } + bool IsOnline() const { return state_.online; } + const Arm6dof_DHParams_t& GetDHParams() const { return dh_params_; } + float GetOffset() const { return q_offset_; } + + // Setters + void SetOffset(float offset) { q_offset_ = offset; } + + void SetTargetAngle(float target) { + state_.target_angle = target; + } + + void SetFeedforwardTorque(float torque) { + state_.feedforward_torque = torque; + } + + float GetFeedforwardTorque() const { return state_.feedforward_torque; } + + int8_t Register() { + return motor_->Register(); + } + + int8_t Enable() { + return motor_->Enable(); + } + + int8_t Update() { + int8_t ret = motor_->Update(); + + float raw = motor_->GetAngle() - q_offset_; + + // 多圈关节（运动范围 > 2π，如 J1/J4/J6）：直接透传多圈绝对值，不截断 + // 单圈关节（运动范围 ≤ 2π，如 J2/J3/J5）：归一化到 [-π, π] + // 目的：消除 DM 电机 0~2π 单圈编码在过零点时的跳变 + // 零点不变：raw=0 时不触发任何折叠 + // 注意：j3 范围 [0,3]、j5 范围 [-1.75,1.75] 均在 [-π,π] 内， + // 归一化后其实际运动段不会碰到 ±π 边界，计算安全 + const bool multi_turn = (dh_params_.qmax - dh_params_.qmin) > 2.0f * M_PI; + if (!multi_turn) { + if (raw > M_PI) raw -= 2.0f * M_PI; + if (raw < -M_PI) raw += 2.0f * M_PI; + } + + state_.current_angle = raw; + state_.current_velocity = motor_->GetVelocity(); + state_.current_torque = motor_->GetTorque(); + state_.online = motor_->IsOnline(); + + return ret; + } + + int8_t Relax() { + return motor_->Relax(); + } + + // 位置控制 + int8_t PositionControl(float target_angle, float dt) { + // 检查限位 + if (target_angle < dh_params_.qmin || target_angle > dh_params_.qmax) { + return -1; + } + + state_.target_angle = target_angle; + + // 总前馈力矩 = 重力补偿等前馈力矩 + float total_feedforward = state_.feedforward_torque; + + // MIT控制参数：优先使用DH参数中配置的kp/kd，否则使用默认值 + float kp = dh_params_.kp; + float kd = dh_params_.kd; + + // 如果未配置（=0），使用默认值（按电机类型） + if (kp <= 0.0f) kp = (id_ < 3) ? 10.0f : 50.0f; // LZ vs DM默认值 + if (kd <= 0.0f) kd = (id_ < 3) ? 0.5f : 3.0f; + + return motor_->PositionControl( + target_angle, + 0.0f, // 速度 + kp, kd, // 刚度和阻尼 + total_feedforward // 前馈力矩（重力补偿） + ); + } + + // 纯力矩控制 + // @param torque: 输出力矩（N·m） + // @return: 0=成功，-1=失败 + int8_t TorqueControl(float torque) { + state_.feedforward_torque = torque; // 存储当前前馈力矩 + return motor_->PositionControl( + state_.current_angle, // 保持当前位置 + 0.0f, // 速度 + 0.0f, 0.0f, // kp=0, kd=0（无刚度无阻尼） + torque // 纯力矩 + ); + } + + // 检查是否到达目标 + bool IsReached(float tolerance) const { + return std::fabs(state_.target_angle - state_.current_angle) < tolerance; + } +}; + +} // namespace arm diff --git a/User/module/motor_base.hpp b/User/module/motor_base.hpp new file mode 100644 index 0000000..958c72a --- /dev/null +++ b/User/module/motor_base.hpp @@ -0,0 +1,179 @@ +/** + * @file motor_base.hpp + * @brief 电机抽象基类 - C++多态实现 + */ + +#pragma once + +#include +#include "component/user_math.h" +#include "device/motor_lz.h" +#include "device/motor_dm.h" + +namespace arm { + +// ============================================================================ +// Motor 抽象基类（接口） +// ============================================================================ +class IMotor { +public: + virtual ~IMotor() = default; + + // 纯虚函数（接口） + virtual int8_t Register() = 0; + virtual int8_t Enable() = 0; + virtual int8_t Update() = 0; + virtual int8_t Relax() = 0; + + virtual float GetAngle() const = 0; + virtual float GetVelocity() const = 0; + virtual float GetTorque() const = 0; + virtual bool IsOnline() const = 0; + + virtual int8_t PositionControl(float angle, float velocity, float kp, float kd, float torque) = 0; +}; + +// ============================================================================ +// LZ电机实现 +// ============================================================================ +class MotorLZ : public IMotor { +private: + MOTOR_LZ_Param_t params_; + MOTOR_LZ_t* motor_instance_; // 指向内部管理器的实例 + +public: + explicit MotorLZ(const MOTOR_LZ_Param_t& params) + : params_(params), motor_instance_(nullptr) {} + + int8_t Register() override { + int8_t ret = MOTOR_LZ_Register(¶ms_); + if (ret == DEVICE_OK) { + motor_instance_ = MOTOR_LZ_GetMotor(¶ms_); + } + return ret; + } + + int8_t Enable() override { + return MOTOR_LZ_Enable(¶ms_); + } + + int8_t Update() override { + int8_t ret = MOTOR_LZ_Update(¶ms_); + if (ret == DEVICE_OK && motor_instance_) { + motor_instance_ = MOTOR_LZ_GetMotor(¶ms_); + } + return ret; + } + + int8_t Relax() override { + return MOTOR_LZ_Relax(¶ms_); + } + + float GetAngle() const override { + if (!motor_instance_) return 0.0f; + + // LZ电机范围: -4π ~ 4π，归一化到 -π ~ π + float angle = motor_instance_->motor.feedback.rotor_abs_angle; + // while (angle > M_PI) { + // angle -= 2.0f * M_PI; + // } + // while (angle < -M_PI) { + // angle += 2.0f * M_PI; + // } + return angle; + } + + float GetVelocity() const override { + return motor_instance_ ? motor_instance_->motor.feedback.rotor_speed : 0.0f; + } + + float GetTorque() const override { + return motor_instance_ ? motor_instance_->motor.feedback.torque_current : 0.0f; + } + + bool IsOnline() const override { + return motor_instance_ ? motor_instance_->motor.header.online : false; + } + + int8_t PositionControl(float angle, float velocity, float kp, float kd, float torque) override { + MOTOR_LZ_MotionParam_t output; + output.target_angle = angle; + output.target_velocity = velocity; + output.kp = kp; + output.kd = kd; + output.torque = torque; + return MOTOR_LZ_MotionControl(¶ms_, &output); + } +}; + +// ============================================================================ +// DM电机实现 +// ============================================================================ +class MotorDM : public IMotor { +private: + MOTOR_DM_Param_t params_; + MOTOR_DM_t* motor_instance_; + +public: + explicit MotorDM(const MOTOR_DM_Param_t& params) + : params_(params), motor_instance_(nullptr) {} + + int8_t Register() override { + int8_t ret = MOTOR_DM_Register(¶ms_); + if (ret == DEVICE_OK) { + motor_instance_ = MOTOR_DM_GetMotor(¶ms_); + } + return ret; + } + + int8_t Enable() override { + return MOTOR_DM_Enable(¶ms_); + } + + int8_t Update() override { + int8_t ret = MOTOR_DM_Update(¶ms_); + if (ret == DEVICE_OK && motor_instance_) { + motor_instance_ = MOTOR_DM_GetMotor(¶ms_); + } + return ret; + } + + int8_t Relax() override { + return MOTOR_DM_Relax(¶ms_); + } + + float GetAngle() const override { + if (!motor_instance_) return 0.0f; + + // DM电机范围: 0 ~ 2π，归一化到 -π ~ π + float angle = motor_instance_->motor.feedback.rotor_abs_angle; + // if (angle > M_PI) { + // angle -= 2.0f * M_PI; // 例如 1.5π → -0.5π + // } + return angle; + } + + float GetVelocity() const override { + return motor_instance_ ? motor_instance_->motor.feedback.rotor_speed : 0.0f; + } + + float GetTorque() const override { + return motor_instance_ ? motor_instance_->motor.feedback.torque_current : 0.0f; + } + + bool IsOnline() const override { + return motor_instance_ ? motor_instance_->motor.header.online : false; + } + + int8_t PositionControl(float angle, float velocity, float kp, float kd, float torque) override { + MOTOR_MIT_Output_t output; + output.angle = angle; + output.velocity = velocity; + output.kp = kp; + output.kd = kd; + output.torque = torque; + return MOTOR_DM_MITCtrl(¶ms_, &output); + } +}; + +} // namespace arm diff --git a/User/task/arm_main.cpp b/User/task/arm_main.cpp new file mode 100644 index 0000000..2d0b30e --- /dev/null +++ b/User/task/arm_main.cpp @@ -0,0 +1,289 @@ +/** + * @file arm_main.cpp + * @brief 机械臂主任务 - C++版本 + */ + +#include "task/user_task.h" + +#include "module/arm_oop.hpp" +#include "module/motor_base.hpp" +#include "module/joint.hpp" +#include "bsp/can.h" +#include "device/motor_lz.h" +#include "device/motor_dm.h" +using namespace arm; + +// ============================================================================ +// 电机和关节配置 +// ============================================================================ + +// LZ电机参数（关节1-3） +static MOTOR_LZ_Param_t lz_params[3] = { + { .can = BSP_CAN_1, .motor_id = 127, .host_id = 0xff, .module = MOTOR_LZ_RSO3, .reverse = true, .mode=MOTOR_LZ_MODE_MOTION}, + { .can = BSP_CAN_1, .motor_id = 126, .host_id = 0xff, .module = MOTOR_LZ_RSO3, .reverse = true, .mode=MOTOR_LZ_MODE_MOTION}, + { .can = BSP_CAN_1, .motor_id = 125, .host_id = 0xff, .module = MOTOR_LZ_RSO3, .reverse = false, .mode=MOTOR_LZ_MODE_MOTION}, +}; + +// DM电机参数（关节4-6） +static MOTOR_DM_Param_t dm_params[3] = { + {.can = BSP_CAN_1, .master_id = 0x14, .can_id = 0x04, .module = MOTOR_DM_J4310, .reverse = false,}, + {.can = BSP_CAN_1, .master_id = 0x15, .can_id = 0x05, .module = MOTOR_DM_J4310, .reverse = false,}, + {.can = BSP_CAN_1, .master_id = 0x16, .can_id = 0x06, .module = MOTOR_DM_J4310, .reverse = false,}, +}; + +// DH参数 (单位: m, rad, kg) +// rc[3]: 质心在 DH 连杆坐标系下的坐标 (m) +// 来源：URDF 经完整RPY旋转矩阵逆变换到 DH 坐标系 +// 变换公式: rc_dh = R_rpy^(-1) * rc_urdf，其中R_rpy来自URDF joint的rpy参数 +// 数据来源：fix2026224/arm/urdf/arm.urdf (2026-02-24 重建模更新) +// +// MIT控制参数kp/kd调优建议： +// - kp：位置刚度，值越大响应越快但易振荡。大关节（J2/J3）可适当降低避免抖动 +// - kd：阻尼系数，值越大越平稳但响应慢。根据实际测试调整 +// - 设置为0则使用默认值（LZ: kp=10,kd=0.5; DM: kp=50,kd=3） +static Arm6dof_DHParams_t dh_params[6] = { + // J1: 腰部旋转（LZ电机，垂直轴，负载小） j1 rpy=(0,0,0) → rc_dh=rc_urdf + {.theta=0.0f, .d=0.0405f, .a=0.0014f, .alpha=-M_PI_2, .theta_offset=0.0f, .qmin=-15.7f, .qmax=15.7f, .m=2.3045f, .rc={ 0.00669710f, -0.00030098f, 0.04424100f}, .kp=10.0f, .kd=0.0f}, + // J2: 大臂俯仰（LZ电机，负载最大，降低kp避免抖动） j2 rpy=(-90,0,-90) + {.theta=0.0f, .d=0.0f, .a=0.388f, .alpha=0.0f, .theta_offset=-M_PI_2, .qmin=-1.57f, .qmax=1.57f, .m=0.8903f, .rc={ 0.26586000f, -0.00044116f, 0.00000094f}, .kp=10.0f, .kd=0.0f}, + // J3: 小臂（LZ电机，负载中等） j3 rpy=(0,0,-90) + {.theta=0.0f, .d=0.002795f, .a=0.047775f, .alpha=-M_PI_2, .theta_offset=-M_PI_2, .qmin=-1.0f, .qmax=3.0f, .m=0.72964f, .rc={ 0.08696299f, 0.00167372f, -0.02500400f}, .kp=20.0f, .kd=0.0f}, + // J4: 腕部旋转（DM电机，roll轴，0到2π循环） j4 rpy=(-90,0,-90) + {.theta=0.0f, .d=0.241f, .a=0.0f, .alpha=M_PI_2, .theta_offset=M_PI, .qmin=0.0f, .qmax=6.3f, .m=0.60215f, .rc={ 0.00207070f, -0.14360000f, 0.00004920f}, .kp=10.0f, .kd=0.0f}, + // J5: 腕部俯仰（DM电机，负载小，可提高响应） j5 rpy=(90,0,-180) + {.theta=0.0f, .d=0.0f, .a=0.1065f, .alpha=-M_PI_2, .theta_offset=0.0f, .qmin=-1.9f, .qmax=1.9f, .m=0.21817f, .rc={ 0.00001750f, 0.00297341f, 0.05816899f}, .kp=15.0f, .kd=0.0f}, + // {.theta=0.0f, .d=0.0f, .a=0.1065f, .alpha=-M_PI_2, .theta_offset=M_PI_2, .qmin=-1.9f, .qmax=1.9f, .m=0.21817f, .rc={ 0.00001750f, 0.00297341f, 0.05816899f}, .kp=15.0f, .kd=0.0f}, + // J6: 末端Roll（DM电机，最小负载，roll轴，0到2π循环） j6 rpy=(90,0,180) + {.theta=0.0f, .d=0.0040025f, .a=0.0f, .alpha=0.0f, .theta_offset=M_PI, .qmin=0.0f, .qmax=6.3f, .m=0.57513f, .rc={-0.00002134f, 0.09993500f, 0.00053860f}, .kp=5.0f, .kd=0.0f}, +}; + +static float q_offset[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}; +// 全局对象指针 +IMotor* motors[6] = {nullptr}; // 多态接口 +MotorLZ* motors_lz[3] = {nullptr}; // 用于调试查看 +MotorDM* motors_dm[3] = {nullptr}; // 用于调试查看 +Joint* joints[6] = {nullptr}; +RoboticArm* robot_arm = nullptr; + +Arm6dof_Pose_t target_pose = {0}; // 控制输入：目标末端位姿，单位 m/rad（内部计算全链路SI单位） + +// ============================================================================ +// 测试用调试变量（可在调试器Watch窗口直接观察和修改） +// ============================================================================ +// 阶段控制：在调试器中修改此值切换测试阶段 +// 0 = 仅计算重力补偿力矩，全部电机松弛，观察 gravity_torques_dbg 是否合理 +// 1 = 仅关节6（Roll轴）输出纯力矩，其余松弛（Roll轴重力补偿接近0，最安全） +// 2 = 全部六轴输出（完整重力补偿，GRAVITY_COMP模式） +// 3 = 笛卡尔空间控制：控制末端目标位姿，逆运动学解算各关节角度 +uint8_t test_stage = 3; +Arm6dof_JointAngles_t current_angles; // 调试观察：当前各关节角度 (rad) +// 重力补偿力矩观察数组（对应关节1~6，单位 N·m） +float gravity_torques_dbg[6] = {0.0f}; +// 末端位姿观察（mm换算，便于观察，不参与计算） +struct { float x, y, z, roll, pitch, yaw; } end_pose_mm_dbg = {0}; +// FK→IK验证：IK解算得到的关节角度 +Arm6dof_JointAngles_t ik_from_fk_result; // IK解算结果 + +int setzero=0; +// 重力补偿缩放系数（可在调试器中实时修改，用于测试补偿强度） +float gravity_comp_scale = 1.0f; +// 轨迹进度观察 [0.0~1.0]，1.0=已到达目标；调试器中可观察运动是否平滑完成 +float traj_progress_dbg = 0.0f; +// 轨迹速度设置：可在调试器中修改这两个值，重启case4生效 +float traj_lin_vel = 0.15f; // 末端线速度 (m/s)，默认 150mm/s +float traj_ang_vel = 1.0f; // 末端角速度 (rad/s)，默认 ~57°/s +// 同步控制：设置为1时将target_pose同步为当前位姿（世界系，供MoveCartesian使用），同步后自动清零 +int sync_target_to_current = 0; +// 航向系同步：设置为1时将target_pose同步为当前位姿的航向系表示（供MoveCartesianHeading使用），同步后自动清零 +// 位置：p_h = Rz(-yaw)*p_w +// 姿态：R_h = Rz(-yaw)*R_cur 提取RPY，同步后直接修改 yaw/pitch/roll 即可直观控制末端各轴旋转 +int sync_heading_to_current = 0; +// 工具系同步：设置为1时将target_pose同步为当前位姿的工具系表示（供MoveCartesianTool使用），同步后自动清零 +// 位置：p_tool = R_cur^T * p_world +// 姿态：roll=pitch=yaw=0 即 R_target=I，机械臂保持当前末端姿态不变 +int sync_tool_to_current = 0; + +extern "C" { + +void Task_arm_main(void* argument) { + + /* 计算任务运行到指定频率需要等待的tick数 */ + const uint32_t delay_tick = osKernelGetTickFreq() / ARM_MAIN_FREQ; + osDelay(ARM_MAIN_INIT_DELAY); /* 延时一段时间再开启任务 */ + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ + + BSP_CAN_Init(); + MOTOR_LZ_Init(); // 注册LZ电机ID解析器 + + // 初始化运动学模型 + Arm6dof_Init(dh_params); + + // 创建电机对象（同时保存到基类和派生类指针） + motors_lz[0] = new MotorLZ(lz_params[0]); + motors_lz[1] = new MotorLZ(lz_params[1]); + motors_lz[2] = new MotorLZ(lz_params[2]); + motors_dm[0] = new MotorDM(dm_params[0]); + motors_dm[1] = new MotorDM(dm_params[1]); + motors_dm[2] = new MotorDM(dm_params[2]); + + // 填充基类指针数组（用于多态管理） + motors[0] = motors_lz[0]; + motors[1] = motors_lz[1]; + motors[2] = motors_lz[2]; + motors[3] = motors_dm[0]; + motors[4] = motors_dm[1]; + motors[5] = motors_dm[2]; + for (int i = 0; i < 6; ++i) { + joints[i] = new Joint(i, motors[i], dh_params[i], q_offset[i], ARM_MAIN_FREQ); + } + robot_arm = new RoboticArm(); + for (int i = 0; i < 6; ++i) { + robot_arm->AddJoint(i, joints[i]); + } + robot_arm->Init(); + robot_arm->Enable(true); + + // 使能重力补偿 + robot_arm->EnableGravityCompensation(true); + robot_arm->SetGravityCompScale(gravity_comp_scale); // 补偿系数，可微调 + + // 设置控制模式（可选以下模式之一）: + // GRAVITY_COMP: 位置保持 + 重力补偿前馈（位控+补偿） + // TEACH: 纯重力补偿力矩输出，零刚度（示教拖动） + // JOINT_POSITION: 关节位置控制 + 重力补偿前馈 + robot_arm->SetMode(ControlMode::GRAVITY_COMP); + + // 将 target_pose 初始化为当前末端位姿，防止 stage4 首次进入时进行大范围跳变 + robot_arm->Update(); // 先跨一帧读取当前状态 + target_pose = robot_arm->GetEndPose(); + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ +/*---------------------------零点校准---------------------------*/ + if (setzero) { + if (setzero <= 3) { + MOTOR_LZ_SetZero(&lz_params[setzero-1]); + } else if (setzero > 3&&setzero < 7) { + MOTOR_DM_SetZero(&dm_params[setzero-4]); + } else if (setzero == 7) { + for (int i = 0; i < 3; ++i) { + MOTOR_LZ_SetZero(&lz_params[i]); + } + for (int i = 0; i < 3; ++i) { + MOTOR_DM_SetZero(&dm_params[i]); + } + } + setzero = 0; + } +/*--------------------------------------------------------------*/ + + // 更新机械臂状态（读取关节角度、FK、重力补偿计算） + robot_arm->Update(); + + // 同步target到current（调试时手动触发） + if (sync_target_to_current) { + target_pose = robot_arm->GetEndPose(); + sync_target_to_current = 0; // 自动清零 + } + // 航向系同步：将target_pose设为当前位姿的航向系表示，供MoveCartesianHeading使用 + // 位置：Rz(-yaw) * p_world → 航向系坐标 + // 姿态：R_heading = Rz(-yaw) * R_cur，提取RPY后写入target_pose + // 同步后 yaw/pitch/roll 即为末端在航向系下的当前姿态，修改哪个轴就转哪个轴 + if (sync_heading_to_current) { + const Arm6dof_Pose_t& ep = robot_arm->GetEndPose(); + float cy = cosf(ep.yaw), sy = sinf(ep.yaw); + // 位置逆变换 + target_pose.x = cy * ep.x + sy * ep.y; + target_pose.y = -sy * ep.x + cy * ep.y; + target_pose.z = ep.z; + // 姿态逆变换：R_h = Rz(-yaw) * R_cur，再提取RPY + float rpy_cur_arr[3] = {ep.yaw, ep.pitch, ep.roll}; + float rpy_mzy_arr[3] = {-ep.yaw, 0.0f, 0.0f}; // Rz(-yaw) + Matrixf<3,3> R_h = robotics::rpy2r(Matrixf<3,1>(rpy_mzy_arr)) + * robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr)); + Matrixf<3,1> rpy_h = robotics::r2rpy(R_h); + target_pose.yaw = rpy_h[0][0]; + target_pose.pitch = rpy_h[1][0]; + target_pose.roll = rpy_h[2][0]; + sync_heading_to_current = 0; // 自动清零 + } + // 工具系同步：将target_pose设为当前位姿的工具系表示，供MoveCartesianTool使用 + // 位置：p_tool = R_cur^T * p_world + // 姿态：roll=pitch=yaw=0 即 R_target=I → R_world=R_cur，保持当前末端姿态不变 + if (sync_tool_to_current) { + const Arm6dof_Pose_t& ep = robot_arm->GetEndPose(); + float rpy_cur_arr[3] = {ep.yaw, ep.pitch, ep.roll}; + Matrixf<3,3> R = robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr)); + float p_arr[3] = {ep.x, ep.y, ep.z}; + Matrixf<3,1> p_tool = R.trans() * Matrixf<3,1>(p_arr); // R^T * p_world + target_pose.x = p_tool[0][0]; + target_pose.y = p_tool[1][0]; + target_pose.z = p_tool[2][0]; + target_pose.yaw = 0.0f; // R_target = I，保持姿态 + target_pose.pitch = 0.0f; + target_pose.roll = 0.0f; + sync_tool_to_current = 0; // 自动清零 + } + + // 每帧同步调试观察变量 + for (int i = 0; i < 6; ++i) { + current_angles.q[i] = joints[i]->GetCurrentAngle(); + gravity_torques_dbg[i] = robot_arm->GetGravityTorque(i); + } + + // 末端位姿换算为mm，便于调试器观察（不参与任何计算） + const Arm6dof_Pose_t& ep = robot_arm->GetEndPose(); + end_pose_mm_dbg = { ep.x * 1000.0f, ep.y * 1000.0f, ep.z * 1000.0f, + ep.roll, ep.pitch, ep.yaw }; + + switch (test_stage) { + case 0: + // 阶段1：仅计算，不输出。观察 gravity_torques_dbg[0~5] 是否量级合理 + for (int i = 0; i < 6; ++i) motors[i]->Relax(); + break; + + case 1: + // 阶段1：测试单个关节力矩输出（用于验证力矩计算） + // 修改下面的索引来测试不同关节：j2=1, j3=2, j5=4 + motors[0]->Relax(); + motors[1]->Relax(); + motors[2]->Relax(); + motors[3]->Relax(); + motors[4]->Relax(); + motors[5]->Relax(); + // joints[1]->TorqueControl(gravity_torques_dbg[1]); // j2大臂 + // joints[2]->TorqueControl(gravity_torques_dbg[2]); // j3小臂 + // joints[4]->TorqueControl(gravity_torques_dbg[4]); // j5腕部 + + // joints[0]->TorqueControl(gravity_torques_dbg[0]); + // joints[3]->TorqueControl(gravity_torques_dbg[3]); + // joints[5]->TorqueControl(gravity_torques_dbg[5]); + break; + case 2: + default: + // 阶段4：全部六轴 GRAVITY_COMP 模式（位置保持 + 重力补偿前馈） + robot_arm->SetMode(ControlMode::GRAVITY_COMP); + robot_arm->Control(); + break; + + case 3: + // 笛卡尔空间轨迹规划控制 + // 修改 target_pose 后，机械臂自动从当前位姿平滑运动到目标位姿 + robot_arm->SetLinVelLimit(traj_lin_vel); + robot_arm->SetAngVelLimit(traj_ang_vel); + robot_arm->SetMode(ControlMode::CARTESIAN_POSITION); + // robot_arm->MoveCartesianTool(target_pose); + // robot_arm->MoveCartesianHeading(target_pose); + robot_arm->MoveCartesian(target_pose); + robot_arm->Control(); + + ik_from_fk_result = robot_arm->GetIkAngles(); + traj_progress_dbg = robot_arm->GetTrajProgress(); + break; + } + + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ + } +} + +} // extern "C" diff --git a/User/task/blink.c b/User/task/blink.c new file mode 100644 index 0000000..8d6a71e --- /dev/null +++ b/User/task/blink.c @@ -0,0 +1,59 @@ +/* + blink Task + +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "cmsis_os2.h" +#include "task/user_task.h" +/* USER INCLUDE BEGIN */ +#include "bsp/pwm.h" +#include +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +void Task_blink(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + + + /* 计算任务运行到指定频率需要等待的tick数 */ + const uint32_t delay_tick = osKernelGetTickFreq() / BLINK_FREQ; + + osDelay(BLINK_INIT_DELAY); /* 延时一段时间再开启任务 */ + + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ + /* USER CODE INIT BEGIN */ + BSP_PWM_SetComp(BSP_PWM_TIM5_CH1, 0.0f); + BSP_PWM_Start(BSP_PWM_TIM5_CH1); + BSP_PWM_SetComp(BSP_PWM_TIM5_CH2, 0.0f); + BSP_PWM_Start(BSP_PWM_TIM5_CH2); + BSP_PWM_SetComp(BSP_PWM_TIM5_CH3, 0.0f); + BSP_PWM_Start(BSP_PWM_TIM5_CH3); + /* USER CODE INIT END */ + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* USER CODE BEGIN */ + // 呼吸灯 - 基于tick的正弦波 + float dutyB = (sinf(tick * 0.003f) + 1.0f) * 0.25f; // 0到1之间的正弦波，加快频率 + float dutyG = (sinf(tick * 0.000f) + 1.0f) * 0.25f; // 0到1之间的正弦波，加快频率 + float dutyR = (sinf(tick * 0.008f) + 1.0f) * 0.25f; // 0到1之间的正弦波，加快频率 + + BSP_PWM_SetComp(BSP_PWM_TIM5_CH1, dutyB); + BSP_PWM_SetComp(BSP_PWM_TIM5_CH2, dutyG); + BSP_PWM_SetComp(BSP_PWM_TIM5_CH3, dutyR); + osDelay(1); + /* USER CODE END */ + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ + } + +} \ No newline at end of file diff --git a/User/task/cmd.cpp b/User/task/cmd.cpp new file mode 100644 index 0000000..a65800f --- /dev/null +++ b/User/task/cmd.cpp @@ -0,0 +1,73 @@ +/* + cmd Task + +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" +/* USER INCLUDE BEGIN */ +#include "device/dr16.h" +#include "device/at9s.h" +#include "module/config.h" +#include "module/chassis.h" +#include "module/arm_oop.hpp" +#include "module/cmd/cmd.h" +#include "bsp/can.h" +//#define DEAD_AREA 0.05f +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ +#if CMD_RCTypeTable_Index == 0 +DR16_t cmd_dr16; +#elif CMD_RCTypeTable_Index == 1 +AT9S_t cmd_at9s; +#endif +Chassis_CMD_t *cmd_for_chassis; + +static CMD_t cmd; +/* USER STRUCT END */ + +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +void Task_cmd(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + + + /* 计算任务运行到指定频率需要等待的tick数 */ + const uint32_t delay_tick = osKernelGetTickFreq() / CMD_FREQ; + + osDelay(CMD_INIT_DELAY); /* 延时一段时间再开启任务 */ + + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ + /* USER CODE INIT BEGIN */ + CMD_Init(&cmd, &Config_GetRobotParam()->cmd_param); + /* 注册CAN接收ID */ + /* USER CODE INIT END */ + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* USER CODE BEGIN */ + #if CMD_RCTypeTable_Index == 0 + osMessageQueueGet(task_runtime.msgq.cmd.rc, &cmd_dr16, NULL, 0); + #elif CMD_RCTypeTable_Index == 1 + osMessageQueueGet(task_runtime.msgq.cmd.rc, &cmd_at9s, NULL, 0); + #endif + + /* 从CAN2接收AI命令 */ + + CMD_Update(&cmd); + + /* 获取命令发送到各模块 */ + cmd_for_chassis = CMD_GetChassisCmd(&cmd); + + osMessageQueueReset(task_runtime.msgq.chassis.cmd); + osMessageQueuePut(task_runtime.msgq.chassis.cmd, cmd_for_chassis, 0, 0); + /* USER CODE END */ + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ + } + +} \ No newline at end of file diff --git a/User/task/config.yaml b/User/task/config.yaml new file mode 100644 index 0000000..8535c59 --- /dev/null +++ b/User/task/config.yaml @@ -0,0 +1,14 @@ +- delay: 0 + description: '' + freq_control: true + frequency: 500.0 + function: Task_blink + name: blink + stack: 256 +- delay: 0 + description: '机械臂控制任务，包含逆运动学计算' + freq_control: true + frequency: 1000.0 + function: Task_arm_main + name: arm_main + stack: 4096 # 增加栈大小到4KB，支持逆运动学矩阵计算 diff --git a/User/task/ctrl_chassis.c b/User/task/ctrl_chassis.c new file mode 100644 index 0000000..51ca867 --- /dev/null +++ b/User/task/ctrl_chassis.c @@ -0,0 +1,63 @@ +/* + ctrl_chassis Task + +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" +/* USER INCLUDE BEGIN */ +#include "module/chassis.h" +#include "module/config.h" +#include "device/supercap.h" +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ +Chassis_t chassis; +Chassis_CMD_t chassis_cmd; +Chassis_IMU_t chassis_imu; +float max =50.0f; + +/* USER STRUCT END */ + +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +void Task_ctrl_chassis(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + + + /* 计算任务运行到指定频率需要等待的tick数 */ + const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_CHASSIS_FREQ; + + osDelay(CTRL_CHASSIS_INIT_DELAY); /* 延时一段时间再开启任务 */ + + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ + /* USER CODE INIT BEGIN */ + + // Config_RobotParam_t *cfg = Config_GetRobotParam(); + // Chassis_Init(&chassis, &cfg->chassis_param, (float)CTRL_CHASSIS_FREQ); + // chassis.mech_zero=0.57f; + /* USER CODE INIT END */ + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* USER CODE BEGIN */ + // float encoder_gimbalYawMotor; + // osMessageQueueGet(task_runtime.msgq.chassis.yaw, &encoder_gimbalYawMotor, NULL, 0); + // osMessageQueueGet(task_runtime.msgq.chassis.cmd, &chassis_cmd, NULL, 0); + // chassis.feedback.encoder_gimbalYawMotor=encoder_gimbalYawMotor; + + // Chassis_UpdateFeedback(&chassis); + // Chassis_Control(&chassis, &chassis_cmd, osKernelGetTickCount()); + + // Chassis_Power_Control(&chassis,max); + + // Chassis_Output(&chassis); + /* USER CODE END */ + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ + } + +} diff --git a/User/task/init.c b/User/task/init.c new file mode 100644 index 0000000..42722bc --- /dev/null +++ b/User/task/init.c @@ -0,0 +1,55 @@ +/* + Init Task + 任务初始化，创建各个线程任务和消息队列 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" + +/* USER INCLUDE BEGIN */ +#include "device/dr16.h" +#include "module/chassis.h" + +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ + +/** + * \brief 初始化 + * + * \param argument 未使用 + */ +void Task_Init(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + /* USER CODE INIT BEGIN */ + + /* USER CODE INIT END */ + osKernelLock(); /* 锁定内核，防止任务切换 */ + + /* 创建任务线程 */ + task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink); + task_runtime.thread.arm_main = osThreadNew(Task_arm_main, NULL, &attr_arm_main); + task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc); + task_runtime.thread.cmd = osThreadNew(Task_cmd, NULL, &attr_cmd); + + // 创建消息队列 + /* USER MESSAGE BEGIN */ + task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL); + #if CMD_RCTypeTable_Index == 0 + task_runtime.msgq.cmd.rc= osMessageQueueNew(3u, sizeof(DR16_t), NULL); +#elif CMD_RCTypeTable_Index == 1 + task_runtime.msgq.cmd.rc= osMessageQueueNew(3u, sizeof(AT9S_t), NULL); +#endif + task_runtime.msgq.chassis.yaw = osMessageQueueNew(2u, sizeof(float), NULL); + task_runtime.msgq.chassis.cmd= osMessageQueueNew(3u, sizeof(Chassis_CMD_t), NULL); + + /* USER MESSAGE END */ + + osKernelUnlock(); // 解锁内核 + osThreadTerminate(osThreadGetId()); // 任务完成后结束自身 +} \ No newline at end of file diff --git a/User/task/rc.c b/User/task/rc.c new file mode 100644 index 0000000..40a2ddf --- /dev/null +++ b/User/task/rc.c @@ -0,0 +1,86 @@ +/* + rc Task + +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" +#include "bsp/uart.h" +/* USER INCLUDE BEGIN */ + +#define DR16 +#ifdef AT9S +#include "device/at9s_pro.h" +#endif + +#ifdef DR16 +#include "device/dr16.h" +#endif +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ +#ifdef AT9S + uint8_t cmd_buffer[AT9S_FRAME_LEN]; + AT9S_t at9s; +#endif +#ifdef DR16 +// 将DR16变量放到RAM_D1 (0x24000000)，用于DMA传输 +// __attribute__((section(".ram_d1"))) __attribute__((aligned(32))) +DR16_t dr16; +#endif +/* USER STRUCT END */ + +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +void Task_rc(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + + + /* 计算任务运行到指定频率需要等待的tick数 */ + const uint32_t delay_tick = osKernelGetTickFreq() / RC_FREQ; + + osDelay(RC_INIT_DELAY); /* 延时一段时间再开启任务 */ + + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ + /* USER CODE INIT BEGIN */ + #ifdef AT9S + AT9S_Init(&at9s); + AT9S_StartDmaRecv(cmd_buffer); + #endif + #ifdef DR16 + // DR16_Init(&dr16); + // DR16_StartDmaRecv(&dr16); // 立即启动第一次接收 + #endif + /* USER CODE INIT END */ + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* USER CODE BEGIN */ + #ifdef AT9S + if (AT9S_WaitDmaCplt(10)) { + AT9S_ParseRaw(cmd_buffer, &at9s); + AT9S_StartDmaRecv(cmd_buffer); + } + osMessageQueuePut(task_runtime.msgq.cmd.rc, &at9s, 0, 0); + #endif + #ifdef DR16 + // // 等待DMA接收完成 + // if (DR16_WaitDmaCplt(20)) { + // DR16_ParseData(&dr16); + // } else { + // DR16_Offline(&dr16); + // } + // osMessageQueueReset(task_runtime.msgq.cmd.rc); + // osMessageQueuePut(task_runtime.msgq.cmd.rc, &dr16, 0, 0); + // // 启动下一次DMA接收 + // DR16_StartDmaRecv(&dr16); + #endif + /* USER CODE END */ + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ + } + +} diff --git a/User/task/user_task.c b/User/task/user_task.c new file mode 100644 index 0000000..a82529d --- /dev/null +++ b/User/task/user_task.c @@ -0,0 +1,36 @@ +#include "task/user_task.h" + +Task_Runtime_t task_runtime; + +const osThreadAttr_t attr_init = { + .name = "Task_Init", + .priority = osPriorityRealtime, + .stack_size = 256 * 4, +}; + +/* User_task */ +const osThreadAttr_t attr_blink = { + .name = "blink", + .priority = osPriorityNormal, + .stack_size = 256 * 4, +}; +const osThreadAttr_t attr_arm_main = { + .name = "arm_main", + .priority = osPriorityNormal, + .stack_size = 2048 * 4, +}; +const osThreadAttr_t attr_rc = { + .name = "rc", + .priority = osPriorityNormal, + .stack_size = 256 * 4, +}; +const osThreadAttr_t attr_ctrl_chassis = { + .name = "ctrl_chassis", + .priority = osPriorityNormal, + .stack_size = 512 * 16, +}; +const osThreadAttr_t attr_cmd = { + .name = "cmd", + .priority = osPriorityNormal, + .stack_size = 256 * 4, +}; \ No newline at end of file diff --git a/User/task/user_task.h b/User/task/user_task.h new file mode 100644 index 0000000..223d970 --- /dev/null +++ b/User/task/user_task.h @@ -0,0 +1,125 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif +/* Includes ----------------------------------------------------------------- */ +#include +#include "FreeRTOS.h" +#include "task.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ +/* Exported constants ------------------------------------------------------- */ +/* 任务运行频率 */ +#define BLINK_FREQ (100.0) +#define ARM_MAIN_FREQ (500.0) +#define RC_FREQ (500.0) +#define CTRL_CHASSIS_FREQ (500.0) +#define CMD_FREQ (500.0) + + + + +/* 任务初始化延时ms */ +#define TASK_INIT_DELAY (100u) +#define BLINK_INIT_DELAY (200) +#define ARM_MAIN_INIT_DELAY (200) +#define RC_INIT_DELAY (0) +#define CTRL_CHASSIS_INIT_DELAY (0) +#define CMD_INIT_DELAY (0) + +/* Exported defines --------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + +/* 任务运行时结构体 */ +typedef struct { + /* 各任务，也可以叫做线程 */ + struct { + osThreadId_t blink; + osThreadId_t arm_main; + osThreadId_t rc; + osThreadId_t ctrl_chassis; + osThreadId_t cmd; + + } thread; + + /* USER MESSAGE BEGIN */ + struct { + osMessageQueueId_t user_msg; /* 用户自定义任务消息队列 */ + struct { + osMessageQueueId_t imu; + osMessageQueueId_t cmd; + osMessageQueueId_t yaw; + }chassis; + struct{ + osMessageQueueId_t rc; + osMessageQueueId_t pc; + osMessageQueueId_t nuc; + osMessageQueueId_t ref; + }cmd; + } msgq; + /* USER MESSAGE END */ + + /* 机器人状态 */ + struct { + float battery; /* 电池电量百分比 */ + float vbat; /* 电池电压 */ + float cpu_temp; /* CPU温度 */ + } status; + + /* USER CONFIG BEGIN */ + + /* USER CONFIG END */ + + /* 各任务的stack使用 */ + struct { + UBaseType_t blink; + UBaseType_t arm_main; + UBaseType_t rc; + UBaseType_t ctrl_chassis; + UBaseType_t cmd; + } stack_water_mark; + + /* 各任务运行频率 */ + struct { + float blink; + float arm_main; + float rc; + float ctrl_chassis; + float cmd; + } freq; + + /* 任务最近运行时间 */ + struct { + float blink; + float arm_main; + float rc; + float ctrl_chassis; + float cmd; + } last_up_time; + +} Task_Runtime_t; + +/* 任务运行时结构体 */ +extern Task_Runtime_t task_runtime; + +/* 初始化任务句柄 */ +extern const osThreadAttr_t attr_init; +extern const osThreadAttr_t attr_blink; +extern const osThreadAttr_t attr_arm_main; +extern const osThreadAttr_t attr_rc; +extern const osThreadAttr_t attr_ctrl_chassis; +extern const osThreadAttr_t attr_cmd; +/* 任务函数声明 */ +void Task_Init(void *argument); +void Task_blink(void *argument); +void Task_arm_main(void *argument); +void Task_rc(void *argument); +void Task_ctrl_chassis(void *argument); +void Task_cmd(void *argument); +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/arm.ioc b/arm.ioc new file mode 100644 index 0000000..df2ef09 --- /dev/null +++ b/arm.ioc @@ -0,0 +1,221 @@ +#MicroXplorer Configuration settings - do not modify +CAD.formats= +CAD.pinconfig= +CAD.provider= +CAN1.BS1=CAN_BS1_10TQ +CAN1.BS2=CAN_BS2_3TQ +CAN1.CalculateBaudRate=1000000 +CAN1.CalculateTimeBit=1000 +CAN1.CalculateTimeQuantum=71.42857142857143 +CAN1.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate,BS1,BS2,Prescaler,NART +CAN1.NART=ENABLE +CAN1.Prescaler=3 +CAN2.BS1=CAN_BS1_10TQ +CAN2.BS2=CAN_BS2_3TQ +CAN2.CalculateBaudRate=1000000 +CAN2.CalculateTimeBit=1000 +CAN2.CalculateTimeQuantum=71.42857142857143 +CAN2.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate,BS1,BS2,Prescaler,NART +CAN2.NART=ENABLE +CAN2.Prescaler=3 +Dma.Request0=USART3_RX +Dma.RequestsNb=1 +Dma.USART3_RX.0.Direction=DMA_PERIPH_TO_MEMORY +Dma.USART3_RX.0.FIFOMode=DMA_FIFOMODE_DISABLE +Dma.USART3_RX.0.Instance=DMA1_Stream1 +Dma.USART3_RX.0.MemDataAlignment=DMA_MDATAALIGN_BYTE +Dma.USART3_RX.0.MemInc=DMA_MINC_ENABLE +Dma.USART3_RX.0.Mode=DMA_NORMAL +Dma.USART3_RX.0.PeriphDataAlignment=DMA_PDATAALIGN_BYTE +Dma.USART3_RX.0.PeriphInc=DMA_PINC_DISABLE +Dma.USART3_RX.0.Priority=DMA_PRIORITY_LOW +Dma.USART3_RX.0.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode +FREERTOS.IPParameters=Tasks01,configTOTAL_HEAP_SIZE +FREERTOS.Tasks01=defaultTask,24,128,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL +FREERTOS.configTOTAL_HEAP_SIZE=20480 +File.Version=6 +GPIO.groupedBy= +KeepUserPlacement=false +Mcu.CPN=STM32F407IGH6 +Mcu.Family=STM32F4 +Mcu.IP0=CAN1 +Mcu.IP1=CAN2 +Mcu.IP2=DMA +Mcu.IP3=FREERTOS +Mcu.IP4=NVIC +Mcu.IP5=RCC +Mcu.IP6=SYS +Mcu.IP7=TIM5 +Mcu.IP8=USART3 +Mcu.IPNb=9 +Mcu.Name=STM32F407I(E-G)Hx +Mcu.Package=UFBGA176 +Mcu.Pin0=PB5 +Mcu.Pin1=PA14 +Mcu.Pin10=PH12 +Mcu.Pin11=PH11 +Mcu.Pin12=PH10 +Mcu.Pin13=VP_FREERTOS_VS_CMSIS_V2 +Mcu.Pin14=VP_SYS_VS_Systick +Mcu.Pin15=VP_TIM5_VS_ClockSourceINT +Mcu.Pin2=PA13 +Mcu.Pin3=PB6 +Mcu.Pin4=PD0 +Mcu.Pin5=PC11 +Mcu.Pin6=PC10 +Mcu.Pin7=PD1 +Mcu.Pin8=PH0-OSC_IN +Mcu.Pin9=PH1-OSC_OUT +Mcu.PinsNb=16 +Mcu.ThirdPartyNb=0 +Mcu.UserConstants= +Mcu.UserName=STM32F407IGHx +MxCube.Version=6.16.1 +MxDb.Version=DB.6.0.161 +NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false +NVIC.CAN1_RX0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true +NVIC.CAN1_RX1_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true +NVIC.CAN1_TX_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true +NVIC.CAN2_RX0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true +NVIC.CAN2_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true +NVIC.CAN2_TX_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true +NVIC.DMA1_Stream1_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true +NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false +NVIC.ForceEnableDMAVector=true +NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false +NVIC.MemoryManagement_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false +NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false +NVIC.PendSV_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:false\:false +NVIC.PriorityGroup=NVIC_PRIORITYGROUP_4 +NVIC.SVCall_IRQn=true\:0\:0\:false\:false\:false\:false\:false\:false\:false +NVIC.SavedPendsvIrqHandlerGenerated=true +NVIC.SavedSvcallIrqHandlerGenerated=true +NVIC.SavedSystickIrqHandlerGenerated=true +NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:true\:true\:false\:true\:false +NVIC.TIM5_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true +NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false +PA13.Mode=Serial_Wire +PA13.Signal=SYS_JTMS-SWDIO +PA14.Mode=Serial_Wire +PA14.Signal=SYS_JTCK-SWCLK +PB5.Mode=CAN_Activate +PB5.Signal=CAN2_RX +PB6.Mode=CAN_Activate +PB6.Signal=CAN2_TX +PC10.Mode=Asynchronous +PC10.Signal=USART3_TX +PC11.Mode=Asynchronous +PC11.Signal=USART3_RX +PCC.Checker=false +PCC.Line=STM32F407/417 +PCC.MCU=STM32F407I(E-G)Hx +PCC.PartNumber=STM32F407IGHx +PCC.Series=STM32F4 +PCC.Temperature=25 +PCC.Vdd=3.3 +PD0.Locked=true +PD0.Mode=CAN_Activate +PD0.Signal=CAN1_RX +PD1.Locked=true +PD1.Mode=CAN_Activate +PD1.Signal=CAN1_TX +PH0-OSC_IN.Mode=HSE-External-Oscillator +PH0-OSC_IN.Signal=RCC_OSC_IN +PH1-OSC_OUT.Mode=HSE-External-Oscillator +PH1-OSC_OUT.Signal=RCC_OSC_OUT +PH10.Locked=true +PH10.Signal=S_TIM5_CH1 +PH11.Locked=true +PH11.Signal=S_TIM5_CH2 +PH12.Locked=true +PH12.Signal=S_TIM5_CH3 +PinOutPanel.CurrentBGAView=Top +PinOutPanel.RotationAngle=0 +ProjectManager.AskForMigrate=true +ProjectManager.BackupPrevious=false +ProjectManager.CompilerLinker=GCC +ProjectManager.CompilerOptimize=6 +ProjectManager.ComputerToolchain=false +ProjectManager.CoupleFile=true +ProjectManager.CustomerFirmwarePackage= +ProjectManager.DefaultFWLocation=true +ProjectManager.DeletePrevious=true +ProjectManager.DeviceId=STM32F407IGHx +ProjectManager.FirmwarePackage=STM32Cube FW_F4 V1.28.3 +ProjectManager.FreePins=false +ProjectManager.FreePinsContext= +ProjectManager.HalAssertFull=false +ProjectManager.HeapSize=0x1000 +ProjectManager.KeepUserCode=true +ProjectManager.LastFirmware=true +ProjectManager.LibraryCopy=0 +ProjectManager.MainLocation=Core/Src +ProjectManager.NoMain=false +ProjectManager.PreviousToolchain= +ProjectManager.ProjectBuild=false +ProjectManager.ProjectFileName=arm.ioc +ProjectManager.ProjectName=arm +ProjectManager.ProjectStructure= +ProjectManager.RegisterCallBack= +ProjectManager.StackSize=0x1000 +ProjectManager.TargetToolchain=CMake +ProjectManager.ToolChainLocation= +ProjectManager.UAScriptAfterPath= +ProjectManager.UAScriptBeforePath= +ProjectManager.UnderRoot=false +ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-MX_CAN1_Init-CAN1-false-HAL-true,5-MX_CAN2_Init-CAN2-false-HAL-true,6-MX_TIM5_Init-TIM5-false-HAL-true,7-MX_USART3_UART_Init-USART3-false-HAL-true +RCC.48MHZClocksFreq_Value=84000000 +RCC.AHBFreq_Value=168000000 +RCC.APB1CLKDivider=RCC_HCLK_DIV4 +RCC.APB1Freq_Value=42000000 +RCC.APB1TimFreq_Value=84000000 +RCC.APB2CLKDivider=RCC_HCLK_DIV2 +RCC.APB2Freq_Value=84000000 +RCC.APB2TimFreq_Value=168000000 +RCC.CortexFreq_Value=168000000 +RCC.EthernetFreq_Value=168000000 +RCC.FCLKCortexFreq_Value=168000000 +RCC.FamilyName=M +RCC.HCLKFreq_Value=168000000 +RCC.HSE_VALUE=12000000 +RCC.HSI_VALUE=16000000 +RCC.I2SClocksFreq_Value=192000000 +RCC.IPParameters=48MHZClocksFreq_Value,AHBFreq_Value,APB1CLKDivider,APB1Freq_Value,APB1TimFreq_Value,APB2CLKDivider,APB2Freq_Value,APB2TimFreq_Value,CortexFreq_Value,EthernetFreq_Value,FCLKCortexFreq_Value,FamilyName,HCLKFreq_Value,HSE_VALUE,HSI_VALUE,I2SClocksFreq_Value,LSE_VALUE,LSI_VALUE,MCO2PinFreq_Value,PLLCLKFreq_Value,PLLM,PLLN,PLLQCLKFreq_Value,PLLSourceVirtual,RTCFreq_Value,RTCHSEDivFreq_Value,SYSCLKFreq_VALUE,SYSCLKSource,VCOI2SOutputFreq_Value,VCOInputFreq_Value,VCOOutputFreq_Value,VcooutputI2S +RCC.LSE_VALUE=32768 +RCC.LSI_VALUE=32000 +RCC.MCO2PinFreq_Value=168000000 +RCC.PLLCLKFreq_Value=168000000 +RCC.PLLM=6 +RCC.PLLN=168 +RCC.PLLQCLKFreq_Value=84000000 +RCC.PLLSourceVirtual=RCC_PLLSOURCE_HSE +RCC.RTCFreq_Value=32000 +RCC.RTCHSEDivFreq_Value=6000000 +RCC.SYSCLKFreq_VALUE=168000000 +RCC.SYSCLKSource=RCC_SYSCLKSOURCE_PLLCLK +RCC.VCOI2SOutputFreq_Value=384000000 +RCC.VCOInputFreq_Value=2000000 +RCC.VCOOutputFreq_Value=336000000 +RCC.VcooutputI2S=192000000 +SH.S_TIM5_CH1.0=TIM5_CH1,PWM Generation1 CH1 +SH.S_TIM5_CH1.ConfNb=1 +SH.S_TIM5_CH2.0=TIM5_CH2,PWM Generation2 CH2 +SH.S_TIM5_CH2.ConfNb=1 +SH.S_TIM5_CH3.0=TIM5_CH3,PWM Generation3 CH3 +SH.S_TIM5_CH3.ConfNb=1 +TIM5.Channel-PWM\ Generation1\ CH1=TIM_CHANNEL_1 +TIM5.Channel-PWM\ Generation2\ CH2=TIM_CHANNEL_2 +TIM5.Channel-PWM\ Generation3\ CH3=TIM_CHANNEL_3 +TIM5.IPParameters=Channel-PWM Generation1 CH1,Channel-PWM Generation2 CH2,Channel-PWM Generation3 CH3,Period,Prescaler +TIM5.Period=65535 +TIM5.Prescaler=0 +USART3.IPParameters=VirtualMode +USART3.VirtualMode=VM_ASYNC +VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2 +VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2 +VP_SYS_VS_Systick.Mode=SysTick +VP_SYS_VS_Systick.Signal=SYS_VS_Systick +VP_TIM5_VS_ClockSourceINT.Mode=Internal +VP_TIM5_VS_ClockSourceINT.Signal=TIM5_VS_ClockSourceINT +board=custom +rtos.0.ip=FREERTOS diff --git a/cmake/gcc-arm-none-eabi.cmake b/cmake/gcc-arm-none-eabi.cmake new file mode 100644 index 0000000..e665e83 --- /dev/null +++ b/cmake/gcc-arm-none-eabi.cmake @@ -0,0 +1,43 @@ +set(CMAKE_SYSTEM_NAME Generic) +set(CMAKE_SYSTEM_PROCESSOR arm) + +set(CMAKE_C_COMPILER_ID GNU) +set(CMAKE_CXX_COMPILER_ID GNU) + +# Some default GCC settings +# arm-none-eabi- must be part of path environment +set(TOOLCHAIN_PREFIX arm-none-eabi-) + +set(CMAKE_C_COMPILER ${TOOLCHAIN_PREFIX}gcc) +set(CMAKE_ASM_COMPILER ${CMAKE_C_COMPILER}) +set(CMAKE_CXX_COMPILER ${TOOLCHAIN_PREFIX}g++) +set(CMAKE_LINKER ${TOOLCHAIN_PREFIX}g++) +set(CMAKE_OBJCOPY ${TOOLCHAIN_PREFIX}objcopy) +set(CMAKE_SIZE ${TOOLCHAIN_PREFIX}size) + +set(CMAKE_EXECUTABLE_SUFFIX_ASM ".elf") +set(CMAKE_EXECUTABLE_SUFFIX_C ".elf") +set(CMAKE_EXECUTABLE_SUFFIX_CXX ".elf") + +set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY) + +# MCU specific flags +set(TARGET_FLAGS "-mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard ") + +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${TARGET_FLAGS}") +set(CMAKE_ASM_FLAGS "${CMAKE_C_FLAGS} -x assembler-with-cpp -MMD -MP") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -fdata-sections -ffunction-sections") + +set(CMAKE_C_FLAGS_DEBUG "-O0 -g3") +set(CMAKE_C_FLAGS_RELEASE "-Os -g0") +set(CMAKE_CXX_FLAGS_DEBUG "-O0 -g3") +set(CMAKE_CXX_FLAGS_RELEASE "-Os -g0") + +set(CMAKE_CXX_FLAGS "${CMAKE_C_FLAGS} -fno-rtti -fno-exceptions -fno-threadsafe-statics") + +set(CMAKE_EXE_LINKER_FLAGS "${TARGET_FLAGS}") +set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -T \"${CMAKE_SOURCE_DIR}/STM32F407XX_FLASH.ld\"") +set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} --specs=nano.specs") +set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,-Map=${CMAKE_PROJECT_NAME}.map -Wl,--gc-sections") +set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--print-memory-usage") +set(TOOLCHAIN_LINK_LIBRARIES "m") diff --git a/cmake/starm-clang.cmake b/cmake/starm-clang.cmake new file mode 100644 index 0000000..b675e8f --- /dev/null +++ b/cmake/starm-clang.cmake @@ -0,0 +1,65 @@ +set(CMAKE_SYSTEM_NAME Generic) +set(CMAKE_SYSTEM_PROCESSOR arm) + +set(CMAKE_C_COMPILER_ID Clang) +set(CMAKE_CXX_COMPILER_ID Clang) + +# Some default llvm settings +set(TOOLCHAIN_PREFIX starm-) + +set(CMAKE_C_COMPILER ${TOOLCHAIN_PREFIX}clang) +set(CMAKE_ASM_COMPILER ${CMAKE_C_COMPILER}) +set(CMAKE_CXX_COMPILER ${TOOLCHAIN_PREFIX}clang++) +set(CMAKE_LINKER ${TOOLCHAIN_PREFIX}clang) +set(CMAKE_OBJCOPY ${TOOLCHAIN_PREFIX}objcopy) +set(CMAKE_SIZE ${TOOLCHAIN_PREFIX}size) + +set(CMAKE_EXECUTABLE_SUFFIX_ASM ".elf") +set(CMAKE_EXECUTABLE_SUFFIX_C ".elf") +set(CMAKE_EXECUTABLE_SUFFIX_CXX ".elf") + +set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY) + +# STARM_TOOLCHAIN_CONFIG allows you to choose the toolchain configuration. +# Possible values are: +# "STARM_HYBRID" : Hybrid configuration using starm-clang Assemler and Compiler and GNU Linker +# "STARM_NEWLIB" : starm-clang toolchain with NEWLIB C library +# "STARM_PICOLIBC" : starm-clang toolchain with PICOLIBC C library +set(STARM_TOOLCHAIN_CONFIG "STARM_HYBRID") + +if(STARM_TOOLCHAIN_CONFIG STREQUAL "STARM_HYBRID") + set(TOOLCHAIN_MULTILIBS "--multi-lib-config=\"$ENV{CLANG_GCC_CMSIS_COMPILER}/multilib.gnu_tools_for_stm32.yaml\" --gcc-toolchain=\"$ENV{GCC_TOOLCHAIN_ROOT}/..\"") +elseif (STARM_TOOLCHAIN_CONFIG STREQUAL "STARM_NEWLIB") + set(TOOLCHAIN_MULTILIBS "--config=newlib.cfg") +endif() + +# MCU specific flags +set(TARGET_FLAGS "-mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard ${TOOLCHAIN_MULTILIBS}") + +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${TARGET_FLAGS}") +set(CMAKE_ASM_FLAGS "${CMAKE_C_FLAGS} -x assembler-with-cpp -MP") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -fdata-sections -ffunction-sections") + +set(CMAKE_C_FLAGS_DEBUG "-O0 -g3") +set(CMAKE_C_FLAGS_RELEASE "-Os -g0") +set(CMAKE_CXX_FLAGS_DEBUG "-O0 -g3") +set(CMAKE_CXX_FLAGS_RELEASE "-Os -g0") + +set(CMAKE_CXX_FLAGS "${CMAKE_C_FLAGS} -fno-rtti -fno-exceptions -fno-threadsafe-statics") + +set(CMAKE_EXE_LINKER_FLAGS "${TARGET_FLAGS}") + +if (STARM_TOOLCHAIN_CONFIG STREQUAL "STARM_HYBRID") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} --gcc-specs=nano.specs") + set(TOOLCHAIN_LINK_LIBRARIES "m") +elseif(STARM_TOOLCHAIN_CONFIG STREQUAL "STARM_NEWLIB") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -lcrt0-nosys") +elseif(STARM_TOOLCHAIN_CONFIG STREQUAL "STARM_PICOLIBC") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -lcrt0-hosted") + +endif() + +set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -T \"${CMAKE_SOURCE_DIR}/STM32F407XX_FLASH.ld\"") +set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,-Map=${CMAKE_PROJECT_NAME}.map -Wl,--gc-sections") +set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -z noexecstack") +set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--print-memory-usage ") diff --git a/cmake/stm32cubemx/CMakeLists.txt b/cmake/stm32cubemx/CMakeLists.txt new file mode 100644 index 0000000..4227996 --- /dev/null +++ b/cmake/stm32cubemx/CMakeLists.txt @@ -0,0 +1,118 @@ +cmake_minimum_required(VERSION 3.22) +# Enable CMake support for ASM and C languages +enable_language(C ASM) +# STM32CubeMX generated symbols (macros) +set(MX_Defines_Syms + USE_HAL_DRIVER + STM32F407xx + $<$:DEBUG> +) + +# STM32CubeMX generated include paths +set(MX_Include_Dirs + ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Inc + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Inc + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy + ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/include + ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 + ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/CMSIS/Device/ST/STM32F4xx/Include + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/CMSIS/Include +) + +# STM32CubeMX generated application sources +set(MX_Application_Src + ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/main.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/gpio.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/freertos.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/can.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/dma.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/tim.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/usart.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_it.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/stm32f4xx_hal_msp.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/sysmem.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/syscalls.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../startup_stm32f407xx.s +) + +# STM32 HAL/LL Drivers +set(STM32_Drivers_Src + ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/system_stm32f4xx.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_uart.c +) + +# Drivers Midllewares + + +set(FreeRTOS_Src + ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/croutine.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/event_groups.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/list.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/queue.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/tasks.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/timers.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c +) + +# Link directories setup +set(MX_LINK_DIRS + +) +# Project static libraries +set(MX_LINK_LIBS + STM32_Drivers + ${TOOLCHAIN_LINK_LIBRARIES} + FreeRTOS +) +# Interface library for includes and symbols +add_library(stm32cubemx INTERFACE) +target_include_directories(stm32cubemx INTERFACE ${MX_Include_Dirs}) +target_compile_definitions(stm32cubemx INTERFACE ${MX_Defines_Syms}) + +# Create STM32_Drivers static library +add_library(STM32_Drivers OBJECT) +target_sources(STM32_Drivers PRIVATE ${STM32_Drivers_Src}) +target_link_libraries(STM32_Drivers PUBLIC stm32cubemx) + + +# Create FreeRTOS static library +add_library(FreeRTOS OBJECT) +target_sources(FreeRTOS PRIVATE ${FreeRTOS_Src}) +target_link_libraries(FreeRTOS PUBLIC stm32cubemx) + +# Add STM32CubeMX generated application sources to the project +target_sources(${CMAKE_PROJECT_NAME} PRIVATE ${MX_Application_Src}) + +# Link directories setup +target_link_directories(${CMAKE_PROJECT_NAME} PRIVATE ${MX_LINK_DIRS}) + +# Add libraries to the project +target_link_libraries(${CMAKE_PROJECT_NAME} ${MX_LINK_LIBS}) + +# Add the map file to the list of files to be removed with 'clean' target +set_target_properties(${CMAKE_PROJECT_NAME} PROPERTIES ADDITIONAL_CLEAN_FILES ${CMAKE_PROJECT_NAME}.map) + +# Validate that STM32CubeMX code is compatible with C standard +if((CMAKE_C_STANDARD EQUAL 90) OR (CMAKE_C_STANDARD EQUAL 99)) + message(ERROR "Generated code requires C11 or higher") +endif() diff --git a/ozone/arm.jdebug b/ozone/arm.jdebug new file mode 100644 index 0000000..f6232bc --- /dev/null +++ b/ozone/arm.jdebug @@ -0,0 +1,361 @@ +/********************************************************************* +* (c) SEGGER Microcontroller GmbH * +* The Embedded Experts * +* www.segger.com * +********************************************************************** + +File : +Created : 26. Feb 2026 22:49 +Ozone Version : V3.40e +*/ + +/********************************************************************* +* +* OnProjectLoad +* +* Function description +* Project load routine. Required. +* +********************************************************************** +*/ +void OnProjectLoad (void) { + // + // Dialog-generated settings + // + Project.AddPathSubstitute ("D:/STM32Projects_HAL/board-F4/arm/ozone", "$(ProjectDir)"); + Project.AddPathSubstitute ("d:/stm32projects_hal/board-f4/arm/ozone", "$(ProjectDir)"); + Project.SetDevice ("STM32F407IG"); + Project.SetHostIF ("USB", ""); + Project.SetTargetIF ("SWD"); + Project.SetTIFSpeed ("4 MHz"); + Project.AddSvdFile ("$(InstallDir)/Config/CPU/Cortex-M4F.svd"); + // + // User settings + // + File.Open ("D:/STM32Projects_HAL/board-F4/arm/build/Debug/arm.elf"); +} + +/********************************************************************* +* +* OnStartupComplete +* +* Function description +* Called when program execution has reached/passed +* the startup completion point. Optional. +* +********************************************************************** +*/ +//void OnStartupComplete (void) { +//} + +/********************************************************************* +* +* TargetReset +* +* Function description +* Replaces the default target device reset routine. Optional. +* +* Notes +* This example demonstrates the usage when +* debugging an application in RAM on a Cortex-M target device. +* +********************************************************************** +*/ +//void TargetReset (void) { +// +// unsigned int SP; +// unsigned int PC; +// unsigned int VectorTableAddr; +// +// VectorTableAddr = Elf.GetBaseAddr(); +// // +// // Set up initial stack pointer +// // +// if (VectorTableAddr != 0xFFFFFFFF) { +// SP = Target.ReadU32(VectorTableAddr); +// Target.SetReg("SP", SP); +// } +// // +// // Set up entry point PC +// // +// PC = Elf.GetEntryPointPC(); +// +// if (PC != 0xFFFFFFFF) { +// Target.SetReg("PC", PC); +// } else if (VectorTableAddr != 0xFFFFFFFF) { +// PC = Target.ReadU32(VectorTableAddr + 4); +// Target.SetReg("PC", PC); +// } else { +// Util.Error("Project file error: failed to set entry point PC", 1); +// } +//} + +/********************************************************************* +* +* BeforeTargetReset +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void BeforeTargetReset (void) { +//} + +/********************************************************************* +* +* AfterTargetReset +* +* Function description +* Event handler routine. Optional. +* The default implementation initializes SP and PC to reset values. +** +********************************************************************** +*/ +void AfterTargetReset (void) { + _SetupTarget(); +} + +/********************************************************************* +* +* DebugStart +* +* Function description +* Replaces the default debug session startup routine. Optional. +* +********************************************************************** +*/ +//void DebugStart (void) { +//} + +/********************************************************************* +* +* TargetConnect +* +* Function description +* Replaces the default target IF connection routine. Optional. +* +********************************************************************** +*/ +//void TargetConnect (void) { +//} + +/********************************************************************* +* +* BeforeTargetConnect +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void BeforeTargetConnect (void) { +//} + +/********************************************************************* +* +* AfterTargetConnect +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void AfterTargetConnect (void) { +//} + +/********************************************************************* +* +* TargetDownload +* +* Function description +* Replaces the default program download routine. Optional. +* +********************************************************************** +*/ +//void TargetDownload (void) { +//} + +/********************************************************************* +* +* BeforeTargetDownload +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void BeforeTargetDownload (void) { +//} + +/********************************************************************* +* +* AfterTargetDownload +* +* Function description +* Event handler routine. Optional. +* The default implementation initializes SP and PC to reset values. +* +********************************************************************** +*/ +void AfterTargetDownload (void) { + _SetupTarget(); +} + +/********************************************************************* +* +* BeforeTargetDisconnect +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void BeforeTargetDisconnect (void) { +//} + +/********************************************************************* +* +* AfterTargetDisconnect +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void AfterTargetDisconnect (void) { +//} + +/********************************************************************* +* +* AfterTargetHalt +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void AfterTargetHalt (void) { +//} + +/********************************************************************* +* +* BeforeTargetResume +* +* Function description +* Event handler routine. Optional. +* +********************************************************************** +*/ +//void BeforeTargetResume (void) { +//} + +/********************************************************************* +* +* OnSnapshotLoad +* +* Function description +* Called upon loading a snapshot. Optional. +* +* Additional information +* This function is used to restore the target state in cases +* where values cannot simply be written to the target. +* Typical use: GPIO clock needs to be enabled, before +* GPIO is configured. +* +********************************************************************** +*/ +//void OnSnapshotLoad (void) { +//} + +/********************************************************************* +* +* OnSnapshotSave +* +* Function description +* Called upon saving a snapshot. Optional. +* +* Additional information +* This function is usually used to save values of the target +* state which can either not be trivially read, +* or need to be restored in a specific way or order. +* Typically use: Memory Mapped Registers, +* such as PLL and GPIO configuration. +* +********************************************************************** +*/ +//void OnSnapshotSave (void) { +//} + +/********************************************************************* +* +* OnError +* +* Function description +* Called when an error ocurred. Optional. +* +********************************************************************** +*/ +//void OnError (void) { +//} + +/********************************************************************* +* +* AfterProjectLoad +* +* Function description +* After Project load routine. Optional. +* +********************************************************************** +*/ +//void AfterProjectLoad (void) { +//} + +/********************************************************************* +* +* OnDebugStartBreakSymbolReached +* +* Function description +* Called when program execution has reached/passed +* the symbol to be breaked at during debug start. Optional. +* +********************************************************************** +*/ +//void OnDebugStartBreakSymReached (void) { +//} + +/********************************************************************* +* +* _SetupTarget +* +* Function description +* Setup the target. +* Called by AfterTargetReset() and AfterTargetDownload(). +* +* Auto-generated function. May be overridden by Ozone. +* +********************************************************************** +*/ +void _SetupTarget(void) { + unsigned int SP; + unsigned int PC; + unsigned int VectorTableAddr; + + VectorTableAddr = Elf.GetBaseAddr(); + // + // Set up initial stack pointer + // + SP = Target.ReadU32(VectorTableAddr); + if (SP != 0xFFFFFFFF) { + Target.SetReg("SP", SP); + } + // + // Set up entry point PC + // + PC = Elf.GetEntryPointPC(); + if (PC != 0xFFFFFFFF) { + Target.SetReg("PC", PC); + } else { + Util.Error("Project script error: failed to set up entry point PC", 1); + } +} \ No newline at end of file diff --git a/ozone/arm.jdebug.user b/ozone/arm.jdebug.user new file mode 100644 index 0000000..01a6b97 --- /dev/null +++ b/ozone/arm.jdebug.user @@ -0,0 +1,37 @@ + + + +Breakpoint=D:/STM32Projects_HAL/board-F4/arm/User/module/arm_oop.hpp:264, State=BP_STATE_ON +Breakpoint=D:/STM32Projects_HAL/board-F4/arm/User/module/arm_oop.hpp:280, State=BP_STATE_ON +OpenDocument="arm6dof.h", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/component/arm_kinematics/arm6dof.h", Line=0 +OpenDocument="can.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/Core/Src/can.c", Line=0 +OpenDocument="arm_oop.hpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/module/arm_oop.hpp", Line=242 +OpenDocument="arm_main.cpp", FilePath="D:/STM32Projects_HAL/board-F4/arm/User/task/arm_main.cpp", Line=139 +OpenDocument="startup_stm32f407xx.s", FilePath="D:/STM32Projects_HAL/board-F4/arm/startup_stm32f407xx.s", Line=39 +OpenDocument="main.c", FilePath="D:/STM32Projects_HAL/board-F4/arm/Core/Src/main.c", Line=45 +OpenToolbar="Debug", Floating=0, x=0, y=0 +OpenWindow="Registers 1", DockArea=RIGHT, x=0, y=0, w=903, h=930, TabPos=2, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, FilteredItems=[], RefreshRate=1 +OpenWindow="Source Files", DockArea=LEFT, x=0, y=0, w=322, h=930, TabPos=0, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Disassembly", DockArea=RIGHT, x=0, y=0, w=903, h=930, TabPos=0, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Memory 1", DockArea=BOTTOM, x=0, y=0, w=463, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, EditorAddress=0x0 +OpenWindow="Watched Data 1", DockArea=RIGHT, x=0, y=0, w=903, h=930, TabPos=1, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Functions", DockArea=LEFT, x=0, y=0, w=322, h=930, TabPos=1, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +OpenWindow="Data Sampling", DockArea=BOTTOM, x=2, y=0, w=725, h=525, TabPos=1, TopOfStack=1, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0, VisibleTab=0, UniformSampleSpacing=0 +OpenWindow="Timeline", DockArea=BOTTOM, x=1, y=0, w=1370, h=544, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=1, DataPaneShown=1, PowerPaneShown=0, CodePaneShown=0, PinCursor="Cursor Movable", TimePerDiv="5 s / Div", TimeStampFormat="Time", DataGraphDrawAsPoints=0, DataGraphLegendShown=1, DataGraphUniformSampleSpacing=0, DataGraphLegendPosition="928;117", DataGraphShowNamesAtCursor=0, PowerGraphDrawAsPoints=0, PowerGraphLegendShown=0, PowerGraphAvgFilterTime=Off, PowerGraphAvgFilterLen=Off, PowerGraphUniformSampleSpacing=0, PowerGraphLegendPosition="1143;-69", CodeGraphLegendShown=0, CodeGraphLegendPosition="1164;0" +OpenWindow="Console", DockArea=BOTTOM, x=2, y=0, w=725, h=525, TabPos=0, TopOfStack=0, FilterBarShown=0, TotalValueBarShown=0, ToolBarShown=0 +SmartViewPlugin="", Page="", Toolbar="Hidden", Window="SmartView 1" +TableHeader="Watched Data 1", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Value";"Location";"Refresh";"Access"], ColWidths=[250;229;145;128;125] +TableHeader="Registers 1", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Description"], ColWidths=[120;144;613] +TableHeader="Functions", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Address";"Size";"#Insts";"Class";"Source"], ColWidths=[1594;104;100;100;27;100] +TableHeader="Power Sampling", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time";"Ch 0"], ColWidths=[100;100;100] +TableHeader="RegisterSelectionDialog", SortCol="None", SortOrder="ASCENDING", VisibleCols=[], ColWidths=[] +TableHeader="Source Files", SortCol="File", SortOrder="ASCENDING", VisibleCols=["File";"Status";"Size";"#Insts";"Path"], ColWidths=[215;100;100;100;1334] +TableHeader="Data Sampling Table", SortCol="None", SortOrder="ASCENDING", VisibleCols=["Index";"Time"], ColWidths=[100;100] +TableHeader="Data Sampling Setup", SortCol="Expression", SortOrder="ASCENDING", VisibleCols=["Expression";"Type";"Value";"Min";"Max";"Average";"# Changes";"Min. Change";"Max. Change"], ColWidths=[118;100;134;100;134;154;110;126;134] +TableHeader="TargetExceptionDialog", SortCol="Name", SortOrder="ASCENDING", VisibleCols=["Name";"Value";"Address";"Description"], ColWidths=[200;100;100;340] +WatchedExpression="robot_arm", RefreshRate=5, Window=Watched Data 1 +WatchedExpression="ik_from_fk_result", RefreshRate=5, Window=Watched Data 1 +WatchedExpression="target_pose", RefreshRate=5, Window=Watched Data 1 +WatchedExpression="current_angles", RefreshRate=5, Window=Watched Data 1 +WatchedExpression="setzero", RefreshRate=5, Window=Watched Data 1 +WatchedExpression="sync_target_to_current", RefreshRate=5, Window=Watched Data 1 \ No newline at end of file diff --git a/scripts/calc_new_dh.py b/scripts/calc_new_dh.py new file mode 100644 index 0000000..05920ac --- /dev/null +++ b/scripts/calc_new_dh.py @@ -0,0 +1,127 @@ +""" +calc_new_dh.py - 计算新URDF的DH参数和质心坐标转换 +""" +import math + +def Rx(angle): + c, s = math.cos(angle), math.sin(angle) + return [[1, 0, 0], [0, c, -s], [0, s, c]] + +def Ry(angle): + c, s = math.cos(angle), math.sin(angle) + return [[c, 0, s], [0, 1, 0], [-s, 0, c]] + +def Rz(angle): + c, s = math.cos(angle), math.sin(angle) + return [[c, -s, 0], [s, c, 0], [0, 0, 1]] + +def mat_mul(A, B): + return [[sum(A[i][k] * B[k][j] for k in range(3)) for j in range(3)] for i in range(3)] + +def mat_vec_mul(M, v): + return tuple(sum(M[i][j] * v[j] for j in range(3)) for i in range(3)) + +def rpy_to_matrix(roll, pitch, yaw): + return mat_mul(mat_mul(Rz(yaw), Ry(pitch)), Rx(roll)) + +def transpose(M): + return [[M[j][i] for j in range(3)] for i in range(3)] + +# ============ NEW URDF (arm.urdf in fix2026224) ============ +print("=" * 72) +print("NEW URDF: Link parameters") +print("=" * 72) + +new_links = [ + ("Link1", (0.0066971, -0.00030098, 0.044241), 2.3045, (0, 0, 0)), + ("Link2", (-3.7501e-8, -0.26586, 0.00044116),0.8903, (-1.5708, 0, -1.5708)), + ("Link3", (0.0016734, -0.086963, -0.025004), 0.72964, (0, 0, -1.5708)), + ("Link4", (4.9724e-5, -0.0020707, 0.1436), 0.60215, (-1.5708, 0, -1.5708)), + ("Link5", (-1.7073e-5, 0.058169, 0.0029732), 0.21817, (1.5708, 0, -3.1416)), + ("Link6", (2.1332e-5, 0.00053897, 0.099935), 0.57513, (1.5708, 0, 3.1416)), +] + +results = [] +for name, rc_urdf, mass, joint_rpy in new_links: + roll, pitch, yaw = joint_rpy + R = rpy_to_matrix(roll, pitch, yaw) + R_inv = transpose(R) + rc_dh = mat_vec_mul(R_inv, rc_urdf) + results.append((name, rc_urdf, rc_dh, mass, joint_rpy)) + print(f"{name} (rpy = {math.degrees(roll):.1f}, {math.degrees(pitch):.1f}, {math.degrees(yaw):.1f})") + print(f" URDF rc = ({rc_urdf[0]:+.8f}, {rc_urdf[1]:+.8f}, {rc_urdf[2]:+.8f})") + print(f" DH rc = ({rc_dh[0]:+.8f}, {rc_dh[1]:+.8f}, {rc_dh[2]:+.8f})") + print(f" mass = {mass}") + print() + +# ============ Joint origin comparison ============ +print("=" * 72) +print("Joint origin comparison: OLD vs NEW") +print("=" * 72) +old_joints = [ + ("j1", (0.18701, -1.0602, 0.042134), (0, 0, 1.5708)), + ("j2", (-0.0208, 0, 0.085), (-1.5708, 0, -1.5708)), + ("j3", (0, -0.404, 0.0563), (0, 0, -1.5708)), + ("j4", (0.047775, -0.0905, -0.0355), (-1.5708, 0, -1.5708)), + ("j5", (0, 0.023, 0.241), (1.5708, 0, 3.1416)), + ("j6", (1.4024e-5, 0.1065, -0.022997), (1.5708, 0, 3.1416)), +] +new_joints = [ + ("j1", (0, 0, 0.0405), (0, 0, 0)), + ("j2", (-0.0014, 0, 0.085), (-1.5708, 0, -1.5708)), + ("j3", (0, -0.388, 0.002795), (0, 0, -1.5708)), + ("j4", (0.047775, -0.0905, -0.001395), (-1.5708, 0, -1.5708)), + ("j5", (0, -0.004, 0.241), (1.5708, 0, -3.1416)), + ("j6", (0, 0.1065, 0.0040025), (1.5708, 0, 3.1416)), +] + +for old, new in zip(old_joints, new_joints): + name = old[0] + xyz_old, rpy_old = old[1], old[2] + xyz_new, rpy_new = new[1], new[2] + changed = "***CHANGED***" if xyz_old != xyz_new or rpy_old != rpy_new else "" + print(f"{name}: {changed}") + print(f" OLD xyz={xyz_old} rpy=({math.degrees(rpy_old[0]):.1f}, {math.degrees(rpy_old[1]):.1f}, {math.degrees(rpy_old[2]):.1f})") + print(f" NEW xyz={xyz_new} rpy=({math.degrees(rpy_new[0]):.1f}, {math.degrees(rpy_new[1]):.1f}, {math.degrees(rpy_new[2]):.1f})") + print() + +# ============ Generate C code ============ +print("=" * 72) +print("C CODE for arm_main.cpp dh_params") +print("=" * 72) + +# DH params extracted from joint origins: +# Standard DH: d comes from z-component of joint origin, a from x/y distance +# These need to be extracted from the URDF joint xyz and rpy carefully +# For the new URDF, the DH parameters: +dh_raw = [ + # d, a, alpha, theta_off, qmin, qmax, effort + (0.0405, 0.0014, -math.pi/2, 0.0, -15.7, 15.7, 60), + (0.0, 0.388, 0.0, -math.pi/2, -1.57, 1.57, 60), + (0.002795, 0.047775, -math.pi/2, -math.pi/2, 0.0, 3.0, 60), + (0.241, 0.0, math.pi/2, math.pi, 0.0, 6.28, 20), + (0.0, 0.1065, -math.pi/2, 0.0, -1.75, 1.75, 20), + (0.0040025, 0.0, 0.0, math.pi, 0.0, 6.3, 20), +] + +alpha_strs = ["-M_PI_2", "0.0f", "-M_PI_2", "M_PI_2", "-M_PI_2", "0.0f"] +toffset_strs = ["0.0f", "-M_PI_2", "-M_PI_2", "M_PI", "0.0f", "M_PI"] +comments = [ + "// J1: yaw", + "// J2: pitch (big arm)", + "// J3: pitch (forearm)", + "// J4: roll (wrist)", + "// J5: pitch (wrist)", + "// J6: roll (end)", +] + +print("static Arm6dof_DHParams_t dh_params[6] = {") +for i, (name, rc_urdf, rc_dh, mass, joint_rpy) in enumerate(results): + d, a, alpha, toff, qmin, qmax, effort = dh_raw[i] + rx, ry, rz = rc_dh + print(f" {comments[i]}") + print(f" {{.theta=0.0f, .d={d}f, .a={a}f, .alpha={alpha_strs[i]}, .theta_offset={toffset_strs[i]}, " + f".qmin={qmin}f, .qmax={qmax}f, .m={mass}f, " + f".rc={{{rx:.8f}f, {ry:.8f}f, {rz:.8f}f}}, .kp=1.0f, .kd=0.0f}},") + +print("};") diff --git a/scripts/quick_calc.py b/scripts/quick_calc.py new file mode 100644 index 0000000..2be658f --- /dev/null +++ b/scripts/quick_calc.py @@ -0,0 +1,32 @@ +import math + +# Link3: rpy="0 0 -1.5708" (纯z轴旋转-90°) +# rc_urdf = (0.0016734, -0.086963, -0.059109) +# Rz(90°) 逆旋转 +c, s = math.cos(math.pi/2), math.sin(math.pi/2) +x, y, z = 0.0016734, -0.086963, -0.059109 +rc_dh_3 = (x*c + y*s, -x*s + y*c, z) +print(f"Link3: rc_dh = ({rc_dh_3[0]:.8f}, {rc_dh_3[1]:.8f}, {rc_dh_3[2]:.8f})") + +# Link4: rpy="-1.5708 0 -1.5708" (roll-90°, yaw-90°) +# rc_urdf = (4.9724e-5, -0.00207071, 0.143600) +# 先Rx(-90°), 再Rz(90°)的逆 +# R = Rz(-90°) * Rx(-90°), R^T = Rx(90°) * Rz(90°) + +# Rx(90°): [1,0,0; 0,0,-1; 0,1,0] +# Rz(90°): [0,-1,0; 1,0,0; 0,0,1] +# R_inv = Rx(90°) * Rz(90°) = [0,-1,0; 0,0,-1; 1,0,0] +x, y, z = 4.9724e-5, -0.00207071, 0.143600 +rc_dh_4 = (-y, -z, x) +print(f"Link4: rc_dh = ({rc_dh_4[0]:.8f}, {rc_dh_4[1]:.8f}, {rc_dh_4[2]:.8f})") + +# Link5: rpy="1.5708 0 3.1416" (roll+90°, yaw+180°) +# rc_urdf = (-1.00614e-5, 0.05816669, -0.02402677) +# R = Rz(180°) * Rx(90°) +# R_inv = Rx(-90°) * Rz(-180°) +# Rx(-90°): [1,0,0; 0,0,1; 0,-1,0] +# Rz(-180°): [-1,0,0; 0,-1,0; 0,0,1] +# R_inv = [[-1,0,0],[0,0,1],[0,1,0]] +x, y, z = -1.00614e-5, 0.05816669, -0.02402677 +rc_dh_5 = (-x, z, y) +print(f"Link5: rc_dh = ({rc_dh_5[0]:.8f}, {rc_dh_5[1]:.8f}, {rc_dh_5[2]:.8f})") diff --git a/scripts/urdf_rc_to_dh.py b/scripts/urdf_rc_to_dh.py new file mode 100644 index 0000000..eb46821 --- /dev/null +++ b/scripts/urdf_rc_to_dh.py @@ -0,0 +1,127 @@ +""" +urdf_rc_to_dh.py +================ +将 URDF 中各连杆的质心坐标 (rc_urdf) 转换到 DH 连杆坐标系下 (rc_dh)。 +纯标准库实现，无需 numpy。 + +关键修正 +-------- +URDF中的Link坐标系由parent joint的完整6DOF变换（xyz + rpy）定义。 +DH坐标系仅用4个参数（d, a, α, θ）定义。 +需要用joint的rpy旋转来转换质心坐标，而不仅仅是theta_offset。 + +转换关系： + rc_dh = R_rpy^(-1) * rc_urdf +其中 R_rpy 是URDF joint中定义的旋转矩阵（roll-pitch-yaw顺序） +""" + +import math + +def Rx(angle): + """绕x轴旋转的旋转矩阵""" + c, s = math.cos(angle), math.sin(angle) + return [[1, 0, 0], [0, c, -s], [0, s, c]] + +def Ry(angle): + """绕y轴旋转的旋转矩阵""" + c, s = math.cos(angle), math.sin(angle) + return [[c, 0, s], [0, 1, 0], [-s, 0, c]] + +def Rz(angle): + """绕z轴旋转的旋转矩阵""" + c, s = math.cos(angle), math.sin(angle) + return [[c, -s, 0], [s, c, 0], [0, 0, 1]] + +def mat_mul(A, B): + """3x3矩阵乘法""" + return [[sum(A[i][k] * B[k][j] for k in range(3)) for j in range(3)] for i in range(3)] + +def mat_vec_mul(M, v): + """3x3矩阵乘3x1向量""" + return tuple(sum(M[i][j] * v[j] for j in range(3)) for i in range(3)) + +def rpy_to_matrix(roll, pitch, yaw): + """RPY(roll-pitch-yaw)转旋转矩阵，顺序：Rz(yaw) * Ry(pitch) * Rx(roll)""" + return mat_mul(mat_mul(Rz(yaw), Ry(pitch)), Rx(roll)) + +def transpose(M): + """3x3矩阵转置""" + return [[M[j][i] for j in range(3)] for i in range(3)] + +# ============================================================ +# 从 URDF 直接提取的数据 +# ============================================================ +links_urdf = [ + # name, rc_urdf (x,y,z), mass, joint_rpy (从URDF joint定义) + ("Link1", ( 0.0070301, -0.00030378, 0.044333 ), 2.3117, (0, 0, 1.5708)), # j1: rpy="0 0 1.5708" + ("Link2", (-9.2767e-6, -0.27664, 0.019763 ), 0.9007, (-1.5708, 0, -1.5708)), # j2: rpy="-1.5708 0 -1.5708" + ("Link3", ( 0.0016734, -0.086963, -0.059109 ), 0.72964, (0, 0, -1.5708)), # j3: rpy="0 0 -1.5708" + ("Link4", ( 4.9724e-5, -0.00207071, 0.143600 ), 0.60215, (-1.5708, 0, -1.5708)), # j4: rpy="-1.5708 0 -1.5708" + ("Link5", (-1.00614e-5, 0.05816669, -0.02402677), 0.21817, (1.5708, 0, 3.1416)), # j5: rpy="1.5708 0 3.1416" + ("Link6", ( 2.13317e-5, 5.38972e-4, 0.099935 ), 0.57513, (1.5708, 0, 3.1416)), # j6: rpy="1.5708 0 3.1416" +] + +print("=" * 72) +print("连杆质心坐标：URDF坐标系 → DH连杆坐标系") +print("=" * 72) +print() + +results = [] +for name, rc_urdf, mass, joint_rpy in links_urdf: + roll, pitch, yaw = joint_rpy + # URDF坐标系 = R_rpy * DH坐标系 + # 所以 rc_dh = R_rpy^T * rc_urdf (逆旋转) + R = rpy_to_matrix(roll, pitch, yaw) + R_inv = transpose(R) # 旋转矩阵的逆 = 转置 + rc_dh = mat_vec_mul(R_inv, rc_urdf) + results.append((name, rc_urdf, rc_dh, mass, joint_rpy)) + + print(f"{name} (joint rpy = {math.degrees(roll):.1f}°, {math.degrees(pitch):.1f}°, {math.degrees(yaw):.1f}°)") + print(f" URDF rc = ({rc_urdf[0]:+.8f}, {rc_urdf[1]:+.8f}, {rc_urdf[2]:+.8f})") + print(f" DH rc = ({rc_dh[0]:+.8f}, {rc_dh[1]:+.8f}, {rc_dh[2]:+.8f})") + print() + +# ============================================================ +# 生成可直接复制到 arm_main.cpp 的 C 代码 +# ============================================================ +print("=" * 72) +print("生成的 C 代码（直接替换 arm_main.cpp 中的 dh_params）") +print("=" * 72) +print() +print("// DH参数 (单位: m, rad, kg)") +print("// rc[3]: 质心在 DH 连杆坐标系下的坐标 (m)，由 URDF inertial/origin 转换") +print("// 转换: rc_dh = R_rpy^(-1) * rc_urdf，其中R_rpy来自URDF joint的rpy") +print("static Arm6dof_DHParams_t dh_params[6] = {") + +dh_raw = [ + # d, a, alpha, theta_off, qmin, qmax + ( 0.042134, 0.0208, -math.pi/2, math.pi/2, -15.7, 15.7 ), + ( 0.0, 0.404, 0.0, -math.pi/2, -1.57, 1.57 ), + ( 0.0563, 0.047775, -math.pi/2, -math.pi/2, 0.0, 3.0 ), + ( 0.241, 0.0, math.pi/2, math.pi, -12.56, 12.56 ), + ( 0.0, 0.1065, -math.pi/2, 0.0, -1.75, 1.75 ), + ( 0.099935, 0.0, 0.0, math.pi, -12.56, 12.56 ), +] + +alpha_strs = ["-M_PI_2", "0.0f", "-M_PI_2", "M_PI_2", "-M_PI_2", "0.0f"] +toffset_strs = ["M_PI_2", "-M_PI_2", "-M_PI_2", "M_PI", "0.0f", "M_PI"] +comments = [ + "// J1: 腰部旋转", + "// J2: 大臂俯仰", + "// J3: 小臂", + "// J4: 腕部旋转", + "// J5: 腕部俯仰", + "// J6: 末端Roll", +] + +for i, (name, rc_urdf, rc_dh, mass, joint_rpy) in enumerate(results): + d, a, alpha, toff, qmin, qmax = dh_raw[i] + rx, ry, rz = rc_dh + roll, pitch, yaw = joint_rpy + print(f" {comments[i]} URDF rc=({rc_urdf[0]:+.4e}, {rc_urdf[1]:+.4e}, {rc_urdf[2]:+.4e}), rpy=({math.degrees(roll):.0f}°,{math.degrees(pitch):.0f}°,{math.degrees(yaw):.0f}°)") + print(f" {{.theta=0.0f, .d={d}f, .a={a}f, .alpha={alpha_strs[i]}, .theta_offset={toffset_strs[i]}, " + f".qmin={qmin}f, .qmax={qmax}f, .m={mass}f, " + f".rc={{{rx:.8f}f, {ry:.8f}f, {rz:.8f}f}}}},") + +print("};") + \ No newline at end of file diff --git a/startup_stm32f407xx.s b/startup_stm32f407xx.s new file mode 100644 index 0000000..cc5f27e --- /dev/null +++ b/startup_stm32f407xx.s @@ -0,0 +1,508 @@ +/** + ****************************************************************************** + * @file startup_stm32f407xx.s + * @author MCD Application Team + * @brief STM32F407xx Devices vector table for GCC based toolchains. + * This module performs: + * - Set the initial SP + * - Set the initial PC == Reset_Handler, + * - Set the vector table entries with the exceptions ISR address + * - Branches to main in the C library (which eventually + * calls main()). + * After Reset the Cortex-M4 processor is in Thread mode, + * priority is Privileged, and the Stack is set to Main. + ****************************************************************************** + * @attention + * + * Copyright (c) 2017 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ + + .syntax unified + .cpu cortex-m4 + .fpu softvfp + .thumb + +.global g_pfnVectors +.global Default_Handler + +/* start address for the initialization values of the .data section. +defined in linker script */ +.word _sidata +/* start address for the .data section. defined in linker script */ +.word _sdata +/* end address for the .data section. defined in linker script */ +.word _edata +/* start address for the .bss section. defined in linker script */ +.word _sbss +/* end address for the .bss section. defined in linker script */ +.word _ebss +/* stack used for SystemInit_ExtMemCtl; always internal RAM used */ + +/** + * @brief This is the code that gets called when the processor first + * starts execution following a reset event. Only the absolutely + * necessary set is performed, after which the application + * supplied main() routine is called. + * @param None + * @retval : None +*/ + + .section .text.Reset_Handler + .weak Reset_Handler + .type Reset_Handler, %function +Reset_Handler: + ldr sp, =_estack /* set stack pointer */ + +/* Call the clock system initialization function.*/ + bl SystemInit + +/* Copy the data segment initializers from flash to SRAM */ + ldr r0, =_sdata + ldr r1, =_edata + ldr r2, =_sidata + movs r3, #0 + b LoopCopyDataInit + +CopyDataInit: + ldr r4, [r2, r3] + str r4, [r0, r3] + adds r3, r3, #4 + +LoopCopyDataInit: + adds r4, r0, r3 + cmp r4, r1 + bcc CopyDataInit + +/* Zero fill the bss segment. */ + ldr r2, =_sbss + ldr r4, =_ebss + movs r3, #0 + b LoopFillZerobss + +FillZerobss: + str r3, [r2] + adds r2, r2, #4 + +LoopFillZerobss: + cmp r2, r4 + bcc FillZerobss + +/* Call static constructors */ + bl __libc_init_array +/* Call the application's entry point.*/ + bl main + bx lr +.size Reset_Handler, .-Reset_Handler + +/** + * @brief This is the code that gets called when the processor receives an + * unexpected interrupt. This simply enters an infinite loop, preserving + * the system state for examination by a debugger. + * @param None + * @retval None +*/ + .section .text.Default_Handler,"ax",%progbits +Default_Handler: +Infinite_Loop: + b Infinite_Loop + .size Default_Handler, .-Default_Handler +/****************************************************************************** +* +* The minimal vector table for a Cortex M3. Note that the proper constructs +* must be placed on this to ensure that it ends up at physical address +* 0x0000.0000. +* +*******************************************************************************/ + .section .isr_vector,"a",%progbits + .type g_pfnVectors, %object + + +g_pfnVectors: + .word _estack + .word Reset_Handler + .word NMI_Handler + .word HardFault_Handler + .word MemManage_Handler + .word BusFault_Handler + .word UsageFault_Handler + .word 0 + .word 0 + .word 0 + .word 0 + .word SVC_Handler + .word DebugMon_Handler + .word 0 + .word PendSV_Handler + .word SysTick_Handler + + /* External Interrupts */ + .word WWDG_IRQHandler /* Window WatchDog */ + .word PVD_IRQHandler /* PVD through EXTI Line detection */ + .word TAMP_STAMP_IRQHandler /* Tamper and TimeStamps through the EXTI line */ + .word RTC_WKUP_IRQHandler /* RTC Wakeup through the EXTI line */ + .word FLASH_IRQHandler /* FLASH */ + .word RCC_IRQHandler /* RCC */ + .word EXTI0_IRQHandler /* EXTI Line0 */ + .word EXTI1_IRQHandler /* EXTI Line1 */ + .word EXTI2_IRQHandler /* EXTI Line2 */ + .word EXTI3_IRQHandler /* EXTI Line3 */ + .word EXTI4_IRQHandler /* EXTI Line4 */ + .word DMA1_Stream0_IRQHandler /* DMA1 Stream 0 */ + .word DMA1_Stream1_IRQHandler /* DMA1 Stream 1 */ + .word DMA1_Stream2_IRQHandler /* DMA1 Stream 2 */ + .word DMA1_Stream3_IRQHandler /* DMA1 Stream 3 */ + .word DMA1_Stream4_IRQHandler /* DMA1 Stream 4 */ + .word DMA1_Stream5_IRQHandler /* DMA1 Stream 5 */ + .word DMA1_Stream6_IRQHandler /* DMA1 Stream 6 */ + .word ADC_IRQHandler /* ADC1, ADC2 and ADC3s */ + .word CAN1_TX_IRQHandler /* CAN1 TX */ + .word CAN1_RX0_IRQHandler /* CAN1 RX0 */ + .word CAN1_RX1_IRQHandler /* CAN1 RX1 */ + .word CAN1_SCE_IRQHandler /* CAN1 SCE */ + .word EXTI9_5_IRQHandler /* External Line[9:5]s */ + .word TIM1_BRK_TIM9_IRQHandler /* TIM1 Break and TIM9 */ + .word TIM1_UP_TIM10_IRQHandler /* TIM1 Update and TIM10 */ + .word TIM1_TRG_COM_TIM11_IRQHandler /* TIM1 Trigger and Commutation and TIM11 */ + .word TIM1_CC_IRQHandler /* TIM1 Capture Compare */ + .word TIM2_IRQHandler /* TIM2 */ + .word TIM3_IRQHandler /* TIM3 */ + .word TIM4_IRQHandler /* TIM4 */ + .word I2C1_EV_IRQHandler /* I2C1 Event */ + .word I2C1_ER_IRQHandler /* I2C1 Error */ + .word I2C2_EV_IRQHandler /* I2C2 Event */ + .word I2C2_ER_IRQHandler /* I2C2 Error */ + .word SPI1_IRQHandler /* SPI1 */ + .word SPI2_IRQHandler /* SPI2 */ + .word USART1_IRQHandler /* USART1 */ + .word USART2_IRQHandler /* USART2 */ + .word USART3_IRQHandler /* USART3 */ + .word EXTI15_10_IRQHandler /* External Line[15:10]s */ + .word RTC_Alarm_IRQHandler /* RTC Alarm (A and B) through EXTI Line */ + .word OTG_FS_WKUP_IRQHandler /* USB OTG FS Wakeup through EXTI line */ + .word TIM8_BRK_TIM12_IRQHandler /* TIM8 Break and TIM12 */ + .word TIM8_UP_TIM13_IRQHandler /* TIM8 Update and TIM13 */ + .word TIM8_TRG_COM_TIM14_IRQHandler /* TIM8 Trigger and Commutation and TIM14 */ + .word TIM8_CC_IRQHandler /* TIM8 Capture Compare */ + .word DMA1_Stream7_IRQHandler /* DMA1 Stream7 */ + .word FSMC_IRQHandler /* FSMC */ + .word SDIO_IRQHandler /* SDIO */ + .word TIM5_IRQHandler /* TIM5 */ + .word SPI3_IRQHandler /* SPI3 */ + .word UART4_IRQHandler /* UART4 */ + .word UART5_IRQHandler /* UART5 */ + .word TIM6_DAC_IRQHandler /* TIM6 and DAC1&2 underrun errors */ + .word TIM7_IRQHandler /* TIM7 */ + .word DMA2_Stream0_IRQHandler /* DMA2 Stream 0 */ + .word DMA2_Stream1_IRQHandler /* DMA2 Stream 1 */ + .word DMA2_Stream2_IRQHandler /* DMA2 Stream 2 */ + .word DMA2_Stream3_IRQHandler /* DMA2 Stream 3 */ + .word DMA2_Stream4_IRQHandler /* DMA2 Stream 4 */ + .word ETH_IRQHandler /* Ethernet */ + .word ETH_WKUP_IRQHandler /* Ethernet Wakeup through EXTI line */ + .word CAN2_TX_IRQHandler /* CAN2 TX */ + .word CAN2_RX0_IRQHandler /* CAN2 RX0 */ + .word CAN2_RX1_IRQHandler /* CAN2 RX1 */ + .word CAN2_SCE_IRQHandler /* CAN2 SCE */ + .word OTG_FS_IRQHandler /* USB OTG FS */ + .word DMA2_Stream5_IRQHandler /* DMA2 Stream 5 */ + .word DMA2_Stream6_IRQHandler /* DMA2 Stream 6 */ + .word DMA2_Stream7_IRQHandler /* DMA2 Stream 7 */ + .word USART6_IRQHandler /* USART6 */ + .word I2C3_EV_IRQHandler /* I2C3 event */ + .word I2C3_ER_IRQHandler /* I2C3 error */ + .word OTG_HS_EP1_OUT_IRQHandler /* USB OTG HS End Point 1 Out */ + .word OTG_HS_EP1_IN_IRQHandler /* USB OTG HS End Point 1 In */ + .word OTG_HS_WKUP_IRQHandler /* USB OTG HS Wakeup through EXTI */ + .word OTG_HS_IRQHandler /* USB OTG HS */ + .word DCMI_IRQHandler /* DCMI */ + .word 0 /* CRYP crypto */ + .word HASH_RNG_IRQHandler /* Hash and Rng */ + .word FPU_IRQHandler /* FPU */ + + + + .size g_pfnVectors, .-g_pfnVectors + +/******************************************************************************* +* +* Provide weak aliases for each Exception handler to the Default_Handler. +* As they are weak aliases, any function with the same name will override +* this definition. +* +*******************************************************************************/ + .weak NMI_Handler + .thumb_set NMI_Handler,Default_Handler + + .weak HardFault_Handler + .thumb_set HardFault_Handler,Default_Handler + + .weak MemManage_Handler + .thumb_set MemManage_Handler,Default_Handler + + .weak BusFault_Handler + .thumb_set BusFault_Handler,Default_Handler + + .weak UsageFault_Handler + .thumb_set UsageFault_Handler,Default_Handler + + .weak SVC_Handler + .thumb_set SVC_Handler,Default_Handler + + .weak DebugMon_Handler + .thumb_set DebugMon_Handler,Default_Handler + + .weak PendSV_Handler + .thumb_set PendSV_Handler,Default_Handler + + .weak SysTick_Handler + .thumb_set SysTick_Handler,Default_Handler + + .weak WWDG_IRQHandler + .thumb_set WWDG_IRQHandler,Default_Handler + + .weak PVD_IRQHandler + .thumb_set PVD_IRQHandler,Default_Handler + + .weak TAMP_STAMP_IRQHandler + .thumb_set TAMP_STAMP_IRQHandler,Default_Handler + + .weak RTC_WKUP_IRQHandler + .thumb_set RTC_WKUP_IRQHandler,Default_Handler + + .weak FLASH_IRQHandler + .thumb_set FLASH_IRQHandler,Default_Handler + + .weak RCC_IRQHandler + .thumb_set RCC_IRQHandler,Default_Handler + + .weak EXTI0_IRQHandler + .thumb_set EXTI0_IRQHandler,Default_Handler + + .weak EXTI1_IRQHandler + .thumb_set EXTI1_IRQHandler,Default_Handler + + .weak EXTI2_IRQHandler + .thumb_set EXTI2_IRQHandler,Default_Handler + + .weak EXTI3_IRQHandler + .thumb_set EXTI3_IRQHandler,Default_Handler + + .weak EXTI4_IRQHandler + .thumb_set EXTI4_IRQHandler,Default_Handler + + .weak DMA1_Stream0_IRQHandler + .thumb_set DMA1_Stream0_IRQHandler,Default_Handler + + .weak DMA1_Stream1_IRQHandler + .thumb_set DMA1_Stream1_IRQHandler,Default_Handler + + .weak DMA1_Stream2_IRQHandler + .thumb_set DMA1_Stream2_IRQHandler,Default_Handler + + .weak DMA1_Stream3_IRQHandler + .thumb_set DMA1_Stream3_IRQHandler,Default_Handler + + .weak DMA1_Stream4_IRQHandler + .thumb_set DMA1_Stream4_IRQHandler,Default_Handler + + .weak DMA1_Stream5_IRQHandler + .thumb_set DMA1_Stream5_IRQHandler,Default_Handler + + .weak DMA1_Stream6_IRQHandler + .thumb_set DMA1_Stream6_IRQHandler,Default_Handler + + .weak ADC_IRQHandler + .thumb_set ADC_IRQHandler,Default_Handler + + .weak CAN1_TX_IRQHandler + .thumb_set CAN1_TX_IRQHandler,Default_Handler + + .weak CAN1_RX0_IRQHandler + .thumb_set CAN1_RX0_IRQHandler,Default_Handler + + .weak CAN1_RX1_IRQHandler + .thumb_set CAN1_RX1_IRQHandler,Default_Handler + + .weak CAN1_SCE_IRQHandler + .thumb_set CAN1_SCE_IRQHandler,Default_Handler + + .weak EXTI9_5_IRQHandler + .thumb_set EXTI9_5_IRQHandler,Default_Handler + + .weak TIM1_BRK_TIM9_IRQHandler + .thumb_set TIM1_BRK_TIM9_IRQHandler,Default_Handler + + .weak TIM1_UP_TIM10_IRQHandler + .thumb_set TIM1_UP_TIM10_IRQHandler,Default_Handler + + .weak TIM1_TRG_COM_TIM11_IRQHandler + .thumb_set TIM1_TRG_COM_TIM11_IRQHandler,Default_Handler + + .weak TIM1_CC_IRQHandler + .thumb_set TIM1_CC_IRQHandler,Default_Handler + + .weak TIM2_IRQHandler + .thumb_set TIM2_IRQHandler,Default_Handler + + .weak TIM3_IRQHandler + .thumb_set TIM3_IRQHandler,Default_Handler + + .weak TIM4_IRQHandler + .thumb_set TIM4_IRQHandler,Default_Handler + + .weak I2C1_EV_IRQHandler + .thumb_set I2C1_EV_IRQHandler,Default_Handler + + .weak I2C1_ER_IRQHandler + .thumb_set I2C1_ER_IRQHandler,Default_Handler + + .weak I2C2_EV_IRQHandler + .thumb_set I2C2_EV_IRQHandler,Default_Handler + + .weak I2C2_ER_IRQHandler + .thumb_set I2C2_ER_IRQHandler,Default_Handler + + .weak SPI1_IRQHandler + .thumb_set SPI1_IRQHandler,Default_Handler + + .weak SPI2_IRQHandler + .thumb_set SPI2_IRQHandler,Default_Handler + + .weak USART1_IRQHandler + .thumb_set USART1_IRQHandler,Default_Handler + + .weak USART2_IRQHandler + .thumb_set USART2_IRQHandler,Default_Handler + + .weak USART3_IRQHandler + .thumb_set USART3_IRQHandler,Default_Handler + + .weak EXTI15_10_IRQHandler + .thumb_set EXTI15_10_IRQHandler,Default_Handler + + .weak RTC_Alarm_IRQHandler + .thumb_set RTC_Alarm_IRQHandler,Default_Handler + + .weak OTG_FS_WKUP_IRQHandler + .thumb_set OTG_FS_WKUP_IRQHandler,Default_Handler + + .weak TIM8_BRK_TIM12_IRQHandler + .thumb_set TIM8_BRK_TIM12_IRQHandler,Default_Handler + + .weak TIM8_UP_TIM13_IRQHandler + .thumb_set TIM8_UP_TIM13_IRQHandler,Default_Handler + + .weak TIM8_TRG_COM_TIM14_IRQHandler + .thumb_set TIM8_TRG_COM_TIM14_IRQHandler,Default_Handler + + .weak TIM8_CC_IRQHandler + .thumb_set TIM8_CC_IRQHandler,Default_Handler + + .weak DMA1_Stream7_IRQHandler + .thumb_set DMA1_Stream7_IRQHandler,Default_Handler + + .weak FSMC_IRQHandler + .thumb_set FSMC_IRQHandler,Default_Handler + + .weak SDIO_IRQHandler + .thumb_set SDIO_IRQHandler,Default_Handler + + .weak TIM5_IRQHandler + .thumb_set TIM5_IRQHandler,Default_Handler + + .weak SPI3_IRQHandler + .thumb_set SPI3_IRQHandler,Default_Handler + + .weak UART4_IRQHandler + .thumb_set UART4_IRQHandler,Default_Handler + + .weak UART5_IRQHandler + .thumb_set UART5_IRQHandler,Default_Handler + + .weak TIM6_DAC_IRQHandler + .thumb_set TIM6_DAC_IRQHandler,Default_Handler + + .weak TIM7_IRQHandler + .thumb_set TIM7_IRQHandler,Default_Handler + + .weak DMA2_Stream0_IRQHandler + .thumb_set DMA2_Stream0_IRQHandler,Default_Handler + + .weak DMA2_Stream1_IRQHandler + .thumb_set DMA2_Stream1_IRQHandler,Default_Handler + + .weak DMA2_Stream2_IRQHandler + .thumb_set DMA2_Stream2_IRQHandler,Default_Handler + + .weak DMA2_Stream3_IRQHandler + .thumb_set DMA2_Stream3_IRQHandler,Default_Handler + + .weak DMA2_Stream4_IRQHandler + .thumb_set DMA2_Stream4_IRQHandler,Default_Handler + + .weak ETH_IRQHandler + .thumb_set ETH_IRQHandler,Default_Handler + + .weak ETH_WKUP_IRQHandler + .thumb_set ETH_WKUP_IRQHandler,Default_Handler + + .weak CAN2_TX_IRQHandler + .thumb_set CAN2_TX_IRQHandler,Default_Handler + + .weak CAN2_RX0_IRQHandler + .thumb_set CAN2_RX0_IRQHandler,Default_Handler + + .weak CAN2_RX1_IRQHandler + .thumb_set CAN2_RX1_IRQHandler,Default_Handler + + .weak CAN2_SCE_IRQHandler + .thumb_set CAN2_SCE_IRQHandler,Default_Handler + + .weak OTG_FS_IRQHandler + .thumb_set OTG_FS_IRQHandler,Default_Handler + + .weak DMA2_Stream5_IRQHandler + .thumb_set DMA2_Stream5_IRQHandler,Default_Handler + + .weak DMA2_Stream6_IRQHandler + .thumb_set DMA2_Stream6_IRQHandler,Default_Handler + + .weak DMA2_Stream7_IRQHandler + .thumb_set DMA2_Stream7_IRQHandler,Default_Handler + + .weak USART6_IRQHandler + .thumb_set USART6_IRQHandler,Default_Handler + + .weak I2C3_EV_IRQHandler + .thumb_set I2C3_EV_IRQHandler,Default_Handler + + .weak I2C3_ER_IRQHandler + .thumb_set I2C3_ER_IRQHandler,Default_Handler + + .weak OTG_HS_EP1_OUT_IRQHandler + .thumb_set OTG_HS_EP1_OUT_IRQHandler,Default_Handler + + .weak OTG_HS_EP1_IN_IRQHandler + .thumb_set OTG_HS_EP1_IN_IRQHandler,Default_Handler + + .weak OTG_HS_WKUP_IRQHandler + .thumb_set OTG_HS_WKUP_IRQHandler,Default_Handler + + .weak OTG_HS_IRQHandler + .thumb_set OTG_HS_IRQHandler,Default_Handler + + .weak DCMI_IRQHandler + .thumb_set DCMI_IRQHandler,Default_Handler + + .weak HASH_RNG_IRQHandler + .thumb_set HASH_RNG_IRQHandler,Default_Handler + + .weak FPU_IRQHandler + .thumb_set FPU_IRQHandler,Default_Handler