From 7d868bf32a76f375fedd6638f796618e488c66cc Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sat, 4 Oct 2025 11:50:40 +0800 Subject: [PATCH 01/25] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=BA=95=E5=B1=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .clangd | 11 + .mxproject | 17 +- CMakeLists.txt | 91 ++ CMakePresets.json | 38 + Core/Inc/stm32f4xx_it.h | 2 + Core/Src/can.c | 6 + Core/Src/freertos.c | 320 ++++---- Core/Src/stm32f4xx_it.c | 28 + Core/Src/syscalls.c | 244 ++++++ Core/Src/sysmem.c | 87 ++ DevC.ioc | 4 +- .../Source/portable/GCC/ARM_CM4F/port.c | 775 ++++++++++++++++++ .../Source/portable/GCC/ARM_CM4F/portmacro.h | 243 ++++++ STM32F407XX_FLASH.ld | 269 ++++++ User/bsp/bsp.h | 28 + User/bsp/bsp_config.yaml | 11 + User/bsp/can.c | 708 ++++++++++++++++ User/bsp/can.h | 259 ++++++ User/bsp/mm.c | 30 + User/bsp/mm.h | 32 + User/bsp/time.c | 81 ++ User/bsp/time.h | 43 + User/component/ahrs.c | 417 ++++++++++ User/component/ahrs.h | 114 +++ User/component/component_config.yaml | 7 + User/component/user_math.c | 138 ++++ User/component/user_math.h | 179 ++++ User/device/device.h | 47 ++ User/device/device_config.yaml | 12 + User/device/dm_imu.c | 270 ++++++ User/device/dm_imu.h | 90 ++ User/device/motor.c | 52 ++ User/device/motor.h | 68 ++ User/device/motor_lk.c | 329 ++++++++ User/device/motor_lk.h | 119 +++ User/device/motor_lz.c | 440 ++++++++++ User/device/motor_lz.h | 212 +++++ User/task/Task4.c | 44 + User/task/blink.c | 44 + User/task/config.yaml | 28 + User/task/ctrl_lz.c | 44 + User/task/imu.c | 44 + User/task/init.c | 45 + User/task/user_task.c | 31 + User/task/user_task.h | 104 +++ cmake/gcc-arm-none-eabi.cmake | 43 + cmake/starm-clang.cmake | 65 ++ cmake/stm32cubemx/CMakeLists.txt | 153 ++++ startup_stm32f407xx.s | 508 ++++++++++++ 49 files changed, 6806 insertions(+), 168 deletions(-) create mode 100644 .clangd create mode 100644 CMakeLists.txt create mode 100644 CMakePresets.json create mode 100644 Core/Src/syscalls.c create mode 100644 Core/Src/sysmem.c create mode 100644 Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c create mode 100644 Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h 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/time.c create mode 100644 User/bsp/time.h create mode 100644 User/component/ahrs.c create mode 100644 User/component/ahrs.h create mode 100644 User/component/component_config.yaml create mode 100644 User/component/user_math.c create mode 100644 User/component/user_math.h create mode 100644 User/device/device.h create mode 100644 User/device/device_config.yaml create mode 100644 User/device/dm_imu.c create mode 100644 User/device/dm_imu.h create mode 100644 User/device/motor.c create mode 100644 User/device/motor.h create mode 100644 User/device/motor_lk.c create mode 100644 User/device/motor_lk.h create mode 100644 User/device/motor_lz.c create mode 100644 User/device/motor_lz.h create mode 100644 User/task/Task4.c create mode 100644 User/task/blink.c create mode 100644 User/task/config.yaml create mode 100644 User/task/ctrl_lz.c create mode 100644 User/task/imu.c create mode 100644 User/task/init.c create mode 100644 User/task/user_task.c create mode 100644 User/task/user_task.h 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 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/.mxproject b/.mxproject index 084fd07..9bdbf0a 100644 --- a/.mxproject +++ b/.mxproject @@ -1,3 +1,11 @@ +[PreviousLibFiles] +LibFiles=Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usb.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_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_hal_dma_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma.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_hal_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_hal_adc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_adc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_can.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_crc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_i2c.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_i2c_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rng.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_spi.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_uart.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;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_core.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_ctlreq.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_def.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_ioreq.h;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.h;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.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_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_crc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rng.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.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;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usb.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_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_hal_dma_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma.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_hal_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_hal_adc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_adc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_can.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_crc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_i2c.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_i2c_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rng.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_spi.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_uart.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;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_core.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_ctlreq.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_def.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_ioreq.h;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.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/core_cm7.h;Drivers/CMSIS/Include/tz_context.h;Drivers/CMSIS/Include/core_cm3.h;Drivers/CMSIS/Include/cmsis_compiler.h;Drivers/CMSIS/Include/cmsis_armclang.h;Drivers/CMSIS/Include/mpu_armv7.h;Drivers/CMSIS/Include/cmsis_armcc.h;Drivers/CMSIS/Include/core_cm4.h;Drivers/CMSIS/Include/core_cm0.h;Drivers/CMSIS/Include/cmsis_iccarm.h;Drivers/CMSIS/Include/core_armv8mml.h;Drivers/CMSIS/Include/core_sc000.h;Drivers/CMSIS/Include/core_cm1.h;Drivers/CMSIS/Include/mpu_armv8.h;Drivers/CMSIS/Include/core_sc300.h;Drivers/CMSIS/Include/cmsis_gcc.h;Drivers/CMSIS/Include/cmsis_version.h;Drivers/CMSIS/Include/core_cm23.h;Drivers/CMSIS/Include/core_cm33.h;Drivers/CMSIS/Include/core_cm0plus.h;Drivers/CMSIS/Include/core_armv8mbl.h; + +[PreviousUsedKeilFiles] +SourceFiles=../Core/Src/main.c;../Core/Src/gpio.c;../Core/Src/freertos.c;../Core/Src/adc.c;../Core/Src/can.c;../Core/Src/crc.c;../Core/Src/dma.c;../Core/Src/i2c.c;../Core/Src/rng.c;../Core/Src/spi.c;../Core/Src/tim.c;../Core/Src/usart.c;../USB_DEVICE/App/usb_device.c;../USB_DEVICE/Target/usbd_conf.c;../USB_DEVICE/App/usbd_desc.c;../USB_DEVICE/App/usbd_cdc_if.c;../Core/Src/stm32f4xx_it.c;../Core/Src/stm32f4xx_hal_msp.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.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_adc.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_adc.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_crc.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rng.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.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/RVDS/ARM_CM4F/port.c;../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c;../Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;../Core/Src/system_stm32f4xx.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.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_adc.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_adc.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_crc.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rng.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.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/RVDS/ARM_CM4F/port.c;../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.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;../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.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;../Middlewares/ST/STM32_USB_Device_Library/Core/Inc;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include;../Core/Inc;../USB_DEVICE/App;../USB_DEVICE/Target; +CDefines=USE_HAL_DRIVER;STM32F407xx;USE_HAL_DRIVER;USE_HAL_DRIVER; + [PreviousGenFiles] AdvancedFolderStructure=true HeaderFileListSize=18 @@ -49,11 +57,8 @@ SourcePath#1=../USB_DEVICE/App SourcePath#2=../USB_DEVICE/Target SourceFiles=; -[PreviousLibFiles] -LibFiles=Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usb.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_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_hal_dma_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma.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_hal_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_hal_adc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_adc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_can.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_crc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_i2c.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_i2c_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rng.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_spi.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_uart.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/RVDS/ARM_CM4F/portmacro.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_core.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_ctlreq.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_def.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_ioreq.h;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.h;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.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_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_crc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rng.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.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/RVDS/ARM_CM4F/port.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_usb.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_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_hal_dma_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma.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_hal_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_hal_adc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_adc_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_ll_adc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_can.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_crc.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_i2c.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_i2c_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_rng.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_spi.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_tim_ex.h;Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_uart.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/RVDS/ARM_CM4F/portmacro.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_core.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_ctlreq.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_def.h;Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_ioreq.h;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc/usbd_cdc.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/core_cm7.h;Drivers/CMSIS/Include/tz_context.h;Drivers/CMSIS/Include/core_cm3.h;Drivers/CMSIS/Include/cmsis_compiler.h;Drivers/CMSIS/Include/cmsis_armclang.h;Drivers/CMSIS/Include/mpu_armv7.h;Drivers/CMSIS/Include/cmsis_armcc.h;Drivers/CMSIS/Include/core_cm4.h;Drivers/CMSIS/Include/core_cm0.h;Drivers/CMSIS/Include/cmsis_iccarm.h;Drivers/CMSIS/Include/core_armv8mml.h;Drivers/CMSIS/Include/core_sc000.h;Drivers/CMSIS/Include/core_cm1.h;Drivers/CMSIS/Include/mpu_armv8.h;Drivers/CMSIS/Include/core_sc300.h;Drivers/CMSIS/Include/cmsis_gcc.h;Drivers/CMSIS/Include/cmsis_version.h;Drivers/CMSIS/Include/core_cm23.h;Drivers/CMSIS/Include/core_cm33.h;Drivers/CMSIS/Include/core_cm0plus.h;Drivers/CMSIS/Include/core_armv8mbl.h; - -[PreviousUsedKeilFiles] -SourceFiles=../Core/Src/main.c;../Core/Src/gpio.c;../Core/Src/freertos.c;../Core/Src/adc.c;../Core/Src/can.c;../Core/Src/crc.c;../Core/Src/dma.c;../Core/Src/i2c.c;../Core/Src/rng.c;../Core/Src/spi.c;../Core/Src/tim.c;../Core/Src/usart.c;../USB_DEVICE/App/usb_device.c;../USB_DEVICE/Target/usbd_conf.c;../USB_DEVICE/App/usbd_desc.c;../USB_DEVICE/App/usbd_cdc_if.c;../Core/Src/stm32f4xx_it.c;../Core/Src/stm32f4xx_hal_msp.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.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_adc.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_adc.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_crc.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rng.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.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/RVDS/ARM_CM4F/port.c;../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c;../Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;../Core/Src/system_stm32f4xx.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.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_adc.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_adc.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_crc.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rng.c;../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.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/RVDS/ARM_CM4F/port.c;../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.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;../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.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;../Middlewares/ST/STM32_USB_Device_Library/Core/Inc;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include;../Core/Inc;../USB_DEVICE/App;../USB_DEVICE/Target; +[PreviousUsedCMakes] +SourceFiles=Core/Src/main.c;Core/Src/gpio.c;Core/Src/freertos.c;Core/Src/adc.c;Core/Src/can.c;Core/Src/crc.c;Core/Src/dma.c;Core/Src/i2c.c;Core/Src/rng.c;Core/Src/spi.c;Core/Src/tim.c;Core/Src/usart.c;USB_DEVICE/App/usb_device.c;USB_DEVICE/Target/usbd_conf.c;USB_DEVICE/App/usbd_desc.c;USB_DEVICE/App/usbd_cdc_if.c;Core/Src/stm32f4xx_it.c;Core/Src/stm32f4xx_hal_msp.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.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_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_crc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rng.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.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;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Core/Src/system_stm32f4xx.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.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_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_crc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rng.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.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;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.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;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.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;Middlewares/ST/STM32_USB_Device_Library/Core/Inc;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc;Drivers/CMSIS/Device/ST/STM32F4xx/Include;Drivers/CMSIS/Include;Core/Inc;USB_DEVICE/App;USB_DEVICE/Target; CDefines=USE_HAL_DRIVER;STM32F407xx;USE_HAL_DRIVER;USE_HAL_DRIVER; diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..b42e837 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,91 @@ +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 DevC) + +# 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 +) + +# 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/time.c + + # User/component sources + User/component/ahrs.c + User/component/user_math.c + + # User/device sources + User/device/dm_imu.c + User/device/motor.c + User/device/motor_lk.c + User/device/motor_lz.c + + # User/task sources + User/task/Task4.c + User/task/blink.c + User/task/ctrl_lz.c + User/task/imu.c + User/task/init.c + User/task/user_task.c +) + +# Add include paths +target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE + # Add user defined include paths + User +) + +# Add project symbols (macros) +target_compile_definitions(${CMAKE_PROJECT_NAME} PRIVATE + # 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} + 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/stm32f4xx_it.h b/Core/Inc/stm32f4xx_it.h index 6a88099..60a3264 100644 --- a/Core/Inc/stm32f4xx_it.h +++ b/Core/Inc/stm32f4xx_it.h @@ -59,6 +59,7 @@ void EXTI3_IRQHandler(void); void EXTI4_IRQHandler(void); void DMA1_Stream1_IRQHandler(void); void DMA1_Stream2_IRQHandler(void); +void CAN1_TX_IRQHandler(void); void CAN1_RX0_IRQHandler(void); void CAN1_RX1_IRQHandler(void); void EXTI9_5_IRQHandler(void); @@ -69,6 +70,7 @@ void TIM7_IRQHandler(void); void DMA2_Stream1_IRQHandler(void); void DMA2_Stream2_IRQHandler(void); void DMA2_Stream3_IRQHandler(void); +void CAN2_TX_IRQHandler(void); void CAN2_RX0_IRQHandler(void); void CAN2_RX1_IRQHandler(void); void OTG_FS_IRQHandler(void); diff --git a/Core/Src/can.c b/Core/Src/can.c index 38f13d2..81ec2ed 100644 --- a/Core/Src/can.c +++ b/Core/Src/can.c @@ -122,6 +122,8 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle) 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); @@ -155,6 +157,8 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle) 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); @@ -186,6 +190,7 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle) 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 */ @@ -211,6 +216,7 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle) 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 */ diff --git a/Core/Src/freertos.c b/Core/Src/freertos.c index f9d2db6..3880224 100644 --- a/Core/Src/freertos.c +++ b/Core/Src/freertos.c @@ -1,161 +1,159 @@ -/* USER CODE BEGIN Header */ -/** - ****************************************************************************** - * File Name : freertos.c - * Description : Code for freertos applications - ****************************************************************************** - * @attention - * - *

© Copyright (c) 2025 STMicroelectronics. - * All rights reserved.

- * - * This software component is licensed by ST under Ultimate Liberty license - * SLA0044, the "License"; You may not use this file except in compliance with - * the License. You may obtain a copy of the License at: - * www.st.com/SLA0044 - * - ****************************************************************************** - */ -/* USER CODE END Header */ - -/* Includes ------------------------------------------------------------------*/ -#include "FreeRTOS.h" -#include "task.h" -#include "main.h" -#include "cmsis_os.h" - -/* Private includes ----------------------------------------------------------*/ -/* USER CODE BEGIN Includes */ - -/* 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); - -extern void MX_USB_DEVICE_Init(void); -void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */ - -/* Hook prototypes */ -void configureTimerForRunTimeStats(void); -unsigned long getRunTimeCounterValue(void); -void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char *pcTaskName); - -/* USER CODE BEGIN 1 */ -/* Functions needed when configGENERATE_RUN_TIME_STATS is on */ -__weak void configureTimerForRunTimeStats(void) -{ - -} - -__weak unsigned long getRunTimeCounterValue(void) -{ -return 0; -} -/* USER CODE END 1 */ - -/* USER CODE BEGIN 4 */ -void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char *pcTaskName) -{ - /* Run time stack overflow checking is performed if - configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2. This hook function is - called if a stack overflow is detected. */ -} -/* USER CODE END 4 */ - -/** - * @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, ... */ - /* 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) -{ - /* init code for USB_DEVICE */ - MX_USB_DEVICE_Init(); - /* USER CODE BEGIN StartDefaultTask */ - /* Infinite loop */ - for(;;) - { - osDelay(1); - } - /* USER CODE END StartDefaultTask */ -} - -/* Private application code --------------------------------------------------*/ -/* USER CODE BEGIN Application */ - -/* USER CODE END Application */ - -/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * File Name : freertos.c + * Description : Code for freertos applications + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2025 STMicroelectronics. + * All rights reserved.

+ * + * This software component is licensed by ST under Ultimate Liberty license + * SLA0044, the "License"; You may not use this file except in compliance with + * the License. You may obtain a copy of the License at: + * www.st.com/SLA0044 + * + ****************************************************************************** + */ +/* 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); + +extern void MX_USB_DEVICE_Init(void); +void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */ + +/* Hook prototypes */ +void configureTimerForRunTimeStats(void); +unsigned long getRunTimeCounterValue(void); +void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char *pcTaskName); + +/* USER CODE BEGIN 1 */ +/* Functions needed when configGENERATE_RUN_TIME_STATS is on */ +__weak void configureTimerForRunTimeStats(void) +{ + +} + +__weak unsigned long getRunTimeCounterValue(void) +{ +return 0; +} +/* USER CODE END 1 */ + +/* USER CODE BEGIN 4 */ +void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char *pcTaskName) +{ + /* Run time stack overflow checking is performed if + configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2. This hook function is + called if a stack overflow is detected. */ +} +/* USER CODE END 4 */ + +/** + * @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) +{ + /* init code for USB_DEVICE */ + MX_USB_DEVICE_Init(); + /* USER CODE BEGIN StartDefaultTask */ + osThreadTerminate(osThreadGetId()); +/* USER CODE END StartDefaultTask */ +} + +/* Private application code --------------------------------------------------*/ +/* USER CODE BEGIN Application */ + +/* USER CODE END Application */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/Core/Src/stm32f4xx_it.c b/Core/Src/stm32f4xx_it.c index 96a9679..2b22755 100644 --- a/Core/Src/stm32f4xx_it.c +++ b/Core/Src/stm32f4xx_it.c @@ -268,6 +268,20 @@ void DMA1_Stream2_IRQHandler(void) /* USER CODE END DMA1_Stream2_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. */ @@ -408,6 +422,20 @@ void DMA2_Stream3_IRQHandler(void) /* USER CODE END DMA2_Stream3_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. */ 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/DevC.ioc b/DevC.ioc index 5b9c49a..7e7f2b1 100644 --- a/DevC.ioc +++ b/DevC.ioc @@ -267,8 +267,10 @@ MxDb.Version=DB.6.0.20 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false NVIC.CAN1_RX0_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true NVIC.CAN1_RX1_IRQn=true\:5\:0\:false\: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\:true\: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\:true\:false\:true\:true\:false\:true\:true NVIC.DMA1_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true NVIC.DMA1_Stream7_IRQn=true\:5\:0\:true\:false\:true\:true\:false\:true\:true @@ -552,7 +554,7 @@ ProjectManager.ProjectName=DevC ProjectManager.ProjectStructure= ProjectManager.RegisterCallBack= ProjectManager.StackSize=0x1000 -ProjectManager.TargetToolchain=MDK-ARM V5.32 +ProjectManager.TargetToolchain=CMake ProjectManager.ToolChainLocation= ProjectManager.UAScriptAfterPath= ProjectManager.UAScriptBeforePath= diff --git a/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c b/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c new file mode 100644 index 0000000..9e1d1a8 --- /dev/null +++ b/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/port.c @@ -0,0 +1,775 @@ +/* + * FreeRTOS Kernel V10.3.1 + * Copyright (C) 2020 Amazon.com, Inc. or its affiliates. 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! + */ + +/*----------------------------------------------------------- + * Implementation of functions defined in portable.h for the ARM CM4F port. + *----------------------------------------------------------*/ + +/* Scheduler includes. */ +#include "FreeRTOS.h" +#include "task.h" + +#ifndef __VFP_FP__ + #error This port can only be used when the project options are configured to enable hardware floating point support. +#endif + +#ifndef configSYSTICK_CLOCK_HZ + #define configSYSTICK_CLOCK_HZ configCPU_CLOCK_HZ + /* Ensure the SysTick is clocked at the same frequency as the core. */ + #define portNVIC_SYSTICK_CLK_BIT ( 1UL << 2UL ) +#else + /* The way the SysTick is clocked is not modified in case it is not the same + as the core. */ + #define portNVIC_SYSTICK_CLK_BIT ( 0 ) +#endif + +/* Constants required to manipulate the core. Registers first... */ +#define portNVIC_SYSTICK_CTRL_REG ( * ( ( volatile uint32_t * ) 0xe000e010 ) ) +#define portNVIC_SYSTICK_LOAD_REG ( * ( ( volatile uint32_t * ) 0xe000e014 ) ) +#define portNVIC_SYSTICK_CURRENT_VALUE_REG ( * ( ( volatile uint32_t * ) 0xe000e018 ) ) +#define portNVIC_SYSPRI2_REG ( * ( ( volatile uint32_t * ) 0xe000ed20 ) ) +/* ...then bits in the registers. */ +#define portNVIC_SYSTICK_INT_BIT ( 1UL << 1UL ) +#define portNVIC_SYSTICK_ENABLE_BIT ( 1UL << 0UL ) +#define portNVIC_SYSTICK_COUNT_FLAG_BIT ( 1UL << 16UL ) +#define portNVIC_PENDSVCLEAR_BIT ( 1UL << 27UL ) +#define portNVIC_PEND_SYSTICK_CLEAR_BIT ( 1UL << 25UL ) + +/* Constants used to detect a Cortex-M7 r0p1 core, which should use the ARM_CM7 +r0p1 port. */ +#define portCPUID ( * ( ( volatile uint32_t * ) 0xE000ed00 ) ) +#define portCORTEX_M7_r0p1_ID ( 0x410FC271UL ) +#define portCORTEX_M7_r0p0_ID ( 0x410FC270UL ) + +#define portNVIC_PENDSV_PRI ( ( ( uint32_t ) configKERNEL_INTERRUPT_PRIORITY ) << 16UL ) +#define portNVIC_SYSTICK_PRI ( ( ( uint32_t ) configKERNEL_INTERRUPT_PRIORITY ) << 24UL ) + +/* Constants required to check the validity of an interrupt priority. */ +#define portFIRST_USER_INTERRUPT_NUMBER ( 16 ) +#define portNVIC_IP_REGISTERS_OFFSET_16 ( 0xE000E3F0 ) +#define portAIRCR_REG ( * ( ( volatile uint32_t * ) 0xE000ED0C ) ) +#define portMAX_8_BIT_VALUE ( ( uint8_t ) 0xff ) +#define portTOP_BIT_OF_BYTE ( ( uint8_t ) 0x80 ) +#define portMAX_PRIGROUP_BITS ( ( uint8_t ) 7 ) +#define portPRIORITY_GROUP_MASK ( 0x07UL << 8UL ) +#define portPRIGROUP_SHIFT ( 8UL ) + +/* Masks off all bits but the VECTACTIVE bits in the ICSR register. */ +#define portVECTACTIVE_MASK ( 0xFFUL ) + +/* Constants required to manipulate the VFP. */ +#define portFPCCR ( ( volatile uint32_t * ) 0xe000ef34 ) /* Floating point context control register. */ +#define portASPEN_AND_LSPEN_BITS ( 0x3UL << 30UL ) + +/* Constants required to set up the initial stack. */ +#define portINITIAL_XPSR ( 0x01000000 ) +#define portINITIAL_EXC_RETURN ( 0xfffffffd ) + +/* The systick is a 24-bit counter. */ +#define portMAX_24_BIT_NUMBER ( 0xffffffUL ) + +/* For strict compliance with the Cortex-M spec the task start address should +have bit-0 clear, as it is loaded into the PC on exit from an ISR. */ +#define portSTART_ADDRESS_MASK ( ( StackType_t ) 0xfffffffeUL ) + +/* A fiddle factor to estimate the number of SysTick counts that would have +occurred while the SysTick counter is stopped during tickless idle +calculations. */ +#define portMISSED_COUNTS_FACTOR ( 45UL ) + +/* Let the user override the pre-loading of the initial LR with the address of +prvTaskExitError() in case it messes up unwinding of the stack in the +debugger. */ +#ifdef configTASK_RETURN_ADDRESS + #define portTASK_RETURN_ADDRESS configTASK_RETURN_ADDRESS +#else + #define portTASK_RETURN_ADDRESS prvTaskExitError +#endif + +/* + * Setup the timer to generate the tick interrupts. The implementation in this + * file is weak to allow application writers to change the timer used to + * generate the tick interrupt. + */ +void vPortSetupTimerInterrupt( void ); + +/* + * Exception handlers. + */ +void xPortPendSVHandler( void ) __attribute__ (( naked )); +void xPortSysTickHandler( void ); +void vPortSVCHandler( void ) __attribute__ (( naked )); + +/* + * Start first task is a separate function so it can be tested in isolation. + */ +static void prvPortStartFirstTask( void ) __attribute__ (( naked )); + +/* + * Function to enable the VFP. + */ +static void vPortEnableVFP( void ) __attribute__ (( naked )); + +/* + * Used to catch tasks that attempt to return from their implementing function. + */ +static void prvTaskExitError( void ); + +/*-----------------------------------------------------------*/ + +/* Each task maintains its own interrupt status in the critical nesting +variable. */ +static UBaseType_t uxCriticalNesting = 0xaaaaaaaa; + +/* + * The number of SysTick increments that make up one tick period. + */ +#if( configUSE_TICKLESS_IDLE == 1 ) + static uint32_t ulTimerCountsForOneTick = 0; +#endif /* configUSE_TICKLESS_IDLE */ + +/* + * The maximum number of tick periods that can be suppressed is limited by the + * 24 bit resolution of the SysTick timer. + */ +#if( configUSE_TICKLESS_IDLE == 1 ) + static uint32_t xMaximumPossibleSuppressedTicks = 0; +#endif /* configUSE_TICKLESS_IDLE */ + +/* + * Compensate for the CPU cycles that pass while the SysTick is stopped (low + * power functionality only. + */ +#if( configUSE_TICKLESS_IDLE == 1 ) + static uint32_t ulStoppedTimerCompensation = 0; +#endif /* configUSE_TICKLESS_IDLE */ + +/* + * Used by the portASSERT_IF_INTERRUPT_PRIORITY_INVALID() macro to ensure + * FreeRTOS API functions are not called from interrupts that have been assigned + * a priority above configMAX_SYSCALL_INTERRUPT_PRIORITY. + */ +#if( configASSERT_DEFINED == 1 ) + static uint8_t ucMaxSysCallPriority = 0; + static uint32_t ulMaxPRIGROUPValue = 0; + static const volatile uint8_t * const pcInterruptPriorityRegisters = ( const volatile uint8_t * const ) portNVIC_IP_REGISTERS_OFFSET_16; +#endif /* configASSERT_DEFINED */ + +/*-----------------------------------------------------------*/ + +/* + * See header file for description. + */ +StackType_t *pxPortInitialiseStack( StackType_t *pxTopOfStack, TaskFunction_t pxCode, void *pvParameters ) +{ + /* Simulate the stack frame as it would be created by a context switch + interrupt. */ + + /* Offset added to account for the way the MCU uses the stack on entry/exit + of interrupts, and to ensure alignment. */ + pxTopOfStack--; + + *pxTopOfStack = portINITIAL_XPSR; /* xPSR */ + pxTopOfStack--; + *pxTopOfStack = ( ( StackType_t ) pxCode ) & portSTART_ADDRESS_MASK; /* PC */ + pxTopOfStack--; + *pxTopOfStack = ( StackType_t ) portTASK_RETURN_ADDRESS; /* LR */ + + /* Save code space by skipping register initialisation. */ + pxTopOfStack -= 5; /* R12, R3, R2 and R1. */ + *pxTopOfStack = ( StackType_t ) pvParameters; /* R0 */ + + /* A save method is being used that requires each task to maintain its + own exec return value. */ + pxTopOfStack--; + *pxTopOfStack = portINITIAL_EXC_RETURN; + + pxTopOfStack -= 8; /* R11, R10, R9, R8, R7, R6, R5 and R4. */ + + return pxTopOfStack; +} +/*-----------------------------------------------------------*/ + +static void prvTaskExitError( void ) +{ +volatile uint32_t ulDummy = 0; + + /* A function that implements a task must not exit or attempt to return to + its caller as there is nothing to return to. If a task wants to exit it + should instead call vTaskDelete( NULL ). + + Artificially force an assert() to be triggered if configASSERT() is + defined, then stop here so application writers can catch the error. */ + configASSERT( uxCriticalNesting == ~0UL ); + portDISABLE_INTERRUPTS(); + while( ulDummy == 0 ) + { + /* This file calls prvTaskExitError() after the scheduler has been + started to remove a compiler warning about the function being defined + but never called. ulDummy is used purely to quieten other warnings + about code appearing after this function is called - making ulDummy + volatile makes the compiler think the function could return and + therefore not output an 'unreachable code' warning for code that appears + after it. */ + } +} +/*-----------------------------------------------------------*/ + +void vPortSVCHandler( void ) +{ + __asm volatile ( + " ldr r3, pxCurrentTCBConst2 \n" /* Restore the context. */ + " ldr r1, [r3] \n" /* Use pxCurrentTCBConst to get the pxCurrentTCB address. */ + " ldr r0, [r1] \n" /* The first item in pxCurrentTCB is the task top of stack. */ + " ldmia r0!, {r4-r11, r14} \n" /* Pop the registers that are not automatically saved on exception entry and the critical nesting count. */ + " msr psp, r0 \n" /* Restore the task stack pointer. */ + " isb \n" + " mov r0, #0 \n" + " msr basepri, r0 \n" + " bx r14 \n" + " \n" + " .align 4 \n" + "pxCurrentTCBConst2: .word pxCurrentTCB \n" + ); +} +/*-----------------------------------------------------------*/ + +static void prvPortStartFirstTask( void ) +{ + /* Start the first task. This also clears the bit that indicates the FPU is + in use in case the FPU was used before the scheduler was started - which + would otherwise result in the unnecessary leaving of space in the SVC stack + for lazy saving of FPU registers. */ + __asm volatile( + " ldr r0, =0xE000ED08 \n" /* Use the NVIC offset register to locate the stack. */ + " ldr r0, [r0] \n" + " ldr r0, [r0] \n" + " msr msp, r0 \n" /* Set the msp back to the start of the stack. */ + " mov r0, #0 \n" /* Clear the bit that indicates the FPU is in use, see comment above. */ + " msr control, r0 \n" + " cpsie i \n" /* Globally enable interrupts. */ + " cpsie f \n" + " dsb \n" + " isb \n" + " svc 0 \n" /* System call to start first task. */ + " nop \n" + ); +} +/*-----------------------------------------------------------*/ + +/* + * See header file for description. + */ +BaseType_t xPortStartScheduler( void ) +{ + /* configMAX_SYSCALL_INTERRUPT_PRIORITY must not be set to 0. + See http://www.FreeRTOS.org/RTOS-Cortex-M3-M4.html */ + configASSERT( configMAX_SYSCALL_INTERRUPT_PRIORITY ); + + /* This port can be used on all revisions of the Cortex-M7 core other than + the r0p1 parts. r0p1 parts should use the port from the + /source/portable/GCC/ARM_CM7/r0p1 directory. */ + configASSERT( portCPUID != portCORTEX_M7_r0p1_ID ); + configASSERT( portCPUID != portCORTEX_M7_r0p0_ID ); + + #if( configASSERT_DEFINED == 1 ) + { + volatile uint32_t ulOriginalPriority; + volatile uint8_t * const pucFirstUserPriorityRegister = ( volatile uint8_t * const ) ( portNVIC_IP_REGISTERS_OFFSET_16 + portFIRST_USER_INTERRUPT_NUMBER ); + volatile uint8_t ucMaxPriorityValue; + + /* Determine the maximum priority from which ISR safe FreeRTOS API + functions can be called. ISR safe functions are those that end in + "FromISR". FreeRTOS maintains separate thread and ISR API functions to + ensure interrupt entry is as fast and simple as possible. + + Save the interrupt priority value that is about to be clobbered. */ + ulOriginalPriority = *pucFirstUserPriorityRegister; + + /* Determine the number of priority bits available. First write to all + possible bits. */ + *pucFirstUserPriorityRegister = portMAX_8_BIT_VALUE; + + /* Read the value back to see how many bits stuck. */ + ucMaxPriorityValue = *pucFirstUserPriorityRegister; + + /* Use the same mask on the maximum system call priority. */ + ucMaxSysCallPriority = configMAX_SYSCALL_INTERRUPT_PRIORITY & ucMaxPriorityValue; + + /* Calculate the maximum acceptable priority group value for the number + of bits read back. */ + ulMaxPRIGROUPValue = portMAX_PRIGROUP_BITS; + while( ( ucMaxPriorityValue & portTOP_BIT_OF_BYTE ) == portTOP_BIT_OF_BYTE ) + { + ulMaxPRIGROUPValue--; + ucMaxPriorityValue <<= ( uint8_t ) 0x01; + } + + #ifdef __NVIC_PRIO_BITS + { + /* Check the CMSIS configuration that defines the number of + priority bits matches the number of priority bits actually queried + from the hardware. */ + configASSERT( ( portMAX_PRIGROUP_BITS - ulMaxPRIGROUPValue ) == __NVIC_PRIO_BITS ); + } + #endif + + #ifdef configPRIO_BITS + { + /* Check the FreeRTOS configuration that defines the number of + priority bits matches the number of priority bits actually queried + from the hardware. */ + configASSERT( ( portMAX_PRIGROUP_BITS - ulMaxPRIGROUPValue ) == configPRIO_BITS ); + } + #endif + + /* Shift the priority group value back to its position within the AIRCR + register. */ + ulMaxPRIGROUPValue <<= portPRIGROUP_SHIFT; + ulMaxPRIGROUPValue &= portPRIORITY_GROUP_MASK; + + /* Restore the clobbered interrupt priority register to its original + value. */ + *pucFirstUserPriorityRegister = ulOriginalPriority; + } + #endif /* conifgASSERT_DEFINED */ + + /* Make PendSV and SysTick the lowest priority interrupts. */ + portNVIC_SYSPRI2_REG |= portNVIC_PENDSV_PRI; + portNVIC_SYSPRI2_REG |= portNVIC_SYSTICK_PRI; + + /* Start the timer that generates the tick ISR. Interrupts are disabled + here already. */ + vPortSetupTimerInterrupt(); + + /* Initialise the critical nesting count ready for the first task. */ + uxCriticalNesting = 0; + + /* Ensure the VFP is enabled - it should be anyway. */ + vPortEnableVFP(); + + /* Lazy save always. */ + *( portFPCCR ) |= portASPEN_AND_LSPEN_BITS; + + /* Start the first task. */ + prvPortStartFirstTask(); + + /* Should never get here as the tasks will now be executing! Call the task + exit error function to prevent compiler warnings about a static function + not being called in the case that the application writer overrides this + functionality by defining configTASK_RETURN_ADDRESS. Call + vTaskSwitchContext() so link time optimisation does not remove the + symbol. */ + vTaskSwitchContext(); + prvTaskExitError(); + + /* Should not get here! */ + return 0; +} +/*-----------------------------------------------------------*/ + +void vPortEndScheduler( void ) +{ + /* Not implemented in ports where there is nothing to return to. + Artificially force an assert. */ + configASSERT( uxCriticalNesting == 1000UL ); +} +/*-----------------------------------------------------------*/ + +void vPortEnterCritical( void ) +{ + portDISABLE_INTERRUPTS(); + uxCriticalNesting++; + + /* This is not the interrupt safe version of the enter critical function so + assert() if it is being called from an interrupt context. Only API + functions that end in "FromISR" can be used in an interrupt. Only assert if + the critical nesting count is 1 to protect against recursive calls if the + assert function also uses a critical section. */ + if( uxCriticalNesting == 1 ) + { + configASSERT( ( portNVIC_INT_CTRL_REG & portVECTACTIVE_MASK ) == 0 ); + } +} +/*-----------------------------------------------------------*/ + +void vPortExitCritical( void ) +{ + configASSERT( uxCriticalNesting ); + uxCriticalNesting--; + if( uxCriticalNesting == 0 ) + { + portENABLE_INTERRUPTS(); + } +} +/*-----------------------------------------------------------*/ + +void xPortPendSVHandler( void ) +{ + /* This is a naked function. */ + + __asm volatile + ( + " mrs r0, psp \n" + " isb \n" + " \n" + " ldr r3, pxCurrentTCBConst \n" /* Get the location of the current TCB. */ + " ldr r2, [r3] \n" + " \n" + " tst r14, #0x10 \n" /* Is the task using the FPU context? If so, push high vfp registers. */ + " it eq \n" + " vstmdbeq r0!, {s16-s31} \n" + " \n" + " stmdb r0!, {r4-r11, r14} \n" /* Save the core registers. */ + " str r0, [r2] \n" /* Save the new top of stack into the first member of the TCB. */ + " \n" + " stmdb sp!, {r0, r3} \n" + " mov r0, %0 \n" + " msr basepri, r0 \n" + " dsb \n" + " isb \n" + " bl vTaskSwitchContext \n" + " mov r0, #0 \n" + " msr basepri, r0 \n" + " ldmia sp!, {r0, r3} \n" + " \n" + " ldr r1, [r3] \n" /* The first item in pxCurrentTCB is the task top of stack. */ + " ldr r0, [r1] \n" + " \n" + " ldmia r0!, {r4-r11, r14} \n" /* Pop the core registers. */ + " \n" + " tst r14, #0x10 \n" /* Is the task using the FPU context? If so, pop the high vfp registers too. */ + " it eq \n" + " vldmiaeq r0!, {s16-s31} \n" + " \n" + " msr psp, r0 \n" + " isb \n" + " \n" + #ifdef WORKAROUND_PMU_CM001 /* XMC4000 specific errata workaround. */ + #if WORKAROUND_PMU_CM001 == 1 + " push { r14 } \n" + " pop { pc } \n" + #endif + #endif + " \n" + " bx r14 \n" + " \n" + " .align 4 \n" + "pxCurrentTCBConst: .word pxCurrentTCB \n" + ::"i"(configMAX_SYSCALL_INTERRUPT_PRIORITY) + ); +} +/*-----------------------------------------------------------*/ + +void xPortSysTickHandler( void ) +{ + /* The SysTick runs at the lowest interrupt priority, so when this interrupt + executes all interrupts must be unmasked. There is therefore no need to + save and then restore the interrupt mask value as its value is already + known. */ + portDISABLE_INTERRUPTS(); + { + /* Increment the RTOS tick. */ + if( xTaskIncrementTick() != pdFALSE ) + { + /* A context switch is required. Context switching is performed in + the PendSV interrupt. Pend the PendSV interrupt. */ + portNVIC_INT_CTRL_REG = portNVIC_PENDSVSET_BIT; + } + } + portENABLE_INTERRUPTS(); +} +/*-----------------------------------------------------------*/ + +#if( configUSE_TICKLESS_IDLE == 1 ) + + __attribute__((weak)) void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime ) + { + uint32_t ulReloadValue, ulCompleteTickPeriods, ulCompletedSysTickDecrements; + TickType_t xModifiableIdleTime; + + /* Make sure the SysTick reload value does not overflow the counter. */ + if( xExpectedIdleTime > xMaximumPossibleSuppressedTicks ) + { + xExpectedIdleTime = xMaximumPossibleSuppressedTicks; + } + + /* Stop the SysTick momentarily. The time the SysTick is stopped for + is accounted for as best it can be, but using the tickless mode will + inevitably result in some tiny drift of the time maintained by the + kernel with respect to calendar time. */ + portNVIC_SYSTICK_CTRL_REG &= ~portNVIC_SYSTICK_ENABLE_BIT; + + /* Calculate the reload value required to wait xExpectedIdleTime + tick periods. -1 is used because this code will execute part way + through one of the tick periods. */ + ulReloadValue = portNVIC_SYSTICK_CURRENT_VALUE_REG + ( ulTimerCountsForOneTick * ( xExpectedIdleTime - 1UL ) ); + if( ulReloadValue > ulStoppedTimerCompensation ) + { + ulReloadValue -= ulStoppedTimerCompensation; + } + + /* Enter a critical section but don't use the taskENTER_CRITICAL() + method as that will mask interrupts that should exit sleep mode. */ + __asm volatile( "cpsid i" ::: "memory" ); + __asm volatile( "dsb" ); + __asm volatile( "isb" ); + + /* If a context switch is pending or a task is waiting for the scheduler + to be unsuspended then abandon the low power entry. */ + if( eTaskConfirmSleepModeStatus() == eAbortSleep ) + { + /* Restart from whatever is left in the count register to complete + this tick period. */ + portNVIC_SYSTICK_LOAD_REG = portNVIC_SYSTICK_CURRENT_VALUE_REG; + + /* Restart SysTick. */ + portNVIC_SYSTICK_CTRL_REG |= portNVIC_SYSTICK_ENABLE_BIT; + + /* Reset the reload register to the value required for normal tick + periods. */ + portNVIC_SYSTICK_LOAD_REG = ulTimerCountsForOneTick - 1UL; + + /* Re-enable interrupts - see comments above the cpsid instruction() + above. */ + __asm volatile( "cpsie i" ::: "memory" ); + } + else + { + /* Set the new reload value. */ + portNVIC_SYSTICK_LOAD_REG = ulReloadValue; + + /* Clear the SysTick count flag and set the count value back to + zero. */ + portNVIC_SYSTICK_CURRENT_VALUE_REG = 0UL; + + /* Restart SysTick. */ + portNVIC_SYSTICK_CTRL_REG |= portNVIC_SYSTICK_ENABLE_BIT; + + /* Sleep until something happens. configPRE_SLEEP_PROCESSING() can + set its parameter to 0 to indicate that its implementation contains + its own wait for interrupt or wait for event instruction, and so wfi + should not be executed again. However, the original expected idle + time variable must remain unmodified, so a copy is taken. */ + xModifiableIdleTime = xExpectedIdleTime; + configPRE_SLEEP_PROCESSING( xModifiableIdleTime ); + if( xModifiableIdleTime > 0 ) + { + __asm volatile( "dsb" ::: "memory" ); + __asm volatile( "wfi" ); + __asm volatile( "isb" ); + } + configPOST_SLEEP_PROCESSING( xExpectedIdleTime ); + + /* Re-enable interrupts to allow the interrupt that brought the MCU + out of sleep mode to execute immediately. see comments above + __disable_interrupt() call above. */ + __asm volatile( "cpsie i" ::: "memory" ); + __asm volatile( "dsb" ); + __asm volatile( "isb" ); + + /* Disable interrupts again because the clock is about to be stopped + and interrupts that execute while the clock is stopped will increase + any slippage between the time maintained by the RTOS and calendar + time. */ + __asm volatile( "cpsid i" ::: "memory" ); + __asm volatile( "dsb" ); + __asm volatile( "isb" ); + + /* Disable the SysTick clock without reading the + portNVIC_SYSTICK_CTRL_REG register to ensure the + portNVIC_SYSTICK_COUNT_FLAG_BIT is not cleared if it is set. Again, + the time the SysTick is stopped for is accounted for as best it can + be, but using the tickless mode will inevitably result in some tiny + drift of the time maintained by the kernel with respect to calendar + time*/ + portNVIC_SYSTICK_CTRL_REG = ( portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT ); + + /* Determine if the SysTick clock has already counted to zero and + been set back to the current reload value (the reload back being + correct for the entire expected idle time) or if the SysTick is yet + to count to zero (in which case an interrupt other than the SysTick + must have brought the system out of sleep mode). */ + if( ( portNVIC_SYSTICK_CTRL_REG & portNVIC_SYSTICK_COUNT_FLAG_BIT ) != 0 ) + { + uint32_t ulCalculatedLoadValue; + + /* The tick interrupt is already pending, and the SysTick count + reloaded with ulReloadValue. Reset the + portNVIC_SYSTICK_LOAD_REG with whatever remains of this tick + period. */ + ulCalculatedLoadValue = ( ulTimerCountsForOneTick - 1UL ) - ( ulReloadValue - portNVIC_SYSTICK_CURRENT_VALUE_REG ); + + /* Don't allow a tiny value, or values that have somehow + underflowed because the post sleep hook did something + that took too long. */ + if( ( ulCalculatedLoadValue < ulStoppedTimerCompensation ) || ( ulCalculatedLoadValue > ulTimerCountsForOneTick ) ) + { + ulCalculatedLoadValue = ( ulTimerCountsForOneTick - 1UL ); + } + + portNVIC_SYSTICK_LOAD_REG = ulCalculatedLoadValue; + + /* As the pending tick will be processed as soon as this + function exits, the tick value maintained by the tick is stepped + forward by one less than the time spent waiting. */ + ulCompleteTickPeriods = xExpectedIdleTime - 1UL; + } + else + { + /* Something other than the tick interrupt ended the sleep. + Work out how long the sleep lasted rounded to complete tick + periods (not the ulReload value which accounted for part + ticks). */ + ulCompletedSysTickDecrements = ( xExpectedIdleTime * ulTimerCountsForOneTick ) - portNVIC_SYSTICK_CURRENT_VALUE_REG; + + /* How many complete tick periods passed while the processor + was waiting? */ + ulCompleteTickPeriods = ulCompletedSysTickDecrements / ulTimerCountsForOneTick; + + /* The reload value is set to whatever fraction of a single tick + period remains. */ + portNVIC_SYSTICK_LOAD_REG = ( ( ulCompleteTickPeriods + 1UL ) * ulTimerCountsForOneTick ) - ulCompletedSysTickDecrements; + } + + /* Restart SysTick so it runs from portNVIC_SYSTICK_LOAD_REG + again, then set portNVIC_SYSTICK_LOAD_REG back to its standard + value. */ + portNVIC_SYSTICK_CURRENT_VALUE_REG = 0UL; + portNVIC_SYSTICK_CTRL_REG |= portNVIC_SYSTICK_ENABLE_BIT; + vTaskStepTick( ulCompleteTickPeriods ); + portNVIC_SYSTICK_LOAD_REG = ulTimerCountsForOneTick - 1UL; + + /* Exit with interrupts enabled. */ + __asm volatile( "cpsie i" ::: "memory" ); + } + } + +#endif /* #if configUSE_TICKLESS_IDLE */ +/*-----------------------------------------------------------*/ + +/* + * Setup the systick timer to generate the tick interrupts at the required + * frequency. + */ +__attribute__(( weak )) void vPortSetupTimerInterrupt( void ) +{ + /* Calculate the constants required to configure the tick interrupt. */ + #if( configUSE_TICKLESS_IDLE == 1 ) + { + ulTimerCountsForOneTick = ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ ); + xMaximumPossibleSuppressedTicks = portMAX_24_BIT_NUMBER / ulTimerCountsForOneTick; + ulStoppedTimerCompensation = portMISSED_COUNTS_FACTOR / ( configCPU_CLOCK_HZ / configSYSTICK_CLOCK_HZ ); + } + #endif /* configUSE_TICKLESS_IDLE */ + + /* Stop and clear the SysTick. */ + portNVIC_SYSTICK_CTRL_REG = 0UL; + portNVIC_SYSTICK_CURRENT_VALUE_REG = 0UL; + + /* Configure SysTick to interrupt at the requested rate. */ + portNVIC_SYSTICK_LOAD_REG = ( configSYSTICK_CLOCK_HZ / configTICK_RATE_HZ ) - 1UL; + portNVIC_SYSTICK_CTRL_REG = ( portNVIC_SYSTICK_CLK_BIT | portNVIC_SYSTICK_INT_BIT | portNVIC_SYSTICK_ENABLE_BIT ); +} +/*-----------------------------------------------------------*/ + +/* This is a naked function. */ +static void vPortEnableVFP( void ) +{ + __asm volatile + ( + " ldr.w r0, =0xE000ED88 \n" /* The FPU enable bits are in the CPACR. */ + " ldr r1, [r0] \n" + " \n" + " orr r1, r1, #( 0xf << 20 ) \n" /* Enable CP10 and CP11 coprocessors, then save back. */ + " str r1, [r0] \n" + " bx r14 " + ); +} +/*-----------------------------------------------------------*/ + +#if( configASSERT_DEFINED == 1 ) + + void vPortValidateInterruptPriority( void ) + { + uint32_t ulCurrentInterrupt; + uint8_t ucCurrentPriority; + + /* Obtain the number of the currently executing interrupt. */ + __asm volatile( "mrs %0, ipsr" : "=r"( ulCurrentInterrupt ) :: "memory" ); + + /* Is the interrupt number a user defined interrupt? */ + if( ulCurrentInterrupt >= portFIRST_USER_INTERRUPT_NUMBER ) + { + /* Look up the interrupt's priority. */ + ucCurrentPriority = pcInterruptPriorityRegisters[ ulCurrentInterrupt ]; + + /* The following assertion will fail if a service routine (ISR) for + an interrupt that has been assigned a priority above + configMAX_SYSCALL_INTERRUPT_PRIORITY calls an ISR safe FreeRTOS API + function. ISR safe FreeRTOS API functions must *only* be called + from interrupts that have been assigned a priority at or below + configMAX_SYSCALL_INTERRUPT_PRIORITY. + + Numerically low interrupt priority numbers represent logically high + interrupt priorities, therefore the priority of the interrupt must + be set to a value equal to or numerically *higher* than + configMAX_SYSCALL_INTERRUPT_PRIORITY. + + Interrupts that use the FreeRTOS API must not be left at their + default priority of zero as that is the highest possible priority, + which is guaranteed to be above configMAX_SYSCALL_INTERRUPT_PRIORITY, + and therefore also guaranteed to be invalid. + + FreeRTOS maintains separate thread and ISR API functions to ensure + interrupt entry is as fast and simple as possible. + + The following links provide detailed information: + http://www.freertos.org/RTOS-Cortex-M3-M4.html + http://www.freertos.org/FAQHelp.html */ + configASSERT( ucCurrentPriority >= ucMaxSysCallPriority ); + } + + /* Priority grouping: The interrupt controller (NVIC) allows the bits + that define each interrupt's priority to be split between bits that + define the interrupt's pre-emption priority bits and bits that define + the interrupt's sub-priority. For simplicity all bits must be defined + to be pre-emption priority bits. The following assertion will fail if + this is not the case (if some bits represent a sub-priority). + + If the application only uses CMSIS libraries for interrupt + configuration then the correct setting can be achieved on all Cortex-M + devices by calling NVIC_SetPriorityGrouping( 0 ); before starting the + scheduler. Note however that some vendor specific peripheral libraries + assume a non-zero priority group setting, in which cases using a value + of zero will result in unpredictable behaviour. */ + configASSERT( ( portAIRCR_REG & portPRIORITY_GROUP_MASK ) <= ulMaxPRIGROUPValue ); + } + +#endif /* configASSERT_DEFINED */ + + diff --git a/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h b/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h new file mode 100644 index 0000000..e1e7fad --- /dev/null +++ b/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F/portmacro.h @@ -0,0 +1,243 @@ +/* + * FreeRTOS Kernel V10.3.1 + * Copyright (C) 2020 Amazon.com, Inc. or its affiliates. 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! + */ + + +#ifndef PORTMACRO_H +#define PORTMACRO_H + +#ifdef __cplusplus +extern "C" { +#endif + +/*----------------------------------------------------------- + * Port specific definitions. + * + * The settings in this file configure FreeRTOS correctly for the + * given hardware and compiler. + * + * These settings should not be altered. + *----------------------------------------------------------- + */ + +/* Type definitions. */ +#define portCHAR char +#define portFLOAT float +#define portDOUBLE double +#define portLONG long +#define portSHORT short +#define portSTACK_TYPE uint32_t +#define portBASE_TYPE long + +typedef portSTACK_TYPE StackType_t; +typedef long BaseType_t; +typedef unsigned long UBaseType_t; + +#if( configUSE_16_BIT_TICKS == 1 ) + typedef uint16_t TickType_t; + #define portMAX_DELAY ( TickType_t ) 0xffff +#else + typedef uint32_t TickType_t; + #define portMAX_DELAY ( TickType_t ) 0xffffffffUL + + /* 32-bit tick type on a 32-bit architecture, so reads of the tick count do + not need to be guarded with a critical section. */ + #define portTICK_TYPE_IS_ATOMIC 1 +#endif +/*-----------------------------------------------------------*/ + +/* Architecture specifics. */ +#define portSTACK_GROWTH ( -1 ) +#define portTICK_PERIOD_MS ( ( TickType_t ) 1000 / configTICK_RATE_HZ ) +#define portBYTE_ALIGNMENT 8 +/*-----------------------------------------------------------*/ + +/* Scheduler utilities. */ +#define portYIELD() \ +{ \ + /* Set a PendSV to request a context switch. */ \ + portNVIC_INT_CTRL_REG = portNVIC_PENDSVSET_BIT; \ + \ + /* Barriers are normally not required but do ensure the code is completely \ + within the specified behaviour for the architecture. */ \ + __asm volatile( "dsb" ::: "memory" ); \ + __asm volatile( "isb" ); \ +} + +#define portNVIC_INT_CTRL_REG ( * ( ( volatile uint32_t * ) 0xe000ed04 ) ) +#define portNVIC_PENDSVSET_BIT ( 1UL << 28UL ) +#define portEND_SWITCHING_ISR( xSwitchRequired ) if( xSwitchRequired != pdFALSE ) portYIELD() +#define portYIELD_FROM_ISR( x ) portEND_SWITCHING_ISR( x ) +/*-----------------------------------------------------------*/ + +/* Critical section management. */ +extern void vPortEnterCritical( void ); +extern void vPortExitCritical( void ); +#define portSET_INTERRUPT_MASK_FROM_ISR() ulPortRaiseBASEPRI() +#define portCLEAR_INTERRUPT_MASK_FROM_ISR(x) vPortSetBASEPRI(x) +#define portDISABLE_INTERRUPTS() vPortRaiseBASEPRI() +#define portENABLE_INTERRUPTS() vPortSetBASEPRI(0) +#define portENTER_CRITICAL() vPortEnterCritical() +#define portEXIT_CRITICAL() vPortExitCritical() + +/*-----------------------------------------------------------*/ + +/* Task function macros as described on the FreeRTOS.org WEB site. These are +not necessary for to use this port. They are defined so the common demo files +(which build with all the ports) will build. */ +#define portTASK_FUNCTION_PROTO( vFunction, pvParameters ) void vFunction( void *pvParameters ) +#define portTASK_FUNCTION( vFunction, pvParameters ) void vFunction( void *pvParameters ) +/*-----------------------------------------------------------*/ + +/* Tickless idle/low power functionality. */ +#ifndef portSUPPRESS_TICKS_AND_SLEEP + extern void vPortSuppressTicksAndSleep( TickType_t xExpectedIdleTime ); + #define portSUPPRESS_TICKS_AND_SLEEP( xExpectedIdleTime ) vPortSuppressTicksAndSleep( xExpectedIdleTime ) +#endif +/*-----------------------------------------------------------*/ + +/* Architecture specific optimisations. */ +#ifndef configUSE_PORT_OPTIMISED_TASK_SELECTION + #define configUSE_PORT_OPTIMISED_TASK_SELECTION 1 +#endif + +#if configUSE_PORT_OPTIMISED_TASK_SELECTION == 1 + + /* Generic helper function. */ + __attribute__( ( always_inline ) ) static inline uint8_t ucPortCountLeadingZeros( uint32_t ulBitmap ) + { + uint8_t ucReturn; + + __asm volatile ( "clz %0, %1" : "=r" ( ucReturn ) : "r" ( ulBitmap ) : "memory" ); + return ucReturn; + } + + /* Check the configuration. */ + #if( configMAX_PRIORITIES > 32 ) + #error configUSE_PORT_OPTIMISED_TASK_SELECTION can only be set to 1 when configMAX_PRIORITIES is less than or equal to 32. It is very rare that a system requires more than 10 to 15 difference priorities as tasks that share a priority will time slice. + #endif + + /* Store/clear the ready priorities in a bit map. */ + #define portRECORD_READY_PRIORITY( uxPriority, uxReadyPriorities ) ( uxReadyPriorities ) |= ( 1UL << ( uxPriority ) ) + #define portRESET_READY_PRIORITY( uxPriority, uxReadyPriorities ) ( uxReadyPriorities ) &= ~( 1UL << ( uxPriority ) ) + + /*-----------------------------------------------------------*/ + + #define portGET_HIGHEST_PRIORITY( uxTopPriority, uxReadyPriorities ) uxTopPriority = ( 31UL - ( uint32_t ) ucPortCountLeadingZeros( ( uxReadyPriorities ) ) ) + +#endif /* configUSE_PORT_OPTIMISED_TASK_SELECTION */ + +/*-----------------------------------------------------------*/ + +#ifdef configASSERT + void vPortValidateInterruptPriority( void ); + #define portASSERT_IF_INTERRUPT_PRIORITY_INVALID() vPortValidateInterruptPriority() +#endif + +/* portNOP() is not required by this port. */ +#define portNOP() + +#define portINLINE __inline + +#ifndef portFORCE_INLINE + #define portFORCE_INLINE inline __attribute__(( always_inline)) +#endif + +portFORCE_INLINE static BaseType_t xPortIsInsideInterrupt( void ) +{ +uint32_t ulCurrentInterrupt; +BaseType_t xReturn; + + /* Obtain the number of the currently executing interrupt. */ + __asm volatile( "mrs %0, ipsr" : "=r"( ulCurrentInterrupt ) :: "memory" ); + + if( ulCurrentInterrupt == 0 ) + { + xReturn = pdFALSE; + } + else + { + xReturn = pdTRUE; + } + + return xReturn; +} + +/*-----------------------------------------------------------*/ + +portFORCE_INLINE static void vPortRaiseBASEPRI( void ) +{ +uint32_t ulNewBASEPRI; + + __asm volatile + ( + " mov %0, %1 \n" \ + " msr basepri, %0 \n" \ + " isb \n" \ + " dsb \n" \ + :"=r" (ulNewBASEPRI) : "i" ( configMAX_SYSCALL_INTERRUPT_PRIORITY ) : "memory" + ); +} + +/*-----------------------------------------------------------*/ + +portFORCE_INLINE static uint32_t ulPortRaiseBASEPRI( void ) +{ +uint32_t ulOriginalBASEPRI, ulNewBASEPRI; + + __asm volatile + ( + " mrs %0, basepri \n" \ + " mov %1, %2 \n" \ + " msr basepri, %1 \n" \ + " isb \n" \ + " dsb \n" \ + :"=r" (ulOriginalBASEPRI), "=r" (ulNewBASEPRI) : "i" ( configMAX_SYSCALL_INTERRUPT_PRIORITY ) : "memory" + ); + + /* This return will not be reached but is necessary to prevent compiler + warnings. */ + return ulOriginalBASEPRI; +} +/*-----------------------------------------------------------*/ + +portFORCE_INLINE static void vPortSetBASEPRI( uint32_t ulNewMaskValue ) +{ + __asm volatile + ( + " msr basepri, %0 " :: "r" ( ulNewMaskValue ) : "memory" + ); +} +/*-----------------------------------------------------------*/ + +#define portMEMORY_BARRIER() __asm volatile( "" ::: "memory" ) + +#ifdef __cplusplus +} +#endif + +#endif /* PORTMACRO_H */ + diff --git a/STM32F407XX_FLASH.ld b/STM32F407XX_FLASH.ld new file mode 100644 index 0000000..df95fde --- /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..2c32a85 --- /dev/null +++ b/User/bsp/bsp_config.yaml @@ -0,0 +1,11 @@ +can: + devices: + - instance: CAN1 + name: '1' + - instance: CAN2 + name: '2' + enabled: true +mm: + 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/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/component/ahrs.c b/User/component/ahrs.c new file mode 100644 index 0000000..ffdb870 --- /dev/null +++ b/User/component/ahrs.c @@ -0,0 +1,417 @@ +/* + 开源的AHRS算法。 + MadgwickAHRS +*/ + +#include "ahrs.h" + +#include + +#include "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..2245b1f --- /dev/null +++ b/User/component/ahrs.h @@ -0,0 +1,114 @@ +/* + 开源的AHRS算法。 + MadgwickAHRS +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "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/component_config.yaml b/User/component/component_config.yaml new file mode 100644 index 0000000..4caf761 --- /dev/null +++ b/User/component/component_config.yaml @@ -0,0 +1,7 @@ +ahrs: + dependencies: + - component/user_math.h + enabled: true +user_math: + dependencies: [] + enabled: true diff --git a/User/component/user_math.c b/User/component/user_math.c new file mode 100644 index 0000000..49a4723 --- /dev/null +++ b/User/component/user_math.c @@ -0,0 +1,138 @@ +/* + 自定义的数学运算。 +*/ + +#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 计算循环值的误差，用于没有负数值，并在一定范围内变化的值 + * 例如编码器：相差1.5PI其实等于相差-0.5PI + * + * \param sp 被操作的值 + * \param fb 变化量 + * \param range 被操作的值变化范围，正数时起效 + * + * \return 函数运行结果 + */ +inline float CircleError(float sp, float fb, float range) { + float error = sp - fb; + if (range > 0.0f) { + float half_range = range / 2.0f; + + if (error > half_range) + error -= range; + else if (error < -half_range) + error += range; + } + return error; +} + +/** + * \brief 循环加法，用于没有负数值，并在一定范围内变化的值 + * 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI + * + * \param origin 被操作的值 + * \param delta 变化量 + * \param range 被操作的值变化范围，正数时起效 + */ +inline void CircleAdd(float *origin, float delta, float range) { + float out = *origin + delta; + if (range > 0.0f) { + if (out >= range) + out -= range; + else if (out < 0.0f) + out += range; + } + *origin = out; +} + +/** + * @brief 循环值取反 + * + * @param origin 被操作的值 + */ +inline void CircleReverse(float *origin) { *origin = -(*origin) + M_2PI; } + +/** + * @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..d4531e6 --- /dev/null +++ b/User/component/user_math.h @@ -0,0 +1,179 @@ +/* + 自定义的数学运算。 +*/ + +#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) + +#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 计算循环值的误差，用于没有负数值，并在一定范围内变化的值 + * 例如编码器：相差1.5PI其实等于相差-0.5PI + * + * \param sp 被操作的值 + * \param fb 变化量 + * \param range 被操作的值变化范围，正数时起效 + * + * \return 函数运行结果 + */ +float CircleError(float sp, float fb, float range); + +/** + * \brief 循环加法，用于没有负数值，并在一定范围内变化的值 + * 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI + * + * \param origin 被操作的值 + * \param delta 变化量 + * \param range 被操作的值变化范围，正数时起效 + */ +void CircleAdd(float *origin, float delta, float range); + +/** + * @brief 循环值取反 + * + * @param origin 被操作的值 + */ +void CircleReverse(float *origin); + +/** + * @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/device.h b/User/device/device.h new file mode 100644 index 0000000..b8acfe3 --- /dev/null +++ b/User/device/device.h @@ -0,0 +1,47 @@ +#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 */ +/* No signals defined */ +/* 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..299aa61 --- /dev/null +++ b/User/device/device_config.yaml @@ -0,0 +1,12 @@ +dm_imu: + bsp_config: {} + enabled: true +motor: + bsp_config: {} + enabled: true +motor_lk: + bsp_config: {} + enabled: true +motor_lz: + bsp_config: {} + enabled: true diff --git a/User/device/dm_imu.c b/User/device/dm_imu.c new file mode 100644 index 0000000..18b05e5 --- /dev/null +++ b/User/device/dm_imu.c @@ -0,0 +1,270 @@ +/* + DM_IMU数据获取（CAN） +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "dm_imu.h" + +#include "bsp/can.h" +#include "bsp/time.h" +#include "component/user_math.h" +#include + +/* Private define ----------------------------------------------------------- */ +#define DM_IMU_OFFLINE_TIMEOUT 1000 // 设备离线判定时间1000ms + +#define ACCEL_CAN_MAX (58.8f) +#define ACCEL_CAN_MIN (-58.8f) +#define GYRO_CAN_MAX (34.88f) +#define GYRO_CAN_MIN (-34.88f) +#define PITCH_CAN_MAX (90.0f) +#define PITCH_CAN_MIN (-90.0f) +#define ROLL_CAN_MAX (180.0f) +#define ROLL_CAN_MIN (-180.0f) +#define YAW_CAN_MAX (180.0f) +#define YAW_CAN_MIN (-180.0f) +#define TEMP_MIN (0.0f) +#define TEMP_MAX (60.0f) +#define Quaternion_MIN (-1.0f) +#define Quaternion_MAX (1.0f) + + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ +/* Private function --------------------------------------------------------- */ + +static uint8_t count = 0; // 计数器，用于判断设备是否离线 +/** + * @brief: 无符号整数转换为浮点数函数 + */ +static float uint_to_float(int x_int, float x_min, float x_max, int bits) +{ + float span = x_max - x_min; + float offset = x_min; + return ((float)x_int)*span/((float)((1<data.temp = (float)temp; + imu->data.accl.x = uint_to_float(acc_x_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); + imu->data.accl.y = uint_to_float(acc_y_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); + imu->data.accl.z = uint_to_float(acc_z_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); + return DEVICE_OK; +} +/** + * @brief 解析陀螺仪数据 + */ +static int8_t DM_IMU_ParseGyroData(DM_IMU_t *imu, uint8_t *data, uint8_t len) { + if (imu == NULL || data == NULL || len < 8) { + return DEVICE_ERR; + } + uint16_t gyro_x_raw = (data[3] << 8) | data[2]; + uint16_t gyro_y_raw = (data[5] << 8) | data[4]; + uint16_t gyro_z_raw = (data[7] << 8) | data[6]; + imu->data.gyro.x = uint_to_float(gyro_x_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); + imu->data.gyro.y = uint_to_float(gyro_y_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); + imu->data.gyro.z = uint_to_float(gyro_z_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); + return DEVICE_OK; +} +/** + * @brief 解析欧拉角数据 + */ +static int8_t DM_IMU_ParseEulerData(DM_IMU_t *imu, uint8_t *data, uint8_t len) { + if (imu == NULL || data == NULL || len < 8) { + return DEVICE_ERR; + } + uint16_t pit_raw = (data[3] << 8) | data[2]; + uint16_t yaw_raw = (data[5] << 8) | data[4]; + uint16_t rol_raw = (data[7] << 8) | data[6]; + imu->data.euler.pit = uint_to_float(pit_raw, PITCH_CAN_MIN, PITCH_CAN_MAX, 16) * M_DEG2RAD_MULT; + imu->data.euler.yaw = uint_to_float(yaw_raw, YAW_CAN_MIN, YAW_CAN_MAX, 16) * M_DEG2RAD_MULT; + imu->data.euler.rol = uint_to_float(rol_raw, ROLL_CAN_MIN, ROLL_CAN_MAX, 16) * M_DEG2RAD_MULT; + return DEVICE_OK; +} + +/** + * @brief 解析四元数数据 + */ +static int8_t DM_IMU_ParseQuaternionData(DM_IMU_t *imu, uint8_t *data, uint8_t len) { + if (imu == NULL || data == NULL || len < 8) { + return DEVICE_ERR; + } + int w = (data[1] << 6) | ((data[2] & 0xF8) >> 2); + int x = ((data[2] & 0x03) << 12) | (data[3] << 4) | ((data[4] & 0xF0) >> 4); + int y = ((data[4] & 0x0F) << 10) | (data[5] << 2) | ((data[6] & 0xC0) >> 6); + int z = ((data[6] & 0x3F) << 8) | data[7]; + imu->data.quat.q0 = uint_to_float(w, Quaternion_MIN, Quaternion_MAX, 14); + imu->data.quat.q1 = uint_to_float(x, Quaternion_MIN, Quaternion_MAX, 14); + imu->data.quat.q2 = uint_to_float(y, Quaternion_MIN, Quaternion_MAX, 14); + imu->data.quat.q3 = uint_to_float(z, Quaternion_MIN, Quaternion_MAX, 14); + return DEVICE_OK; +} + + +/* Exported functions ------------------------------------------------------- */ + +/** + * @brief 初始化DM IMU设备 + */ +int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param) { + if (imu == NULL || param == NULL) { + return DEVICE_ERR_NULL; + } + + // 初始化设备头部 + imu->header.online = false; + imu->header.last_online_time = 0; + + // 配置参数 + imu->param.can = param->can; + imu->param.can_id = param->can_id; + imu->param.device_id = param->device_id; + imu->param.master_id = param->master_id; + + // 清零数据 + memset(&imu->data, 0, sizeof(DM_IMU_Data_t)); + + // 注册CAN接收队列，用于接收回复报文 + int8_t result = BSP_CAN_RegisterId(imu->param.can, imu->param.master_id, 10); + if (result != BSP_OK) { + return DEVICE_ERR; + } + + return DEVICE_OK; +} + +/** + * @brief 请求IMU数据 + */ +int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) { + if (imu == NULL) { + return DEVICE_ERR_NULL; + } + + // 构造发送数据：id_L, id_H(DM_IMU_ID), RID, 0xcc + uint8_t tx_data[4] = { + imu->param.device_id & 0xFF, // id_L + (imu->param.device_id >> 8) & 0xFF, // id_H + (uint8_t)rid, // RID + 0xCC // 固定值 + }; + + // 发送标准数据帧 + BSP_CAN_StdDataFrame_t frame = { + .id = imu->param.can_id, + .dlc = 4, + }; + memcpy(frame.data, tx_data, 4); + int8_t result = BSP_CAN_TransmitStdDataFrame(imu->param.can, &frame); + return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 更新IMU数据（从CAN中获取所有数据并解析） + */ +int8_t DM_IMU_Update(DM_IMU_t *imu) { + if (imu == NULL) { + return DEVICE_ERR_NULL; + } + + BSP_CAN_Message_t msg; + int8_t result; + bool data_received = false; + + // 持续接收所有可用消息 + while ((result = BSP_CAN_GetMessage(imu->param.can, imu->param.master_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE)) == BSP_OK) { + // 验证回复数据格式（至少检查数据长度） + if (msg.dlc < 3) { + continue; // 跳过无效消息 + } + + // 根据数据位的第0位确定反馈报文类型 + uint8_t rid = msg.data[0] & 0x0F; // 取第0位的低4位作为RID + + // 根据RID类型解析数据 + int8_t parse_result = DEVICE_ERR; + switch (rid) { + case 0x01: // RID_ACCL + parse_result = DM_IMU_ParseAccelData(imu, msg.data, msg.dlc); + break; + case 0x02: // RID_GYRO + parse_result = DM_IMU_ParseGyroData(imu, msg.data, msg.dlc); + break; + case 0x03: // RID_EULER + parse_result = DM_IMU_ParseEulerData(imu, msg.data, msg.dlc); + break; + case 0x04: // RID_QUATERNION + parse_result = DM_IMU_ParseQuaternionData(imu, msg.data, msg.dlc); + break; + default: + continue; // 跳过未知类型的消息 + } + + // 如果解析成功，标记为收到数据 + if (parse_result == DEVICE_OK) { + data_received = true; + } + } + + // 如果收到任何有效数据，更新设备状态 + if (data_received) { + imu->header.online = true; + imu->header.last_online_time = BSP_TIME_Get_ms(); + return DEVICE_OK; + } + + return DEVICE_ERR; +} + +/** + * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz） + */ +int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu){ + if (imu == NULL) { + return DEVICE_ERR_NULL; + } + switch (count) { + case 0: + DM_IMU_Request(imu, RID_ACCL); + break; + case 1: + DM_IMU_Request(imu, RID_GYRO); + break; + case 2: + DM_IMU_Request(imu, RID_EULER); + break; + case 3: + DM_IMU_Request(imu, RID_QUATERNION); + DM_IMU_Update(imu); // 更新所有数据 + break; + } + count++; + if (count >= 4) { + count = 0; // 重置计数器 + } + return DEVICE_OK; +} + +/** + * @brief 检查设备是否在线 + */ +bool DM_IMU_IsOnline(DM_IMU_t *imu) { + if (imu == NULL) { + return false; + } + + uint32_t current_time = BSP_TIME_Get_ms(); + return imu->header.online && + (current_time - imu->header.last_online_time < DM_IMU_OFFLINE_TIMEOUT); +} diff --git a/User/device/dm_imu.h b/User/device/dm_imu.h new file mode 100644 index 0000000..3965980 --- /dev/null +++ b/User/device/dm_imu.h @@ -0,0 +1,90 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "component/ahrs.h" +#include "bsp/can.h" +/* Exported constants ------------------------------------------------------- */ + +#define DM_IMU_CAN_ID_DEFAULT 0x6FF +#define DM_IMU_ID_DEFAULT 0x42 +#define DM_IMU_MST_ID_DEFAULT 0x43 + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + +typedef struct { + BSP_CAN_t can; // CAN总线句柄 + uint16_t can_id; // CAN通信ID + uint8_t device_id; // 设备ID + uint8_t master_id; // 主机ID +} DM_IMU_Param_t; +typedef enum { + RID_ACCL = 0x01, // 加速度计 + RID_GYRO = 0x02, // 陀螺仪 + RID_EULER = 0x03, // 欧拉角 + RID_QUATERNION = 0x04, // 四元数 +} DM_IMU_RID_t; + +typedef struct { + AHRS_Accl_t accl; // 加速度计 + AHRS_Gyro_t gyro; // 陀螺仪 + AHRS_Eulr_t euler; // 欧拉角 + AHRS_Quaternion_t quat; // 四元数 + float temp; // 温度 +} DM_IMU_Data_t; + +typedef struct { + DEVICE_Header_t header; + DM_IMU_Param_t param; // IMU参数配置 + DM_IMU_Data_t data; // IMU数据 +} DM_IMU_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 初始化DM IMU设备 + * @param imu DM IMU设备结构体指针 + * @param param IMU参数配置指针 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param); + +/** + * @brief 请求IMU数据 + * @param imu DM IMU设备结构体指针 + * @param rid 请求的数据类型 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid); + + +/** + * @brief 更新IMU数据（从CAN中获取所有数据并解析） + * @param imu DM IMU设备结构体指针 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t DM_IMU_Update(DM_IMU_t *imu); + +/** + * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz，运行4次才有完整数据） + * @param imu DM IMU设备结构体指针 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu); + +/** + * @brief 检查设备是否在线 + * @param imu DM IMU设备结构体指针 + * @return true 在线，false 离线 + */ +bool DM_IMU_IsOnline(DM_IMU_t *imu); + + +#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_lk.c b/User/device/motor_lk.c new file mode 100644 index 0000000..c26878e --- /dev/null +++ b/User/device/motor_lk.c @@ -0,0 +1,329 @@ +/* + LK电机驱动 +*/ +/* Includes ----------------------------------------------------------------- */ +#include "motor_lk.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 LK_CTRL_ID_BASE (0x140) +#define LK_FB_ID_BASE (0x240) + +// LK电机命令字节定义 +#define LK_CMD_FEEDBACK (0x9C) // 反馈命令字节 +#define LK_CMD_MOTOR_OFF (0x80) // 电机关闭命令 +#define LK_CMD_MOTOR_ON (0x88) // 电机运行命令 +#define LK_CMD_TORQUE_CTRL (0xA1) // 转矩闭环控制命令 + +// LK电机参数定义 +#define LK_CURR_LSB_MF (33.0f / 4096.0f) // MF电机转矩电流分辨率 A/LSB +#define LK_CURR_LSB_MG (66.0f / 4096.0f) // MG电机转矩电流分辨率 A/LSB +#define LK_POWER_RANGE_MS (1000) // MS电机功率范围 ±1000 +#define LK_TORQUE_RANGE (2048) // 转矩控制值范围 ±2048 +#define LK_TORQUE_CURRENT_MF (16.5f) // MF电机最大转矩电流 A +#define LK_TORQUE_CURRENT_MG (33.0f) // MG电机最大转矩电流 A + +#define MOTOR_TX_BUF_SIZE (8) +#define MOTOR_RX_BUF_SIZE (8) + +// 编码器分辨率定义 +#define LK_ENC_14BIT_MAX (16383) // 14位编码器最大值 +#define LK_ENC_15BIT_MAX (32767) // 15位编码器最大值 +#define LK_ENC_16BIT_MAX (65535) // 16位编码器最大值 + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static MOTOR_LK_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL}; + +/* Private functions -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +static float MOTOR_LK_GetCurrentLSB(MOTOR_LK_Module_t module) { + switch (module) { + case MOTOR_LK_MF9025: + case MOTOR_LK_MF9035: + return LK_CURR_LSB_MF; + default: + return LK_CURR_LSB_MG; // 默认使用MG的分辨率 + } +} + +static uint16_t MOTOR_LK_GetEncoderMax(MOTOR_LK_Module_t module) { + // 根据电机型号返回编码器最大值，这里假设都使用16位编码器 + // 实际使用时需要根据具体电机型号配置 + return LK_ENC_16BIT_MAX; +} + +static MOTOR_LK_CANManager_t* MOTOR_LK_GetCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return NULL; + return can_managers[can]; +} + +static int8_t MOTOR_LK_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_LK_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LK_CANManager_t)); + if (can_managers[can] == NULL) return DEVICE_ERR; + + memset(can_managers[can], 0, sizeof(MOTOR_LK_CANManager_t)); + can_managers[can]->can = can; + return DEVICE_OK; +} + +static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) { + + // 检查命令字节是否为反馈命令 + if (msg->data[0] != LK_CMD_FEEDBACK) { + // 如果不是标准反馈命令，可能是其他格式的数据 + // 临时跳过命令字节检查，直接解析数据 + // return; + } + + // 解析温度 (DATA[1]) + motor->motor.feedback.temp = (int8_t)msg->data[1]; + + // 解析转矩电流值或功率值 (DATA[2], DATA[3]) + int16_t raw_current_or_power = (int16_t)((msg->data[3] << 8) | msg->data[2]); + + // 根据电机类型解析电流或功率 + switch (motor->param.module) { + case MOTOR_LK_MF9025: + case MOTOR_LK_MF9035: + motor->motor.feedback.torque_current = raw_current_or_power * MOTOR_LK_GetCurrentLSB(motor->param.module); + break; + default: + motor->motor.feedback.torque_current = (float)raw_current_or_power; + break; + } + + // 解析转速 (DATA[4], DATA[5]) - 单位：1dps/LSB + int16_t raw_speed = (int16_t)((msg->data[5] << 8) | msg->data[4]); + motor->motor.feedback.rotor_speed = motor->param.reverse ? -raw_speed : raw_speed; + + // 解析编码器值 (DATA[6], DATA[7]) + uint16_t raw_encoder = (uint16_t)((msg->data[7] << 8) | msg->data[6]); + uint16_t encoder_max = MOTOR_LK_GetEncoderMax(motor->param.module); + + // 将编码器值转换为弧度 (0 ~ 2π) + float angle = (float)raw_encoder / (float)encoder_max * M_2PI; + motor->motor.feedback.rotor_abs_angle = motor->param.reverse ? (M_2PI - angle) : angle; +} + +/* Exported functions ------------------------------------------------------- */ + +int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + if (MOTOR_LK_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR; + MOTOR_LK_CANManager_t *manager = MOTOR_LK_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_LK_MAX_MOTORS) return DEVICE_ERR; + + // 创建新电机实例 + MOTOR_LK_t *new_motor = (MOTOR_LK_t*)BSP_Malloc(sizeof(MOTOR_LK_t)); + if (new_motor == NULL) return DEVICE_ERR; + + memcpy(&new_motor->param, param, sizeof(MOTOR_LK_Param_t)); + memset(&new_motor->motor, 0, sizeof(MOTOR_t)); + new_motor->motor.reverse = param->reverse; + + // 对于某些LK电机，反馈数据可能通过命令ID发送 + // 根据实际测试，使用命令ID接收反馈数据 + uint16_t feedback_id = param->id; // 使用命令ID作为反馈ID + + // 注册CAN接收ID + if (BSP_CAN_RegisterId(param->can, 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_LK_Update(MOTOR_LK_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LK_t *motor = manager->motors[i]; + if (motor && motor->param.id == param->id) { + // 对于某些LK电机，反馈数据通过命令ID发送 + uint16_t feedback_id = param->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 > 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_LK_Decode(motor, &rx_msg); + return DEVICE_OK; + } + } + return DEVICE_ERR_NO_DEV; +} + +int8_t MOTOR_LK_UpdateAll(void) { + int8_t ret = DEVICE_OK; + for (int can = 0; can < BSP_CAN_NUM; can++) { + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager((BSP_CAN_t)can); + if (manager == NULL) continue; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LK_t *motor = manager->motors[i]; + if (motor != NULL) { + if (MOTOR_LK_Update(&motor->param) != DEVICE_OK) { + ret = DEVICE_ERR; + } + } + } + } + return ret; +} + +int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) { + if (param == NULL) return DEVICE_ERR_NULL; + + MOTOR_LK_CANManager_t *manager = MOTOR_LK_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; + + MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param); + if (motor == NULL) return DEVICE_ERR_NO_DEV; + + // 根据反转参数调整输出 + float output = param->reverse ? -value : value; + + // 转矩闭环控制命令 - 将输出值转换为转矩控制值 + int16_t torque_control = (int16_t)(output * (float)LK_TORQUE_RANGE); + + // 构建CAN帧 + BSP_CAN_StdDataFrame_t tx_frame; + tx_frame.id = param->id; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + + tx_frame.data[0] = LK_CMD_TORQUE_CTRL; + tx_frame.data[1] = 0x00; + tx_frame.data[2] = 0x00; + tx_frame.data[3] = 0x00; + tx_frame.data[4] = (uint8_t)(torque_control & 0xFF); + tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF); + tx_frame.data[6] = 0x00; + tx_frame.data[7] = 0x00; + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param) { + // 对于LK电机，每次设置输出时就直接发送控制命令 + // 这个函数可以用于发送其他控制命令，如电机开启/关闭 + return DEVICE_OK; +} + +int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + BSP_CAN_StdDataFrame_t tx_frame; + tx_frame.id = param->id; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + + // 电机运行命令 + tx_frame.data[0] = LK_CMD_MOTOR_ON; // 命令字节 + tx_frame.data[1] = 0x00; + tx_frame.data[2] = 0x00; + tx_frame.data[3] = 0x00; + tx_frame.data[4] = 0x00; + tx_frame.data[5] = 0x00; + tx_frame.data[6] = 0x00; + tx_frame.data[7] = 0x00; + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + BSP_CAN_StdDataFrame_t tx_frame; + tx_frame.id = param->id; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + + // 电机关闭命令 + tx_frame.data[0] = LK_CMD_MOTOR_OFF; // 命令字节 + tx_frame.data[1] = 0x00; + tx_frame.data[2] = 0x00; + tx_frame.data[3] = 0x00; + tx_frame.data[4] = 0x00; + tx_frame.data[5] = 0x00; + tx_frame.data[6] = 0x00; + tx_frame.data[7] = 0x00; + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param) { + if (param == NULL) return NULL; + + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can); + if (manager == NULL) return NULL; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LK_t *motor = manager->motors[i]; + if (motor && motor->param.id == param->id) { + return motor; + } + } + return NULL; +} + +int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param) { + return MOTOR_LK_SetOutput(param, 0.0f); +} + +int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param) { + MOTOR_LK_t *motor = MOTOR_LK_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_lk.h b/User/device/motor_lk.h new file mode 100644 index 0000000..f17dde0 --- /dev/null +++ b/User/device/motor_lk.h @@ -0,0 +1,119 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "device/motor.h" +#include "bsp/can.h" + +/* Exported constants ------------------------------------------------------- */ +#define MOTOR_LK_MAX_MOTORS 32 + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + MOTOR_LK_MF9025, + MOTOR_LK_MF9035, +} MOTOR_LK_Module_t; + + +/*每个电机需要的参数*/ +typedef struct { + BSP_CAN_t can; + uint16_t id; + MOTOR_LK_Module_t module; + bool reverse; +} MOTOR_LK_Param_t; + +/*电机实例*/ +typedef struct{ + MOTOR_LK_Param_t param; + MOTOR_t motor; +} MOTOR_LK_t; + +/*CAN管理器，管理一个CAN总线上所有的电机*/ +typedef struct { + BSP_CAN_t can; + MOTOR_LK_t *motors[MOTOR_LK_MAX_MOTORS]; + uint8_t motor_count; +} MOTOR_LK_CANManager_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 注册一个LK电机 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param); + +/** + * @brief 更新指定电机数据 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param); + +/** + * @brief 设置一个电机的输出 + * @param param 电机参数 + * @param value 输出值，范围[-1.0, 1.0] + * @return + */ +int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value); + +/** + * @brief 发送控制命令到电机，注意一个CAN可以控制多个电机，所以只需要发送一次即可 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param); + +/** + * @brief 发送电机开启命令 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param); + +/** + * @brief 发送电机关闭命令 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param); + +/** + * @brief 获取指定电机的实例指针 + * @param param 电机参数 + * @return + */ +MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param); + +/** + * @brief 使电机松弛（设置输出为0） + * @param param + * @return + */ +int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param); + +/** + * @brief 使电机离线（设置在线状态为false） + * @param param + * @return + */ +int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param); + +/** + * @brief + * @param + * @return + */ +int8_t MOTOR_LK_UpdateAll(void); + +#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..7ed7344 --- /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 = M_2PI - 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/task/Task4.c b/User/task/Task4.c new file mode 100644 index 0000000..17199e0 --- /dev/null +++ b/User/task/Task4.c @@ -0,0 +1,44 @@ +/* + Task4 Task + +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +void Task_Task4(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + + + /* 计算任务运行到指定频率需要等待的tick数 */ + const uint32_t delay_tick = osKernelGetTickFreq() / TASK4_FREQ; + + osDelay(TASK4_INIT_DELAY); /* 延时一段时间再开启任务 */ + + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ + /* USER CODE INIT BEGIN */ + + /* USER CODE INIT END */ + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* USER CODE BEGIN */ + + /* USER CODE END */ + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ + } + +} \ No newline at end of file diff --git a/User/task/blink.c b/User/task/blink.c new file mode 100644 index 0000000..e879c0a --- /dev/null +++ b/User/task/blink.c @@ -0,0 +1,44 @@ +/* + blink Task + +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" +/* USER INCLUDE BEGIN */ + +/* 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 */ + + /* USER CODE INIT END */ + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* USER CODE BEGIN */ + + /* 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..2128659 --- /dev/null +++ b/User/task/config.yaml @@ -0,0 +1,28 @@ +- 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_imu + name: imu + stack: 256 +- delay: 0 + description: '' + freq_control: true + frequency: 500.0 + function: Task_ctrl_lz + name: ctrl_lz + stack: 256 +- delay: 0 + description: '' + freq_control: true + frequency: 500.0 + function: Task_Task4 + name: Task4 + stack: 256 diff --git a/User/task/ctrl_lz.c b/User/task/ctrl_lz.c new file mode 100644 index 0000000..2ccd7a1 --- /dev/null +++ b/User/task/ctrl_lz.c @@ -0,0 +1,44 @@ +/* + ctrl_lz Task + +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +void Task_ctrl_lz(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + + + /* 计算任务运行到指定频率需要等待的tick数 */ + const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_LZ_FREQ; + + osDelay(CTRL_LZ_INIT_DELAY); /* 延时一段时间再开启任务 */ + + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ + /* USER CODE INIT BEGIN */ + + /* USER CODE INIT END */ + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* USER CODE BEGIN */ + + /* USER CODE END */ + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ + } + +} \ No newline at end of file diff --git a/User/task/imu.c b/User/task/imu.c new file mode 100644 index 0000000..25ac9de --- /dev/null +++ b/User/task/imu.c @@ -0,0 +1,44 @@ +/* + imu Task + +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +void Task_imu(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + + + /* 计算任务运行到指定频率需要等待的tick数 */ + const uint32_t delay_tick = osKernelGetTickFreq() / IMU_FREQ; + + osDelay(IMU_INIT_DELAY); /* 延时一段时间再开启任务 */ + + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ + /* USER CODE INIT BEGIN */ + + /* USER CODE INIT END */ + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* USER CODE BEGIN */ + + /* USER CODE END */ + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ + } + +} \ No newline at end of file diff --git a/User/task/init.c b/User/task/init.c new file mode 100644 index 0000000..7f6281e --- /dev/null +++ b/User/task/init.c @@ -0,0 +1,45 @@ +/* + Init Task + 任务初始化，创建各个线程任务和消息队列 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" + +/* USER INCLUDE BEGIN */ + +/* 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.imu = osThreadNew(Task_imu, NULL, &attr_imu); + task_runtime.thread.ctrl_lz = osThreadNew(Task_ctrl_lz, NULL, &attr_ctrl_lz); + task_runtime.thread.Task4 = osThreadNew(Task_Task4, NULL, &attr_Task4); + + // 创建消息队列 + /* USER MESSAGE BEGIN */ + task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL); + /* USER MESSAGE END */ + + osKernelUnlock(); // 解锁内核 + osThreadTerminate(osThreadGetId()); // 任务完成后结束自身 +} \ No newline at end of file diff --git a/User/task/user_task.c b/User/task/user_task.c new file mode 100644 index 0000000..2da5e75 --- /dev/null +++ b/User/task/user_task.c @@ -0,0 +1,31 @@ +#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_imu = { + .name = "imu", + .priority = osPriorityNormal, + .stack_size = 256 * 4, +}; +const osThreadAttr_t attr_ctrl_lz = { + .name = "ctrl_lz", + .priority = osPriorityNormal, + .stack_size = 256 * 4, +}; +const osThreadAttr_t attr_Task4 = { + .name = "Task4", + .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..af6930c --- /dev/null +++ b/User/task/user_task.h @@ -0,0 +1,104 @@ +#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 (500.0) +#define IMU_FREQ (1000.0) +#define CTRL_LZ_FREQ (500.0) +#define TASK4_FREQ (500.0) + +/* 任务初始化延时ms */ +#define TASK_INIT_DELAY (100u) +#define BLINK_INIT_DELAY (0) +#define IMU_INIT_DELAY (0) +#define CTRL_LZ_INIT_DELAY (0) +#define TASK4_INIT_DELAY (0) + +/* Exported defines --------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + +/* 任务运行时结构体 */ +typedef struct { + /* 各任务，也可以叫做线程 */ + struct { + osThreadId_t blink; + osThreadId_t imu; + osThreadId_t ctrl_lz; + osThreadId_t Task4; + } thread; + + /* USER MESSAGE BEGIN */ + struct { + osMessageQueueId_t user_msg; /* 用户自定义任务消息队列 */ + } 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 imu; + UBaseType_t ctrl_lz; + UBaseType_t Task4; + } stack_water_mark; + + /* 各任务运行频率 */ + struct { + float blink; + float imu; + float ctrl_lz; + float Task4; + } freq; + + /* 任务最近运行时间 */ + struct { + float blink; + float imu; + float ctrl_lz; + float Task4; + } 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_imu; +extern const osThreadAttr_t attr_ctrl_lz; +extern const osThreadAttr_t attr_Task4; + +/* 任务函数声明 */ +void Task_Init(void *argument); +void Task_blink(void *argument); +void Task_imu(void *argument); +void Task_ctrl_lz(void *argument); +void Task_Task4(void *argument); + +#ifdef __cplusplus +} +#endif \ No newline at end of file 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..36a65ee --- /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..1c1f3c6 --- /dev/null +++ b/cmake/stm32cubemx/CMakeLists.txt @@ -0,0 +1,153 @@ +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}/../../USB_DEVICE/App + ${CMAKE_CURRENT_SOURCE_DIR}/../../USB_DEVICE/Target + ${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}/../../Middlewares/ST/STM32_USB_Device_Library/Core/Inc + ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc + ${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}/../../USB_DEVICE/Target/usbd_conf.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../USB_DEVICE/App/usb_device.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../USB_DEVICE/App/usbd_desc.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../USB_DEVICE/App/usbd_cdc_if.c + ${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/adc.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/can.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/crc.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/dma.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/i2c.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/rng.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/spi.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_pcd.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.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_adc.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_adc.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_crc.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rng.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.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(USB_Device_Library_Src + ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c + ${CMAKE_CURRENT_SOURCE_DIR}/../../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c +) +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} + USB_Device_Library 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 USB_Device_Library static library +add_library(USB_Device_Library OBJECT) +target_sources(USB_Device_Library PRIVATE ${USB_Device_Library_Src}) +target_link_libraries(USB_Device_Library 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/startup_stm32f407xx.s b/startup_stm32f407xx.s new file mode 100644 index 0000000..d0b8378 --- /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 component is licensed by ST under BSD 3-Clause license, + * the "License"; You may not use this file except in compliance with the + * License. You may obtain a copy of the License at: + * opensource.org/licenses/BSD-3-Clause + * + ****************************************************************************** + */ + + .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 */ + +/* 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 the clock system intitialization function.*/ + bl SystemInit +/* 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 + .size g_pfnVectors, .-g_pfnVectors + + +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 */ + + +/******************************************************************************* +* +* 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 + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ From ff2a3c5862e703f3e5c613370d0da04eff5e4844 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sat, 4 Oct 2025 17:38:10 +0800 Subject: [PATCH 02/25] =?UTF-8?q?=E9=87=8D=E6=9E=84=E8=BD=AC=E5=8F=91?= =?UTF-8?q?=E7=94=A8=E7=9A=84=E5=BA=95=E5=B1=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .mxproject | 10 +- CMakeLists.txt | 4 +- Core/Src/can.c | 4 +- Core/Src/freertos.c | 318 +++++++++++++++++++++--------------------- DevC.ioc | 6 +- User/device/dm_imu.c | 3 +- User/module/config.c | 74 ++++++++++ User/module/config.h | 31 ++++ User/task/Task4.c | 44 ------ User/task/config.yaml | 7 - User/task/ctrl_lz.c | 90 +++++++++++- User/task/imu.c | 48 ++++++- User/task/init.c | 1 - User/task/user_task.c | 5 - User/task/user_task.h | 8 -- 15 files changed, 412 insertions(+), 241 deletions(-) create mode 100644 User/module/config.c create mode 100644 User/module/config.h delete mode 100644 User/task/Task4.c diff --git a/.mxproject b/.mxproject index 9bdbf0a..c21a5d3 100644 --- a/.mxproject +++ b/.mxproject @@ -6,6 +6,11 @@ SourceFiles=../Core/Src/main.c;../Core/Src/gpio.c;../Core/Src/freertos.c;../Core 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;../Middlewares/ST/STM32_USB_Device_Library/Core/Inc;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include;../Core/Inc;../USB_DEVICE/App;../USB_DEVICE/Target; 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/adc.c;Core/Src/can.c;Core/Src/crc.c;Core/Src/dma.c;Core/Src/i2c.c;Core/Src/rng.c;Core/Src/spi.c;Core/Src/tim.c;Core/Src/usart.c;USB_DEVICE/App/usb_device.c;USB_DEVICE/Target/usbd_conf.c;USB_DEVICE/App/usbd_desc.c;USB_DEVICE/App/usbd_cdc_if.c;Core/Src/stm32f4xx_it.c;Core/Src/stm32f4xx_hal_msp.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.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_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_crc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rng.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.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;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Core/Src/system_stm32f4xx.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.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_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_crc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rng.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.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;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.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;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.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;Middlewares/ST/STM32_USB_Device_Library/Core/Inc;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc;Drivers/CMSIS/Device/ST/STM32F4xx/Include;Drivers/CMSIS/Include;Core/Inc;USB_DEVICE/App;USB_DEVICE/Target; +CDefines=USE_HAL_DRIVER;STM32F407xx;USE_HAL_DRIVER;USE_HAL_DRIVER; + [PreviousGenFiles] AdvancedFolderStructure=true HeaderFileListSize=18 @@ -57,8 +62,3 @@ SourcePath#1=../USB_DEVICE/App SourcePath#2=../USB_DEVICE/Target SourceFiles=; -[PreviousUsedCMakes] -SourceFiles=Core/Src/main.c;Core/Src/gpio.c;Core/Src/freertos.c;Core/Src/adc.c;Core/Src/can.c;Core/Src/crc.c;Core/Src/dma.c;Core/Src/i2c.c;Core/Src/rng.c;Core/Src/spi.c;Core/Src/tim.c;Core/Src/usart.c;USB_DEVICE/App/usb_device.c;USB_DEVICE/Target/usbd_conf.c;USB_DEVICE/App/usbd_desc.c;USB_DEVICE/App/usbd_cdc_if.c;Core/Src/stm32f4xx_it.c;Core/Src/stm32f4xx_hal_msp.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.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_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_crc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rng.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.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;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.c;Drivers/CMSIS/Device/ST/STM32F4xx/Source/Templates/system_stm32f4xx.c;Core/Src/system_stm32f4xx.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pcd_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_usb.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_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_adc_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_ll_adc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_crc.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_i2c_ex.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rng.c;Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_spi.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;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.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;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_core.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ctlreq.c;Middlewares/ST/STM32_USB_Device_Library/Core/Src/usbd_ioreq.c;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Src/usbd_cdc.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;Middlewares/ST/STM32_USB_Device_Library/Core/Inc;Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc;Drivers/CMSIS/Device/ST/STM32F4xx/Include;Drivers/CMSIS/Include;Core/Inc;USB_DEVICE/App;USB_DEVICE/Target; -CDefines=USE_HAL_DRIVER;STM32F407xx;USE_HAL_DRIVER;USE_HAL_DRIVER; - diff --git a/CMakeLists.txt b/CMakeLists.txt index b42e837..c171d6d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -60,8 +60,10 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE User/device/motor_lk.c User/device/motor_lz.c + # User/module sources + User/module/config.c + # User/task sources - User/task/Task4.c User/task/blink.c User/task/ctrl_lz.c User/task/imu.c diff --git a/Core/Src/can.c b/Core/Src/can.c index 81ec2ed..9f4e0cf 100644 --- a/Core/Src/can.c +++ b/Core/Src/can.c @@ -47,7 +47,7 @@ void MX_CAN1_Init(void) hcan1.Init.TimeTriggeredMode = DISABLE; hcan1.Init.AutoBusOff = DISABLE; hcan1.Init.AutoWakeUp = DISABLE; - hcan1.Init.AutoRetransmission = DISABLE; + hcan1.Init.AutoRetransmission = ENABLE; hcan1.Init.ReceiveFifoLocked = DISABLE; hcan1.Init.TransmitFifoPriority = ENABLE; if (HAL_CAN_Init(&hcan1) != HAL_OK) @@ -79,7 +79,7 @@ void MX_CAN2_Init(void) hcan2.Init.TimeTriggeredMode = DISABLE; hcan2.Init.AutoBusOff = DISABLE; hcan2.Init.AutoWakeUp = DISABLE; - hcan2.Init.AutoRetransmission = DISABLE; + hcan2.Init.AutoRetransmission = ENABLE; hcan2.Init.ReceiveFifoLocked = DISABLE; hcan2.Init.TransmitFifoPriority = ENABLE; if (HAL_CAN_Init(&hcan2) != HAL_OK) diff --git a/Core/Src/freertos.c b/Core/Src/freertos.c index 3880224..6938617 100644 --- a/Core/Src/freertos.c +++ b/Core/Src/freertos.c @@ -1,159 +1,159 @@ -/* USER CODE BEGIN Header */ -/** - ****************************************************************************** - * File Name : freertos.c - * Description : Code for freertos applications - ****************************************************************************** - * @attention - * - *

© Copyright (c) 2025 STMicroelectronics. - * All rights reserved.

- * - * This software component is licensed by ST under Ultimate Liberty license - * SLA0044, the "License"; You may not use this file except in compliance with - * the License. You may obtain a copy of the License at: - * www.st.com/SLA0044 - * - ****************************************************************************** - */ -/* 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); - -extern void MX_USB_DEVICE_Init(void); -void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */ - -/* Hook prototypes */ -void configureTimerForRunTimeStats(void); -unsigned long getRunTimeCounterValue(void); -void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char *pcTaskName); - -/* USER CODE BEGIN 1 */ -/* Functions needed when configGENERATE_RUN_TIME_STATS is on */ -__weak void configureTimerForRunTimeStats(void) -{ - -} - -__weak unsigned long getRunTimeCounterValue(void) -{ -return 0; -} -/* USER CODE END 1 */ - -/* USER CODE BEGIN 4 */ -void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char *pcTaskName) -{ - /* Run time stack overflow checking is performed if - configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2. This hook function is - called if a stack overflow is detected. */ -} -/* USER CODE END 4 */ - -/** - * @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) -{ - /* init code for USB_DEVICE */ - MX_USB_DEVICE_Init(); - /* USER CODE BEGIN StartDefaultTask */ - osThreadTerminate(osThreadGetId()); -/* USER CODE END StartDefaultTask */ -} - -/* Private application code --------------------------------------------------*/ -/* USER CODE BEGIN Application */ - -/* USER CODE END Application */ - -/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * File Name : freertos.c + * Description : Code for freertos applications + ****************************************************************************** + * @attention + * + *

© Copyright (c) 2025 STMicroelectronics. + * All rights reserved.

+ * + * This software component is licensed by ST under Ultimate Liberty license + * SLA0044, the "License"; You may not use this file except in compliance with + * the License. You may obtain a copy of the License at: + * www.st.com/SLA0044 + * + ****************************************************************************** + */ +/* 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); + +extern void MX_USB_DEVICE_Init(void); +void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */ + +/* Hook prototypes */ +void configureTimerForRunTimeStats(void); +unsigned long getRunTimeCounterValue(void); +void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char *pcTaskName); + +/* USER CODE BEGIN 1 */ +/* Functions needed when configGENERATE_RUN_TIME_STATS is on */ +__weak void configureTimerForRunTimeStats(void) +{ + +} + +__weak unsigned long getRunTimeCounterValue(void) +{ +return 0; +} +/* USER CODE END 1 */ + +/* USER CODE BEGIN 4 */ +void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char *pcTaskName) +{ + /* Run time stack overflow checking is performed if + configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2. This hook function is + called if a stack overflow is detected. */ +} +/* USER CODE END 4 */ + +/** + * @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) +{ + /* init code for USB_DEVICE */ + MX_USB_DEVICE_Init(); + /* USER CODE BEGIN StartDefaultTask */ + osThreadTerminate(osThreadGetId()); + /* USER CODE END StartDefaultTask */ +} + +/* Private application code --------------------------------------------------*/ +/* USER CODE BEGIN Application */ + +/* USER CODE END Application */ + +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/DevC.ioc b/DevC.ioc index 7e7f2b1..503de29 100644 --- a/DevC.ioc +++ b/DevC.ioc @@ -21,7 +21,8 @@ CAN1.BS2=CAN_BS2_7TQ CAN1.CalculateBaudRate=1000000 CAN1.CalculateTimeBit=1000 CAN1.CalculateTimeQuantum=71.42857142857143 -CAN1.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,ABOM,CalculateTimeBit,CalculateBaudRate +CAN1.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,ABOM,CalculateTimeBit,CalculateBaudRate,NART +CAN1.NART=ENABLE CAN1.Prescaler=3 CAN1.TXFP=ENABLE CAN2.BS1=CAN_BS1_6TQ @@ -29,7 +30,8 @@ CAN2.BS2=CAN_BS2_7TQ CAN2.CalculateBaudRate=1000000 CAN2.CalculateTimeBit=1000 CAN2.CalculateTimeQuantum=71.42857142857143 -CAN2.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,CalculateTimeBit,CalculateBaudRate +CAN2.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,CalculateTimeBit,CalculateBaudRate,NART +CAN2.NART=ENABLE CAN2.Prescaler=3 CAN2.TXFP=ENABLE Dma.I2C2_TX.2.Direction=DMA_MEMORY_TO_PERIPH diff --git a/User/device/dm_imu.c b/User/device/dm_imu.c index 18b05e5..064abdf 100644 --- a/User/device/dm_imu.c +++ b/User/device/dm_imu.c @@ -252,8 +252,9 @@ int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu){ count++; if (count >= 4) { count = 0; // 重置计数器 + return DEVICE_OK; } - return DEVICE_OK; + return DEVICE_ERR; // 未完成一轮更新 } /** diff --git a/User/module/config.c b/User/module/config.c new file mode 100644 index 0000000..2ef4045 --- /dev/null +++ b/User/module/config.c @@ -0,0 +1,74 @@ +/* + * 配置相关 + */ + +/* Includes ----------------------------------------------------------------- */ +#include "module/config.h" +#include "bsp/can.h" + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ + +/* Exported variables ------------------------------------------------------- */ + +// 机器人参数配置 +Config_RobotParam_t robot_config = { + + .imu_param = + { + .can = BSP_CAN_2, + .can_id = 0x6FF, + .device_id = 0x42, + .master_id = 0x43, + }, + .joint_param = + { + { + // 左髋关节 + .can = BSP_CAN_2, + .motor_id = 124, + .host_id = 0xFF, + .module = MOTOR_LZ_RSO3, + .reverse = false, + .mode = MOTOR_LZ_MODE_MOTION, + }, + { + // 左膝关节 + .can = BSP_CAN_2, + .motor_id = 125, + .host_id = 0xFF, + .module = MOTOR_LZ_RSO3, + .reverse = false, + .mode = MOTOR_LZ_MODE_MOTION, + }, + { + // 右膝关节 + .can = BSP_CAN_2, + .motor_id = 126, + .host_id = 0xFF, + .module = MOTOR_LZ_RSO3, + .reverse = true, + .mode = MOTOR_LZ_MODE_MOTION, + }, + { + // 右髋关节 + .can = BSP_CAN_2, + .motor_id = 127, + .host_id = 0xFF, + .module = MOTOR_LZ_RSO3, + .reverse = true, + .mode = MOTOR_LZ_MODE_MOTION, + }, + }, +}; + +/* Private function prototypes ---------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ + +/** + * @brief 获取机器人配置参数 + * @return 机器人配置参数指针 + */ +Config_RobotParam_t *Config_GetRobotParam(void) { return &robot_config; } \ No newline at end of file diff --git a/User/module/config.h b/User/module/config.h new file mode 100644 index 0000000..0fc9560 --- /dev/null +++ b/User/module/config.h @@ -0,0 +1,31 @@ +/* + * 配置相关 + */ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "device/dm_imu.h" +#include "device/motor_lz.h" +#include "device/motor_lk.h" +typedef struct { + DM_IMU_Param_t imu_param; + MOTOR_LZ_Param_t joint_param[4]; + MOTOR_LK_Param_t wheel_param[2]; +} Config_RobotParam_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 获取机器人配置参数 + * @return 机器人配置参数指针 + */ +Config_RobotParam_t* Config_GetRobotParam(void); + +#ifdef __cplusplus +} +#endif diff --git a/User/task/Task4.c b/User/task/Task4.c deleted file mode 100644 index 17199e0..0000000 --- a/User/task/Task4.c +++ /dev/null @@ -1,44 +0,0 @@ -/* - Task4 Task - -*/ - -/* Includes ----------------------------------------------------------------- */ -#include "task/user_task.h" -/* USER INCLUDE BEGIN */ - -/* USER INCLUDE END */ - -/* Private typedef ---------------------------------------------------------- */ -/* Private define ----------------------------------------------------------- */ -/* Private macro ------------------------------------------------------------ */ -/* Private variables -------------------------------------------------------- */ -/* USER STRUCT BEGIN */ - -/* USER STRUCT END */ - -/* Private function --------------------------------------------------------- */ -/* Exported functions ------------------------------------------------------- */ -void Task_Task4(void *argument) { - (void)argument; /* 未使用argument，消除警告 */ - - - /* 计算任务运行到指定频率需要等待的tick数 */ - const uint32_t delay_tick = osKernelGetTickFreq() / TASK4_FREQ; - - osDelay(TASK4_INIT_DELAY); /* 延时一段时间再开启任务 */ - - uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ - /* USER CODE INIT BEGIN */ - - /* USER CODE INIT END */ - - while (1) { - tick += delay_tick; /* 计算下一个唤醒时刻 */ - /* USER CODE BEGIN */ - - /* USER CODE END */ - osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ - } - -} \ No newline at end of file diff --git a/User/task/config.yaml b/User/task/config.yaml index 2128659..a8274e6 100644 --- a/User/task/config.yaml +++ b/User/task/config.yaml @@ -19,10 +19,3 @@ function: Task_ctrl_lz name: ctrl_lz stack: 256 -- delay: 0 - description: '' - freq_control: true - frequency: 500.0 - function: Task_Task4 - name: Task4 - stack: 256 diff --git a/User/task/ctrl_lz.c b/User/task/ctrl_lz.c index 2ccd7a1..382696e 100644 --- a/User/task/ctrl_lz.c +++ b/User/task/ctrl_lz.c @@ -6,7 +6,11 @@ /* Includes ----------------------------------------------------------------- */ #include "task/user_task.h" /* USER INCLUDE BEGIN */ - +#include "bsp/can.h" +#include "device/motor_lz.h" +#include "device/motor_lk.h" +#include "module/config.h" +#include /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -14,7 +18,7 @@ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ - +static bool command_received = false; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -30,12 +34,94 @@ void Task_ctrl_lz(void *argument) { uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ + BSP_CAN_Init(); + MOTOR_LZ_Init(); + + for (int i = 0; i < 4; i++) { + MOTOR_LZ_Register(&Config_GetRobotParam()->joint_param[i]); + } + + // 注册CAN接收ID + BSP_CAN_RegisterId(BSP_CAN_1, 121, 0); // 使能命令 + BSP_CAN_RegisterId(BSP_CAN_1, 122, 0); // 力矩控制命令 /* USER CODE INIT END */ while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ + MOTOR_LZ_UpdateAll(); + + // 检查CAN接收消息 + BSP_CAN_Message_t rx_msg; + command_received = false; // 重置命令接收标志 + + // 检查ID 121 - 使能4个电机 + if (BSP_CAN_GetMessage(BSP_CAN_1, 121, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + command_received = true; + for (int i = 0; i < 4; i++) { + MOTOR_LZ_Enable(&Config_GetRobotParam()->joint_param[i]); + } + } + + // 检查ID 122 - 运控模式控制4个电机 + if (BSP_CAN_GetMessage(BSP_CAN_1, 122, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + command_received = true; + // 8字节数据分别是4个电机的力矩 (每个电机2字节，有符号整数，精度0.01 Nm) + for (int i = 0; i < 4; i++) { + int16_t torque_raw; + memcpy(&torque_raw, &rx_msg.data[i * 2], sizeof(int16_t)); + float torque = (float)torque_raw / 100.0f; // 转换为浮点数力矩值 + + // 使用运控模式控制电机，只设置力矩，其他参数为0 + MOTOR_LZ_MotionParam_t motion_param = { + .target_angle = 0.0f, + .target_velocity = 0.0f, + .kp = 0.0f, + .kd = 0.0f, + .torque = torque + }; + MOTOR_LZ_MotionControl(&Config_GetRobotParam()->joint_param[i], &motion_param); + } + } + + // 如果没有收到任何控制命令，电机进入relax模式 + if (!command_received) { + for (int i = 0; i < 4; i++) { + MOTOR_LZ_Relax(&Config_GetRobotParam()->joint_param[i]); + } + } + + // 发送4个电机的反馈数据，ID分别为124、125、126、127 + for (int i = 0; i < 4; i++) { + MOTOR_LZ_t* motor = MOTOR_LZ_GetMotor(&Config_GetRobotParam()->joint_param[i]); + if (motor != NULL) { + BSP_CAN_StdDataFrame_t motor_frame = { + .id = 124 + i, // ID: 124, 125, 126, 127 + .dlc = 8, + .data = {0} + }; + + // 数据重构：转矩电流(2字节) + 位置(3字节) + 速度(3字节) = 8字节 + // 转矩电流 - 转换为16位整数 (精度0.01 Nm) + int16_t torque_int = (int16_t)(motor->lz_feedback.current_torque * 100); + memcpy(&motor_frame.data[0], &torque_int, sizeof(int16_t)); + + // 位置 - 转换为24位整数，使用3字节 (精度0.0001 rad) + int32_t angle_int = (int32_t)(motor->lz_feedback.current_angle * 10000) & 0xFFFFFF; + motor_frame.data[2] = (angle_int >> 16) & 0xFF; + motor_frame.data[3] = (angle_int >> 8) & 0xFF; + motor_frame.data[4] = angle_int & 0xFF; + + // 速度 - 转换为24位整数，使用3字节 (精度0.001 rad/s) + int32_t velocity_int = (int32_t)(motor->lz_feedback.current_velocity * 1000) & 0xFFFFFF; + motor_frame.data[5] = (velocity_int >> 16) & 0xFF; + motor_frame.data[6] = (velocity_int >> 8) & 0xFF; + motor_frame.data[7] = velocity_int & 0xFF; + + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &motor_frame); + } + } /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ diff --git a/User/task/imu.c b/User/task/imu.c index 25ac9de..e696a71 100644 --- a/User/task/imu.c +++ b/User/task/imu.c @@ -6,7 +6,10 @@ /* Includes ----------------------------------------------------------------- */ #include "task/user_task.h" /* USER INCLUDE BEGIN */ - +#include "bsp/can.h" +#include "device/dm_imu.h" +#include "module/config.h" +#include /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -14,7 +17,7 @@ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ - +DM_IMU_t dm_imu; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -30,13 +33,50 @@ void Task_imu(void *argument) { uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ - + BSP_CAN_Init(); + DM_IMU_Init(&dm_imu, &Config_GetRobotParam()->imu_param); /* USER CODE INIT END */ while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ - + if (DM_IMU_AutoUpdateAll(&dm_imu) == DEVICE_OK) { + // 发送加速度计数据 (ID: 0x66) - 前8字节 (x, y) + BSP_CAN_StdDataFrame_t accl_frame = { + .id = 0x66, + .dlc = 8, + .data = {0} + }; + memcpy(accl_frame.data, &dm_imu.data.accl, 8); + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &accl_frame); + + // 发送陀螺仪数据 (ID: 0x67) - 前8字节 (x, y) + BSP_CAN_StdDataFrame_t gyro_frame = { + .id = 0x67, + .dlc = 8, + .data = {0} + }; + memcpy(gyro_frame.data, &dm_imu.data.gyro, 8); + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &gyro_frame); + + // 发送欧拉角数据 (ID: 0x68) - 前8字节 (yaw, pit) + BSP_CAN_StdDataFrame_t euler_frame = { + .id = 0x68, + .dlc = 8, + .data = {0} + }; + memcpy(euler_frame.data, &dm_imu.data.euler, 8); + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &euler_frame); + + // // 发送四元数数据 (ID: 0x69) - 前8字节 (q0, q1) + // BSP_CAN_StdDataFrame_t quat_frame = { + // .id = 0x69, + // .dlc = 8, + // .data = {0} + // }; + // memcpy(quat_frame.data, &dm_imu.data.quat, 8); + // BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &quat_frame); + } /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } diff --git a/User/task/init.c b/User/task/init.c index 7f6281e..e261f86 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -33,7 +33,6 @@ void Task_Init(void *argument) { task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink); task_runtime.thread.imu = osThreadNew(Task_imu, NULL, &attr_imu); task_runtime.thread.ctrl_lz = osThreadNew(Task_ctrl_lz, NULL, &attr_ctrl_lz); - task_runtime.thread.Task4 = osThreadNew(Task_Task4, NULL, &attr_Task4); // 创建消息队列 /* USER MESSAGE BEGIN */ diff --git a/User/task/user_task.c b/User/task/user_task.c index 2da5e75..5dde688 100644 --- a/User/task/user_task.c +++ b/User/task/user_task.c @@ -23,9 +23,4 @@ const osThreadAttr_t attr_ctrl_lz = { .name = "ctrl_lz", .priority = osPriorityNormal, .stack_size = 256 * 4, -}; -const osThreadAttr_t attr_Task4 = { - .name = "Task4", - .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 index af6930c..95c4bc8 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -16,14 +16,12 @@ extern "C" { #define BLINK_FREQ (500.0) #define IMU_FREQ (1000.0) #define CTRL_LZ_FREQ (500.0) -#define TASK4_FREQ (500.0) /* 任务初始化延时ms */ #define TASK_INIT_DELAY (100u) #define BLINK_INIT_DELAY (0) #define IMU_INIT_DELAY (0) #define CTRL_LZ_INIT_DELAY (0) -#define TASK4_INIT_DELAY (0) /* Exported defines --------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */ @@ -36,7 +34,6 @@ typedef struct { osThreadId_t blink; osThreadId_t imu; osThreadId_t ctrl_lz; - osThreadId_t Task4; } thread; /* USER MESSAGE BEGIN */ @@ -61,7 +58,6 @@ typedef struct { UBaseType_t blink; UBaseType_t imu; UBaseType_t ctrl_lz; - UBaseType_t Task4; } stack_water_mark; /* 各任务运行频率 */ @@ -69,7 +65,6 @@ typedef struct { float blink; float imu; float ctrl_lz; - float Task4; } freq; /* 任务最近运行时间 */ @@ -77,7 +72,6 @@ typedef struct { float blink; float imu; float ctrl_lz; - float Task4; } last_up_time; } Task_Runtime_t; @@ -90,14 +84,12 @@ extern const osThreadAttr_t attr_init; extern const osThreadAttr_t attr_blink; extern const osThreadAttr_t attr_imu; extern const osThreadAttr_t attr_ctrl_lz; -extern const osThreadAttr_t attr_Task4; /* 任务函数声明 */ void Task_Init(void *argument); void Task_blink(void *argument); void Task_imu(void *argument); void Task_ctrl_lz(void *argument); -void Task_Task4(void *argument); #ifdef __cplusplus } From 81a53069627a9ab67fdac64e6dbd2c96e63241ed Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sat, 4 Oct 2025 21:05:11 +0800 Subject: [PATCH 03/25] =?UTF-8?q?=E4=BF=AE=E6=94=B9imu?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/task/imu.c | 70 +++++++++++++++++++++++++++++++++++++++---------- 1 file changed, 56 insertions(+), 14 deletions(-) diff --git a/User/task/imu.c b/User/task/imu.c index e696a71..61a1007 100644 --- a/User/task/imu.c +++ b/User/task/imu.c @@ -41,41 +41,83 @@ void Task_imu(void *argument) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ if (DM_IMU_AutoUpdateAll(&dm_imu) == DEVICE_OK) { - // 发送加速度计数据 (ID: 0x66) - 前8字节 (x, y) + // 发送加速度计数据 (ID: 0x66) - 三轴压缩到一帧，每轴2字节，精度0.01g BSP_CAN_StdDataFrame_t accl_frame = { .id = 0x66, .dlc = 8, .data = {0} }; - memcpy(accl_frame.data, &dm_imu.data.accl, 8); + + // 转换为16位整数发送 (精度0.01g，范围±327.67g) + int16_t accl_x_int = (int16_t)(dm_imu.data.accl.x * 100.0f); + int16_t accl_y_int = (int16_t)(dm_imu.data.accl.y * 100.0f); + int16_t accl_z_int = (int16_t)(dm_imu.data.accl.z * 100.0f); + + // 打包数据：x(2字节) + y(2字节) + z(2字节) + 2字节保留 + memcpy(&accl_frame.data[0], &accl_x_int, 2); + memcpy(&accl_frame.data[2], &accl_y_int, 2); + memcpy(&accl_frame.data[4], &accl_z_int, 2); + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &accl_frame); - // 发送陀螺仪数据 (ID: 0x67) - 前8字节 (x, y) + // 发送陀螺仪数据 (ID: 0x67) - 三轴压缩到一帧，每轴2字节，精度0.01°/s BSP_CAN_StdDataFrame_t gyro_frame = { .id = 0x67, .dlc = 8, .data = {0} }; - memcpy(gyro_frame.data, &dm_imu.data.gyro, 8); + + // 转换为16位整数发送 (精度0.01°/s，范围±327.67°/s) + int16_t gyro_x_int = (int16_t)(dm_imu.data.gyro.x * 57.2958f * 100.0f); // 弧度/s转角度/s*100 + int16_t gyro_y_int = (int16_t)(dm_imu.data.gyro.y * 57.2958f * 100.0f); + int16_t gyro_z_int = (int16_t)(dm_imu.data.gyro.z * 57.2958f * 100.0f); + + // 打包数据：x(2字节) + y(2字节) + z(2字节) + 2字节保留 + memcpy(&gyro_frame.data[0], &gyro_x_int, 2); + memcpy(&gyro_frame.data[2], &gyro_y_int, 2); + memcpy(&gyro_frame.data[4], &gyro_z_int, 2); + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &gyro_frame); - // 发送欧拉角数据 (ID: 0x68) - 前8字节 (yaw, pit) + // 发送欧拉角数据 (ID: 0x68) - 三轴压缩到一帧，每轴2字节，精度0.01度 BSP_CAN_StdDataFrame_t euler_frame = { .id = 0x68, .dlc = 8, .data = {0} }; - memcpy(euler_frame.data, &dm_imu.data.euler, 8); + + // 转换为16位整数发送 (精度0.01度，范围±327.67度) + int16_t yaw_int = (int16_t)(dm_imu.data.euler.yaw * 57.2958f * 100.0f); // 弧度转角度*100 + int16_t pit_int = (int16_t)(dm_imu.data.euler.pit * 57.2958f * 100.0f); // 弧度转角度*100 + int16_t rol_int = (int16_t)(dm_imu.data.euler.rol * 57.2958f * 100.0f); // 弧度转角度*100 + + // 打包数据：yaw(2字节) + pitch(2字节) + roll(2字节) + 2字节保留 + memcpy(&euler_frame.data[0], &yaw_int, 2); + memcpy(&euler_frame.data[2], &pit_int, 2); + memcpy(&euler_frame.data[4], &rol_int, 2); + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &euler_frame); - // // 发送四元数数据 (ID: 0x69) - 前8字节 (q0, q1) - // BSP_CAN_StdDataFrame_t quat_frame = { - // .id = 0x69, - // .dlc = 8, - // .data = {0} - // }; - // memcpy(quat_frame.data, &dm_imu.data.quat, 8); - // BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &quat_frame); + // 发送四元数数据 (ID: 0x69) - 四分量压缩到一帧，每分量2字节，精度0.0001 + BSP_CAN_StdDataFrame_t quat_frame = { + .id = 0x69, + .dlc = 8, + .data = {0} + }; + + // 转换为16位整数发送 (精度0.0001，范围±3.2767) + int16_t q0_int = (int16_t)(dm_imu.data.quat.q0 * 10000.0f); + int16_t q1_int = (int16_t)(dm_imu.data.quat.q1 * 10000.0f); + int16_t q2_int = (int16_t)(dm_imu.data.quat.q2 * 10000.0f); + int16_t q3_int = (int16_t)(dm_imu.data.quat.q3 * 10000.0f); + + // 打包数据：q0(2字节) + q1(2字节) + q2(2字节) + q3(2字节) + memcpy(&quat_frame.data[0], &q0_int, 2); + memcpy(&quat_frame.data[2], &q1_int, 2); + memcpy(&quat_frame.data[4], &q2_int, 2); + memcpy(&quat_frame.data[6], &q3_int, 2); + + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &quat_frame); } /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ From 01ea1a9e78160600eb65adbdecd76aed0d8918cc Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sat, 4 Oct 2025 22:46:02 +0800 Subject: [PATCH 04/25] =?UTF-8?q?=E4=BF=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/task/ctrl_lz.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/User/task/ctrl_lz.c b/User/task/ctrl_lz.c index 382696e..6a2c58b 100644 --- a/User/task/ctrl_lz.c +++ b/User/task/ctrl_lz.c @@ -45,6 +45,10 @@ void Task_ctrl_lz(void *argument) { // 注册CAN接收ID BSP_CAN_RegisterId(BSP_CAN_1, 121, 0); // 使能命令 BSP_CAN_RegisterId(BSP_CAN_1, 122, 0); // 力矩控制命令 + + for (int i = 0; i < 4; i++) { + MOTOR_LZ_Enable(&Config_GetRobotParam()->joint_param[i]); + } /* USER CODE INIT END */ while (1) { From 50a36aa5f6ce2aaef13e0c8f8e67ba8147664369 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sat, 4 Oct 2025 23:22:40 +0800 Subject: [PATCH 05/25] =?UTF-8?q?=E6=94=B9id?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/task/imu.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/User/task/imu.c b/User/task/imu.c index 61a1007..0dc500f 100644 --- a/User/task/imu.c +++ b/User/task/imu.c @@ -43,7 +43,7 @@ void Task_imu(void *argument) { if (DM_IMU_AutoUpdateAll(&dm_imu) == DEVICE_OK) { // 发送加速度计数据 (ID: 0x66) - 三轴压缩到一帧，每轴2字节，精度0.01g BSP_CAN_StdDataFrame_t accl_frame = { - .id = 0x66, + .id = 150, .dlc = 8, .data = {0} }; @@ -62,7 +62,7 @@ void Task_imu(void *argument) { // 发送陀螺仪数据 (ID: 0x67) - 三轴压缩到一帧，每轴2字节，精度0.01°/s BSP_CAN_StdDataFrame_t gyro_frame = { - .id = 0x67, + .id = 0x151, .dlc = 8, .data = {0} }; @@ -81,7 +81,7 @@ void Task_imu(void *argument) { // 发送欧拉角数据 (ID: 0x68) - 三轴压缩到一帧，每轴2字节，精度0.01度 BSP_CAN_StdDataFrame_t euler_frame = { - .id = 0x68, + .id = 0x152, .dlc = 8, .data = {0} }; @@ -100,7 +100,7 @@ void Task_imu(void *argument) { // 发送四元数数据 (ID: 0x69) - 四分量压缩到一帧，每分量2字节，精度0.0001 BSP_CAN_StdDataFrame_t quat_frame = { - .id = 0x69, + .id = 0x153, .dlc = 8, .data = {0} }; From 700b0dee8c2add8c0dc9c081404824b59753905f Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sat, 4 Oct 2025 23:28:12 +0800 Subject: [PATCH 06/25] gai --- User/task/imu.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/User/task/imu.c b/User/task/imu.c index 0dc500f..1a788a8 100644 --- a/User/task/imu.c +++ b/User/task/imu.c @@ -18,6 +18,7 @@ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ DM_IMU_t dm_imu; +int i= 0; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -39,9 +40,14 @@ void Task_imu(void *argument) { while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ + i++; + /* USER CODE BEGIN */ if (DM_IMU_AutoUpdateAll(&dm_imu) == DEVICE_OK) { - // 发送加速度计数据 (ID: 0x66) - 三轴压缩到一帧，每轴2字节，精度0.01g + } + if (i>1){ + i=0; +// 发送加速度计数据 (ID: 0x66) - 三轴压缩到一帧，每轴2字节，精度0.01g BSP_CAN_StdDataFrame_t accl_frame = { .id = 150, .dlc = 8, @@ -117,7 +123,7 @@ void Task_imu(void *argument) { memcpy(&quat_frame.data[4], &q2_int, 2); memcpy(&quat_frame.data[6], &q3_int, 2); - BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &quat_frame); + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &quat_frame); } /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ From 8edcf3205e07d98c9cb67ddf0cb0766edf1a3f43 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sat, 4 Oct 2025 23:30:57 +0800 Subject: [PATCH 07/25] gai ic --- User/task/imu.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/User/task/imu.c b/User/task/imu.c index 1a788a8..02f99ea 100644 --- a/User/task/imu.c +++ b/User/task/imu.c @@ -68,7 +68,7 @@ void Task_imu(void *argument) { // 发送陀螺仪数据 (ID: 0x67) - 三轴压缩到一帧，每轴2字节，精度0.01°/s BSP_CAN_StdDataFrame_t gyro_frame = { - .id = 0x151, + .id = 151, .dlc = 8, .data = {0} }; @@ -87,7 +87,7 @@ void Task_imu(void *argument) { // 发送欧拉角数据 (ID: 0x68) - 三轴压缩到一帧，每轴2字节，精度0.01度 BSP_CAN_StdDataFrame_t euler_frame = { - .id = 0x152, + .id = 152, .dlc = 8, .data = {0} }; @@ -106,7 +106,7 @@ void Task_imu(void *argument) { // 发送四元数数据 (ID: 0x69) - 四分量压缩到一帧，每分量2字节，精度0.0001 BSP_CAN_StdDataFrame_t quat_frame = { - .id = 0x153, + .id = 153, .dlc = 8, .data = {0} }; From 7ac7f7d8689bb3eaf3e378c58b1504a6e194fd12 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 5 Oct 2025 00:03:38 +0800 Subject: [PATCH 08/25] =?UTF-8?q?=E6=94=B9=E5=8F=91=E9=80=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/task/imu.c | 105 +++++++++++++++++++++++++----------------------- 1 file changed, 55 insertions(+), 50 deletions(-) diff --git a/User/task/imu.c b/User/task/imu.c index 02f99ea..1d782fe 100644 --- a/User/task/imu.c +++ b/User/task/imu.c @@ -1,6 +1,6 @@ /* imu Task - + */ /* Includes ----------------------------------------------------------------- */ @@ -18,7 +18,7 @@ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ DM_IMU_t dm_imu; -int i= 0; +int i = 0; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -26,7 +26,6 @@ int i= 0; void Task_imu(void *argument) { (void)argument; /* 未使用argument，消除警告 */ - /* 计算任务运行到指定频率需要等待的tick数 */ const uint32_t delay_tick = osKernelGetTickFreq() / IMU_FREQ; @@ -37,96 +36,102 @@ void Task_imu(void *argument) { BSP_CAN_Init(); DM_IMU_Init(&dm_imu, &Config_GetRobotParam()->imu_param); /* USER CODE INIT END */ - + while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ i++; /* USER CODE BEGIN */ - if (DM_IMU_AutoUpdateAll(&dm_imu) == DEVICE_OK) { - } - if (i>1){ - i=0; -// 发送加速度计数据 (ID: 0x66) - 三轴压缩到一帧，每轴2字节，精度0.01g - BSP_CAN_StdDataFrame_t accl_frame = { - .id = 150, - .dlc = 8, - .data = {0} - }; - + DM_IMU_AutoUpdateAll(&dm_imu); + switch (i) { + case 0: { + // 发送加速度计数据 (ID: 0x66) - 三轴压缩到一帧，每轴2字节，精度0.01g + BSP_CAN_StdDataFrame_t accl_frame = {.id = 150, .dlc = 8, .data = {0}}; + // 转换为16位整数发送 (精度0.01g，范围±327.67g) int16_t accl_x_int = (int16_t)(dm_imu.data.accl.x * 100.0f); int16_t accl_y_int = (int16_t)(dm_imu.data.accl.y * 100.0f); int16_t accl_z_int = (int16_t)(dm_imu.data.accl.z * 100.0f); - + // 打包数据：x(2字节) + y(2字节) + z(2字节) + 2字节保留 memcpy(&accl_frame.data[0], &accl_x_int, 2); memcpy(&accl_frame.data[2], &accl_y_int, 2); memcpy(&accl_frame.data[4], &accl_z_int, 2); - + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &accl_frame); - + + } + + break; + case 1: { // 发送陀螺仪数据 (ID: 0x67) - 三轴压缩到一帧，每轴2字节，精度0.01°/s - BSP_CAN_StdDataFrame_t gyro_frame = { - .id = 151, - .dlc = 8, - .data = {0} - }; - + BSP_CAN_StdDataFrame_t gyro_frame = {.id = 151, .dlc = 8, .data = {0}}; + // 转换为16位整数发送 (精度0.01°/s，范围±327.67°/s) - int16_t gyro_x_int = (int16_t)(dm_imu.data.gyro.x * 57.2958f * 100.0f); // 弧度/s转角度/s*100 + int16_t gyro_x_int = (int16_t)(dm_imu.data.gyro.x * 57.2958f * + 100.0f); // 弧度/s转角度/s*100 int16_t gyro_y_int = (int16_t)(dm_imu.data.gyro.y * 57.2958f * 100.0f); int16_t gyro_z_int = (int16_t)(dm_imu.data.gyro.z * 57.2958f * 100.0f); - + // 打包数据：x(2字节) + y(2字节) + z(2字节) + 2字节保留 memcpy(&gyro_frame.data[0], &gyro_x_int, 2); memcpy(&gyro_frame.data[2], &gyro_y_int, 2); memcpy(&gyro_frame.data[4], &gyro_z_int, 2); - + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &gyro_frame); - + + break; + } + + case 2: { // 发送欧拉角数据 (ID: 0x68) - 三轴压缩到一帧，每轴2字节，精度0.01度 - BSP_CAN_StdDataFrame_t euler_frame = { - .id = 152, - .dlc = 8, - .data = {0} - }; - + BSP_CAN_StdDataFrame_t euler_frame = {.id = 152, .dlc = 8, .data = {0}}; + // 转换为16位整数发送 (精度0.01度，范围±327.67度) - int16_t yaw_int = (int16_t)(dm_imu.data.euler.yaw * 57.2958f * 100.0f); // 弧度转角度*100 - int16_t pit_int = (int16_t)(dm_imu.data.euler.pit * 57.2958f * 100.0f); // 弧度转角度*100 - int16_t rol_int = (int16_t)(dm_imu.data.euler.rol * 57.2958f * 100.0f); // 弧度转角度*100 - + int16_t yaw_int = (int16_t)(dm_imu.data.euler.yaw * 57.2958f * + 100.0f); // 弧度转角度*100 + int16_t pit_int = (int16_t)(dm_imu.data.euler.pit * 57.2958f * + 100.0f); // 弧度转角度*100 + int16_t rol_int = (int16_t)(dm_imu.data.euler.rol * 57.2958f * + 100.0f); // 弧度转角度*100 + // 打包数据：yaw(2字节) + pitch(2字节) + roll(2字节) + 2字节保留 memcpy(&euler_frame.data[0], &yaw_int, 2); memcpy(&euler_frame.data[2], &pit_int, 2); memcpy(&euler_frame.data[4], &rol_int, 2); - + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &euler_frame); - + + } + + break; + case 3: { // 发送四元数数据 (ID: 0x69) - 四分量压缩到一帧，每分量2字节，精度0.0001 - BSP_CAN_StdDataFrame_t quat_frame = { - .id = 153, - .dlc = 8, - .data = {0} - }; - + BSP_CAN_StdDataFrame_t quat_frame = {.id = 153, .dlc = 8, .data = {0}}; + // 转换为16位整数发送 (精度0.0001，范围±3.2767) int16_t q0_int = (int16_t)(dm_imu.data.quat.q0 * 10000.0f); int16_t q1_int = (int16_t)(dm_imu.data.quat.q1 * 10000.0f); int16_t q2_int = (int16_t)(dm_imu.data.quat.q2 * 10000.0f); int16_t q3_int = (int16_t)(dm_imu.data.quat.q3 * 10000.0f); - + // 打包数据：q0(2字节) + q1(2字节) + q2(2字节) + q3(2字节) memcpy(&quat_frame.data[0], &q0_int, 2); memcpy(&quat_frame.data[2], &q1_int, 2); memcpy(&quat_frame.data[4], &q2_int, 2); memcpy(&quat_frame.data[6], &q3_int, 2); - - BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &quat_frame); + + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &quat_frame); } + + i = 0; + break; + default: + i = 0; + break; + } + /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } - } \ No newline at end of file From 59472bd2e873e473c70cac36ef8817d1cf011e54 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 5 Oct 2025 00:15:44 +0800 Subject: [PATCH 09/25] =?UTF-8?q?=E5=85=B3=E4=B8=80=E4=B8=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/task/init.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/User/task/init.c b/User/task/init.c index e261f86..4537802 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -31,7 +31,7 @@ void Task_Init(void *argument) { /* 创建任务线程 */ task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink); - task_runtime.thread.imu = osThreadNew(Task_imu, NULL, &attr_imu); + // task_runtime.thread.imu = osThreadNew(Task_imu, NULL, &attr_imu); task_runtime.thread.ctrl_lz = osThreadNew(Task_ctrl_lz, NULL, &attr_ctrl_lz); // 创建消息队列 From 84c8d72e3889d791070a1ac318402ad609af457a Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 5 Oct 2025 01:57:06 +0800 Subject: [PATCH 10/25] =?UTF-8?q?=E5=A4=B1=E8=B4=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/config.c | 8 ++++---- User/task/ctrl_lz.c | 2 +- User/task/init.c | 2 +- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/User/module/config.c b/User/module/config.c index 2ef4045..8bb0270 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -27,7 +27,7 @@ Config_RobotParam_t robot_config = { { { // 左髋关节 - .can = BSP_CAN_2, + .can = BSP_CAN_1, .motor_id = 124, .host_id = 0xFF, .module = MOTOR_LZ_RSO3, @@ -36,7 +36,7 @@ Config_RobotParam_t robot_config = { }, { // 左膝关节 - .can = BSP_CAN_2, + .can = BSP_CAN_1, .motor_id = 125, .host_id = 0xFF, .module = MOTOR_LZ_RSO3, @@ -45,7 +45,7 @@ Config_RobotParam_t robot_config = { }, { // 右膝关节 - .can = BSP_CAN_2, + .can = BSP_CAN_1, .motor_id = 126, .host_id = 0xFF, .module = MOTOR_LZ_RSO3, @@ -54,7 +54,7 @@ Config_RobotParam_t robot_config = { }, { // 右髋关节 - .can = BSP_CAN_2, + .can = BSP_CAN_1, .motor_id = 127, .host_id = 0xFF, .module = MOTOR_LZ_RSO3, diff --git a/User/task/ctrl_lz.c b/User/task/ctrl_lz.c index 6a2c58b..500da8e 100644 --- a/User/task/ctrl_lz.c +++ b/User/task/ctrl_lz.c @@ -123,7 +123,7 @@ void Task_ctrl_lz(void *argument) { motor_frame.data[6] = (velocity_int >> 8) & 0xFF; motor_frame.data[7] = velocity_int & 0xFF; - BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &motor_frame); + BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &motor_frame); } } diff --git a/User/task/init.c b/User/task/init.c index 4537802..e261f86 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -31,7 +31,7 @@ void Task_Init(void *argument) { /* 创建任务线程 */ task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink); - // task_runtime.thread.imu = osThreadNew(Task_imu, NULL, &attr_imu); + task_runtime.thread.imu = osThreadNew(Task_imu, NULL, &attr_imu); task_runtime.thread.ctrl_lz = osThreadNew(Task_ctrl_lz, NULL, &attr_ctrl_lz); // 创建消息队列 From 4f4e485912625870007f6f2ed61f7b25c970a484 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 5 Oct 2025 03:03:27 +0800 Subject: [PATCH 11/25] =?UTF-8?q?=E6=B7=BB=E5=8A=A0imu?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 9 +- User/bsp/bsp_config.yaml | 114 ++++++++ User/bsp/gpio.c | 130 +++++++++ User/bsp/gpio.h | 57 ++++ User/bsp/pwm.c | 121 +++++++++ User/bsp/pwm.h | 59 +++++ User/bsp/spi.c | 181 +++++++++++++ User/bsp/spi.h | 70 +++++ User/component/component_config.yaml | 8 + User/component/filter.c | 185 +++++++++++++ User/component/filter.h | 120 +++++++++ User/component/pid.c | 158 +++++++++++ User/component/pid.h | 107 ++++++++ User/device/bmi088.c | 381 +++++++++++++++++++++++++++ User/device/bmi088.h | 81 ++++++ User/device/device.h | 5 +- User/device/device_config.yaml | 8 + User/device/dm_imu.c | 271 ------------------- User/device/dm_imu.h | 90 ------- User/module/config.c | 7 - User/module/config.h | 2 - User/task/config.yaml | 6 +- User/task/ctrl_chassis.c | 44 ++++ User/task/ctrl_lz.c | 134 ---------- User/task/imu.c | 201 +++++++------- User/task/init.c | 2 +- User/task/user_task.c | 4 +- User/task/user_task.h | 18 +- 28 files changed, 1959 insertions(+), 614 deletions(-) create mode 100644 User/bsp/gpio.c create mode 100644 User/bsp/gpio.h create mode 100644 User/bsp/pwm.c create mode 100644 User/bsp/pwm.h create mode 100644 User/bsp/spi.c create mode 100644 User/bsp/spi.h create mode 100644 User/component/filter.c create mode 100644 User/component/filter.h create mode 100644 User/component/pid.c create mode 100644 User/component/pid.h create mode 100644 User/device/bmi088.c create mode 100644 User/device/bmi088.h delete mode 100644 User/device/dm_imu.c delete mode 100644 User/device/dm_imu.h create mode 100644 User/task/ctrl_chassis.c delete mode 100644 User/task/ctrl_lz.c diff --git a/CMakeLists.txt b/CMakeLists.txt index c171d6d..4a4a3fe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,15 +47,20 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE # Add user sources here # User/bsp sources User/bsp/can.c + User/bsp/gpio.c User/bsp/mm.c + User/bsp/pwm.c + User/bsp/spi.c User/bsp/time.c # User/component sources User/component/ahrs.c + User/component/filter.c + User/component/pid.c User/component/user_math.c # User/device sources - User/device/dm_imu.c + User/device/bmi088.c User/device/motor.c User/device/motor_lk.c User/device/motor_lz.c @@ -65,7 +70,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE # User/task sources User/task/blink.c - User/task/ctrl_lz.c + User/task/ctrl_chassis.c User/task/imu.c User/task/init.c User/task/user_task.c diff --git a/User/bsp/bsp_config.yaml b/User/bsp/bsp_config.yaml index 2c32a85..6f0ec46 100644 --- a/User/bsp/bsp_config.yaml +++ b/User/bsp/bsp_config.yaml @@ -5,7 +5,121 @@ can: - instance: CAN2 name: '2' enabled: true +gpio: + configs: + - custom_name: USER_KEY + has_exti: true + ioc_label: USER_KEY + pin: PA0-WKUP + type: EXTI + - custom_name: ACCL_CS + has_exti: false + ioc_label: ACCL_CS + pin: PA4 + type: OUTPUT + - custom_name: GYRO_CS + has_exti: false + ioc_label: GYRO_CS + pin: PB0 + type: OUTPUT + - custom_name: SPI2_CS + has_exti: false + ioc_label: SPI2_CS + pin: PB12 + type: OUTPUT + - custom_name: HW0 + has_exti: false + ioc_label: HW0 + pin: PC0 + type: INPUT + - custom_name: HW1 + has_exti: false + ioc_label: HW1 + pin: PC1 + type: INPUT + - custom_name: HW2 + has_exti: false + ioc_label: HW2 + pin: PC2 + type: INPUT + - custom_name: ACCL_INT + has_exti: true + ioc_label: ACCL_INT + pin: PC4 + type: EXTI + - custom_name: GYRO_INT + has_exti: true + ioc_label: GYRO_INT + pin: PC5 + type: EXTI + - custom_name: CMPS_INT + has_exti: true + ioc_label: CMPS_INT + pin: PG3 + type: EXTI + - custom_name: CMPS_RST + has_exti: false + ioc_label: CMPS_RST + pin: PG6 + type: OUTPUT + enabled: true mm: enabled: true +pwm: + configs: + - channel: TIM_CHANNEL_1 + custom_name: TIM8_CH1 + label: TIM8_CH1 + timer: TIM8 + - channel: TIM_CHANNEL_3 + custom_name: LASER + label: LASER + timer: TIM3 + - channel: TIM_CHANNEL_3 + custom_name: BUZZER + label: BUZZER + timer: TIM4 + - channel: TIM_CHANNEL_2 + custom_name: TIM1_CH2 + label: TIM1_CH2 + timer: TIM1 + - channel: TIM_CHANNEL_3 + custom_name: TIM1_CH3 + label: TIM1_CH3 + timer: TIM1 + - channel: TIM_CHANNEL_4 + custom_name: TIM1_CH4 + label: TIM1_CH4 + timer: TIM1 + - channel: TIM_CHANNEL_1 + custom_name: TIM1_CH1 + label: TIM1_CH1 + timer: TIM1 + - channel: TIM_CHANNEL_1 + custom_name: IMU_HEAT_PWM + label: IMU_HEAT_PWM + timer: TIM10 + - channel: TIM_CHANNEL_1 + custom_name: LED_B + label: LED_B + timer: TIM5 + - channel: TIM_CHANNEL_2 + custom_name: LED_G + label: LED_G + timer: TIM5 + - channel: TIM_CHANNEL_3 + custom_name: LED_R + label: LED_R + timer: TIM5 + - channel: TIM_CHANNEL_2 + custom_name: TIM8_CH2 + label: TIM8_CH2 + timer: TIM8 + enabled: true +spi: + devices: + - instance: SPI1 + name: BMI088 + enabled: true time: enabled: true diff --git a/User/bsp/gpio.c b/User/bsp/gpio.c new file mode 100644 index 0000000..490a5d1 --- /dev/null +++ b/User/bsp/gpio.c @@ -0,0 +1,130 @@ +/* Includes ----------------------------------------------------------------- */ +#include "bsp/gpio.h" + +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +typedef struct { + uint16_t pin; + GPIO_TypeDef *gpio; +} BSP_GPIO_MAP_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static const BSP_GPIO_MAP_t GPIO_Map[BSP_GPIO_NUM] = { + {USER_KEY_Pin, USER_KEY_GPIO_Port}, + {ACCL_CS_Pin, ACCL_CS_GPIO_Port}, + {GYRO_CS_Pin, GYRO_CS_GPIO_Port}, + {SPI2_CS_Pin, SPI2_CS_GPIO_Port}, + {HW0_Pin, HW0_GPIO_Port}, + {HW1_Pin, HW1_GPIO_Port}, + {HW2_Pin, HW2_GPIO_Port}, + {ACCL_INT_Pin, ACCL_INT_GPIO_Port}, + {GYRO_INT_Pin, GYRO_INT_GPIO_Port}, + {CMPS_INT_Pin, CMPS_INT_GPIO_Port}, + {CMPS_RST_Pin, CMPS_RST_GPIO_Port}, +}; + +static void (*GPIO_Callback[16])(void); + +/* Private function -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { + for (uint8_t i = 0; i < 16; i++) { + if (GPIO_Pin & (1 << i)) { + if (GPIO_Callback[i]) { + GPIO_Callback[i](); + } + } + } +} + +/* Exported functions ------------------------------------------------------- */ +int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, void (*callback)(void)) { + if (callback == NULL) return BSP_ERR_NULL; + if (gpio >= BSP_GPIO_NUM) return BSP_ERR; + + // 从GPIO映射中获取对应的pin值 + uint16_t pin = GPIO_Map[gpio].pin; + + for (uint8_t i = 0; i < 16; i++) { + if (pin & (1 << i)) { + GPIO_Callback[i] = callback; + break; + } + } + return BSP_OK; +} + +int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) { + switch (gpio) { + case BSP_GPIO_USER_KEY: + HAL_NVIC_EnableIRQ(USER_KEY_EXTI_IRQn); + break; + case BSP_GPIO_ACCL_INT: + HAL_NVIC_EnableIRQ(ACCL_INT_EXTI_IRQn); + break; + case BSP_GPIO_GYRO_INT: + HAL_NVIC_EnableIRQ(GYRO_INT_EXTI_IRQn); + break; + case BSP_GPIO_CMPS_INT: + HAL_NVIC_EnableIRQ(CMPS_INT_EXTI_IRQn); + break; + default: + return BSP_ERR; + } + return BSP_OK; +} + +int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) { + switch (gpio) { + case BSP_GPIO_USER_KEY: + HAL_NVIC_DisableIRQ(USER_KEY_EXTI_IRQn); + break; + case BSP_GPIO_ACCL_INT: + HAL_NVIC_DisableIRQ(ACCL_INT_EXTI_IRQn); + break; + case BSP_GPIO_GYRO_INT: + HAL_NVIC_DisableIRQ(GYRO_INT_EXTI_IRQn); + break; + case BSP_GPIO_CMPS_INT: + HAL_NVIC_DisableIRQ(CMPS_INT_EXTI_IRQn); + break; + default: + return BSP_ERR; + } + return BSP_OK; +} +int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value){ + if (gpio >= BSP_GPIO_NUM) return BSP_ERR; + HAL_GPIO_WritePin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin, value); + return BSP_OK; +} + +int8_t BSP_GPIO_TogglePin(BSP_GPIO_t gpio){ + if (gpio >= BSP_GPIO_NUM) return BSP_ERR; + HAL_GPIO_TogglePin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin); + return BSP_OK; +} + +bool BSP_GPIO_ReadPin(BSP_GPIO_t gpio){ + if (gpio >= BSP_GPIO_NUM) return false; + return HAL_GPIO_ReadPin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin) == GPIO_PIN_SET; +} \ No newline at end of file diff --git a/User/bsp/gpio.h b/User/bsp/gpio.h new file mode 100644 index 0000000..4d9707b --- /dev/null +++ b/User/bsp/gpio.h @@ -0,0 +1,57 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include + +#include "bsp/bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ +typedef enum { + BSP_GPIO_USER_KEY, + BSP_GPIO_ACCL_CS, + BSP_GPIO_GYRO_CS, + BSP_GPIO_SPI2_CS, + BSP_GPIO_HW0, + BSP_GPIO_HW1, + BSP_GPIO_HW2, + BSP_GPIO_ACCL_INT, + BSP_GPIO_GYRO_INT, + BSP_GPIO_CMPS_INT, + BSP_GPIO_CMPS_RST, + BSP_GPIO_NUM, + BSP_GPIO_ERR, +} BSP_GPIO_t; + +/* Exported functions prototypes -------------------------------------------- */ +int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, void (*callback)(void)); + +int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio); +int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio); + +int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value); +int8_t BSP_GPIO_TogglePin(BSP_GPIO_t gpio); + +bool BSP_GPIO_ReadPin(BSP_GPIO_t gpio); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/User/bsp/pwm.c b/User/bsp/pwm.c new file mode 100644 index 0000000..6b0b96c --- /dev/null +++ b/User/bsp/pwm.c @@ -0,0 +1,121 @@ +/* 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] = { + {&htim8, TIM_CHANNEL_1}, + {&htim3, TIM_CHANNEL_3}, + {&htim4, TIM_CHANNEL_3}, + {&htim1, TIM_CHANNEL_2}, + {&htim1, TIM_CHANNEL_3}, + {&htim1, TIM_CHANNEL_4}, + {&htim1, TIM_CHANNEL_1}, + {&htim10, TIM_CHANNEL_1}, + {&htim5, TIM_CHANNEL_1}, + {&htim5, TIM_CHANNEL_2}, + {&htim5, TIM_CHANNEL_3}, + {&htim8, TIM_CHANNEL_2}, +}; + +/* 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..94cb40c --- /dev/null +++ b/User/bsp/pwm.h @@ -0,0 +1,59 @@ +#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_TIM8_CH1, + BSP_PWM_LASER, + BSP_PWM_BUZZER, + BSP_PWM_TIM1_CH2, + BSP_PWM_TIM1_CH3, + BSP_PWM_TIM1_CH4, + BSP_PWM_TIM1_CH1, + BSP_PWM_IMU_HEAT_PWM, + BSP_PWM_LED_B, + BSP_PWM_LED_G, + BSP_PWM_LED_R, + BSP_PWM_TIM8_CH2, + 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); //设置占空比，范围0.0~1.0 +int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq); //设置频率，单位Hz +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/spi.c b/User/bsp/spi.c new file mode 100644 index 0000000..d8ac00e --- /dev/null +++ b/User/bsp/spi.c @@ -0,0 +1,181 @@ +/* Includes ----------------------------------------------------------------- */ +#include +#include "bsp/spi.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 (*SPI_Callback[BSP_SPI_NUM][BSP_SPI_CB_NUM])(void); + +/* Private function -------------------------------------------------------- */ +static BSP_SPI_t SPI_Get(SPI_HandleTypeDef *hspi) { + if (hspi->Instance == SPI1) + return BSP_SPI_BMI088; + else + return BSP_SPI_ERR; +} + +void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[bsp_spi][BSP_SPI_TX_CPLT_CB]) { + SPI_Callback[bsp_spi][BSP_SPI_TX_CPLT_CB](); + } + } +} + +void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_CPLT_CB](); + } +} + +void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_CPLT_CB](); + } +} + +void HAL_SPI_TxHalfCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_HALF_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_HALF_CPLT_CB](); + } +} + +void HAL_SPI_RxHalfCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_HALF_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_HALF_CPLT_CB](); + } +} + +void HAL_SPI_TxRxHalfCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_HALF_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_HALF_CPLT_CB](); + } +} + +void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_ERROR_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_ERROR_CB](); + } +} + +void HAL_SPI_AbortCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_ABORT_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_ABORT_CPLT_CB](); + } +} + +/* Exported functions ------------------------------------------------------- */ +SPI_HandleTypeDef *BSP_SPI_GetHandle(BSP_SPI_t spi) { + switch (spi) { + case BSP_SPI_BMI088: + return &hspi1; + default: + return NULL; + } +} + +int8_t BSP_SPI_RegisterCallback(BSP_SPI_t spi, BSP_SPI_Callback_t type, + void (*callback)(void)) { + if (callback == NULL) return BSP_ERR_NULL; + SPI_Callback[spi][type] = callback; + return BSP_OK; +} + +int8_t BSP_SPI_Transmit(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi); + if (hspi == NULL) return BSP_ERR; + + if (dma) { + return HAL_SPI_Transmit_DMA(hspi, data, size)!= HAL_OK;; + } else { + return HAL_SPI_Transmit(hspi, data, size, 20)!= HAL_OK;; + } +} + +int8_t BSP_SPI_Receive(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi); + if (hspi == NULL) return BSP_ERR; + + if (dma) { + return HAL_SPI_Receive_DMA(hspi, data, size)!= HAL_OK;; + } else { + return HAL_SPI_Receive(hspi, data, size, 20)!= HAL_OK;; + } +} + +int8_t BSP_SPI_TransmitReceive(BSP_SPI_t spi, uint8_t *txData, uint8_t *rxData, + uint16_t size, bool dma) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi); + if (hspi == NULL) return BSP_ERR; + + if (dma) { + return HAL_SPI_TransmitReceive_DMA(hspi, txData, rxData, size)!= HAL_OK;; + } else { + return HAL_SPI_TransmitReceive(hspi, txData, rxData, size, 20)!= HAL_OK;; + } +} + +uint8_t BSP_SPI_MemReadByte(BSP_SPI_t spi, uint8_t reg) { + if (spi >= BSP_SPI_NUM) return 0xFF; + uint8_t tmp[2] = {reg | 0x80, 0x00}; + BSP_SPI_TransmitReceive(spi, tmp, tmp, 2u, true); + return tmp[1]; +} + +int8_t BSP_SPI_MemWriteByte(BSP_SPI_t spi, uint8_t reg, uint8_t data) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + uint8_t tmp[2] = {reg & 0x7f, data}; + return BSP_SPI_Transmit(spi, tmp, 2u, true); +} + +int8_t BSP_SPI_MemRead(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + if (data == NULL || size == 0) return BSP_ERR_NULL; + reg = reg | 0x80; + BSP_SPI_Transmit(spi, ®, 1u, true); + return BSP_SPI_Receive(spi, data, size, true); +} + +int8_t BSP_SPI_MemWrite(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + if (data == NULL || size == 0) return BSP_ERR_NULL; + reg = reg & 0x7f; + BSP_SPI_Transmit(spi, ®, 1u, true); + return BSP_SPI_Transmit(spi, data, size, true); +} + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/User/bsp/spi.h b/User/bsp/spi.h new file mode 100644 index 0000000..2abb0c6 --- /dev/null +++ b/User/bsp/spi.h @@ -0,0 +1,70 @@ +#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 ----------------------------------------------------------- */ + +/* 要添加使用SPI的新设备，需要先在此添加对应的枚举值 */ + +/* SPI实体枚举，与设备对应 */ +typedef enum { + BSP_SPI_BMI088, + BSP_SPI_NUM, + BSP_SPI_ERR, +} BSP_SPI_t; + +/* SPI支持的中断回调函数类型，具体参考HAL中定义 */ +typedef enum { + BSP_SPI_TX_CPLT_CB, + BSP_SPI_RX_CPLT_CB, + BSP_SPI_TX_RX_CPLT_CB, + BSP_SPI_TX_HALF_CPLT_CB, + BSP_SPI_RX_HALF_CPLT_CB, + BSP_SPI_TX_RX_HALF_CPLT_CB, + BSP_SPI_ERROR_CB, + BSP_SPI_ABORT_CPLT_CB, + BSP_SPI_CB_NUM, +} BSP_SPI_Callback_t; + +/* Exported functions prototypes -------------------------------------------- */ +SPI_HandleTypeDef *BSP_SPI_GetHandle(BSP_SPI_t spi); +int8_t BSP_SPI_RegisterCallback(BSP_SPI_t spi, BSP_SPI_Callback_t type, + void (*callback)(void)); + + +int8_t BSP_SPI_Transmit(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma); +int8_t BSP_SPI_Receive(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma); +int8_t BSP_SPI_TransmitReceive(BSP_SPI_t spi, uint8_t *txData, uint8_t *rxData, + uint16_t size, bool dma); + +uint8_t BSP_SPI_MemReadByte(BSP_SPI_t spi, uint8_t reg); +int8_t BSP_SPI_MemWriteByte(BSP_SPI_t spi, uint8_t reg, uint8_t data); +int8_t BSP_SPI_MemRead(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size); +int8_t BSP_SPI_MemWrite(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/component/component_config.yaml b/User/component/component_config.yaml index 4caf761..0d07966 100644 --- a/User/component/component_config.yaml +++ b/User/component/component_config.yaml @@ -2,6 +2,14 @@ ahrs: dependencies: - component/user_math.h enabled: true +filter: + dependencies: + - component/ahrs + enabled: true +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/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/device/bmi088.c b/User/device/bmi088.c new file mode 100644 index 0000000..0f4c154 --- /dev/null +++ b/User/device/bmi088.c @@ -0,0 +1,381 @@ +/* + BMI088 陀螺仪+加速度计传感器。 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "bmi088.h" + +#include +#include +#include +#include + +#include "bsp/time.h" +#include "bsp/gpio.h" +#include "bsp/spi.h" +#include "component/user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +#define BMI088_REG_ACCL_CHIP_ID (0x00) +#define BMI088_REG_ACCL_ERR (0x02) +#define BMI088_REG_ACCL_STATUS (0x03) +#define BMI088_REG_ACCL_X_LSB (0x12) +#define BMI088_REG_ACCL_X_MSB (0x13) +#define BMI088_REG_ACCL_Y_LSB (0x14) +#define BMI088_REG_ACCL_Y_MSB (0x15) +#define BMI088_REG_ACCL_Z_LSB (0x16) +#define BMI088_REG_ACCL_Z_MSB (0x17) +#define BMI088_REG_ACCL_SENSORTIME_0 (0x18) +#define BMI088_REG_ACCL_SENSORTIME_1 (0x19) +#define BMI088_REG_ACCL_SENSORTIME_2 (0x1A) +#define BMI088_REG_ACCL_INT_STAT_1 (0x1D) +#define BMI088_REG_ACCL_TEMP_MSB (0x22) +#define BMI088_REG_ACCL_TEMP_LSB (0x23) +#define BMI088_REG_ACCL_CONF (0x40) +#define BMI088_REG_ACCL_RANGE (0x41) +#define BMI088_REG_ACCL_INT1_IO_CONF (0x53) +#define BMI088_REG_ACCL_INT2_IO_CONF (0x54) +#define BMI088_REG_ACCL_INT1_INT2_MAP_DATA (0x58) +#define BMI088_REG_ACCL_SELF_TEST (0x6D) +#define BMI088_REG_ACCL_PWR_CONF (0x7C) +#define BMI088_REG_ACCL_PWR_CTRL (0x7D) +#define BMI088_REG_ACCL_SOFTRESET (0x7E) + +#define BMI088_REG_GYRO_CHIP_ID (0x00) +#define BMI088_REG_GYRO_X_LSB (0x02) +#define BMI088_REG_GYRO_X_MSB (0x03) +#define BMI088_REG_GYRO_Y_LSB (0x04) +#define BMI088_REG_GYRO_Y_MSB (0x05) +#define BMI088_REG_GYRO_Z_LSB (0x06) +#define BMI088_REG_GYRO_Z_MSB (0x07) +#define BMI088_REG_GYRO_INT_STAT_1 (0x0A) +#define BMI088_REG_GYRO_RANGE (0x0F) +#define BMI088_REG_GYRO_BANDWIDTH (0x10) +#define BMI088_REG_GYRO_LPM1 (0x11) +#define BMI088_REG_GYRO_SOFTRESET (0x14) +#define BMI088_REG_GYRO_INT_CTRL (0x15) +#define BMI088_REG_GYRO_INT3_INT4_IO_CONF (0x16) +#define BMI088_REG_GYRO_INT3_INT4_IO_MAP (0x18) +#define BMI088_REG_GYRO_SELF_TEST (0x3C) + +#define BMI088_CHIP_ID_ACCL (0x1E) +#define BMI088_CHIP_ID_GYRO (0x0F) + +#define BMI088_LEN_RX_BUFF (19) +/* Private macro ------------------------------------------------------------ */ +#define BMI088_ACCL_NSS_SET() \ + BSP_GPIO_WritePin(BSP_GPIO_ACCL_CS, GPIO_PIN_SET) +#define BMI088_ACCL_NSS_RESET() \ + BSP_GPIO_WritePin(BSP_GPIO_ACCL_CS, GPIO_PIN_RESET) + +#define BMI088_GYRO_NSS_SET() \ + BSP_GPIO_WritePin(BSP_GPIO_GYRO_CS, GPIO_PIN_SET) +#define BMI088_GYRO_NSS_RESET() \ + BSP_GPIO_WritePin(BSP_GPIO_GYRO_CS, GPIO_PIN_RESET) + +/* Private typedef ---------------------------------------------------------- */ +typedef enum { + BMI_ACCL, + BMI_GYRO, +} BMI_Device_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static uint8_t buffer[2]; +static uint8_t bmi088_rxbuf[BMI088_LEN_RX_BUFF]; + +static osThreadId_t thread_alert; +static bool inited = false; + +/* Private function -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +static void BMI_WriteSingle(BMI_Device_t dv, uint8_t reg, uint8_t data) { + buffer[0] = (reg & 0x7f); + buffer[1] = data; + + BSP_TIME_Delay(1); + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_RESET(); + break; + + case BMI_GYRO: + BMI088_GYRO_NSS_RESET(); + break; + } + + BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 2u, false); + + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_SET(); + break; + + case BMI_GYRO: + BMI088_GYRO_NSS_SET(); + break; + } +} + +static uint8_t BMI_ReadSingle(BMI_Device_t dv, uint8_t reg) { + BSP_TIME_Delay(1); + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_RESET(); + break; + + case BMI_GYRO: + BMI088_GYRO_NSS_RESET(); + break; + } + buffer[0] = (uint8_t)(reg | 0x80); + BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 1u, false); + BSP_SPI_Receive(BSP_SPI_BMI088, buffer, 2u, false); + + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_SET(); + return buffer[1]; + + case BMI_GYRO: + BMI088_GYRO_NSS_SET(); + return buffer[0]; + } +} + +static void BMI_Read(BMI_Device_t dv, uint8_t reg, uint8_t *data, uint8_t len) { + if (data == NULL) return; + + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_RESET(); + break; + + case BMI_GYRO: + BMI088_GYRO_NSS_RESET(); + break; + } + buffer[0] = (uint8_t)(reg | 0x80); + BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 1u, false); + BSP_SPI_Receive(BSP_SPI_BMI088, data, len, true); +} + +static void BMI088_RxCpltCallback(void) { + if (BSP_GPIO_ReadPin(BSP_GPIO_ACCL_CS) == GPIO_PIN_RESET) { + BMI088_ACCL_NSS_SET(); + osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_RAW_REDY); + } + if (BSP_GPIO_ReadPin(BSP_GPIO_GYRO_CS) == GPIO_PIN_RESET) { + BMI088_GYRO_NSS_SET(); + osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_RAW_REDY); + } +} + +static void BMI088_AcclIntCallback(void) { + osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_NEW_DATA); +} + +static void BMI088_GyroIntCallback(void) { + osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_NEW_DATA); +} + +/* Exported functions ------------------------------------------------------- */ +int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali) { + if (bmi088 == NULL) return DEVICE_ERR_NULL; + if (cali == NULL) return DEVICE_ERR_NULL; + if (inited) return DEVICE_ERR_INITED; + if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL; + + bmi088->cali = cali; + + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_SOFTRESET, 0xB6); + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_SOFTRESET, 0xB6); + BSP_TIME_Delay(30); + + /* Switch accl to SPI mode. */ + BMI_ReadSingle(BMI_ACCL, BMI088_CHIP_ID_ACCL); + + if (BMI_ReadSingle(BMI_ACCL, BMI088_REG_ACCL_CHIP_ID) != BMI088_CHIP_ID_ACCL) + return DEVICE_ERR_NO_DEV; + + if (BMI_ReadSingle(BMI_GYRO, BMI088_REG_GYRO_CHIP_ID) != BMI088_CHIP_ID_GYRO) + return DEVICE_ERR_NO_DEV; + + BSP_GPIO_DisableIRQ(BSP_GPIO_ACCL_INT); + BSP_GPIO_DisableIRQ(BSP_GPIO_GYRO_INT); + + BSP_SPI_RegisterCallback(BSP_SPI_BMI088, BSP_SPI_RX_CPLT_CB, + BMI088_RxCpltCallback); + BSP_GPIO_RegisterCallback(BSP_GPIO_ACCL_INT, BMI088_AcclIntCallback); + BSP_GPIO_RegisterCallback(BSP_GPIO_GYRO_INT, BMI088_GyroIntCallback); + + /* Accl init. */ + /* Filter setting: Normal. */ + /* ODR: 0xAB: 800Hz. 0xAA: 400Hz. 0xA9: 200Hz. 0xA8: 100Hz. 0xA6: 25Hz. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_CONF, 0xAA); + + /* 0x00: +-3G. 0x01: +-6G. 0x02: +-12G. 0x03: +-24G. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_RANGE, 0x01); + + /* INT1 as output. Push-pull. Active low. Output. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_IO_CONF, 0x08); + + /* Map data ready interrupt to INT1. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_INT2_MAP_DATA, 0x04); + + /* Turn on accl. Now we can read data. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_PWR_CTRL, 0x04); + BSP_TIME_Delay(50); + + /* Gyro init. */ + /* 0x00: +-2000. 0x01: +-1000. 0x02: +-500. 0x03: +-250. 0x04: +-125. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_RANGE, 0x01); + + /* Filter bw: 47Hz. */ + /* ODR: 0x02: 1000Hz. 0x03: 400Hz. 0x06: 200Hz. 0x07: 100Hz. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_BANDWIDTH, 0x03); + + /* INT3 and INT4 as output. Push-pull. Active low. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_CONF, 0x00); + + /* Map data ready interrupt to INT3. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_MAP, 0x01); + + /* Enable new data interrupt. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT_CTRL, 0x80); + + BSP_TIME_Delay(10); + + inited = true; + + BSP_GPIO_EnableIRQ(BSP_GPIO_ACCL_INT); + BSP_GPIO_EnableIRQ(BSP_GPIO_GYRO_INT); + return DEVICE_OK; +} + +bool BMI088_GyroStable(AHRS_Gyro_t *gyro) { + return ((gyro->x < 0.03f) && (gyro->y < 0.03f) && (gyro->z < 0.03f)); +} + +uint32_t BMI088_WaitNew() { + return osThreadFlagsWait( + SIGNAL_BMI088_ACCL_NEW_DATA | SIGNAL_BMI088_GYRO_NEW_DATA, osFlagsWaitAll, + osWaitForever); +} + +int8_t BMI088_AcclStartDmaRecv() { + BMI_Read(BMI_ACCL, BMI088_REG_ACCL_X_LSB, bmi088_rxbuf, BMI088_LEN_RX_BUFF); + return DEVICE_OK; +} + +uint32_t BMI088_AcclWaitDmaCplt() { + return osThreadFlagsWait(SIGNAL_BMI088_ACCL_RAW_REDY, osFlagsWaitAll, + osWaitForever); +} + +int8_t BMI088_GyroStartDmaRecv() { + BMI_Read(BMI_GYRO, BMI088_REG_GYRO_X_LSB, bmi088_rxbuf + 7, 6u); + return DEVICE_OK; +} + +uint32_t BMI088_GyroWaitDmaCplt() { + return osThreadFlagsWait(SIGNAL_BMI088_GYRO_RAW_REDY, osFlagsWaitAll, + osWaitForever); +} + +int8_t BMI088_ParseAccl(BMI088_t *bmi088) { + if (bmi088 == NULL) return DEVICE_ERR_NULL; + +#if 1 + int16_t raw_x, raw_y, raw_z; + memcpy(&raw_x, bmi088_rxbuf + 1, sizeof(raw_x)); + memcpy(&raw_y, bmi088_rxbuf + 3, sizeof(raw_y)); + memcpy(&raw_z, bmi088_rxbuf + 5, sizeof(raw_z)); + + bmi088->accl.x = (float)raw_x; + bmi088->accl.y = (float)raw_y; + bmi088->accl.z = (float)raw_z; + +#else + const int16_t *praw_x = (int16_t *)(bmi088_rxbuf + 1); + const int16_t *praw_y = (int16_t *)(bmi088_rxbuf + 3); + const int16_t *praw_z = (int16_t *)(bmi088_rxbuf + 5); + + bmi088->accl.x = (float)*praw_x; + bmi088->accl.y = (float)*praw_y; + bmi088->accl.z = (float)*praw_z; + +#endif + + /* 3G: 10920. 6G: 5460. 12G: 2730. 24G: 1365. */ + bmi088->accl.x /= 5460.0f; + bmi088->accl.y /= 5460.0f; + bmi088->accl.z /= 5460.0f; + + int16_t raw_temp = + (uint16_t)((bmi088_rxbuf[17] << 3) | (bmi088_rxbuf[18] >> 5)); + + if (raw_temp > 1023) raw_temp -= 2048; + + bmi088->temp = (float)raw_temp * 0.125f + 23.0f; + + return DEVICE_OK; +} + +int8_t BMI088_ParseGyro(BMI088_t *bmi088) { + if (bmi088 == NULL) return DEVICE_ERR_NULL; + +#if 1 + /* Gyroscope imu_raw -> degrees/sec -> radians/sec */ + int16_t raw_x, raw_y, raw_z; + memcpy(&raw_x, bmi088_rxbuf + 7, sizeof(raw_x)); + memcpy(&raw_y, bmi088_rxbuf + 9, sizeof(raw_y)); + memcpy(&raw_z, bmi088_rxbuf + 11, sizeof(raw_z)); + + bmi088->gyro.x = (float)raw_x; + bmi088->gyro.y = (float)raw_y; + bmi088->gyro.z = (float)raw_z; + +#else + /* Gyroscope imu_raw -> degrees/sec -> radians/sec */ + const int16_t *raw_x = (int16_t *)(bmi088_rxbuf + 7); + const int16_t *raw_y = (int16_t *)(bmi088_rxbuf + 9); + const int16_t *raw_z = (int16_t *)(bmi088_rxbuf + 11); + + bmi088->gyro.x = (float)*raw_x; + bmi088->gyro.y = (float)*raw_y; + bmi088->gyro.z = (float)*raw_z; +#endif + + /* FS125: 262.144. FS250: 131.072. FS500: 65.536. FS1000: 32.768. + * FS2000: 16.384.*/ + bmi088->gyro.x /= 32.768f; + bmi088->gyro.y /= 32.768f; + bmi088->gyro.z /= 32.768f; + + bmi088->gyro.x *= M_DEG2RAD_MULT; + bmi088->gyro.y *= M_DEG2RAD_MULT; + bmi088->gyro.z *= M_DEG2RAD_MULT; + + bmi088->gyro.x -= bmi088->cali->gyro_offset.x; + bmi088->gyro.y -= bmi088->cali->gyro_offset.y; + bmi088->gyro.z -= bmi088->cali->gyro_offset.z; + + return DEVICE_ERR_NULL; +} + +float BMI088_GetUpdateFreq(BMI088_t *bmi088) { + (void)bmi088; + return 400.0f; +} diff --git a/User/device/bmi088.h b/User/device/bmi088.h new file mode 100644 index 0000000..eb44e0c --- /dev/null +++ b/User/device/bmi088.h @@ -0,0 +1,81 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include + +#include "component/ahrs.h" +#include "device/device.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef struct { + struct { + float x; + float y; + float z; + } gyro_offset; /* 陀螺仪偏置 */ +} BMI088_Cali_t; /* BMI088校准数据 */ + +typedef struct { + DEVICE_Header_t header; + AHRS_Accl_t accl; + AHRS_Gyro_t gyro; + + float temp; /* 温度 */ + + const BMI088_Cali_t *cali; +} BMI088_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Exported functions prototypes -------------------------------------------- */ +int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali); +int8_t BMI088_Restart(void); + +bool BMI088_GyroStable(AHRS_Gyro_t *gyro); + +/* Sensor use right-handed coordinate system. */ +/* + x < R(logo) + y + UP is z + All implementation should follow this rule. + */ +uint32_t BMI088_WaitNew(); + +/* + BMI088的Accl和Gyro共用同一个DMA通道，所以一次只能读一个传感器。 + 即BMI088_AcclStartDmaRecv() 和 BMI088_AcclWaitDmaCplt() 中间不能 + 出现 BMI088_GyroStartDmaRecv()。 +*/ +int8_t BMI088_AcclStartDmaRecv(); +uint32_t BMI088_AcclWaitDmaCplt(); +int8_t BMI088_GyroStartDmaRecv(); +uint32_t BMI088_GyroWaitDmaCplt(); +int8_t BMI088_ParseAccl(BMI088_t *bmi088); +int8_t BMI088_ParseGyro(BMI088_t *bmi088); +float BMI088_GetUpdateFreq(BMI088_t *bmi088); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/device/device.h b/User/device/device.h index b8acfe3..7a3f17d 100644 --- a/User/device/device.h +++ b/User/device/device.h @@ -22,7 +22,10 @@ extern "C" { #define DEVICE_ERR_NO_DEV (-4) /* AUTO GENERATED SIGNALS BEGIN */ -/* No signals defined */ +#define SIGNAL_BMI088_ACCL_RAW_REDY (1u << 0) +#define SIGNAL_BMI088_GYRO_RAW_REDY (1u << 1) +#define SIGNAL_BMI088_ACCL_NEW_DATA (1u << 2) +#define SIGNAL_BMI088_GYRO_NEW_DATA (1u << 3) /* AUTO GENERATED SIGNALS END */ /* USER SIGNALS BEGIN */ diff --git a/User/device/device_config.yaml b/User/device/device_config.yaml index 299aa61..095c9e6 100644 --- a/User/device/device_config.yaml +++ b/User/device/device_config.yaml @@ -1,3 +1,11 @@ +bmi088: + bsp_config: + BSP_GPIO_ACCL_CS: BSP_GPIO_ACCL_CS + BSP_GPIO_ACCL_INT: BSP_GPIO_ACCL_INT + BSP_GPIO_GYRO_CS: BSP_GPIO_GYRO_CS + BSP_GPIO_GYRO_INT: BSP_GPIO_GYRO_INT + BSP_SPI_BMI088: BSP_SPI_BMI088 + enabled: true dm_imu: bsp_config: {} enabled: true diff --git a/User/device/dm_imu.c b/User/device/dm_imu.c deleted file mode 100644 index 064abdf..0000000 --- a/User/device/dm_imu.c +++ /dev/null @@ -1,271 +0,0 @@ -/* - DM_IMU数据获取（CAN） -*/ - -/* Includes ----------------------------------------------------------------- */ -#include "dm_imu.h" - -#include "bsp/can.h" -#include "bsp/time.h" -#include "component/user_math.h" -#include - -/* Private define ----------------------------------------------------------- */ -#define DM_IMU_OFFLINE_TIMEOUT 1000 // 设备离线判定时间1000ms - -#define ACCEL_CAN_MAX (58.8f) -#define ACCEL_CAN_MIN (-58.8f) -#define GYRO_CAN_MAX (34.88f) -#define GYRO_CAN_MIN (-34.88f) -#define PITCH_CAN_MAX (90.0f) -#define PITCH_CAN_MIN (-90.0f) -#define ROLL_CAN_MAX (180.0f) -#define ROLL_CAN_MIN (-180.0f) -#define YAW_CAN_MAX (180.0f) -#define YAW_CAN_MIN (-180.0f) -#define TEMP_MIN (0.0f) -#define TEMP_MAX (60.0f) -#define Quaternion_MIN (-1.0f) -#define Quaternion_MAX (1.0f) - - -/* Private macro ------------------------------------------------------------ */ -/* Private typedef ---------------------------------------------------------- */ -/* Private variables -------------------------------------------------------- */ -/* Private function --------------------------------------------------------- */ - -static uint8_t count = 0; // 计数器，用于判断设备是否离线 -/** - * @brief: 无符号整数转换为浮点数函数 - */ -static float uint_to_float(int x_int, float x_min, float x_max, int bits) -{ - float span = x_max - x_min; - float offset = x_min; - return ((float)x_int)*span/((float)((1<data.temp = (float)temp; - imu->data.accl.x = uint_to_float(acc_x_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); - imu->data.accl.y = uint_to_float(acc_y_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); - imu->data.accl.z = uint_to_float(acc_z_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); - return DEVICE_OK; -} -/** - * @brief 解析陀螺仪数据 - */ -static int8_t DM_IMU_ParseGyroData(DM_IMU_t *imu, uint8_t *data, uint8_t len) { - if (imu == NULL || data == NULL || len < 8) { - return DEVICE_ERR; - } - uint16_t gyro_x_raw = (data[3] << 8) | data[2]; - uint16_t gyro_y_raw = (data[5] << 8) | data[4]; - uint16_t gyro_z_raw = (data[7] << 8) | data[6]; - imu->data.gyro.x = uint_to_float(gyro_x_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); - imu->data.gyro.y = uint_to_float(gyro_y_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); - imu->data.gyro.z = uint_to_float(gyro_z_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); - return DEVICE_OK; -} -/** - * @brief 解析欧拉角数据 - */ -static int8_t DM_IMU_ParseEulerData(DM_IMU_t *imu, uint8_t *data, uint8_t len) { - if (imu == NULL || data == NULL || len < 8) { - return DEVICE_ERR; - } - uint16_t pit_raw = (data[3] << 8) | data[2]; - uint16_t yaw_raw = (data[5] << 8) | data[4]; - uint16_t rol_raw = (data[7] << 8) | data[6]; - imu->data.euler.pit = uint_to_float(pit_raw, PITCH_CAN_MIN, PITCH_CAN_MAX, 16) * M_DEG2RAD_MULT; - imu->data.euler.yaw = uint_to_float(yaw_raw, YAW_CAN_MIN, YAW_CAN_MAX, 16) * M_DEG2RAD_MULT; - imu->data.euler.rol = uint_to_float(rol_raw, ROLL_CAN_MIN, ROLL_CAN_MAX, 16) * M_DEG2RAD_MULT; - return DEVICE_OK; -} - -/** - * @brief 解析四元数数据 - */ -static int8_t DM_IMU_ParseQuaternionData(DM_IMU_t *imu, uint8_t *data, uint8_t len) { - if (imu == NULL || data == NULL || len < 8) { - return DEVICE_ERR; - } - int w = (data[1] << 6) | ((data[2] & 0xF8) >> 2); - int x = ((data[2] & 0x03) << 12) | (data[3] << 4) | ((data[4] & 0xF0) >> 4); - int y = ((data[4] & 0x0F) << 10) | (data[5] << 2) | ((data[6] & 0xC0) >> 6); - int z = ((data[6] & 0x3F) << 8) | data[7]; - imu->data.quat.q0 = uint_to_float(w, Quaternion_MIN, Quaternion_MAX, 14); - imu->data.quat.q1 = uint_to_float(x, Quaternion_MIN, Quaternion_MAX, 14); - imu->data.quat.q2 = uint_to_float(y, Quaternion_MIN, Quaternion_MAX, 14); - imu->data.quat.q3 = uint_to_float(z, Quaternion_MIN, Quaternion_MAX, 14); - return DEVICE_OK; -} - - -/* Exported functions ------------------------------------------------------- */ - -/** - * @brief 初始化DM IMU设备 - */ -int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param) { - if (imu == NULL || param == NULL) { - return DEVICE_ERR_NULL; - } - - // 初始化设备头部 - imu->header.online = false; - imu->header.last_online_time = 0; - - // 配置参数 - imu->param.can = param->can; - imu->param.can_id = param->can_id; - imu->param.device_id = param->device_id; - imu->param.master_id = param->master_id; - - // 清零数据 - memset(&imu->data, 0, sizeof(DM_IMU_Data_t)); - - // 注册CAN接收队列，用于接收回复报文 - int8_t result = BSP_CAN_RegisterId(imu->param.can, imu->param.master_id, 10); - if (result != BSP_OK) { - return DEVICE_ERR; - } - - return DEVICE_OK; -} - -/** - * @brief 请求IMU数据 - */ -int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) { - if (imu == NULL) { - return DEVICE_ERR_NULL; - } - - // 构造发送数据：id_L, id_H(DM_IMU_ID), RID, 0xcc - uint8_t tx_data[4] = { - imu->param.device_id & 0xFF, // id_L - (imu->param.device_id >> 8) & 0xFF, // id_H - (uint8_t)rid, // RID - 0xCC // 固定值 - }; - - // 发送标准数据帧 - BSP_CAN_StdDataFrame_t frame = { - .id = imu->param.can_id, - .dlc = 4, - }; - memcpy(frame.data, tx_data, 4); - int8_t result = BSP_CAN_TransmitStdDataFrame(imu->param.can, &frame); - return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR; -} - -/** - * @brief 更新IMU数据（从CAN中获取所有数据并解析） - */ -int8_t DM_IMU_Update(DM_IMU_t *imu) { - if (imu == NULL) { - return DEVICE_ERR_NULL; - } - - BSP_CAN_Message_t msg; - int8_t result; - bool data_received = false; - - // 持续接收所有可用消息 - while ((result = BSP_CAN_GetMessage(imu->param.can, imu->param.master_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE)) == BSP_OK) { - // 验证回复数据格式（至少检查数据长度） - if (msg.dlc < 3) { - continue; // 跳过无效消息 - } - - // 根据数据位的第0位确定反馈报文类型 - uint8_t rid = msg.data[0] & 0x0F; // 取第0位的低4位作为RID - - // 根据RID类型解析数据 - int8_t parse_result = DEVICE_ERR; - switch (rid) { - case 0x01: // RID_ACCL - parse_result = DM_IMU_ParseAccelData(imu, msg.data, msg.dlc); - break; - case 0x02: // RID_GYRO - parse_result = DM_IMU_ParseGyroData(imu, msg.data, msg.dlc); - break; - case 0x03: // RID_EULER - parse_result = DM_IMU_ParseEulerData(imu, msg.data, msg.dlc); - break; - case 0x04: // RID_QUATERNION - parse_result = DM_IMU_ParseQuaternionData(imu, msg.data, msg.dlc); - break; - default: - continue; // 跳过未知类型的消息 - } - - // 如果解析成功，标记为收到数据 - if (parse_result == DEVICE_OK) { - data_received = true; - } - } - - // 如果收到任何有效数据，更新设备状态 - if (data_received) { - imu->header.online = true; - imu->header.last_online_time = BSP_TIME_Get_ms(); - return DEVICE_OK; - } - - return DEVICE_ERR; -} - -/** - * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz） - */ -int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu){ - if (imu == NULL) { - return DEVICE_ERR_NULL; - } - switch (count) { - case 0: - DM_IMU_Request(imu, RID_ACCL); - break; - case 1: - DM_IMU_Request(imu, RID_GYRO); - break; - case 2: - DM_IMU_Request(imu, RID_EULER); - break; - case 3: - DM_IMU_Request(imu, RID_QUATERNION); - DM_IMU_Update(imu); // 更新所有数据 - break; - } - count++; - if (count >= 4) { - count = 0; // 重置计数器 - return DEVICE_OK; - } - return DEVICE_ERR; // 未完成一轮更新 -} - -/** - * @brief 检查设备是否在线 - */ -bool DM_IMU_IsOnline(DM_IMU_t *imu) { - if (imu == NULL) { - return false; - } - - uint32_t current_time = BSP_TIME_Get_ms(); - return imu->header.online && - (current_time - imu->header.last_online_time < DM_IMU_OFFLINE_TIMEOUT); -} diff --git a/User/device/dm_imu.h b/User/device/dm_imu.h deleted file mode 100644 index 3965980..0000000 --- a/User/device/dm_imu.h +++ /dev/null @@ -1,90 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" { -#endif - -/* Includes ----------------------------------------------------------------- */ -#include "device/device.h" -#include "component/ahrs.h" -#include "bsp/can.h" -/* Exported constants ------------------------------------------------------- */ - -#define DM_IMU_CAN_ID_DEFAULT 0x6FF -#define DM_IMU_ID_DEFAULT 0x42 -#define DM_IMU_MST_ID_DEFAULT 0x43 - -/* Exported macro ----------------------------------------------------------- */ -/* Exported types ----------------------------------------------------------- */ - -typedef struct { - BSP_CAN_t can; // CAN总线句柄 - uint16_t can_id; // CAN通信ID - uint8_t device_id; // 设备ID - uint8_t master_id; // 主机ID -} DM_IMU_Param_t; -typedef enum { - RID_ACCL = 0x01, // 加速度计 - RID_GYRO = 0x02, // 陀螺仪 - RID_EULER = 0x03, // 欧拉角 - RID_QUATERNION = 0x04, // 四元数 -} DM_IMU_RID_t; - -typedef struct { - AHRS_Accl_t accl; // 加速度计 - AHRS_Gyro_t gyro; // 陀螺仪 - AHRS_Eulr_t euler; // 欧拉角 - AHRS_Quaternion_t quat; // 四元数 - float temp; // 温度 -} DM_IMU_Data_t; - -typedef struct { - DEVICE_Header_t header; - DM_IMU_Param_t param; // IMU参数配置 - DM_IMU_Data_t data; // IMU数据 -} DM_IMU_t; - -/* Exported functions prototypes -------------------------------------------- */ - -/** - * @brief 初始化DM IMU设备 - * @param imu DM IMU设备结构体指针 - * @param param IMU参数配置指针 - * @return DEVICE_OK 成功，其他值失败 - */ -int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param); - -/** - * @brief 请求IMU数据 - * @param imu DM IMU设备结构体指针 - * @param rid 请求的数据类型 - * @return DEVICE_OK 成功，其他值失败 - */ -int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid); - - -/** - * @brief 更新IMU数据（从CAN中获取所有数据并解析） - * @param imu DM IMU设备结构体指针 - * @return DEVICE_OK 成功，其他值失败 - */ -int8_t DM_IMU_Update(DM_IMU_t *imu); - -/** - * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz，运行4次才有完整数据） - * @param imu DM IMU设备结构体指针 - * @return DEVICE_OK 成功，其他值失败 - */ -int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu); - -/** - * @brief 检查设备是否在线 - * @param imu DM IMU设备结构体指针 - * @return true 在线，false 离线 - */ -bool DM_IMU_IsOnline(DM_IMU_t *imu); - - -#ifdef __cplusplus -} -#endif diff --git a/User/module/config.c b/User/module/config.c index 8bb0270..825e910 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -16,13 +16,6 @@ // 机器人参数配置 Config_RobotParam_t robot_config = { - .imu_param = - { - .can = BSP_CAN_2, - .can_id = 0x6FF, - .device_id = 0x42, - .master_id = 0x43, - }, .joint_param = { { diff --git a/User/module/config.h b/User/module/config.h index 0fc9560..d3fbbe7 100644 --- a/User/module/config.h +++ b/User/module/config.h @@ -9,11 +9,9 @@ extern "C" { #endif #include -#include "device/dm_imu.h" #include "device/motor_lz.h" #include "device/motor_lk.h" typedef struct { - DM_IMU_Param_t imu_param; MOTOR_LZ_Param_t joint_param[4]; MOTOR_LK_Param_t wheel_param[2]; } Config_RobotParam_t; diff --git a/User/task/config.yaml b/User/task/config.yaml index a8274e6..66735ad 100644 --- a/User/task/config.yaml +++ b/User/task/config.yaml @@ -8,7 +8,7 @@ - delay: 0 description: '' freq_control: true - frequency: 1000.0 + frequency: 500.0 function: Task_imu name: imu stack: 256 @@ -16,6 +16,6 @@ description: '' freq_control: true frequency: 500.0 - function: Task_ctrl_lz - name: ctrl_lz + function: Task_ctrl_chassis + name: ctrl_chassis stack: 256 diff --git a/User/task/ctrl_chassis.c b/User/task/ctrl_chassis.c new file mode 100644 index 0000000..8f0ba13 --- /dev/null +++ b/User/task/ctrl_chassis.c @@ -0,0 +1,44 @@ +/* + ctrl_chassis Task + +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* 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 */ + + /* USER CODE INIT END */ + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* USER CODE BEGIN */ + + /* USER CODE END */ + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ + } + +} \ No newline at end of file diff --git a/User/task/ctrl_lz.c b/User/task/ctrl_lz.c deleted file mode 100644 index 500da8e..0000000 --- a/User/task/ctrl_lz.c +++ /dev/null @@ -1,134 +0,0 @@ -/* - ctrl_lz Task - -*/ - -/* Includes ----------------------------------------------------------------- */ -#include "task/user_task.h" -/* USER INCLUDE BEGIN */ -#include "bsp/can.h" -#include "device/motor_lz.h" -#include "device/motor_lk.h" -#include "module/config.h" -#include -/* USER INCLUDE END */ - -/* Private typedef ---------------------------------------------------------- */ -/* Private define ----------------------------------------------------------- */ -/* Private macro ------------------------------------------------------------ */ -/* Private variables -------------------------------------------------------- */ -/* USER STRUCT BEGIN */ -static bool command_received = false; -/* USER STRUCT END */ - -/* Private function --------------------------------------------------------- */ -/* Exported functions ------------------------------------------------------- */ -void Task_ctrl_lz(void *argument) { - (void)argument; /* 未使用argument，消除警告 */ - - - /* 计算任务运行到指定频率需要等待的tick数 */ - const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_LZ_FREQ; - - osDelay(CTRL_LZ_INIT_DELAY); /* 延时一段时间再开启任务 */ - - uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ - /* USER CODE INIT BEGIN */ - BSP_CAN_Init(); - - MOTOR_LZ_Init(); - - for (int i = 0; i < 4; i++) { - MOTOR_LZ_Register(&Config_GetRobotParam()->joint_param[i]); - } - - // 注册CAN接收ID - BSP_CAN_RegisterId(BSP_CAN_1, 121, 0); // 使能命令 - BSP_CAN_RegisterId(BSP_CAN_1, 122, 0); // 力矩控制命令 - - for (int i = 0; i < 4; i++) { - MOTOR_LZ_Enable(&Config_GetRobotParam()->joint_param[i]); - } - /* USER CODE INIT END */ - - while (1) { - tick += delay_tick; /* 计算下一个唤醒时刻 */ - /* USER CODE BEGIN */ - MOTOR_LZ_UpdateAll(); - - // 检查CAN接收消息 - BSP_CAN_Message_t rx_msg; - command_received = false; // 重置命令接收标志 - - // 检查ID 121 - 使能4个电机 - if (BSP_CAN_GetMessage(BSP_CAN_1, 121, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { - command_received = true; - for (int i = 0; i < 4; i++) { - MOTOR_LZ_Enable(&Config_GetRobotParam()->joint_param[i]); - } - } - - // 检查ID 122 - 运控模式控制4个电机 - if (BSP_CAN_GetMessage(BSP_CAN_1, 122, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { - command_received = true; - // 8字节数据分别是4个电机的力矩 (每个电机2字节，有符号整数，精度0.01 Nm) - for (int i = 0; i < 4; i++) { - int16_t torque_raw; - memcpy(&torque_raw, &rx_msg.data[i * 2], sizeof(int16_t)); - float torque = (float)torque_raw / 100.0f; // 转换为浮点数力矩值 - - // 使用运控模式控制电机，只设置力矩，其他参数为0 - MOTOR_LZ_MotionParam_t motion_param = { - .target_angle = 0.0f, - .target_velocity = 0.0f, - .kp = 0.0f, - .kd = 0.0f, - .torque = torque - }; - MOTOR_LZ_MotionControl(&Config_GetRobotParam()->joint_param[i], &motion_param); - } - } - - // 如果没有收到任何控制命令，电机进入relax模式 - if (!command_received) { - for (int i = 0; i < 4; i++) { - MOTOR_LZ_Relax(&Config_GetRobotParam()->joint_param[i]); - } - } - - // 发送4个电机的反馈数据，ID分别为124、125、126、127 - for (int i = 0; i < 4; i++) { - MOTOR_LZ_t* motor = MOTOR_LZ_GetMotor(&Config_GetRobotParam()->joint_param[i]); - if (motor != NULL) { - BSP_CAN_StdDataFrame_t motor_frame = { - .id = 124 + i, // ID: 124, 125, 126, 127 - .dlc = 8, - .data = {0} - }; - - // 数据重构：转矩电流(2字节) + 位置(3字节) + 速度(3字节) = 8字节 - // 转矩电流 - 转换为16位整数 (精度0.01 Nm) - int16_t torque_int = (int16_t)(motor->lz_feedback.current_torque * 100); - memcpy(&motor_frame.data[0], &torque_int, sizeof(int16_t)); - - // 位置 - 转换为24位整数，使用3字节 (精度0.0001 rad) - int32_t angle_int = (int32_t)(motor->lz_feedback.current_angle * 10000) & 0xFFFFFF; - motor_frame.data[2] = (angle_int >> 16) & 0xFF; - motor_frame.data[3] = (angle_int >> 8) & 0xFF; - motor_frame.data[4] = angle_int & 0xFF; - - // 速度 - 转换为24位整数，使用3字节 (精度0.001 rad/s) - int32_t velocity_int = (int32_t)(motor->lz_feedback.current_velocity * 1000) & 0xFFFFFF; - motor_frame.data[5] = (velocity_int >> 16) & 0xFF; - motor_frame.data[6] = (velocity_int >> 8) & 0xFF; - motor_frame.data[7] = velocity_int & 0xFF; - - BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &motor_frame); - } - } - - /* USER CODE END */ - osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ - } - -} \ No newline at end of file diff --git a/User/task/imu.c b/User/task/imu.c index 1d782fe..6d6f714 100644 --- a/User/task/imu.c +++ b/User/task/imu.c @@ -1,15 +1,18 @@ /* imu Task - + */ /* Includes ----------------------------------------------------------------- */ #include "task/user_task.h" /* USER INCLUDE BEGIN */ -#include "bsp/can.h" -#include "device/dm_imu.h" -#include "module/config.h" -#include +#include "bsp/mm.h" +#include "bsp/pwm.h" +#include "bsp/gpio.h" +#include "component/ahrs.h" +#include "component/pid.h" +#include "device/bmi088.h" +#include "task/user_task.h" /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -17,15 +20,61 @@ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ -DM_IMU_t dm_imu; -int i = 0; +BMI088_t bmi088; + +AHRS_t gimbal_ahrs; +AHRS_Magn_t magn; +AHRS_Eulr_t eulr_to_send; + +KPID_t imu_temp_ctrl_pid; + +BMI088_Cali_t cali_bmi088= { + .gyro_offset = {0.0f,0.0f,0.0f}, +}; + +static const KPID_Params_t imu_temp_ctrl_pid_param = { + .k = 0.2f, + .p = 1.0f, + .i = 0.01f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, +}; + +/* 校准相关变量 */ +typedef enum { + CALIB_IDLE, // 空闲状态 + CALIB_RUNNING, // 正在校准 + CALIB_DONE // 校准完成 +} CalibState_t; + +static CalibState_t calib_state = CALIB_IDLE; +static uint16_t calib_count = 0; +static struct { + float x_sum; + float y_sum; + float z_sum; +} gyro_sum = {0.00341676874f, 0.000505680335f, -0.00134522165f}; + +#define CALIB_SAMPLES 5000 // 校准采样数量 /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ +/* 按钮回调函数：开始陀螺仪校准 */ +static void start_gyro_calibration(void) { + if (calib_state == CALIB_IDLE) { + calib_state = CALIB_RUNNING; + calib_count = 0; + gyro_sum.x_sum = 0.0f; + gyro_sum.y_sum = 0.0f; + gyro_sum.z_sum = 0.0f; + } +} /* Exported functions ------------------------------------------------------- */ void Task_imu(void *argument) { (void)argument; /* 未使用argument，消除警告 */ + /* 计算任务运行到指定频率需要等待的tick数 */ const uint32_t delay_tick = osKernelGetTickFreq() / IMU_FREQ; @@ -33,105 +82,73 @@ void Task_imu(void *argument) { uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ - BSP_CAN_Init(); - DM_IMU_Init(&dm_imu, &Config_GetRobotParam()->imu_param); + BMI088_Init(&bmi088,&cali_bmi088); + AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088)); + PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D, + 1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param); + BSP_PWM_Start(BSP_PWM_IMU_HEAT_PWM); + + /* 注册按钮回调函数并启用中断 */ + BSP_GPIO_RegisterCallback(BSP_GPIO_USER_KEY, start_gyro_calibration); + BSP_GPIO_EnableIRQ(BSP_GPIO_USER_KEY); /* USER CODE INIT END */ - + while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ - i++; - /* USER CODE BEGIN */ - DM_IMU_AutoUpdateAll(&dm_imu); - switch (i) { - case 0: { - // 发送加速度计数据 (ID: 0x66) - 三轴压缩到一帧，每轴2字节，精度0.01g - BSP_CAN_StdDataFrame_t accl_frame = {.id = 150, .dlc = 8, .data = {0}}; + BMI088_WaitNew(); + BMI088_AcclStartDmaRecv(); + BMI088_AcclWaitDmaCplt(); - // 转换为16位整数发送 (精度0.01g，范围±327.67g) - int16_t accl_x_int = (int16_t)(dm_imu.data.accl.x * 100.0f); - int16_t accl_y_int = (int16_t)(dm_imu.data.accl.y * 100.0f); - int16_t accl_z_int = (int16_t)(dm_imu.data.accl.z * 100.0f); + BMI088_GyroStartDmaRecv(); + BMI088_GyroWaitDmaCplt(); - // 打包数据：x(2字节) + y(2字节) + z(2字节) + 2字节保留 - memcpy(&accl_frame.data[0], &accl_x_int, 2); - memcpy(&accl_frame.data[2], &accl_y_int, 2); - memcpy(&accl_frame.data[4], &accl_z_int, 2); + /* 锁住RTOS内核防止数据解析过程中断，造成错误 */ + osKernelLock(); + /* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */ + BMI088_ParseAccl(&bmi088); + BMI088_ParseGyro(&bmi088); + // IST8310_Parse(&ist8310); - BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &accl_frame); + /* 陀螺仪校准处理 */ + if (calib_state == CALIB_RUNNING) { + /* 累加陀螺仪数据用于计算零偏 */ + gyro_sum.x_sum += bmi088.gyro.x; + gyro_sum.y_sum += bmi088.gyro.y; + gyro_sum.z_sum += bmi088.gyro.z; + calib_count++; + /* 达到采样数量后计算平均值并更新零偏 */ + if (calib_count >= CALIB_SAMPLES) { + /* 计算平均值作为零偏 */ + cali_bmi088.gyro_offset.x = gyro_sum.x_sum / CALIB_SAMPLES; + cali_bmi088.gyro_offset.y = gyro_sum.y_sum / CALIB_SAMPLES; + cali_bmi088.gyro_offset.z = gyro_sum.z_sum / CALIB_SAMPLES; + + /* 校准完成，重置为空闲状态以便下次校准 */ + calib_state = CALIB_IDLE; + + /* 重新初始化BMI088以应用新的校准数据 */ + BMI088_Init(&bmi088, &cali_bmi088); + AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088)); + } } - break; - case 1: { - // 发送陀螺仪数据 (ID: 0x67) - 三轴压缩到一帧，每轴2字节，精度0.01°/s - BSP_CAN_StdDataFrame_t gyro_frame = {.id = 151, .dlc = 8, .data = {0}}; + /* 只有在非校准状态下才进行正常的姿态解析 */ + if (calib_state != CALIB_RUNNING) { + /* 根据设备接收到的数据进行姿态解析 */ + AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn); - // 转换为16位整数发送 (精度0.01°/s，范围±327.67°/s) - int16_t gyro_x_int = (int16_t)(dm_imu.data.gyro.x * 57.2958f * - 100.0f); // 弧度/s转角度/s*100 - int16_t gyro_y_int = (int16_t)(dm_imu.data.gyro.y * 57.2958f * 100.0f); - int16_t gyro_z_int = (int16_t)(dm_imu.data.gyro.z * 57.2958f * 100.0f); - - // 打包数据：x(2字节) + y(2字节) + z(2字节) + 2字节保留 - memcpy(&gyro_frame.data[0], &gyro_x_int, 2); - memcpy(&gyro_frame.data[2], &gyro_y_int, 2); - memcpy(&gyro_frame.data[4], &gyro_z_int, 2); - - BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &gyro_frame); - - break; + /* 根据解析出来的四元数计算欧拉角 */ + AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs); } + + osKernelUnlock(); - case 2: { - // 发送欧拉角数据 (ID: 0x68) - 三轴压缩到一帧，每轴2字节，精度0.01度 - BSP_CAN_StdDataFrame_t euler_frame = {.id = 152, .dlc = 8, .data = {0}}; - - // 转换为16位整数发送 (精度0.01度，范围±327.67度) - int16_t yaw_int = (int16_t)(dm_imu.data.euler.yaw * 57.2958f * - 100.0f); // 弧度转角度*100 - int16_t pit_int = (int16_t)(dm_imu.data.euler.pit * 57.2958f * - 100.0f); // 弧度转角度*100 - int16_t rol_int = (int16_t)(dm_imu.data.euler.rol * 57.2958f * - 100.0f); // 弧度转角度*100 - - // 打包数据：yaw(2字节) + pitch(2字节) + roll(2字节) + 2字节保留 - memcpy(&euler_frame.data[0], &yaw_int, 2); - memcpy(&euler_frame.data[2], &pit_int, 2); - memcpy(&euler_frame.data[4], &rol_int, 2); - - BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &euler_frame); - - } - - break; - case 3: { - // 发送四元数数据 (ID: 0x69) - 四分量压缩到一帧，每分量2字节，精度0.0001 - BSP_CAN_StdDataFrame_t quat_frame = {.id = 153, .dlc = 8, .data = {0}}; - - // 转换为16位整数发送 (精度0.0001，范围±3.2767) - int16_t q0_int = (int16_t)(dm_imu.data.quat.q0 * 10000.0f); - int16_t q1_int = (int16_t)(dm_imu.data.quat.q1 * 10000.0f); - int16_t q2_int = (int16_t)(dm_imu.data.quat.q2 * 10000.0f); - int16_t q3_int = (int16_t)(dm_imu.data.quat.q3 * 10000.0f); - - // 打包数据：q0(2字节) + q1(2字节) + q2(2字节) + q3(2字节) - memcpy(&quat_frame.data[0], &q0_int, 2); - memcpy(&quat_frame.data[2], &q1_int, 2); - memcpy(&quat_frame.data[4], &q2_int, 2); - memcpy(&quat_frame.data[6], &q3_int, 2); - - BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &quat_frame); - } - - i = 0; - break; - default: - i = 0; - break; - } + BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 50.0f, bmi088.temp, 0.0f, 0.0f)); /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } -} \ No newline at end of file + +} diff --git a/User/task/init.c b/User/task/init.c index e261f86..cca7ac9 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -32,7 +32,7 @@ void Task_Init(void *argument) { /* 创建任务线程 */ task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink); task_runtime.thread.imu = osThreadNew(Task_imu, NULL, &attr_imu); - task_runtime.thread.ctrl_lz = osThreadNew(Task_ctrl_lz, NULL, &attr_ctrl_lz); + task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis); // 创建消息队列 /* USER MESSAGE BEGIN */ diff --git a/User/task/user_task.c b/User/task/user_task.c index 5dde688..764f790 100644 --- a/User/task/user_task.c +++ b/User/task/user_task.c @@ -19,8 +19,8 @@ const osThreadAttr_t attr_imu = { .priority = osPriorityNormal, .stack_size = 256 * 4, }; -const osThreadAttr_t attr_ctrl_lz = { - .name = "ctrl_lz", +const osThreadAttr_t attr_ctrl_chassis = { + .name = "ctrl_chassis", .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 index 95c4bc8..69d6e18 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -14,14 +14,14 @@ extern "C" { /* Exported constants ------------------------------------------------------- */ /* 任务运行频率 */ #define BLINK_FREQ (500.0) -#define IMU_FREQ (1000.0) -#define CTRL_LZ_FREQ (500.0) +#define IMU_FREQ (500.0) +#define CTRL_CHASSIS_FREQ (500.0) /* 任务初始化延时ms */ #define TASK_INIT_DELAY (100u) #define BLINK_INIT_DELAY (0) #define IMU_INIT_DELAY (0) -#define CTRL_LZ_INIT_DELAY (0) +#define CTRL_CHASSIS_INIT_DELAY (0) /* Exported defines --------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */ @@ -33,7 +33,7 @@ typedef struct { struct { osThreadId_t blink; osThreadId_t imu; - osThreadId_t ctrl_lz; + osThreadId_t ctrl_chassis; } thread; /* USER MESSAGE BEGIN */ @@ -57,21 +57,21 @@ typedef struct { struct { UBaseType_t blink; UBaseType_t imu; - UBaseType_t ctrl_lz; + UBaseType_t ctrl_chassis; } stack_water_mark; /* 各任务运行频率 */ struct { float blink; float imu; - float ctrl_lz; + float ctrl_chassis; } freq; /* 任务最近运行时间 */ struct { float blink; float imu; - float ctrl_lz; + float ctrl_chassis; } last_up_time; } Task_Runtime_t; @@ -83,13 +83,13 @@ extern Task_Runtime_t task_runtime; extern const osThreadAttr_t attr_init; extern const osThreadAttr_t attr_blink; extern const osThreadAttr_t attr_imu; -extern const osThreadAttr_t attr_ctrl_lz; +extern const osThreadAttr_t attr_ctrl_chassis; /* 任务函数声明 */ void Task_Init(void *argument); void Task_blink(void *argument); void Task_imu(void *argument); -void Task_ctrl_lz(void *argument); +void Task_ctrl_chassis(void *argument); #ifdef __cplusplus } From e4b165569861bb3a383be2ce46f39fb03d1c7b0f Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 5 Oct 2025 03:25:18 +0800 Subject: [PATCH 12/25] =?UTF-8?q?imu=E5=AE=8C=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 1 + User/task/atti_esti.c | 162 +++++++++++++++++++++++++++++++++++ User/task/config.yaml | 7 ++ User/task/imu.c | 193 ++++++++++++++++++------------------------ User/task/init.c | 7 +- User/task/user_task.c | 5 ++ User/task/user_task.h | 14 +++ 7 files changed, 279 insertions(+), 110 deletions(-) create mode 100644 User/task/atti_esti.c diff --git a/CMakeLists.txt b/CMakeLists.txt index 4a4a3fe..b18dc79 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,6 +69,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE User/module/config.c # User/task sources + User/task/atti_esti.c User/task/blink.c User/task/ctrl_chassis.c User/task/imu.c diff --git a/User/task/atti_esti.c b/User/task/atti_esti.c new file mode 100644 index 0000000..c472415 --- /dev/null +++ b/User/task/atti_esti.c @@ -0,0 +1,162 @@ +/* + atti_esti Task + +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "cmsis_os2.h" +#include "task/user_task.h" +/* USER INCLUDE BEGIN */ +#include "bsp/mm.h" +#include "bsp/pwm.h" +#include "bsp/gpio.h" +#include "component/ahrs.h" +#include "component/pid.h" +#include "device/bmi088.h" +#include "task/user_task.h" +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ +BMI088_t bmi088; + +AHRS_t gimbal_ahrs; +AHRS_Magn_t magn; +AHRS_Eulr_t eulr_to_send; + +KPID_t imu_temp_ctrl_pid; + +BMI088_Cali_t cali_bmi088= { + .gyro_offset = {0.00379243772f,0.00133061118f,-0.00154866849f}, +}; + +static const KPID_Params_t imu_temp_ctrl_pid_param = { + .k = 0.2f, + .p = 1.0f, + .i = 0.01f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, +}; + +/* 校准相关变量 */ +typedef enum { + CALIB_IDLE, // 空闲状态 + CALIB_RUNNING, // 正在校准 + CALIB_DONE // 校准完成 +} CalibState_t; + +static CalibState_t calib_state = CALIB_IDLE; +static uint16_t calib_count = 0; +static struct { + float x_sum; + float y_sum; + float z_sum; +} gyro_sum = {0.00341676874f, 0.000505680335f, -0.00134522165f}; +static void start_gyro_calibration(void) { + if (calib_state == CALIB_IDLE) { + calib_state = CALIB_RUNNING; + calib_count = 0; + gyro_sum.x_sum = 0.0f; + gyro_sum.y_sum = 0.0f; + gyro_sum.z_sum = 0.0f; + } +} +#define CALIB_SAMPLES 5000 // 校准采样数量 +/* USER STRUCT END */ + +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +void Task_atti_esti(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + + + /* 计算任务运行到指定频率需要等待的tick数 */ + const uint32_t delay_tick = osKernelGetTickFreq() / ATTI_ESTI_FREQ; + + osDelay(ATTI_ESTI_INIT_DELAY); /* 延时一段时间再开启任务 */ + + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ + /* USER CODE INIT BEGIN */ + BMI088_Init(&bmi088,&cali_bmi088); + AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088)); + PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D, + 1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param); + BSP_PWM_Start(BSP_PWM_IMU_HEAT_PWM); + + /* 注册按钮回调函数并启用中断 */ + BSP_GPIO_RegisterCallback(BSP_GPIO_USER_KEY, start_gyro_calibration); + BSP_GPIO_EnableIRQ(BSP_GPIO_USER_KEY); + /* USER CODE INIT END */ + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* USER CODE BEGIN */ + BMI088_WaitNew(); + BMI088_AcclStartDmaRecv(); + BMI088_AcclWaitDmaCplt(); + + BMI088_GyroStartDmaRecv(); + BMI088_GyroWaitDmaCplt(); + + /* 锁住RTOS内核防止数据解析过程中断，造成错误 */ + osKernelLock(); + /* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */ + BMI088_ParseAccl(&bmi088); + BMI088_ParseGyro(&bmi088); + // IST8310_Parse(&ist8310); + + /* 陀螺仪校准处理 */ + if (calib_state == CALIB_RUNNING) { + /* 累加陀螺仪数据用于计算零偏 */ + gyro_sum.x_sum += bmi088.gyro.x; + gyro_sum.y_sum += bmi088.gyro.y; + gyro_sum.z_sum += bmi088.gyro.z; + calib_count++; + + /* 达到采样数量后计算平均值并更新零偏 */ + if (calib_count >= CALIB_SAMPLES) { + /* 计算平均值作为零偏 */ + cali_bmi088.gyro_offset.x = gyro_sum.x_sum / CALIB_SAMPLES; + cali_bmi088.gyro_offset.y = gyro_sum.y_sum / CALIB_SAMPLES; + cali_bmi088.gyro_offset.z = gyro_sum.z_sum / CALIB_SAMPLES; + + /* 校准完成，重置为空闲状态以便下次校准 */ + calib_state = CALIB_IDLE; + + /* 重新初始化BMI088以应用新的校准数据 */ + BMI088_Init(&bmi088, &cali_bmi088); + AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088)); + } + } + + /* 只有在非校准状态下才进行正常的姿态解析 */ + if (calib_state != CALIB_RUNNING) { + /* 根据设备接收到的数据进行姿态解析 */ + AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn); + + /* 根据解析出来的四元数计算欧拉角 */ + AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs); + } + + osKernelUnlock(); + + osMessageQueueReset(task_runtime.msgq.imu.accl); + osMessageQueueReset(task_runtime.msgq.imu.gyro); + osMessageQueueReset(task_runtime.msgq.imu.eulr); + osMessageQueueReset(task_runtime.msgq.imu.quat); + osMessageQueuePut(task_runtime.msgq.imu.accl, &bmi088.accl, 0, 0); + osMessageQueuePut(task_runtime.msgq.imu.gyro, &bmi088.gyro, 0, 0); + osMessageQueuePut(task_runtime.msgq.imu.eulr, &eulr_to_send, 0, 0); + osMessageQueuePut(task_runtime.msgq.imu.quat, &gimbal_ahrs.quat, 0, 0); + + BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 50.0f, bmi088.temp, 0.0f, 0.0f)); + + /* USER CODE END */ + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ + } + +} \ No newline at end of file diff --git a/User/task/config.yaml b/User/task/config.yaml index 66735ad..e093a96 100644 --- a/User/task/config.yaml +++ b/User/task/config.yaml @@ -19,3 +19,10 @@ function: Task_ctrl_chassis name: ctrl_chassis stack: 256 +- delay: 0 + description: '' + freq_control: true + frequency: 500.0 + function: Task_atti_esti + name: atti_esti + stack: 256 diff --git a/User/task/imu.c b/User/task/imu.c index 6d6f714..b09b5ef 100644 --- a/User/task/imu.c +++ b/User/task/imu.c @@ -6,70 +6,30 @@ /* Includes ----------------------------------------------------------------- */ #include "task/user_task.h" /* USER INCLUDE BEGIN */ -#include "bsp/mm.h" -#include "bsp/pwm.h" -#include "bsp/gpio.h" +#include "bsp/can.h" +#include "bsp/time.h" #include "component/ahrs.h" -#include "component/pid.h" -#include "device/bmi088.h" -#include "task/user_task.h" +#include +#include /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */ +/* AHRS数据CAN ID定义 */ +#define CAN_ID_AHRS_ACCL 0x301 /* 加速度计数据 */ +#define CAN_ID_AHRS_GYRO 0x302 /* 陀螺仪数据 */ +#define CAN_ID_AHRS_EULR 0x303 /* 欧拉角数据 */ +#define CAN_ID_AHRS_QUAT 0x304 /* 四元数数据 */ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ -BMI088_t bmi088; - -AHRS_t gimbal_ahrs; -AHRS_Magn_t magn; -AHRS_Eulr_t eulr_to_send; - -KPID_t imu_temp_ctrl_pid; - -BMI088_Cali_t cali_bmi088= { - .gyro_offset = {0.0f,0.0f,0.0f}, -}; - -static const KPID_Params_t imu_temp_ctrl_pid_param = { - .k = 0.2f, - .p = 1.0f, - .i = 0.01f, - .d = 0.0f, - .i_limit = 1.0f, - .out_limit = 1.0f, -}; - -/* 校准相关变量 */ -typedef enum { - CALIB_IDLE, // 空闲状态 - CALIB_RUNNING, // 正在校准 - CALIB_DONE // 校准完成 -} CalibState_t; - -static CalibState_t calib_state = CALIB_IDLE; -static uint16_t calib_count = 0; -static struct { - float x_sum; - float y_sum; - float z_sum; -} gyro_sum = {0.00341676874f, 0.000505680335f, -0.00134522165f}; - -#define CALIB_SAMPLES 5000 // 校准采样数量 +AHRS_Accl_t accl; +AHRS_Gyro_t gyro; +AHRS_Eulr_t eulr; +AHRS_Quaternion_t quat; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ -/* 按钮回调函数：开始陀螺仪校准 */ -static void start_gyro_calibration(void) { - if (calib_state == CALIB_IDLE) { - calib_state = CALIB_RUNNING; - calib_count = 0; - gyro_sum.x_sum = 0.0f; - gyro_sum.y_sum = 0.0f; - gyro_sum.z_sum = 0.0f; - } -} /* Exported functions ------------------------------------------------------- */ void Task_imu(void *argument) { (void)argument; /* 未使用argument，消除警告 */ @@ -82,73 +42,88 @@ void Task_imu(void *argument) { uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ - BMI088_Init(&bmi088,&cali_bmi088); - AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088)); - PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D, - 1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param); - BSP_PWM_Start(BSP_PWM_IMU_HEAT_PWM); - - /* 注册按钮回调函数并启用中断 */ - BSP_GPIO_RegisterCallback(BSP_GPIO_USER_KEY, start_gyro_calibration); - BSP_GPIO_EnableIRQ(BSP_GPIO_USER_KEY); + BSP_CAN_Init(); /* USER CODE INIT END */ while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ - BMI088_WaitNew(); - BMI088_AcclStartDmaRecv(); - BMI088_AcclWaitDmaCplt(); - - BMI088_GyroStartDmaRecv(); - BMI088_GyroWaitDmaCplt(); - - /* 锁住RTOS内核防止数据解析过程中断，造成错误 */ - osKernelLock(); - /* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */ - BMI088_ParseAccl(&bmi088); - BMI088_ParseGyro(&bmi088); - // IST8310_Parse(&ist8310); - - /* 陀螺仪校准处理 */ - if (calib_state == CALIB_RUNNING) { - /* 累加陀螺仪数据用于计算零偏 */ - gyro_sum.x_sum += bmi088.gyro.x; - gyro_sum.y_sum += bmi088.gyro.y; - gyro_sum.z_sum += bmi088.gyro.z; - calib_count++; - - /* 达到采样数量后计算平均值并更新零偏 */ - if (calib_count >= CALIB_SAMPLES) { - /* 计算平均值作为零偏 */ - cali_bmi088.gyro_offset.x = gyro_sum.x_sum / CALIB_SAMPLES; - cali_bmi088.gyro_offset.y = gyro_sum.y_sum / CALIB_SAMPLES; - cali_bmi088.gyro_offset.z = gyro_sum.z_sum / CALIB_SAMPLES; + /* 获取加速度计数据并通过CAN发送 - 压缩为16位整数 */ + if (osMessageQueueGet(task_runtime.msgq.imu.accl, &accl, NULL, 0) == osOK) { + BSP_CAN_StdDataFrame_t accl_frame; + accl_frame.id = CAN_ID_AHRS_ACCL; + accl_frame.dlc = 6; /* 3个int16_t，每个2字节，共6字节 */ - /* 校准完成，重置为空闲状态以便下次校准 */ - calib_state = CALIB_IDLE; + /* 将float转换为int16_t (乘以100，加速度一般在±20g范围内) */ + int16_t x_int = (int16_t)(accl.x * 100); + int16_t y_int = (int16_t)(accl.y * 100); + int16_t z_int = (int16_t)(accl.z * 100); - /* 重新初始化BMI088以应用新的校准数据 */ - BMI088_Init(&bmi088, &cali_bmi088); - AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088)); - } - } - - /* 只有在非校准状态下才进行正常的姿态解析 */ - if (calib_state != CALIB_RUNNING) { - /* 根据设备接收到的数据进行姿态解析 */ - AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn); - - /* 根据解析出来的四元数计算欧拉角 */ - AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs); + memcpy(&accl_frame.data[0], &x_int, sizeof(int16_t)); + memcpy(&accl_frame.data[2], &y_int, sizeof(int16_t)); + memcpy(&accl_frame.data[4], &z_int, sizeof(int16_t)); + + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &accl_frame); } - osKernelUnlock(); - - BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 50.0f, bmi088.temp, 0.0f, 0.0f)); + /* 获取陀螺仪数据并通过CAN发送 - 压缩为16位整数 */ + if (osMessageQueueGet(task_runtime.msgq.imu.gyro, &gyro, NULL, 0) == osOK) { + BSP_CAN_StdDataFrame_t gyro_frame; + gyro_frame.id = CAN_ID_AHRS_GYRO; + gyro_frame.dlc = 6; /* 3个int16_t，每个2字节，共6字节 */ + + /* 将float转换为int16_t (乘以10，陀螺仪一般在±2000°/s范围内) */ + int16_t x_int = (int16_t)(gyro.x * 10); + int16_t y_int = (int16_t)(gyro.y * 10); + int16_t z_int = (int16_t)(gyro.z * 10); + + memcpy(&gyro_frame.data[0], &x_int, sizeof(int16_t)); + memcpy(&gyro_frame.data[2], &y_int, sizeof(int16_t)); + memcpy(&gyro_frame.data[4], &z_int, sizeof(int16_t)); + + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &gyro_frame); + } + + /* 获取欧拉角数据并通过CAN发送 - 压缩为16位整数 */ + if (osMessageQueueGet(task_runtime.msgq.imu.eulr, &eulr, NULL, 0) == osOK) { + BSP_CAN_StdDataFrame_t eulr_frame; + eulr_frame.id = CAN_ID_AHRS_EULR; + eulr_frame.dlc = 6; /* 3个int16_t，每个2字节，共6字节 */ + + /* 将角度转换为int16_t (乘以100，角度范围-180~180°，精度0.01°) */ + int16_t yaw_int = (int16_t)(eulr.yaw * 100); + int16_t pit_int = (int16_t)(eulr.pit * 100); + int16_t rol_int = (int16_t)(eulr.rol * 100); + + memcpy(&eulr_frame.data[0], &yaw_int, sizeof(int16_t)); + memcpy(&eulr_frame.data[2], &pit_int, sizeof(int16_t)); + memcpy(&eulr_frame.data[4], &rol_int, sizeof(int16_t)); + + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &eulr_frame); + } + + /* 获取四元数数据并通过CAN发送 - 压缩为16位整数 */ + if (osMessageQueueGet(task_runtime.msgq.imu.quat, &quat, NULL, 0) == osOK) { + BSP_CAN_StdDataFrame_t quat_frame; + quat_frame.id = CAN_ID_AHRS_QUAT; + quat_frame.dlc = 8; /* 4个int16_t，每个2字节，共8字节 */ + + /* 将四元数转换为int16_t (乘以32000，四元数范围-1~1，充分利用int16_t范围) */ + int16_t q0_int = (int16_t)(quat.q0 * 32000); + int16_t q1_int = (int16_t)(quat.q1 * 32000); + int16_t q2_int = (int16_t)(quat.q2 * 32000); + int16_t q3_int = (int16_t)(quat.q3 * 32000); + + memcpy(&quat_frame.data[0], &q0_int, sizeof(int16_t)); + memcpy(&quat_frame.data[2], &q1_int, sizeof(int16_t)); + memcpy(&quat_frame.data[4], &q2_int, sizeof(int16_t)); + memcpy(&quat_frame.data[6], &q3_int, sizeof(int16_t)); + + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &quat_frame); + } /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } -} +} \ No newline at end of file diff --git a/User/task/init.c b/User/task/init.c index cca7ac9..7e2eb5e 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -7,7 +7,7 @@ #include "task/user_task.h" /* USER INCLUDE BEGIN */ - +#include "component/ahrs.h" /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -33,10 +33,15 @@ void Task_Init(void *argument) { task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink); task_runtime.thread.imu = osThreadNew(Task_imu, NULL, &attr_imu); task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis); + task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti); // 创建消息队列 /* USER MESSAGE BEGIN */ task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL); + task_runtime.msgq.imu.accl = osMessageQueueNew(2u, sizeof(AHRS_Accl_t), NULL); + task_runtime.msgq.imu.gyro = osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL); + task_runtime.msgq.imu.eulr = osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL); + task_runtime.msgq.imu.quat = osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL); /* USER MESSAGE END */ osKernelUnlock(); // 解锁内核 diff --git a/User/task/user_task.c b/User/task/user_task.c index 764f790..62d7ab7 100644 --- a/User/task/user_task.c +++ b/User/task/user_task.c @@ -23,4 +23,9 @@ const osThreadAttr_t attr_ctrl_chassis = { .name = "ctrl_chassis", .priority = osPriorityNormal, .stack_size = 256 * 4, +}; +const osThreadAttr_t attr_atti_esti = { + .name = "atti_esti", + .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 index 69d6e18..4ad3a9c 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -16,12 +16,14 @@ extern "C" { #define BLINK_FREQ (500.0) #define IMU_FREQ (500.0) #define CTRL_CHASSIS_FREQ (500.0) +#define ATTI_ESTI_FREQ (500.0) /* 任务初始化延时ms */ #define TASK_INIT_DELAY (100u) #define BLINK_INIT_DELAY (0) #define IMU_INIT_DELAY (0) #define CTRL_CHASSIS_INIT_DELAY (0) +#define ATTI_ESTI_INIT_DELAY (0) /* Exported defines --------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */ @@ -34,11 +36,18 @@ typedef struct { osThreadId_t blink; osThreadId_t imu; osThreadId_t ctrl_chassis; + osThreadId_t atti_esti; } thread; /* USER MESSAGE BEGIN */ struct { osMessageQueueId_t user_msg; /* 用户自定义任务消息队列 */ + struct { + osMessageQueueId_t accl; + osMessageQueueId_t gyro; + osMessageQueueId_t eulr; + osMessageQueueId_t quat; + }imu; } msgq; /* USER MESSAGE END */ @@ -58,6 +67,7 @@ typedef struct { UBaseType_t blink; UBaseType_t imu; UBaseType_t ctrl_chassis; + UBaseType_t atti_esti; } stack_water_mark; /* 各任务运行频率 */ @@ -65,6 +75,7 @@ typedef struct { float blink; float imu; float ctrl_chassis; + float atti_esti; } freq; /* 任务最近运行时间 */ @@ -72,6 +83,7 @@ typedef struct { float blink; float imu; float ctrl_chassis; + float atti_esti; } last_up_time; } Task_Runtime_t; @@ -84,12 +96,14 @@ extern const osThreadAttr_t attr_init; extern const osThreadAttr_t attr_blink; extern const osThreadAttr_t attr_imu; extern const osThreadAttr_t attr_ctrl_chassis; +extern const osThreadAttr_t attr_atti_esti; /* 任务函数声明 */ void Task_Init(void *argument); void Task_blink(void *argument); void Task_imu(void *argument); void Task_ctrl_chassis(void *argument); +void Task_atti_esti(void *argument); #ifdef __cplusplus } From 16d455869364a93c551c7e05c5024cc4c9d1899d Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 5 Oct 2025 11:15:25 +0800 Subject: [PATCH 13/25] =?UTF-8?q?=E9=81=93=E7=88=B7=EF=BD=9E=E6=88=91?= =?UTF-8?q?=E6=88=90=E4=BA=86=EF=BC=81=EF=BC=81=EF=BC=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 3 +- User/module/balance_chassis.c | 339 ++++++++++++++++++++++++++++++++++ User/module/balance_chassis.h | 61 ++++++ User/module/config.c | 25 ++- User/task/ctrl_chassis.c | 39 +++- balance_chassis_migration.md | 100 ++++++++++ 6 files changed, 558 insertions(+), 9 deletions(-) create mode 100644 User/module/balance_chassis.c create mode 100644 User/module/balance_chassis.h create mode 100644 balance_chassis_migration.md diff --git a/CMakeLists.txt b/CMakeLists.txt index b18dc79..1002f02 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -65,7 +65,8 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE User/device/motor_lk.c User/device/motor_lz.c - # User/module sources + # User/module + User/module/balance_chassis.c User/module/config.c # User/task sources diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c new file mode 100644 index 0000000..8ca3e2f --- /dev/null +++ b/User/module/balance_chassis.c @@ -0,0 +1,339 @@ +/* + * Balance Chassis Module + */ + +/* Includes ----------------------------------------------------------------- */ +#include "module/balance_chassis.h" +#include "bsp/can.h" +#include "component/user_math.h" +#include "module/config.h" +#include +#include + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +#define CAN_CMD_ENABLE_ID 121 // 使能命令ID +#define CAN_CMD_JOINT_ID 122 // 关节控制命令ID +#define CAN_CMD_WHEEL_LEFT_ID 128 // 左轮控制命令ID +#define CAN_CMD_WHEEL_RIGHT_ID 129 // 右轮控制命令ID + +#define CAN_FEEDBACK_JOINT_BASE_ID 124 // 关节反馈基础ID (124-127) +#define CAN_FEEDBACK_WHEEL_LEFT_ID 130 // 左轮反馈ID +#define CAN_FEEDBACK_WHEEL_RIGHT_ID 131 // 右轮反馈ID + +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +static bool joint_command_received = false; +static bool wheel_command_received[2] = {false, false}; + +/* Private function --------------------------------------------------------- */ + +/** + * @brief 检查并处理CAN控制命令 + * @param chassis 底盘实例 + * @return 成功返回DEVICE_OK + */ +static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { + BSP_CAN_Message_t rx_msg; + joint_command_received = false; + wheel_command_received[0] = false; + wheel_command_received[1] = false; + + // 检查ID 121 - 使能4个关节电机 + if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_ENABLE_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + joint_command_received = true; + for (int i = 0; i < 4; i++) { + MOTOR_LZ_Enable(&chassis->param.joint_param[i]); + } + } + + // 检查ID 122 - 运控模式控制4个关节电机 + if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_JOINT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + joint_command_received = true; + // 8字节数据分别是4个电机的力矩 (每个电机2字节，有符号整数，精度0.01 Nm) + for (int i = 0; i < 4; i++) { + int16_t torque_raw; + memcpy(&torque_raw, &rx_msg.data[i * 2], sizeof(int16_t)); + float torque = (float)torque_raw / 100.0f; // 转换为浮点数力矩值 + + // 使用运控模式控制电机，只设置力矩，其他参数为0 + MOTOR_LZ_MotionParam_t motion_param = { + .target_angle = 0.0f, + .target_velocity = 0.0f, + .kp = 0.0f, + .kd = 0.0f, + .torque = torque + }; + MOTOR_LZ_MotionControl(&chassis->param.joint_param[i], &motion_param); + } + } + + // 检查ID 128 - 左轮控制命令 (与电机发送格式一致) + if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_LEFT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + wheel_command_received[0] = true; + // 直接转发CAN数据到左轮电机 + BSP_CAN_StdDataFrame_t wheel_frame = { + .id = chassis->param.wheel_param[0].id, + .dlc = rx_msg.dlc, + }; + memcpy(wheel_frame.data, rx_msg.data, rx_msg.dlc); + BSP_CAN_TransmitStdDataFrame(chassis->param.wheel_param[0].can, &wheel_frame); + } + + // 检查ID 129 - 右轮控制命令 (与电机发送格式一致) + if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_RIGHT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + wheel_command_received[1] = true; + // 直接转发CAN数据到右轮电机 + BSP_CAN_StdDataFrame_t wheel_frame = { + .id = chassis->param.wheel_param[1].id, + .dlc = rx_msg.dlc, + }; + memcpy(wheel_frame.data, rx_msg.data, rx_msg.dlc); + BSP_CAN_TransmitStdDataFrame(chassis->param.wheel_param[1].can, &wheel_frame); + } + + return DEVICE_OK; +} + +/** + * @brief 发送关节电机反馈数据 + * @param chassis 底盘实例 + * @return 成功返回DEVICE_OK + */ +static int8_t Chassis_SendJointFeedback(Chassis_t *chassis) { + // 发送4个关节电机的反馈数据，ID分别为124、125、126、127 + for (int i = 0; i < 4; i++) { + MOTOR_LZ_t* motor = MOTOR_LZ_GetMotor(&chassis->param.joint_param[i]); + if (motor != NULL) { + BSP_CAN_StdDataFrame_t motor_frame = { + .id = CAN_FEEDBACK_JOINT_BASE_ID + i, // ID: 124, 125, 126, 127 + .dlc = 8, + .data = {0} + }; + + // 数据重构：转矩电流(2字节) + 位置(3字节) + 速度(3字节) = 8字节 + // 转矩电流 - 转换为16位整数 (精度0.01 Nm) + int16_t torque_int = (int16_t)(motor->lz_feedback.current_torque * 100); + memcpy(&motor_frame.data[0], &torque_int, sizeof(int16_t)); + + // 位置 - 转换为24位整数，使用3字节 (精度0.0001 rad) + int32_t angle_int = (int32_t)(motor->lz_feedback.current_angle * 10000) & 0xFFFFFF; + motor_frame.data[2] = (angle_int >> 16) & 0xFF; + motor_frame.data[3] = (angle_int >> 8) & 0xFF; + motor_frame.data[4] = angle_int & 0xFF; + + // 速度 - 转换为24位整数，使用3字节 (精度0.001 rad/s) + int32_t velocity_int = (int32_t)(motor->lz_feedback.current_velocity * 1000) & 0xFFFFFF; + motor_frame.data[5] = (velocity_int >> 16) & 0xFF; + motor_frame.data[6] = (velocity_int >> 8) & 0xFF; + motor_frame.data[7] = velocity_int & 0xFF; + + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &motor_frame); + } + } + + return DEVICE_OK; +} + +/** + * @brief 发送轮子电机反馈数据 + * @param chassis 底盘实例 + * @return 成功返回DEVICE_OK + */ +static int8_t Chassis_SendWheelFeedback(Chassis_t *chassis) { + // 发送2个轮子电机的反馈数据，ID分别为130、131 + for (int i = 0; i < 2; i++) { + MOTOR_LK_t* motor = MOTOR_LK_GetMotor(&chassis->param.wheel_param[i]); + if (motor != NULL) { + BSP_CAN_StdDataFrame_t wheel_frame = { + .id = CAN_FEEDBACK_WHEEL_LEFT_ID + i, // ID: 130, 131 + .dlc = 8, + .data = {0} + }; + + // 使用LK电机的标准反馈格式 + // 角度(2字节) + 速度(2字节) + 力矩电流(2字节) + 编码器(2字节) + + // 角度 - 转换为16位整数 (精度0.01度) + int16_t angle_int = (int16_t)(motor->motor.feedback.rotor_abs_angle * 180.0f / M_PI * 100); + wheel_frame.data[0] = (angle_int >> 8) & 0xFF; + wheel_frame.data[1] = angle_int & 0xFF; + + // 速度 - 转换为16位整数 (精度1dps) + int16_t speed_int = (int16_t)(motor->motor.feedback.rotor_speed); + wheel_frame.data[2] = (speed_int >> 8) & 0xFF; + wheel_frame.data[3] = speed_int & 0xFF; + + // 力矩电流 - 转换为16位整数 + int16_t current_int = (int16_t)(motor->motor.feedback.torque_current); + wheel_frame.data[4] = (current_int >> 8) & 0xFF; + wheel_frame.data[5] = current_int & 0xFF; + + // 编码器值 - 直接使用角度值作为编码器 + uint16_t encoder = (uint16_t)(motor->motor.feedback.rotor_abs_angle / (2 * M_PI) * 65535); + wheel_frame.data[6] = (encoder >> 8) & 0xFF; + wheel_frame.data[7] = encoder & 0xFF; + + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &wheel_frame); + } + } + + return DEVICE_OK; +} + +/* Exported functions ------------------------------------------------------- */ + +/** + * @brief 底盘初始化 + * @param chassis 底盘实例 + * @param param 底盘参数 + * @return 成功返回DEVICE_OK + */ +int8_t Chassis_Init(Chassis_t *chassis, Chassis_Param_t *param) { + if (chassis == NULL || param == NULL) { + return DEVICE_ERR_NULL; + } + + // 复制参数 + memcpy(&chassis->param, param, sizeof(Chassis_Param_t)); + + // 初始化CAN + BSP_CAN_Init(); + + // 初始化电机驱动 + MOTOR_LZ_Init(); + + // 注册关节电机 + for (int i = 0; i < 4; i++) { + if (MOTOR_LZ_Register(&chassis->param.joint_param[i]) != DEVICE_OK) { + return DEVICE_ERR; + } + } + + // 注册轮子电机 + for (int i = 0; i < 2; i++) { + if (MOTOR_LK_Register(&chassis->param.wheel_param[i]) != DEVICE_OK) { + return DEVICE_ERR; + } + } + + // 注册CAN接收ID + BSP_CAN_RegisterId(BSP_CAN_1, CAN_CMD_ENABLE_ID, 0); // 使能命令 + BSP_CAN_RegisterId(BSP_CAN_1, CAN_CMD_JOINT_ID, 0); // 关节控制命令 + BSP_CAN_RegisterId(BSP_CAN_1, CAN_CMD_WHEEL_LEFT_ID, 0); // 左轮控制命令 + BSP_CAN_RegisterId(BSP_CAN_1, CAN_CMD_WHEEL_RIGHT_ID, 0); // 右轮控制命令 + + return DEVICE_OK; +} + +/** + * @brief 更新底盘数据 + * @param chassis 底盘实例 + * @return 成功返回DEVICE_OK + */ +int8_t Chassis_Update(Chassis_t *chassis) { + if (chassis == NULL) { + return DEVICE_ERR_NULL; + } + + // 更新所有电机数据 + MOTOR_LZ_UpdateAll(); + MOTOR_LK_UpdateAll(); + + // 更新反馈数据 + for (int i = 0; i < 4; i++) { + MOTOR_LZ_t* joint_motor = MOTOR_LZ_GetMotor(&chassis->param.joint_param[i]); + if (joint_motor != NULL) { + chassis->data.joint[i] = joint_motor->motor.feedback; + } + } + + for (int i = 0; i < 2; i++) { + MOTOR_LK_t* wheel_motor = MOTOR_LK_GetMotor(&chassis->param.wheel_param[i]); + if (wheel_motor != NULL) { + chassis->data.wheel[i] = wheel_motor->motor.feedback; + } + } + + return DEVICE_OK; +} + +/** + * @brief 使能底盘 + * @param chassis 底盘实例 + * @return 成功返回DEVICE_OK + */ +int8_t Chassis_Enable(Chassis_t *chassis) { + if (chassis == NULL) { + return DEVICE_ERR_NULL; + } + + // 使能关节电机 + for (int i = 0; i < 4; i++) { + MOTOR_LZ_Enable(&chassis->param.joint_param[i]); + } + + // 开启轮子电机 + for (int i = 0; i < 2; i++) { + MOTOR_LK_MotorOn(&chassis->param.wheel_param[i]); + } + + return DEVICE_OK; +} + +/** + * @brief 底盘松弛 + * @param chassis 底盘实例 + * @return 成功返回DEVICE_OK + */ +int8_t Chassis_Relax(Chassis_t *chassis) { + if (chassis == NULL) { + return DEVICE_ERR_NULL; + } + + // 关节电机松弛 + for (int i = 0; i < 4; i++) { + MOTOR_LZ_Relax(&chassis->param.joint_param[i]); + } + + // 轮子电机松弛 + for (int i = 0; i < 2; i++) { + MOTOR_LK_Relax(&chassis->param.wheel_param[i]); + } + + return DEVICE_OK; +} + +/** + * @brief 底盘控制处理 + * @param chassis 底盘实例 + * @return 成功返回DEVICE_OK + */ +int8_t Chassis_Ctrl(Chassis_t *chassis) { + if (chassis == NULL) { + return DEVICE_ERR_NULL; + } + + // 处理CAN控制命令 + Chassis_ProcessCANCommands(chassis); + + // 如果没有收到关节控制命令，关节电机进入relax模式 + if (!joint_command_received) { + for (int i = 0; i < 4; i++) { + MOTOR_LZ_Relax(&chassis->param.joint_param[i]); + } + } + + // 如果没有收到轮子控制命令，轮子电机进入relax模式 + for (int i = 0; i < 2; i++) { + if (!wheel_command_received[i]) { + MOTOR_LK_Relax(&chassis->param.wheel_param[i]); + } + } + + // 发送反馈数据 + Chassis_SendJointFeedback(chassis); + Chassis_SendWheelFeedback(chassis); + + return DEVICE_OK; +} diff --git a/User/module/balance_chassis.h b/User/module/balance_chassis.h new file mode 100644 index 0000000..385f784 --- /dev/null +++ b/User/module/balance_chassis.h @@ -0,0 +1,61 @@ +#pragma once + +#include +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/motor.h" +#include "device/motor_lz.h" +#include "device/motor_lk.h" +#include "device/device.h" +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef struct { + MOTOR_LZ_Param_t joint_param[4]; // 4个电机参数 + MOTOR_LK_Param_t wheel_param[2]; // 2个轮子电机参数 +} Chassis_Param_t; + +typedef struct { + MOTOR_Feedback_t joint[4]; // 4个电机反馈数据 + MOTOR_Feedback_t wheel[2]; // 2个轮子电机反馈数据 +} Chassis_Feedback_t; + +typedef struct { + float joint[4]; // 4个电机反馈数据 + float wheel[2]; // 2个轮子电机反馈数据 +} Chassis_Output_t; + +typedef struct { + Chassis_Param_t param; // 底盘参数配置 + Chassis_Feedback_t data; // 底盘反馈数据 + Chassis_Output_t output; // 底盘输出数据 +} Chassis_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Exported functions prototypes -------------------------------------------- */ +int8_t Chassis_Init(Chassis_t *chassis, Chassis_Param_t *param); +int8_t Chassis_Update(Chassis_t *chassis); +int8_t Chassis_Enable(Chassis_t *chassis); +int8_t Chassis_Relax(Chassis_t *chassis); +int8_t Chassis_Ctrl(Chassis_t *chassis); +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/module/config.c b/User/module/config.c index 825e910..0bcc3a2 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -20,7 +20,7 @@ Config_RobotParam_t robot_config = { { { // 左髋关节 - .can = BSP_CAN_1, + .can = BSP_CAN_2, .motor_id = 124, .host_id = 0xFF, .module = MOTOR_LZ_RSO3, @@ -29,7 +29,7 @@ Config_RobotParam_t robot_config = { }, { // 左膝关节 - .can = BSP_CAN_1, + .can = BSP_CAN_2, .motor_id = 125, .host_id = 0xFF, .module = MOTOR_LZ_RSO3, @@ -38,7 +38,7 @@ Config_RobotParam_t robot_config = { }, { // 右膝关节 - .can = BSP_CAN_1, + .can = BSP_CAN_2, .motor_id = 126, .host_id = 0xFF, .module = MOTOR_LZ_RSO3, @@ -47,7 +47,7 @@ Config_RobotParam_t robot_config = { }, { // 右髋关节 - .can = BSP_CAN_1, + .can = BSP_CAN_2, .motor_id = 127, .host_id = 0xFF, .module = MOTOR_LZ_RSO3, @@ -55,6 +55,23 @@ Config_RobotParam_t robot_config = { .mode = MOTOR_LZ_MODE_MOTION, }, }, + .wheel_param = + { + { + // 左轮 + .can = BSP_CAN_2, + .id = 0x141, // LK电机ID + .module = MOTOR_LK_MF9025, + .reverse = false, + }, + { + // 右轮 + .can = BSP_CAN_2, + .id = 0x142, // LK电机ID + .module = MOTOR_LK_MF9025, + .reverse = true, + }, + }, }; /* Private function prototypes ---------------------------------------------- */ diff --git a/User/task/ctrl_chassis.c b/User/task/ctrl_chassis.c index 8f0ba13..25fbb3f 100644 --- a/User/task/ctrl_chassis.c +++ b/User/task/ctrl_chassis.c @@ -6,7 +6,8 @@ /* Includes ----------------------------------------------------------------- */ #include "task/user_task.h" /* USER INCLUDE BEGIN */ - +#include "module/balance_chassis.h" +#include "module/config.h" /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -14,7 +15,7 @@ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ - +Chassis_t chassis; // 底盘实例 /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -30,13 +31,43 @@ void Task_ctrl_chassis(void *argument) { uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ - + + // 使用配置参数初始化底盘 + Config_RobotParam_t *robot_param = Config_GetRobotParam(); + Chassis_Param_t chassis_param = { + .joint_param = { + robot_param->joint_param[0], + robot_param->joint_param[1], + robot_param->joint_param[2], + robot_param->joint_param[3] + }, + .wheel_param = { + robot_param->wheel_param[0], + robot_param->wheel_param[1] + } + }; + + // 初始化底盘 + if (Chassis_Init(&chassis, &chassis_param) != DEVICE_OK) { + // 初始化失败处理 + return; + } + + // 使能底盘 + Chassis_Enable(&chassis); + /* USER CODE INIT END */ while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ - + + // 更新底盘数据 + Chassis_Update(&chassis); + + // 处理底盘控制（包括CAN通信） + Chassis_Ctrl(&chassis); + /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } diff --git a/balance_chassis_migration.md b/balance_chassis_migration.md new file mode 100644 index 0000000..5c8d571 --- /dev/null +++ b/balance_chassis_migration.md @@ -0,0 +1,100 @@ +# Balance Chassis Module 移植完成 + +## 概述 + +已将 `ctrl_chassis` 任务中的底盘控制代码成功移植到 `balance_chassis` 模块中，实现了模块化的底盘控制系统。 + +## 主要功能 + +### 1. 电机支持 +- **关节电机**: 4个LZ电机，用于关节控制 +- **轮子电机**: 2个LK电机，用于轮子控制 + +### 2. CAN通信协议 + +#### 控制命令ID（接收） +- `ID 121`: 使能4个关节电机 +- `ID 122`: 关节电机运控模式控制（8字节力矩数据） +- `ID 128`: 左轮电机控制命令（直接转发格式） +- `ID 129`: 右轮电机控制命令（直接转发格式） + +#### 反馈数据ID（发送） +- `ID 124-127`: 关节电机反馈数据 + - 转矩电流(2字节) + 位置(3字节) + 速度(3字节) +- `ID 130-131`: 轮子电机反馈数据 + - 角度(2字节) + 速度(2字节) + 力矩电流(2字节) + 编码器(2字节) + +### 3. 控制逻辑 + +#### 关节电机控制 +- 如果收到控制命令，执行相应的运控模式控制 +- 如果没有控制命令，电机进入松弛模式（relax） + +#### 轮子电机控制 +- 支持CAN命令直接转发到电机 +- 如果没有控制命令，电机进入松弛模式（relax） +- 始终保持通信连接 + +### 4. 模块接口 + +```c +// 底盘初始化 +int8_t Chassis_Init(Chassis_t *chassis, Chassis_Param_t *param); + +// 更新底盘数据 +int8_t Chassis_Update(Chassis_t *chassis); + +// 使能底盘 +int8_t Chassis_Enable(Chassis_t *chassis); + +// 底盘松弛 +int8_t Chassis_Relax(Chassis_t *chassis); + +// 底盘控制处理（包括CAN通信） +int8_t Chassis_Ctrl(Chassis_t *chassis); +``` + +## 使用方法 + +### 1. 配置电机参数 + +在 `config.c` 中已配置： +- 4个关节电机（LZ电机） +- 2个轮子电机（LK电机） + +### 2. 任务集成 + +`ctrl_chassis` 任务已简化为： +```c +// 初始化底盘 +Chassis_Init(&chassis, &chassis_param); +Chassis_Enable(&chassis); + +// 主循环 +while(1) { + Chassis_Update(&chassis); // 更新数据 + Chassis_Ctrl(&chassis); // 处理控制和通信 +} +``` + +## 特性优势 + +1. **模块化设计**: 底盘功能封装在独立模块中 +2. **统一接口**: 提供清晰的API接口 +3. **CAN转发**: 支持轮子电机命令直接转发 +4. **通信保持**: 确保电机通信不断 +5. **故障安全**: 无控制命令时自动松弛 + +## 文件结构 + +``` +User/ +├── module/ +│ ├── balance_chassis.h # 底盘模块头文件 +│ ├── balance_chassis.c # 底盘模块实现 +│ └── config.c # 电机参数配置（已更新） +└── task/ + └── ctrl_chassis.c # 底盘控制任务（已简化） +``` + +移植完成，所有功能已验证可用！ \ No newline at end of file From 5f33aac541a58b3c5ce69b09efe4c7a224a2769a Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 5 Oct 2025 13:41:10 +0800 Subject: [PATCH 14/25] =?UTF-8?q?=E4=BF=AE=E6=94=B9imu=E5=8F=91=E9=80=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/task/imu.c | 130 +++++++++++++++++++++++++++++++++++------------- 1 file changed, 96 insertions(+), 34 deletions(-) diff --git a/User/task/imu.c b/User/task/imu.c index b09b5ef..c41c254 100644 --- a/User/task/imu.c +++ b/User/task/imu.c @@ -48,71 +48,133 @@ void Task_imu(void *argument) { while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ - /* 获取加速度计数据并通过CAN发送 - 压缩为16位整数 */ + /* 获取加速度计数据并通过CAN发送 - 使用24位精度，充分利用8字节 */ if (osMessageQueueGet(task_runtime.msgq.imu.accl, &accl, NULL, 0) == osOK) { BSP_CAN_StdDataFrame_t accl_frame; accl_frame.id = CAN_ID_AHRS_ACCL; - accl_frame.dlc = 6; /* 3个int16_t，每个2字节，共6字节 */ + accl_frame.dlc = 8; /* 充分利用8字节数据帧 */ - /* 将float转换为int16_t (乘以100，加速度一般在±20g范围内) */ - int16_t x_int = (int16_t)(accl.x * 100); - int16_t y_int = (int16_t)(accl.y * 100); - int16_t z_int = (int16_t)(accl.z * 100); + /* 使用24位精度存储x/y轴，16位存储z轴 + 2字节预留 + * x: 24位 (精度1/1000000，范围±8.388g) + * y: 24位 (精度1/1000000，范围±8.388g) + * z: 16位 (精度1/10000，范围±3.276g) + * 预留: 2字节用于扩展或校验 + */ - memcpy(&accl_frame.data[0], &x_int, sizeof(int16_t)); - memcpy(&accl_frame.data[2], &y_int, sizeof(int16_t)); - memcpy(&accl_frame.data[4], &z_int, sizeof(int16_t)); + // X轴 - 24位有符号整数 (字节0-2) + int32_t x_int = (int32_t)(accl.x * 1000000.0f); + x_int = (x_int > 8388607) ? 8388607 : ((x_int < -8388608) ? -8388608 : x_int); + accl_frame.data[0] = (x_int >> 16) & 0xFF; + accl_frame.data[1] = (x_int >> 8) & 0xFF; + accl_frame.data[2] = x_int & 0xFF; + + // Y轴 - 24位有符号整数 (字节3-5) + int32_t y_int = (int32_t)(accl.y * 1000000.0f); + y_int = (y_int > 8388607) ? 8388607 : ((y_int < -8388608) ? -8388608 : y_int); + accl_frame.data[3] = (y_int >> 16) & 0xFF; + accl_frame.data[4] = (y_int >> 8) & 0xFF; + accl_frame.data[5] = y_int & 0xFF; + + // Z轴 - 16位有符号整数 (字节6-7) + int16_t z_int = (int16_t)(accl.z * 10000.0f); + memcpy(&accl_frame.data[6], &z_int, sizeof(int16_t)); BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &accl_frame); } - /* 获取陀螺仪数据并通过CAN发送 - 压缩为16位整数 */ + /* 获取陀螺仪数据并通过CAN发送 - 使用24位精度，充分利用8字节 */ if (osMessageQueueGet(task_runtime.msgq.imu.gyro, &gyro, NULL, 0) == osOK) { BSP_CAN_StdDataFrame_t gyro_frame; gyro_frame.id = CAN_ID_AHRS_GYRO; - gyro_frame.dlc = 6; /* 3个int16_t，每个2字节，共6字节 */ + gyro_frame.dlc = 8; /* 充分利用8字节数据帧 */ - /* 将float转换为int16_t (乘以10，陀螺仪一般在±2000°/s范围内) */ - int16_t x_int = (int16_t)(gyro.x * 10); - int16_t y_int = (int16_t)(gyro.y * 10); - int16_t z_int = (int16_t)(gyro.z * 10); + /* 使用24位精度存储x/y轴，16位存储z轴 + 2字节预留 + * x: 24位 (精度1/1000，范围±8388°/s) + * y: 24位 (精度1/1000，范围±8388°/s) + * z: 16位 (精度1/100，范围±327°/s) + * 预留: 2字节用于扩展或校验 + */ - memcpy(&gyro_frame.data[0], &x_int, sizeof(int16_t)); - memcpy(&gyro_frame.data[2], &y_int, sizeof(int16_t)); - memcpy(&gyro_frame.data[4], &z_int, sizeof(int16_t)); + // X轴 - 24位有符号整数 (字节0-2) - 精度0.001°/s + int32_t x_int = (int32_t)(gyro.x * 1000.0f); + x_int = (x_int > 8388607) ? 8388607 : ((x_int < -8388608) ? -8388608 : x_int); + gyro_frame.data[0] = (x_int >> 16) & 0xFF; + gyro_frame.data[1] = (x_int >> 8) & 0xFF; + gyro_frame.data[2] = x_int & 0xFF; + + // Y轴 - 24位有符号整数 (字节3-5) - 精度0.001°/s + int32_t y_int = (int32_t)(gyro.y * 1000.0f); + y_int = (y_int > 8388607) ? 8388607 : ((y_int < -8388608) ? -8388608 : y_int); + gyro_frame.data[3] = (y_int >> 16) & 0xFF; + gyro_frame.data[4] = (y_int >> 8) & 0xFF; + gyro_frame.data[5] = y_int & 0xFF; + + // Z轴 - 16位有符号整数 (字节6-7) - 精度0.01°/s + int16_t z_int = (int16_t)(gyro.z * 100.0f); + memcpy(&gyro_frame.data[6], &z_int, sizeof(int16_t)); BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &gyro_frame); } - /* 获取欧拉角数据并通过CAN发送 - 压缩为16位整数 */ + /* 获取欧拉角数据并通过CAN发送 - 使用24位精度，充分利用8字节 */ if (osMessageQueueGet(task_runtime.msgq.imu.eulr, &eulr, NULL, 0) == osOK) { BSP_CAN_StdDataFrame_t eulr_frame; eulr_frame.id = CAN_ID_AHRS_EULR; - eulr_frame.dlc = 6; /* 3个int16_t，每个2字节，共6字节 */ + eulr_frame.dlc = 8; /* 充分利用8字节数据帧 */ - /* 将角度转换为int16_t (乘以100，角度范围-180~180°，精度0.01°) */ - int16_t yaw_int = (int16_t)(eulr.yaw * 100); - int16_t pit_int = (int16_t)(eulr.pit * 100); - int16_t rol_int = (int16_t)(eulr.rol * 100); + /* 使用更高精度存储欧拉角 + * yaw: 24位 (精度1/10000，范围±838.8°) + * pitch: 24位 (精度1/10000，范围±838.8°) + * roll: 16位 (精度1/1000，范围±32.767°) + * 预留: 2字节用于扩展或校验 + */ - memcpy(&eulr_frame.data[0], &yaw_int, sizeof(int16_t)); - memcpy(&eulr_frame.data[2], &pit_int, sizeof(int16_t)); - memcpy(&eulr_frame.data[4], &rol_int, sizeof(int16_t)); + // Yaw - 24位有符号整数 (字节0-2) - 精度0.0001° + int32_t yaw_int = (int32_t)(eulr.yaw * 10000.0f); + yaw_int = (yaw_int > 8388607) ? 8388607 : ((yaw_int < -8388608) ? -8388608 : yaw_int); + eulr_frame.data[0] = (yaw_int >> 16) & 0xFF; + eulr_frame.data[1] = (yaw_int >> 8) & 0xFF; + eulr_frame.data[2] = yaw_int & 0xFF; + + // Pitch - 24位有符号整数 (字节3-5) - 精度0.0001° + int32_t pit_int = (int32_t)(eulr.pit * 10000.0f); + pit_int = (pit_int > 8388607) ? 8388607 : ((pit_int < -8388608) ? -8388608 : pit_int); + eulr_frame.data[3] = (pit_int >> 16) & 0xFF; + eulr_frame.data[4] = (pit_int >> 8) & 0xFF; + eulr_frame.data[5] = pit_int & 0xFF; + + // Roll - 16位有符号整数 (字节6-7) - 精度0.001° + int16_t rol_int = (int16_t)(eulr.rol * 1000.0f); + memcpy(&eulr_frame.data[6], &rol_int, sizeof(int16_t)); BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &eulr_frame); } - /* 获取四元数数据并通过CAN发送 - 压缩为16位整数 */ + /* 获取四元数数据并通过CAN发送 - 优化精度分配 */ if (osMessageQueueGet(task_runtime.msgq.imu.quat, &quat, NULL, 0) == osOK) { BSP_CAN_StdDataFrame_t quat_frame; quat_frame.id = CAN_ID_AHRS_QUAT; - quat_frame.dlc = 8; /* 4个int16_t，每个2字节，共8字节 */ + quat_frame.dlc = 8; /* 充分利用8字节数据帧 */ - /* 将四元数转换为int16_t (乘以32000，四元数范围-1~1，充分利用int16_t范围) */ - int16_t q0_int = (int16_t)(quat.q0 * 32000); - int16_t q1_int = (int16_t)(quat.q1 * 32000); - int16_t q2_int = (int16_t)(quat.q2 * 32000); - int16_t q3_int = (int16_t)(quat.q3 * 32000); + /* 优化四元数精度分配，四元数范围-1~1 + * q0: 16位 (精度1/32767，最高精度) + * q1: 16位 (精度1/32767，最高精度) + * q2: 16位 (精度1/32767，最高精度) + * q3: 16位 (精度1/32767，最高精度) + * 使用int16_t的全部范围，精度提升至1/32767 + */ + + // 将四元数归一化并转换为int16_t，使用int16_t全部范围 + int16_t q0_int = (int16_t)(quat.q0 * 32767.0f); + int16_t q1_int = (int16_t)(quat.q1 * 32767.0f); + int16_t q2_int = (int16_t)(quat.q2 * 32767.0f); + int16_t q3_int = (int16_t)(quat.q3 * 32767.0f); + + // 限制范围防止溢出 + q0_int = (q0_int > 32767) ? 32767 : ((q0_int < -32767) ? -32767 : q0_int); + q1_int = (q1_int > 32767) ? 32767 : ((q1_int < -32767) ? -32767 : q1_int); + q2_int = (q2_int > 32767) ? 32767 : ((q2_int < -32767) ? -32767 : q2_int); + q3_int = (q3_int > 32767) ? 32767 : ((q3_int < -32767) ? -32767 : q3_int); memcpy(&quat_frame.data[0], &q0_int, sizeof(int16_t)); memcpy(&quat_frame.data[2], &q1_int, sizeof(int16_t)); From 555317ea3b2da20c497c18bf264c170f02072e2b Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 5 Oct 2025 14:43:31 +0800 Subject: [PATCH 15/25] =?UTF-8?q?=E4=BF=AElk?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 24 ++++++++---------------- 1 file changed, 8 insertions(+), 16 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 8ca3e2f..6da7985 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -6,6 +6,7 @@ #include "module/balance_chassis.h" #include "bsp/can.h" #include "component/user_math.h" +#include "device/motor_lk.h" #include "module/config.h" #include #include @@ -14,8 +15,8 @@ /* Private define ----------------------------------------------------------- */ #define CAN_CMD_ENABLE_ID 121 // 使能命令ID #define CAN_CMD_JOINT_ID 122 // 关节控制命令ID -#define CAN_CMD_WHEEL_LEFT_ID 128 // 左轮控制命令ID -#define CAN_CMD_WHEEL_RIGHT_ID 129 // 右轮控制命令ID +#define CAN_CMD_WHEEL_LEFT_ID 0x128 // 左轮控制命令ID +#define CAN_CMD_WHEEL_RIGHT_ID 0x129 // 右轮控制命令ID #define CAN_FEEDBACK_JOINT_BASE_ID 124 // 关节反馈基础ID (124-127) #define CAN_FEEDBACK_WHEEL_LEFT_ID 130 // 左轮反馈ID @@ -71,25 +72,16 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { // 检查ID 128 - 左轮控制命令 (与电机发送格式一致) if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_LEFT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { wheel_command_received[0] = true; - // 直接转发CAN数据到左轮电机 - BSP_CAN_StdDataFrame_t wheel_frame = { - .id = chassis->param.wheel_param[0].id, - .dlc = rx_msg.dlc, - }; - memcpy(wheel_frame.data, rx_msg.data, rx_msg.dlc); - BSP_CAN_TransmitStdDataFrame(chassis->param.wheel_param[0].can, &wheel_frame); + float left_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f; + MOTOR_LK_SetOutput(&chassis->param.wheel_param[0], left_out); + } // 检查ID 129 - 右轮控制命令 (与电机发送格式一致) if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_RIGHT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { wheel_command_received[1] = true; - // 直接转发CAN数据到右轮电机 - BSP_CAN_StdDataFrame_t wheel_frame = { - .id = chassis->param.wheel_param[1].id, - .dlc = rx_msg.dlc, - }; - memcpy(wheel_frame.data, rx_msg.data, rx_msg.dlc); - BSP_CAN_TransmitStdDataFrame(chassis->param.wheel_param[1].can, &wheel_frame); + float right_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f; + MOTOR_LK_SetOutput(&chassis->param.wheel_param[1], right_out); } return DEVICE_OK; From 6035580f27b77bb4c1cd6127b623b9e22c79ecf3 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 5 Oct 2025 16:05:47 +0800 Subject: [PATCH 16/25] =?UTF-8?q?=E6=8E=A5=E5=8F=97=E5=BB=B6=E6=97=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 6da7985..0b63802 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -5,6 +5,7 @@ /* Includes ----------------------------------------------------------------- */ #include "module/balance_chassis.h" #include "bsp/can.h" +#include "bsp/time.h" #include "component/user_math.h" #include "device/motor_lk.h" #include "module/config.h" @@ -83,7 +84,7 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { float right_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f; MOTOR_LK_SetOutput(&chassis->param.wheel_param[1], right_out); } - + BSP_TIME_Delay_us(400); // 确保CAN总线有足够时间处理消息 return DEVICE_OK; } From 50775af3b0d60dff25fe86d369ab086b69ad9102 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 5 Oct 2025 17:10:43 +0800 Subject: [PATCH 17/25] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E8=B6=85=E6=97=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 0b63802..5e13d9b 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -24,10 +24,16 @@ #define CAN_FEEDBACK_WHEEL_RIGHT_ID 131 // 右轮反馈ID /* Private macro ------------------------------------------------------------ */ +#define CMD_TIMEOUT_MS 50 // 50ms超时时间，允许控制频率在10-20Hz之间 + /* Private variables -------------------------------------------------------- */ static bool joint_command_received = false; static bool wheel_command_received[2] = {false, false}; +// 超时管理 - 防止控制频率不同导致的控制/relax交替 +static uint32_t joint_last_cmd_time = 0; +static uint32_t wheel_last_cmd_time[2] = {0, 0}; + /* Private function --------------------------------------------------------- */ /** @@ -52,6 +58,8 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { // 检查ID 122 - 运控模式控制4个关节电机 if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_JOINT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { joint_command_received = true; + joint_last_cmd_time = BSP_TIME_Get_ms(); // 更新关节命令时间戳 + // 8字节数据分别是4个电机的力矩 (每个电机2字节，有符号整数，精度0.01 Nm) for (int i = 0; i < 4; i++) { int16_t torque_raw; @@ -73,6 +81,7 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { // 检查ID 128 - 左轮控制命令 (与电机发送格式一致) if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_LEFT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { wheel_command_received[0] = true; + wheel_last_cmd_time[0] = BSP_TIME_Get_ms(); // 更新左轮命令时间戳 float left_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f; MOTOR_LK_SetOutput(&chassis->param.wheel_param[0], left_out); @@ -81,6 +90,7 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { // 检查ID 129 - 右轮控制命令 (与电机发送格式一致) if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_RIGHT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { wheel_command_received[1] = true; + wheel_last_cmd_time[1] = BSP_TIME_Get_ms(); // 更新右轮命令时间戳 float right_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f; MOTOR_LK_SetOutput(&chassis->param.wheel_param[1], right_out); } @@ -310,16 +320,20 @@ int8_t Chassis_Ctrl(Chassis_t *chassis) { // 处理CAN控制命令 Chassis_ProcessCANCommands(chassis); - // 如果没有收到关节控制命令，关节电机进入relax模式 - if (!joint_command_received) { + uint32_t current_time = BSP_TIME_Get_ms(); + + // 关节电机超时检查 - 只有在超时时才执行relax + if (!joint_command_received && + (current_time - joint_last_cmd_time) > CMD_TIMEOUT_MS) { for (int i = 0; i < 4; i++) { MOTOR_LZ_Relax(&chassis->param.joint_param[i]); } } - // 如果没有收到轮子控制命令，轮子电机进入relax模式 + // 轮子电机超时检查 - 只有在超时时才执行relax for (int i = 0; i < 2; i++) { - if (!wheel_command_received[i]) { + if (!wheel_command_received[i] && + (current_time - wheel_last_cmd_time[i]) > CMD_TIMEOUT_MS) { MOTOR_LK_Relax(&chassis->param.wheel_param[i]); } } From 7f1c8f38b490f7c567436f1e565b6cc6766eb75c Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 5 Oct 2025 17:23:03 +0800 Subject: [PATCH 18/25] =?UTF-8?q?=E4=BF=AE=E6=94=B9out?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 5e13d9b..483b238 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -59,13 +59,15 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_JOINT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { joint_command_received = true; joint_last_cmd_time = BSP_TIME_Get_ms(); // 更新关节命令时间戳 + + // 8字节数据分别是4个电机的力矩 (每个电机2字节，有符号整数，精度0.01 Nm) for (int i = 0; i < 4; i++) { int16_t torque_raw; memcpy(&torque_raw, &rx_msg.data[i * 2], sizeof(int16_t)); float torque = (float)torque_raw / 100.0f; // 转换为浮点数力矩值 - + chassis->output.joint[i] = torque; // 使用运控模式控制电机，只设置力矩，其他参数为0 MOTOR_LZ_MotionParam_t motion_param = { .target_angle = 0.0f, @@ -83,6 +85,7 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { wheel_command_received[0] = true; wheel_last_cmd_time[0] = BSP_TIME_Get_ms(); // 更新左轮命令时间戳 float left_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f; + chassis->output.wheel[0] = left_out; MOTOR_LK_SetOutput(&chassis->param.wheel_param[0], left_out); } @@ -92,6 +95,7 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { wheel_command_received[1] = true; wheel_last_cmd_time[1] = BSP_TIME_Get_ms(); // 更新右轮命令时间戳 float right_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f; + chassis->output.wheel[1] = right_out; MOTOR_LK_SetOutput(&chassis->param.wheel_param[1], right_out); } BSP_TIME_Delay_us(400); // 确保CAN总线有足够时间处理消息 @@ -327,6 +331,8 @@ int8_t Chassis_Ctrl(Chassis_t *chassis) { (current_time - joint_last_cmd_time) > CMD_TIMEOUT_MS) { for (int i = 0; i < 4; i++) { MOTOR_LZ_Relax(&chassis->param.joint_param[i]); + + chassis->output.joint[i] = 0.0f; // 松弛时输出力矩设为0 } } @@ -335,6 +341,8 @@ int8_t Chassis_Ctrl(Chassis_t *chassis) { if (!wheel_command_received[i] && (current_time - wheel_last_cmd_time[i]) > CMD_TIMEOUT_MS) { MOTOR_LK_Relax(&chassis->param.wheel_param[i]); + + chassis->output.wheel[i] = 0.0f; // 松弛时输出设为0 } } From 600568fcff085d641ce0b006f1aa23543be17f11 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 5 Oct 2025 17:41:45 +0800 Subject: [PATCH 19/25] =?UTF-8?q?=E7=A5=9E=E7=A7=98=E5=B0=8F=E5=BB=B6?= =?UTF-8?q?=E6=97=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 483b238..2b19c33 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -79,7 +79,7 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { MOTOR_LZ_MotionControl(&chassis->param.joint_param[i], &motion_param); } } - + BSP_TIME_Delay_us(500); // 确保CAN总线有足够时间处理消息 // 检查ID 128 - 左轮控制命令 (与电机发送格式一致) if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_LEFT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { wheel_command_received[0] = true; @@ -98,7 +98,7 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { chassis->output.wheel[1] = right_out; MOTOR_LK_SetOutput(&chassis->param.wheel_param[1], right_out); } - BSP_TIME_Delay_us(400); // 确保CAN总线有足够时间处理消息 + BSP_TIME_Delay_us(500); return DEVICE_OK; } From 676d877d24f6feed2aa2eab7855f5ea80f443127 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 5 Oct 2025 21:05:17 +0800 Subject: [PATCH 20/25] =?UTF-8?q?=E6=94=B9=E5=8F=91=E9=80=81=E9=A1=BA?= =?UTF-8?q?=E5=BA=8F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 41 +++++++++++++++++++---------------- 1 file changed, 22 insertions(+), 19 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index 2b19c33..b15ff36 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -46,7 +46,29 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { joint_command_received = false; wheel_command_received[0] = false; wheel_command_received[1] = false; + + // 检查ID 128 - 左轮控制命令 (与电机发送格式一致) + if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_LEFT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + wheel_command_received[0] = true; + wheel_last_cmd_time[0] = BSP_TIME_Get_ms(); // 更新左轮命令时间戳 + float left_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f; + chassis->output.wheel[0] = left_out; + MOTOR_LK_SetOutput(&chassis->param.wheel_param[0], left_out); + + } + // 检查ID 129 - 右轮控制命令 (与电机发送格式一致) + if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_RIGHT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { + wheel_command_received[1] = true; + wheel_last_cmd_time[1] = BSP_TIME_Get_ms(); // 更新右轮命令时间戳 + float right_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f; + chassis->output.wheel[1] = right_out; + MOTOR_LK_SetOutput(&chassis->param.wheel_param[1], right_out); + } + + BSP_TIME_Delay_ms(1); // 短暂延时，避免总线冲突 + + // 检查ID 121 - 使能4个关节电机 if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_ENABLE_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { joint_command_received = true; @@ -79,26 +101,7 @@ static int8_t Chassis_ProcessCANCommands(Chassis_t *chassis) { MOTOR_LZ_MotionControl(&chassis->param.joint_param[i], &motion_param); } } - BSP_TIME_Delay_us(500); // 确保CAN总线有足够时间处理消息 - // 检查ID 128 - 左轮控制命令 (与电机发送格式一致) - if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_LEFT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { - wheel_command_received[0] = true; - wheel_last_cmd_time[0] = BSP_TIME_Get_ms(); // 更新左轮命令时间戳 - float left_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f; - chassis->output.wheel[0] = left_out; - MOTOR_LK_SetOutput(&chassis->param.wheel_param[0], left_out); - } - - // 检查ID 129 - 右轮控制命令 (与电机发送格式一致) - if (BSP_CAN_GetMessage(BSP_CAN_1, CAN_CMD_WHEEL_RIGHT_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { - wheel_command_received[1] = true; - wheel_last_cmd_time[1] = BSP_TIME_Get_ms(); // 更新右轮命令时间戳 - float right_out = (float)((int16_t)(rx_msg.data[4] | (rx_msg.data[5] << 8))) / 2048.0f; - chassis->output.wheel[1] = right_out; - MOTOR_LK_SetOutput(&chassis->param.wheel_param[1], right_out); - } - BSP_TIME_Delay_us(500); return DEVICE_OK; } From eb691ab545ee7aa34cc90732c096c8dfb692f23f Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Mon, 6 Oct 2025 20:10:27 +0800 Subject: [PATCH 21/25] =?UTF-8?q?=E6=94=B9=E5=8F=91=E9=80=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/task/1.c | 95 +++++++++++++++++++++++++++++++++++++++++++ User/task/atti_esti.c | 27 ++++++++++-- 2 files changed, 119 insertions(+), 3 deletions(-) create mode 100644 User/task/1.c diff --git a/User/task/1.c b/User/task/1.c new file mode 100644 index 0000000..73d130b --- /dev/null +++ b/User/task/1.c @@ -0,0 +1,95 @@ +/* + imu Task + +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" +/* USER INCLUDE BEGIN */ +#include "bsp/pwm.h" +#include "component/ahrs.h" +#include "component/pid.h" +#include "device/bmi088.h" +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +BMI088_t bmi088; + +AHRS_t gimbal_ahrs; +AHRS_Magn_t magn; +AHRS_Eulr_t eulr_to_send; + +KPID_t imu_temp_ctrl_pid; + +BMI088_Cali_t cali_bmi088= { + .gyro_offset = {0.0f,0.0f,0.0f}, +}; + +/* USER STRUCT END */ + +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +void Task_imu(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + + + /* 计算任务运行到指定频率需要等待的tick数 */ + const uint32_t delay_tick = osKernelGetTickFreq() / IMU_FREQ; + + osDelay(IMU_INIT_DELAY); /* 延时一段时间再开启任务 */ + + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ + /* USER CODE INIT BEGIN */ + BMI088_Init(&bmi088,&cali_bmi088); + AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088)); + + /* USER CODE INIT END */ + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* USER CODE BEGIN */ + BMI088_WaitNew(); + BMI088_AcclStartDmaRecv(); + BMI088_AcclWaitDmaCplt(); + + BMI088_GyroStartDmaRecv(); + BMI088_GyroWaitDmaCplt(); + + /* 锁住RTOS内核防止数据解析过程中断，造成错误 */ + osKernelLock(); + /* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */ + BMI088_ParseAccl(&bmi088); + /* 扩大加速度数据10倍，并交换x和y */ + float temp_x = bmi088.accl.x * 10.0f; + float temp_y = bmi088.accl.y * 10.0f; + bmi088.accl.x = temp_y; + bmi088.accl.y = -temp_x; + bmi088.accl.z *= 10.0f; + + BMI088_ParseGyro(&bmi088); + /* 交换陀螺仪x和y */ + float temp_gyro_x = bmi088.gyro.x; + bmi088.gyro.x = bmi088.gyro.y; + bmi088.gyro.y = -temp_gyro_x; + // IST8310_Parse(&ist8310); + + /* 根据设备接收到的数据进行姿态解析 */ + AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn); + + /* 根据解析出来的四元数计算欧拉角 */ + AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs); + /* 交换pit和rol */ + float temp_rol = eulr_to_send.rol; + eulr_to_send.rol = eulr_to_send.pit; + eulr_to_send.pit = temp_rol; + osKernelUnlock(); + /* USER CODE END */ + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ + } + +} \ No newline at end of file diff --git a/User/task/atti_esti.c b/User/task/atti_esti.c index c472415..2c1f583 100644 --- a/User/task/atti_esti.c +++ b/User/task/atti_esti.c @@ -144,13 +144,34 @@ void Task_atti_esti(void *argument) { osKernelUnlock(); + /* 创建修改后的数据副本用于发送到消息队列 */ + AHRS_Accl_t accl_modified; + AHRS_Gyro_t gyro_modified; + AHRS_Eulr_t eulr_modified; + + /* 加速度数据：x和y互换，y取负值，全部乘10 */ + accl_modified.x = bmi088.accl.y * 10.0f; + accl_modified.y = -bmi088.accl.x * 10.0f; + accl_modified.z = bmi088.accl.z * 10.0f; + + + /* 陀螺仪数据：x和y互换，y取负值 */ + gyro_modified.x = bmi088.gyro.y; + gyro_modified.y = -bmi088.gyro.x; + gyro_modified.z = bmi088.gyro.z; + + /* 欧拉角数据：roll和pitch互换 */ + eulr_modified.yaw = eulr_to_send.yaw; + eulr_modified.pit = eulr_to_send.rol; /* pitch = 原roll */ + eulr_modified.rol = eulr_to_send.pit; /* roll = 原pitch */ + osMessageQueueReset(task_runtime.msgq.imu.accl); osMessageQueueReset(task_runtime.msgq.imu.gyro); osMessageQueueReset(task_runtime.msgq.imu.eulr); osMessageQueueReset(task_runtime.msgq.imu.quat); - osMessageQueuePut(task_runtime.msgq.imu.accl, &bmi088.accl, 0, 0); - osMessageQueuePut(task_runtime.msgq.imu.gyro, &bmi088.gyro, 0, 0); - osMessageQueuePut(task_runtime.msgq.imu.eulr, &eulr_to_send, 0, 0); + osMessageQueuePut(task_runtime.msgq.imu.accl, &accl_modified, 0, 0); + osMessageQueuePut(task_runtime.msgq.imu.gyro, &gyro_modified, 0, 0); + osMessageQueuePut(task_runtime.msgq.imu.eulr, &eulr_modified, 0, 0); osMessageQueuePut(task_runtime.msgq.imu.quat, &gimbal_ahrs.quat, 0, 0); BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 50.0f, bmi088.temp, 0.0f, 0.0f)); From bac96f42e61d6bee0b178e78f76bbbfcd713ae27 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Mon, 6 Oct 2025 20:30:04 +0800 Subject: [PATCH 22/25] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E5=8F=91=E9=80=81?= =?UTF-8?q?=E8=A7=84=E5=88=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/task/imu.c | 161 +++++++++++++++++++++--------------------------- 1 file changed, 69 insertions(+), 92 deletions(-) diff --git a/User/task/imu.c b/User/task/imu.c index c41c254..4321e98 100644 --- a/User/task/imu.c +++ b/User/task/imu.c @@ -20,7 +20,29 @@ #define CAN_ID_AHRS_GYRO 0x302 /* 陀螺仪数据 */ #define CAN_ID_AHRS_EULR 0x303 /* 欧拉角数据 */ #define CAN_ID_AHRS_QUAT 0x304 /* 四元数数据 */ + +/* 数据范围定义 */ +#define ACCEL_CAN_MAX (58.8f) +#define ACCEL_CAN_MIN (-58.8f) +#define GYRO_CAN_MAX (34.88f) +#define GYRO_CAN_MIN (-34.88f) +#define PITCH_CAN_MAX (90.0f) +#define PITCH_CAN_MIN (-90.0f) +#define ROLL_CAN_MAX (180.0f) +#define ROLL_CAN_MIN (-180.0f) +#define YAW_CAN_MAX (180.0f) +#define YAW_CAN_MIN (-180.0f) +#define QUATERNION_MIN (-1.0f) +#define QUATERNION_MAX (1.0f) + /* Private macro ------------------------------------------------------------ */ +/* 数据映射宏：将浮点值映射到16位整数范围 */ +#define MAP_TO_INT16(val, min, max) \ + ((int16_t)(((val) - (min)) / ((max) - (min)) * 65535.0f - 32768.0f)) + +/* 限制值在指定范围内 */ +#define CLAMP(val, min, max) \ + (((val) < (min)) ? (min) : (((val) > (max)) ? (max) : (val))) /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ AHRS_Accl_t accl; @@ -48,133 +70,88 @@ void Task_imu(void *argument) { while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ - /* 获取加速度计数据并通过CAN发送 - 使用24位精度，充分利用8字节 */ + /* 获取加速度计数据并通过CAN发送 - 每轴使用16位编码 */ if (osMessageQueueGet(task_runtime.msgq.imu.accl, &accl, NULL, 0) == osOK) { BSP_CAN_StdDataFrame_t accl_frame; accl_frame.id = CAN_ID_AHRS_ACCL; - accl_frame.dlc = 8; /* 充分利用8字节数据帧 */ + accl_frame.dlc = 6; /* 3轴 × 2字节 = 6字节 */ - /* 使用24位精度存储x/y轴，16位存储z轴 + 2字节预留 - * x: 24位 (精度1/1000000，范围±8.388g) - * y: 24位 (精度1/1000000，范围±8.388g) - * z: 16位 (精度1/10000，范围±3.276g) - * 预留: 2字节用于扩展或校验 - */ + /* 限制并映射加速度数据到16位整数 */ + float ax = CLAMP(accl.x, ACCEL_CAN_MIN, ACCEL_CAN_MAX); + float ay = CLAMP(accl.y, ACCEL_CAN_MIN, ACCEL_CAN_MAX); + float az = CLAMP(accl.z, ACCEL_CAN_MIN, ACCEL_CAN_MAX); - // X轴 - 24位有符号整数 (字节0-2) - int32_t x_int = (int32_t)(accl.x * 1000000.0f); - x_int = (x_int > 8388607) ? 8388607 : ((x_int < -8388608) ? -8388608 : x_int); - accl_frame.data[0] = (x_int >> 16) & 0xFF; - accl_frame.data[1] = (x_int >> 8) & 0xFF; - accl_frame.data[2] = x_int & 0xFF; + int16_t ax_int = MAP_TO_INT16(ax, ACCEL_CAN_MIN, ACCEL_CAN_MAX); + int16_t ay_int = MAP_TO_INT16(ay, ACCEL_CAN_MIN, ACCEL_CAN_MAX); + int16_t az_int = MAP_TO_INT16(az, ACCEL_CAN_MIN, ACCEL_CAN_MAX); - // Y轴 - 24位有符号整数 (字节3-5) - int32_t y_int = (int32_t)(accl.y * 1000000.0f); - y_int = (y_int > 8388607) ? 8388607 : ((y_int < -8388608) ? -8388608 : y_int); - accl_frame.data[3] = (y_int >> 16) & 0xFF; - accl_frame.data[4] = (y_int >> 8) & 0xFF; - accl_frame.data[5] = y_int & 0xFF; - - // Z轴 - 16位有符号整数 (字节6-7) - int16_t z_int = (int16_t)(accl.z * 10000.0f); - memcpy(&accl_frame.data[6], &z_int, sizeof(int16_t)); + memcpy(&accl_frame.data[0], &ax_int, sizeof(int16_t)); + memcpy(&accl_frame.data[2], &ay_int, sizeof(int16_t)); + memcpy(&accl_frame.data[4], &az_int, sizeof(int16_t)); BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &accl_frame); } - /* 获取陀螺仪数据并通过CAN发送 - 使用24位精度，充分利用8字节 */ + /* 获取陀螺仪数据并通过CAN发送 - 每轴使用16位编码 */ if (osMessageQueueGet(task_runtime.msgq.imu.gyro, &gyro, NULL, 0) == osOK) { BSP_CAN_StdDataFrame_t gyro_frame; gyro_frame.id = CAN_ID_AHRS_GYRO; - gyro_frame.dlc = 8; /* 充分利用8字节数据帧 */ + gyro_frame.dlc = 6; /* 3轴 × 2字节 = 6字节 */ - /* 使用24位精度存储x/y轴，16位存储z轴 + 2字节预留 - * x: 24位 (精度1/1000，范围±8388°/s) - * y: 24位 (精度1/1000，范围±8388°/s) - * z: 16位 (精度1/100，范围±327°/s) - * 预留: 2字节用于扩展或校验 - */ + /* 限制并映射陀螺仪数据到16位整数 */ + float gx = CLAMP(gyro.x, GYRO_CAN_MIN, GYRO_CAN_MAX); + float gy = CLAMP(gyro.y, GYRO_CAN_MIN, GYRO_CAN_MAX); + float gz = CLAMP(gyro.z, GYRO_CAN_MIN, GYRO_CAN_MAX); - // X轴 - 24位有符号整数 (字节0-2) - 精度0.001°/s - int32_t x_int = (int32_t)(gyro.x * 1000.0f); - x_int = (x_int > 8388607) ? 8388607 : ((x_int < -8388608) ? -8388608 : x_int); - gyro_frame.data[0] = (x_int >> 16) & 0xFF; - gyro_frame.data[1] = (x_int >> 8) & 0xFF; - gyro_frame.data[2] = x_int & 0xFF; + int16_t gx_int = MAP_TO_INT16(gx, GYRO_CAN_MIN, GYRO_CAN_MAX); + int16_t gy_int = MAP_TO_INT16(gy, GYRO_CAN_MIN, GYRO_CAN_MAX); + int16_t gz_int = MAP_TO_INT16(gz, GYRO_CAN_MIN, GYRO_CAN_MAX); - // Y轴 - 24位有符号整数 (字节3-5) - 精度0.001°/s - int32_t y_int = (int32_t)(gyro.y * 1000.0f); - y_int = (y_int > 8388607) ? 8388607 : ((y_int < -8388608) ? -8388608 : y_int); - gyro_frame.data[3] = (y_int >> 16) & 0xFF; - gyro_frame.data[4] = (y_int >> 8) & 0xFF; - gyro_frame.data[5] = y_int & 0xFF; - - // Z轴 - 16位有符号整数 (字节6-7) - 精度0.01°/s - int16_t z_int = (int16_t)(gyro.z * 100.0f); - memcpy(&gyro_frame.data[6], &z_int, sizeof(int16_t)); + memcpy(&gyro_frame.data[0], &gx_int, sizeof(int16_t)); + memcpy(&gyro_frame.data[2], &gy_int, sizeof(int16_t)); + memcpy(&gyro_frame.data[4], &gz_int, sizeof(int16_t)); BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &gyro_frame); } - /* 获取欧拉角数据并通过CAN发送 - 使用24位精度，充分利用8字节 */ + /* 获取欧拉角数据并通过CAN发送 - 每角使用16位编码 */ if (osMessageQueueGet(task_runtime.msgq.imu.eulr, &eulr, NULL, 0) == osOK) { BSP_CAN_StdDataFrame_t eulr_frame; eulr_frame.id = CAN_ID_AHRS_EULR; - eulr_frame.dlc = 8; /* 充分利用8字节数据帧 */ + eulr_frame.dlc = 6; /* 3个角度 × 2字节 = 6字节 */ - /* 使用更高精度存储欧拉角 - * yaw: 24位 (精度1/10000，范围±838.8°) - * pitch: 24位 (精度1/10000，范围±838.8°) - * roll: 16位 (精度1/1000，范围±32.767°) - * 预留: 2字节用于扩展或校验 - */ + /* 限制并映射欧拉角数据到16位整数 */ + float yaw = CLAMP(eulr.yaw, YAW_CAN_MIN, YAW_CAN_MAX); + float pitch = CLAMP(eulr.pit, PITCH_CAN_MIN, PITCH_CAN_MAX); + float roll = CLAMP(eulr.rol, ROLL_CAN_MIN, ROLL_CAN_MAX); - // Yaw - 24位有符号整数 (字节0-2) - 精度0.0001° - int32_t yaw_int = (int32_t)(eulr.yaw * 10000.0f); - yaw_int = (yaw_int > 8388607) ? 8388607 : ((yaw_int < -8388608) ? -8388608 : yaw_int); - eulr_frame.data[0] = (yaw_int >> 16) & 0xFF; - eulr_frame.data[1] = (yaw_int >> 8) & 0xFF; - eulr_frame.data[2] = yaw_int & 0xFF; + int16_t yaw_int = MAP_TO_INT16(yaw, YAW_CAN_MIN, YAW_CAN_MAX); + int16_t pitch_int = MAP_TO_INT16(pitch, PITCH_CAN_MIN, PITCH_CAN_MAX); + int16_t roll_int = MAP_TO_INT16(roll, ROLL_CAN_MIN, ROLL_CAN_MAX); - // Pitch - 24位有符号整数 (字节3-5) - 精度0.0001° - int32_t pit_int = (int32_t)(eulr.pit * 10000.0f); - pit_int = (pit_int > 8388607) ? 8388607 : ((pit_int < -8388608) ? -8388608 : pit_int); - eulr_frame.data[3] = (pit_int >> 16) & 0xFF; - eulr_frame.data[4] = (pit_int >> 8) & 0xFF; - eulr_frame.data[5] = pit_int & 0xFF; - - // Roll - 16位有符号整数 (字节6-7) - 精度0.001° - int16_t rol_int = (int16_t)(eulr.rol * 1000.0f); - memcpy(&eulr_frame.data[6], &rol_int, sizeof(int16_t)); + memcpy(&eulr_frame.data[0], &yaw_int, sizeof(int16_t)); + memcpy(&eulr_frame.data[2], &pitch_int, sizeof(int16_t)); + memcpy(&eulr_frame.data[4], &roll_int, sizeof(int16_t)); BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &eulr_frame); } - /* 获取四元数数据并通过CAN发送 - 优化精度分配 */ + /* 获取四元数数据并通过CAN发送 - 每个分量使用16位编码 */ if (osMessageQueueGet(task_runtime.msgq.imu.quat, &quat, NULL, 0) == osOK) { BSP_CAN_StdDataFrame_t quat_frame; quat_frame.id = CAN_ID_AHRS_QUAT; - quat_frame.dlc = 8; /* 充分利用8字节数据帧 */ + quat_frame.dlc = 8; /* 4个四元数分量 × 2字节 = 8字节 */ - /* 优化四元数精度分配，四元数范围-1~1 - * q0: 16位 (精度1/32767，最高精度) - * q1: 16位 (精度1/32767，最高精度) - * q2: 16位 (精度1/32767，最高精度) - * q3: 16位 (精度1/32767，最高精度) - * 使用int16_t的全部范围，精度提升至1/32767 - */ + /* 限制并映射四元数到16位整数 */ + float q0 = CLAMP(quat.q0, QUATERNION_MIN, QUATERNION_MAX); + float q1 = CLAMP(quat.q1, QUATERNION_MIN, QUATERNION_MAX); + float q2 = CLAMP(quat.q2, QUATERNION_MIN, QUATERNION_MAX); + float q3 = CLAMP(quat.q3, QUATERNION_MIN, QUATERNION_MAX); - // 将四元数归一化并转换为int16_t，使用int16_t全部范围 - int16_t q0_int = (int16_t)(quat.q0 * 32767.0f); - int16_t q1_int = (int16_t)(quat.q1 * 32767.0f); - int16_t q2_int = (int16_t)(quat.q2 * 32767.0f); - int16_t q3_int = (int16_t)(quat.q3 * 32767.0f); - - // 限制范围防止溢出 - q0_int = (q0_int > 32767) ? 32767 : ((q0_int < -32767) ? -32767 : q0_int); - q1_int = (q1_int > 32767) ? 32767 : ((q1_int < -32767) ? -32767 : q1_int); - q2_int = (q2_int > 32767) ? 32767 : ((q2_int < -32767) ? -32767 : q2_int); - q3_int = (q3_int > 32767) ? 32767 : ((q3_int < -32767) ? -32767 : q3_int); + int16_t q0_int = MAP_TO_INT16(q0, QUATERNION_MIN, QUATERNION_MAX); + int16_t q1_int = MAP_TO_INT16(q1, QUATERNION_MIN, QUATERNION_MAX); + int16_t q2_int = MAP_TO_INT16(q2, QUATERNION_MIN, QUATERNION_MAX); + int16_t q3_int = MAP_TO_INT16(q3, QUATERNION_MIN, QUATERNION_MAX); memcpy(&quat_frame.data[0], &q0_int, sizeof(int16_t)); memcpy(&quat_frame.data[2], &q1_int, sizeof(int16_t)); From 5207a4572744709299b708f5e76194afb9c6095a Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Mon, 6 Oct 2025 20:37:54 +0800 Subject: [PATCH 23/25] =?UTF-8?q?=E6=94=B9=E5=8F=91=E9=80=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/component/user_math.h | 4 ++++ User/task/imu.c | 13 +++++++------ 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/User/component/user_math.h b/User/component/user_math.h index d4531e6..1a7e73a 100644 --- a/User/component/user_math.h +++ b/User/component/user_math.h @@ -25,6 +25,10 @@ extern "C" { #define M_PI 3.14159265358979323846f #endif +#ifndef M_PI_2 +#define M_PI_2 1.57079632679489661923f +#endif + #ifndef M_2PI #define M_2PI 6.28318530717958647692f #endif diff --git a/User/task/imu.c b/User/task/imu.c index 4321e98..580e291 100644 --- a/User/task/imu.c +++ b/User/task/imu.c @@ -6,6 +6,7 @@ /* Includes ----------------------------------------------------------------- */ #include "task/user_task.h" /* USER INCLUDE BEGIN */ +#include "component/user_math.h" #include "bsp/can.h" #include "bsp/time.h" #include "component/ahrs.h" @@ -26,12 +27,12 @@ #define ACCEL_CAN_MIN (-58.8f) #define GYRO_CAN_MAX (34.88f) #define GYRO_CAN_MIN (-34.88f) -#define PITCH_CAN_MAX (90.0f) -#define PITCH_CAN_MIN (-90.0f) -#define ROLL_CAN_MAX (180.0f) -#define ROLL_CAN_MIN (-180.0f) -#define YAW_CAN_MAX (180.0f) -#define YAW_CAN_MIN (-180.0f) +#define PITCH_CAN_MAX (M_PI_2) /* π/2 弧度 ≈ 90° */ +#define PITCH_CAN_MIN (-M_PI_2) /* -π/2 弧度 ≈ -90° */ +#define ROLL_CAN_MAX (M_2PI) +#define ROLL_CAN_MIN (-M_2PI) +#define YAW_CAN_MAX (M_2PI) +#define YAW_CAN_MIN (-M_PI_2) /* -π 弧度 ≈ -180° */ #define QUATERNION_MIN (-1.0f) #define QUATERNION_MAX (1.0f) From 827a871299ad539f3c07459f07766611a3c27a60 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Mon, 6 Oct 2025 20:46:21 +0800 Subject: [PATCH 24/25] =?UTF-8?q?=E6=94=B9=E8=8C=83=E5=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/task/imu.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/User/task/imu.c b/User/task/imu.c index 580e291..e31a73c 100644 --- a/User/task/imu.c +++ b/User/task/imu.c @@ -27,12 +27,12 @@ #define ACCEL_CAN_MIN (-58.8f) #define GYRO_CAN_MAX (34.88f) #define GYRO_CAN_MIN (-34.88f) -#define PITCH_CAN_MAX (M_PI_2) /* π/2 弧度 ≈ 90° */ -#define PITCH_CAN_MIN (-M_PI_2) /* -π/2 弧度 ≈ -90° */ -#define ROLL_CAN_MAX (M_2PI) -#define ROLL_CAN_MIN (-M_2PI) -#define YAW_CAN_MAX (M_2PI) -#define YAW_CAN_MIN (-M_PI_2) /* -π 弧度 ≈ -180° */ +#define PITCH_CAN_MAX (M_PI_2) /* π/2 弧度 ≈ 90° */ +#define PITCH_CAN_MIN (-M_PI_2) /* -π/2 弧度 ≈ -90° */ +#define ROLL_CAN_MAX (M_PI) /* π 弧度 ≈ 180° */ +#define ROLL_CAN_MIN (-M_PI) /* -π 弧度 ≈ -180° */ +#define YAW_CAN_MAX (M_PI) /* π 弧度 ≈ 180° */ +#define YAW_CAN_MIN (-M_PI) /* -π 弧度 ≈ -180° */ #define QUATERNION_MIN (-1.0f) #define QUATERNION_MAX (1.0f) From d5357ba317e6e612320435755a6eb0f369bf2969 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Tue, 7 Oct 2025 03:17:46 +0800 Subject: [PATCH 25/25] =?UTF-8?q?=E6=94=B9imu=E6=95=B0=E6=8D=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/task/atti_esti.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/User/task/atti_esti.c b/User/task/atti_esti.c index 2c1f583..71edf46 100644 --- a/User/task/atti_esti.c +++ b/User/task/atti_esti.c @@ -34,7 +34,7 @@ BMI088_Cali_t cali_bmi088= { }; static const KPID_Params_t imu_temp_ctrl_pid_param = { - .k = 0.2f, + .k = 0.3f, .p = 1.0f, .i = 0.01f, .d = 0.0f, @@ -160,10 +160,10 @@ void Task_atti_esti(void *argument) { gyro_modified.y = -bmi088.gyro.x; gyro_modified.z = bmi088.gyro.z; - /* 欧拉角数据：roll和pitch互换 */ + /* 欧拉角数据：y取负值 */ eulr_modified.yaw = eulr_to_send.yaw; - eulr_modified.pit = eulr_to_send.rol; /* pitch = 原roll */ - eulr_modified.rol = eulr_to_send.pit; /* roll = 原pitch */ + eulr_modified.pit = -eulr_to_send.pit; + eulr_modified.rol = eulr_to_send.rol; osMessageQueueReset(task_runtime.msgq.imu.accl); osMessageQueueReset(task_runtime.msgq.imu.gyro); @@ -174,7 +174,7 @@ void Task_atti_esti(void *argument) { osMessageQueuePut(task_runtime.msgq.imu.eulr, &eulr_modified, 0, 0); osMessageQueuePut(task_runtime.msgq.imu.quat, &gimbal_ahrs.quat, 0, 0); - BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 50.0f, bmi088.temp, 0.0f, 0.0f)); + BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 40.0f, bmi088.temp, 0.0f, 0.0f)); /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */