xm/arm
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

batch 1: core project files (exclude drivers/middlewares/arm_model)

Browse Source
This commit is contained in:
xxxxm 2026-03-07 20:31:52 +08:00
commit e3179c0495
131 changed files with 24648 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
.clangd Normal file
View File

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

5
.gitignore vendored Normal file
View File

@ -0,0 +1,5 @@
build
mx.scratch
!.settings
.venv/
arm_model/*.zip

42
.mxproject Normal file
View File

File diff suppressed because one or more lines are too long

147
.settings/bundles-lock.store.json Normal file
View File

@ -0,0 +1,147 @@
{
"resolved": [
{
"name": "cmake",
"version": "4.0.1+st.3",
"platform": "darwin",
"selected_by": [
{
"name": "cmake",
"version": "4.0.1+st.3"
}
]
},
{
"name": "cmake",
"version": "4.0.1+st.3",
"platform": "x86_64-linux",
"selected_by": [
{
"name": "cmake",
"version": "4.0.1+st.3"
}
]
},
{
"name": "cmake",
"version": "4.0.1+st.3",
"platform": "x86_64-windows",
"selected_by": [
{
"name": "cmake",
"version": "4.0.1+st.3"
}
]
},
{
"name": "gnu-tools-for-stm32",
"version": "13.3.1+st.9",
"platform": "darwin",
"selected_by": [
{
"name": "gnu-tools-for-stm32",
"version": "13.3.1+st.9"
}
]
},
{
"name": "gnu-tools-for-stm32",
"version": "13.3.1+st.9",
"platform": "x86_64-linux",
"selected_by": [
{
"name": "gnu-tools-for-stm32",
"version": "13.3.1+st.9"
}
]
},
{
"name": "gnu-tools-for-stm32",
"version": "13.3.1+st.9",
"platform": "x86_64-windows",
"selected_by": [
{
"name": "gnu-tools-for-stm32",
"version": "13.3.1+st.9"
}
]
},
{
"name": "gnu-tools-for-stm32-13_3_1-description",
"version": "1.0.1+st.1",
"platform": "all",
"selected_by": [
{
"name": "gnu-tools-for-stm32-13_3_1-description",
"version": ">=0.0.1"
}
]
},
{
"name": "ninja",
"version": "1.13.1+st.1",
"platform": "darwin",
"selected_by": [
{
"name": "ninja",
"version": "1.13.1+st.1"
}
]
},
{
"name": "ninja",
"version": "1.13.1+st.1",
"platform": "x86_64-linux",
"selected_by": [
{
"name": "ninja",
"version": "1.13.1+st.1"
}
]
},
{
"name": "ninja",
"version": "1.13.1+st.1",
"platform": "x86_64-windows",
"selected_by": [
{
"name": "ninja",
"version": "1.13.1+st.1"
}
]
},
{
"name": "st-arm-clangd",
"version": "19.1.2+st.3",
"platform": "darwin",
"selected_by": [
{
"name": "st-arm-clangd",
"version": "19.1.2+st.3"
}
]
},
{
"name": "st-arm-clangd",
"version": "19.1.2+st.3",
"platform": "x86_64-linux",
"selected_by": [
{
"name": "st-arm-clangd",
"version": "19.1.2+st.3"
}
]
},
{
"name": "st-arm-clangd",
"version": "19.1.2+st.3",
"platform": "x86_64-windows",
"selected_by": [
{
"name": "st-arm-clangd",
"version": "19.1.2+st.3"
}
]
}
]
}

20
.settings/bundles.store.json Normal file
View File

@ -0,0 +1,20 @@
{
"bundles": [
{
"name": "cmake",
"version": "4.0.1+st.3"
},
{
"name": "ninja",
"version": "1.13.1+st.1"
},
{
"name": "gnu-tools-for-stm32",
"version": "13.3.1+st.9"
},
{
"name": "st-arm-clangd",
"version": "19.1.2+st.3"
}
]
}

6
.settings/ide.store.json Normal file
View File

@ -0,0 +1,6 @@
{
"device": "STM32F407IGH6",
"core": "Cortex-M4",
"order": 0,
"toolchain": "GCC"
}

8
.vscode/c_cpp_properties.json vendored Normal file
View File

@ -0,0 +1,8 @@
{
"configurations": [
{
"name": "STM32",
"compileCommands": "${workspaceFolder}/build/Debug/compile_commands.json"
}
]
}

15
.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,15 @@
{
"cmake.cmakePath": "cube-cmake",
"cmake.configureArgs": [
"-DCMAKE_COMMAND=cube-cmake"
],
"cmake.preferredGenerators": [
"Ninja"
],
"stm32cube-ide-clangd.path": "cube",
"stm32cube-ide-clangd.arguments": [
"starm-clangd",
"--query-driver=${env:CUBE_BUNDLE_PATH}/gnu-tools-for-stm32/13.3.1+st.9/bin/arm-none-eabi-gcc.exe",
"--query-driver=${env:CUBE_BUNDLE_PATH}/gnu-tools-for-stm32/13.3.1+st.9/bin/arm-none-eabi-g++.exe"
]
}

122
CMakeLists.txt Normal file
View File

@ -0,0 +1,122 @@
cmake_minimum_required(VERSION 3.22)
#
# This file is generated only once,
# and is not re-generated if converter is called multiple times.
#
# User is free to modify the file as much as necessary
#
# Setup compiler settings
set(CMAKE_C_STANDARD 11)
set(CMAKE_C_STANDARD_REQUIRED ON)
set(CMAKE_C_EXTENSIONS ON)
# Define the build type
if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE "Debug")
endif()
# Set the project name
set(CMAKE_PROJECT_NAME arm)
# Enable compile command to ease indexing with e.g. clangd
set(CMAKE_EXPORT_COMPILE_COMMANDS TRUE)
# Core project settings
project(${CMAKE_PROJECT_NAME})
message("Build type: " ${CMAKE_BUILD_TYPE})
# Enable CMake support for ASM and C languages
enable_language(C ASM)
# Create an executable object type
add_executable(${CMAKE_PROJECT_NAME})
# Add STM32CubeMX generated sources
add_subdirectory(cmake/stm32cubemx)
# Link directories setup
target_link_directories(${CMAKE_PROJECT_NAME} PRIVATE
# Add user defined library search paths
Middlewares/ST/ARM/DSP/Lib
)
# Add sources to executable
target_sources(${CMAKE_PROJECT_NAME} PRIVATE
# Add user sources here
# User/bsp sources
User/bsp/can.c
User/bsp/mm.c
User/bsp/pwm.c
User/bsp/time.c
User/bsp/uart.c
# User/component sources
User/component/pid.c
User/component/user_math.c
User/component/filter.c
User/component/PowerControl.c
# User/component/toolbox sources (C++)
User/component/toolbox/matrix.cpp
User/component/toolbox/robotics.cpp
User/component/toolbox/utils.cpp
# User/component/arm sources (C++)
User/component/arm_kinematics/arm6dof.cpp
# User/device sources
User/device/motor.c
User/device/motor_dm.c
User/device/motor_lz.c
User/device/motor_rm.c
User/device/dr16.c
# User/module sources (C++)
User/module/arm.cpp
User/module/config.c
User/module/chassis.c
User/module/cmd/cmd.c
User/module/cmd/cmd_adapter.c
User/module/cmd/cmd_behavior.c
User/module/cmd/cmd_example.c
# User/task sources
User/task/arm_main.cpp
User/task/blink.c
User/task/init.c
User/task/user_task.c
User/task/cmd.cpp
User/task/rc.c
User/task/ctrl_chassis.c
)
# Add include paths
target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE
# Add user defined include paths
User
User/component
User/component/toolbox
# ARM CMSIS DSP library include
Middlewares/ST/ARM/DSP/Inc
)
# Add project symbols (macros)
target_compile_definitions(${CMAKE_PROJECT_NAME} PRIVATE
ARM_MATH_CM4
ARM_MATH_MATRIX_CHECK
ARM_MATH_ROUNDING
# Add user defined symbols
)
# Remove wrong libob.a library dependency when using cpp files
list(REMOVE_ITEM CMAKE_C_IMPLICIT_LINK_LIBRARIES ob)
# Add linked libraries
target_link_libraries(${CMAKE_PROJECT_NAME}
# ARM CMSIS DSP library (Cortex-M4 with FPU)
${CMAKE_SOURCE_DIR}/Middlewares/ST/ARM/DSP/Lib/libarm_cortexM4lf_math.a
stm32cubemx
# Add user defined libraries
)

38
CMakePresets.json Normal file
View File

@ -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"
}
]
}

171
Core/Inc/FreeRTOSConfig.h Normal file
View File

@ -0,0 +1,171 @@
/* USER CODE BEGIN Header */
/*
* FreeRTOS Kernel V10.3.1
* Portion Copyright (C) 2017 Amazon.com, Inc. or its affiliates. All Rights Reserved.
* Portion Copyright (C) 2019 StMicroelectronics, Inc. All Rights Reserved.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of
* this software and associated documentation files (the "Software"), to deal in
* the Software without restriction, including without limitation the rights to
* use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
* the Software, and to permit persons to whom the Software is furnished to do so,
* subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
* COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
* IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*
* http://www.FreeRTOS.org
* http://aws.amazon.com/freertos
*
* 1 tab == 4 spaces!
*/
/* USER CODE END Header */
#ifndef FREERTOS_CONFIG_H
#define FREERTOS_CONFIG_H
/*-----------------------------------------------------------
* Application specific definitions.
*
* These definitions should be adjusted for your particular hardware and
* application requirements.
*
* These parameters and more are described within the 'configuration' section of the
* FreeRTOS API documentation available on the FreeRTOS.org web site.
*
* See http://www.freertos.org/a00110.html
*----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* Section where include file can be added */
/* USER CODE END Includes */
/* Ensure definitions are only used by the compiler, and not by the assembler. */
#if defined(__ICCARM__) || defined(__CC_ARM) || defined(__GNUC__)
#include <stdint.h>
extern uint32_t SystemCoreClock;
void xPortSysTickHandler(void);
#endif
#ifndef CMSIS_device_header
#define CMSIS_device_header "stm32f4xx.h"
#endif /* CMSIS_device_header */
#define configENABLE_FPU 0
#define configENABLE_MPU 0
#define configUSE_PREEMPTION 1
#define configSUPPORT_STATIC_ALLOCATION 1
#define configSUPPORT_DYNAMIC_ALLOCATION 1
#define configUSE_IDLE_HOOK 0
#define configUSE_TICK_HOOK 0
#define configCPU_CLOCK_HZ ( SystemCoreClock )
#define configTICK_RATE_HZ ((TickType_t)1000)
#define configMAX_PRIORITIES ( 56 )
#define configMINIMAL_STACK_SIZE ((uint16_t)128)
#define configTOTAL_HEAP_SIZE ((size_t)20480)
#define configMAX_TASK_NAME_LEN ( 16 )
#define configUSE_TRACE_FACILITY 1
#define configUSE_16_BIT_TICKS 0
#define configUSE_MUTEXES 1
#define configQUEUE_REGISTRY_SIZE 8
#define configUSE_RECURSIVE_MUTEXES 1
#define configUSE_COUNTING_SEMAPHORES 1
#define configUSE_PORT_OPTIMISED_TASK_SELECTION 0
/* USER CODE BEGIN MESSAGE_BUFFER_LENGTH_TYPE */
/* Defaults to size_t for backward compatibility, but can be changed
if lengths will always be less than the number of bytes in a size_t. */
#define configMESSAGE_BUFFER_LENGTH_TYPE size_t
/* USER CODE END MESSAGE_BUFFER_LENGTH_TYPE */
/* Co-routine definitions. */
#define configUSE_CO_ROUTINES 0
#define configMAX_CO_ROUTINE_PRIORITIES ( 2 )
/* Software timer definitions. */
#define configUSE_TIMERS 1
#define configTIMER_TASK_PRIORITY ( 2 )
#define configTIMER_QUEUE_LENGTH 10
#define configTIMER_TASK_STACK_DEPTH 256
/* CMSIS-RTOS V2 flags */
#define configUSE_OS2_THREAD_SUSPEND_RESUME 1
#define configUSE_OS2_THREAD_ENUMERATE 1
#define configUSE_OS2_EVENTFLAGS_FROM_ISR 1
#define configUSE_OS2_THREAD_FLAGS 1
#define configUSE_OS2_TIMER 1
#define configUSE_OS2_MUTEX 1
/* Set the following definitions to 1 to include the API function, or zero
to exclude the API function. */
#define INCLUDE_vTaskPrioritySet 1
#define INCLUDE_uxTaskPriorityGet 1
#define INCLUDE_vTaskDelete 1
#define INCLUDE_vTaskCleanUpResources 0
#define INCLUDE_vTaskSuspend 1
#define INCLUDE_vTaskDelayUntil 1
#define INCLUDE_vTaskDelay 1
#define INCLUDE_xTaskGetSchedulerState 1
#define INCLUDE_xTimerPendFunctionCall 1
#define INCLUDE_xQueueGetMutexHolder 1
#define INCLUDE_uxTaskGetStackHighWaterMark 1
#define INCLUDE_xTaskGetCurrentTaskHandle 1
#define INCLUDE_eTaskGetState 1
/*
* The CMSIS-RTOS V2 FreeRTOS wrapper is dependent on the heap implementation used
* by the application thus the correct define need to be enabled below
*/
#define USE_FreeRTOS_HEAP_4
/* Cortex-M specific definitions. */
#ifdef __NVIC_PRIO_BITS
/* __BVIC_PRIO_BITS will be specified when CMSIS is being used. */
#define configPRIO_BITS __NVIC_PRIO_BITS
#else
#define configPRIO_BITS 4
#endif
/* The lowest interrupt priority that can be used in a call to a "set priority"
function. */
#define configLIBRARY_LOWEST_INTERRUPT_PRIORITY 15
/* The highest interrupt priority that can be used by any interrupt service
routine that makes calls to interrupt safe FreeRTOS API functions. DO NOT CALL
INTERRUPT SAFE FREERTOS API FUNCTIONS FROM ANY INTERRUPT THAT HAS A HIGHER
PRIORITY THAN THIS! (higher priorities are lower numeric values. */
#define configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY 5
/* Interrupt priorities used by the kernel port layer itself. These are generic
to all Cortex-M ports, and do not rely on any particular library functions. */
#define configKERNEL_INTERRUPT_PRIORITY ( configLIBRARY_LOWEST_INTERRUPT_PRIORITY << (8 - configPRIO_BITS) )
/* !!!! configMAX_SYSCALL_INTERRUPT_PRIORITY must not be set to zero !!!!
See http://www.FreeRTOS.org/RTOS-Cortex-M3-M4.html. */
#define configMAX_SYSCALL_INTERRUPT_PRIORITY ( configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY << (8 - configPRIO_BITS) )
/* Normal assert() semantics without relying on the provision of an assert.h
header file. */
/* USER CODE BEGIN 1 */
#define configASSERT( x ) if ((x) == 0) {taskDISABLE_INTERRUPTS(); for( ;; );}
/* USER CODE END 1 */
/* Definitions that map the FreeRTOS port interrupt handlers to their CMSIS
standard names. */
#define vPortSVCHandler SVC_Handler
#define xPortPendSVHandler PendSV_Handler
/* IMPORTANT: After 10.3.1 update, Systick_Handler comes from NVIC (if SYS timebase = systick), otherwise from cmsis_os2.c */
#define USE_CUSTOM_SYSTICK_HANDLER_IMPLEMENTATION 1
/* USER CODE BEGIN Defines */
/* Section where parameter definitions can be added (for instance, to override default ones in FreeRTOS.h) */
/* USER CODE END Defines */
#endif /* FREERTOS_CONFIG_H */

55
Core/Inc/can.h Normal file
View File

@ -0,0 +1,55 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file can.h
* @brief This file contains all the function prototypes for
* the can.c file
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __CAN_H__
#define __CAN_H__
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "main.h"
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
void MX_CAN1_Init(void);
void MX_CAN2_Init(void);
/* USER CODE BEGIN Prototypes */
/* USER CODE END Prototypes */
#ifdef __cplusplus
}
#endif
#endif /* __CAN_H__ */

52
Core/Inc/dma.h Normal file
View File

@ -0,0 +1,52 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file dma.h
* @brief This file contains all the function prototypes for
* the dma.c file
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __DMA_H__
#define __DMA_H__
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "main.h"
/* DMA memory to memory transfer handles -------------------------------------*/
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
void MX_DMA_Init(void);
/* USER CODE BEGIN Prototypes */
/* USER CODE END Prototypes */
#ifdef __cplusplus
}
#endif
#endif /* __DMA_H__ */

49
Core/Inc/gpio.h Normal file
View File

@ -0,0 +1,49 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file gpio.h
* @brief This file contains all the function prototypes for
* the gpio.c file
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __GPIO_H__
#define __GPIO_H__
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "main.h"
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
void MX_GPIO_Init(void);
/* USER CODE BEGIN Prototypes */
/* USER CODE END Prototypes */
#ifdef __cplusplus
}
#endif
#endif /*__ GPIO_H__ */

69
Core/Inc/main.h Normal file
View File

@ -0,0 +1,69 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.h
* @brief : Header for main.c file.
* This file contains the common defines of the application.
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __MAIN_H
#define __MAIN_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* Exported types ------------------------------------------------------------*/
/* USER CODE BEGIN ET */
/* USER CODE END ET */
/* Exported constants --------------------------------------------------------*/
/* USER CODE BEGIN EC */
/* USER CODE END EC */
/* Exported macro ------------------------------------------------------------*/
/* USER CODE BEGIN EM */
/* USER CODE END EM */
/* Exported functions prototypes ---------------------------------------------*/
void Error_Handler(void);
/* USER CODE BEGIN EFP */
/* USER CODE END EFP */
/* Private defines -----------------------------------------------------------*/
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
#ifdef __cplusplus
}
#endif
#endif /* __MAIN_H */

25
Core/Inc/retarget.h Normal file
View File

@ -0,0 +1,25 @@
/**
******************************************************************************
* @file retarget.h
* @brief Printf 重定向头文件
******************************************************************************
*/
#ifndef __RETARGET_H
#define __RETARGET_H
#ifdef __cplusplus
extern "C" {
#endif
/**
* @brief 初始化 ITM（用于 SWO/printf 输出）
* @note 必须在使用 printf 之前调用
*/
void ITM_Init(void);
#ifdef __cplusplus
}
#endif
#endif /* __RETARGET_H */

495
Core/Inc/stm32f4xx_hal_conf.h Normal file
View File

@ -0,0 +1,495 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file stm32f4xx_hal_conf_template.h
* @author MCD Application Team
* @brief HAL configuration template file.
* This file should be copied to the application folder and renamed
* to stm32f4xx_hal_conf.h.
******************************************************************************
* @attention
*
* Copyright (c) 2017 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __STM32F4xx_HAL_CONF_H
#define __STM32F4xx_HAL_CONF_H
#ifdef __cplusplus
extern "C" {
#endif
/* Exported types ------------------------------------------------------------*/
/* Exported constants --------------------------------------------------------*/
/* ########################## Module Selection ############################## */
/**
* @brief This is the list of modules to be used in the HAL driver
*/
#define HAL_MODULE_ENABLED
/* #define HAL_CRYP_MODULE_ENABLED */
/* #define HAL_ADC_MODULE_ENABLED */
#define HAL_CAN_MODULE_ENABLED
/* #define HAL_CRC_MODULE_ENABLED */
/* #define HAL_CAN_LEGACY_MODULE_ENABLED */
/* #define HAL_DAC_MODULE_ENABLED */
/* #define HAL_DCMI_MODULE_ENABLED */
/* #define HAL_DMA2D_MODULE_ENABLED */
/* #define HAL_ETH_MODULE_ENABLED */
/* #define HAL_ETH_LEGACY_MODULE_ENABLED */
/* #define HAL_NAND_MODULE_ENABLED */
/* #define HAL_NOR_MODULE_ENABLED */
/* #define HAL_PCCARD_MODULE_ENABLED */
/* #define HAL_SRAM_MODULE_ENABLED */
/* #define HAL_SDRAM_MODULE_ENABLED */
/* #define HAL_HASH_MODULE_ENABLED */
/* #define HAL_I2C_MODULE_ENABLED */
/* #define HAL_I2S_MODULE_ENABLED */
/* #define HAL_IWDG_MODULE_ENABLED */
/* #define HAL_LTDC_MODULE_ENABLED */
/* #define HAL_RNG_MODULE_ENABLED */
/* #define HAL_RTC_MODULE_ENABLED */
/* #define HAL_SAI_MODULE_ENABLED */
/* #define HAL_SD_MODULE_ENABLED */
/* #define HAL_MMC_MODULE_ENABLED */
/* #define HAL_SPI_MODULE_ENABLED */
#define HAL_TIM_MODULE_ENABLED
#define HAL_UART_MODULE_ENABLED
/* #define HAL_USART_MODULE_ENABLED */
/* #define HAL_IRDA_MODULE_ENABLED */
/* #define HAL_SMARTCARD_MODULE_ENABLED */
/* #define HAL_SMBUS_MODULE_ENABLED */
/* #define HAL_WWDG_MODULE_ENABLED */
/* #define HAL_PCD_MODULE_ENABLED */
/* #define HAL_HCD_MODULE_ENABLED */
/* #define HAL_DSI_MODULE_ENABLED */
/* #define HAL_QSPI_MODULE_ENABLED */
/* #define HAL_QSPI_MODULE_ENABLED */
/* #define HAL_CEC_MODULE_ENABLED */
/* #define HAL_FMPI2C_MODULE_ENABLED */
/* #define HAL_FMPSMBUS_MODULE_ENABLED */
/* #define HAL_SPDIFRX_MODULE_ENABLED */
/* #define HAL_DFSDM_MODULE_ENABLED */
/* #define HAL_LPTIM_MODULE_ENABLED */
#define HAL_GPIO_MODULE_ENABLED
#define HAL_EXTI_MODULE_ENABLED
#define HAL_DMA_MODULE_ENABLED
#define HAL_RCC_MODULE_ENABLED
#define HAL_FLASH_MODULE_ENABLED
#define HAL_PWR_MODULE_ENABLED
#define HAL_CORTEX_MODULE_ENABLED
/* ########################## HSE/HSI Values adaptation ##################### */
/**
* @brief Adjust the value of External High Speed oscillator (HSE) used in your application.
* This value is used by the RCC HAL module to compute the system frequency
* (when HSE is used as system clock source, directly or through the PLL).
*/
#if !defined (HSE_VALUE)
#define HSE_VALUE 12000000U /*!< Value of the External oscillator in Hz */
#endif /* HSE_VALUE */
#if !defined (HSE_STARTUP_TIMEOUT)
#define HSE_STARTUP_TIMEOUT 100U /*!< Time out for HSE start up, in ms */
#endif /* HSE_STARTUP_TIMEOUT */
/**
* @brief Internal High Speed oscillator (HSI) value.
* This value is used by the RCC HAL module to compute the system frequency
* (when HSI is used as system clock source, directly or through the PLL).
*/
#if !defined (HSI_VALUE)
#define HSI_VALUE ((uint32_t)16000000U) /*!< Value of the Internal oscillator in Hz*/
#endif /* HSI_VALUE */
/**
* @brief Internal Low Speed oscillator (LSI) value.
*/
#if !defined (LSI_VALUE)
#define LSI_VALUE 32000U /*!< LSI Typical Value in Hz*/
#endif /* LSI_VALUE */ /*!< Value of the Internal Low Speed oscillator in Hz
The real value may vary depending on the variations
in voltage and temperature.*/
/**
* @brief External Low Speed oscillator (LSE) value.
*/
#if !defined (LSE_VALUE)
#define LSE_VALUE 32768U /*!< Value of the External Low Speed oscillator in Hz */
#endif /* LSE_VALUE */
#if !defined (LSE_STARTUP_TIMEOUT)
#define LSE_STARTUP_TIMEOUT 5000U /*!< Time out for LSE start up, in ms */
#endif /* LSE_STARTUP_TIMEOUT */
/**
* @brief External clock source for I2S peripheral
* This value is used by the I2S HAL module to compute the I2S clock source
* frequency, this source is inserted directly through I2S_CKIN pad.
*/
#if !defined (EXTERNAL_CLOCK_VALUE)
#define EXTERNAL_CLOCK_VALUE 12288000U /*!< Value of the External audio frequency in Hz*/
#endif /* EXTERNAL_CLOCK_VALUE */
/* Tip: To avoid modifying this file each time you need to use different HSE,
=== you can define the HSE value in your toolchain compiler preprocessor. */
/* ########################### System Configuration ######################### */
/**
* @brief This is the HAL system configuration section
*/
#define VDD_VALUE 3300U /*!< Value of VDD in mv */
#define TICK_INT_PRIORITY 15U /*!< tick interrupt priority */
#define USE_RTOS 0U
#define PREFETCH_ENABLE 1U
#define INSTRUCTION_CACHE_ENABLE 1U
#define DATA_CACHE_ENABLE 1U
#define USE_HAL_ADC_REGISTER_CALLBACKS 0U /* ADC register callback disabled */
#define USE_HAL_CAN_REGISTER_CALLBACKS 0U /* CAN register callback disabled */
#define USE_HAL_CEC_REGISTER_CALLBACKS 0U /* CEC register callback disabled */
#define USE_HAL_CRYP_REGISTER_CALLBACKS 0U /* CRYP register callback disabled */
#define USE_HAL_DAC_REGISTER_CALLBACKS 0U /* DAC register callback disabled */
#define USE_HAL_DCMI_REGISTER_CALLBACKS 0U /* DCMI register callback disabled */
#define USE_HAL_DFSDM_REGISTER_CALLBACKS 0U /* DFSDM register callback disabled */
#define USE_HAL_DMA2D_REGISTER_CALLBACKS 0U /* DMA2D register callback disabled */
#define USE_HAL_DSI_REGISTER_CALLBACKS 0U /* DSI register callback disabled */
#define USE_HAL_ETH_REGISTER_CALLBACKS 0U /* ETH register callback disabled */
#define USE_HAL_HASH_REGISTER_CALLBACKS 0U /* HASH register callback disabled */
#define USE_HAL_HCD_REGISTER_CALLBACKS 0U /* HCD register callback disabled */
#define USE_HAL_I2C_REGISTER_CALLBACKS 0U /* I2C register callback disabled */
#define USE_HAL_FMPI2C_REGISTER_CALLBACKS 0U /* FMPI2C register callback disabled */
#define USE_HAL_FMPSMBUS_REGISTER_CALLBACKS 0U /* FMPSMBUS register callback disabled */
#define USE_HAL_I2S_REGISTER_CALLBACKS 0U /* I2S register callback disabled */
#define USE_HAL_IRDA_REGISTER_CALLBACKS 0U /* IRDA register callback disabled */
#define USE_HAL_LPTIM_REGISTER_CALLBACKS 0U /* LPTIM register callback disabled */
#define USE_HAL_LTDC_REGISTER_CALLBACKS 0U /* LTDC register callback disabled */
#define USE_HAL_MMC_REGISTER_CALLBACKS 0U /* MMC register callback disabled */
#define USE_HAL_NAND_REGISTER_CALLBACKS 0U /* NAND register callback disabled */
#define USE_HAL_NOR_REGISTER_CALLBACKS 0U /* NOR register callback disabled */
#define USE_HAL_PCCARD_REGISTER_CALLBACKS 0U /* PCCARD register callback disabled */
#define USE_HAL_PCD_REGISTER_CALLBACKS 0U /* PCD register callback disabled */
#define USE_HAL_QSPI_REGISTER_CALLBACKS 0U /* QSPI register callback disabled */
#define USE_HAL_RNG_REGISTER_CALLBACKS 0U /* RNG register callback disabled */
#define USE_HAL_RTC_REGISTER_CALLBACKS 0U /* RTC register callback disabled */
#define USE_HAL_SAI_REGISTER_CALLBACKS 0U /* SAI register callback disabled */
#define USE_HAL_SD_REGISTER_CALLBACKS 0U /* SD register callback disabled */
#define USE_HAL_SMARTCARD_REGISTER_CALLBACKS 0U /* SMARTCARD register callback disabled */
#define USE_HAL_SDRAM_REGISTER_CALLBACKS 0U /* SDRAM register callback disabled */
#define USE_HAL_SRAM_REGISTER_CALLBACKS 0U /* SRAM register callback disabled */
#define USE_HAL_SPDIFRX_REGISTER_CALLBACKS 0U /* SPDIFRX register callback disabled */
#define USE_HAL_SMBUS_REGISTER_CALLBACKS 0U /* SMBUS register callback disabled */
#define USE_HAL_SPI_REGISTER_CALLBACKS 0U /* SPI register callback disabled */
#define USE_HAL_TIM_REGISTER_CALLBACKS 0U /* TIM register callback disabled */
#define USE_HAL_UART_REGISTER_CALLBACKS 0U /* UART register callback disabled */
#define USE_HAL_USART_REGISTER_CALLBACKS 0U /* USART register callback disabled */
#define USE_HAL_WWDG_REGISTER_CALLBACKS 0U /* WWDG register callback disabled */
/* ########################## Assert Selection ############################## */
/**
* @brief Uncomment the line below to expanse the "assert_param" macro in the
* HAL drivers code
*/
/* #define USE_FULL_ASSERT 1U */
/* ################## Ethernet peripheral configuration ##################### */
/* Section 1 : Ethernet peripheral configuration */
/* MAC ADDRESS: MAC_ADDR0:MAC_ADDR1:MAC_ADDR2:MAC_ADDR3:MAC_ADDR4:MAC_ADDR5 */
#define MAC_ADDR0 2U
#define MAC_ADDR1 0U
#define MAC_ADDR2 0U
#define MAC_ADDR3 0U
#define MAC_ADDR4 0U
#define MAC_ADDR5 0U
/* Definition of the Ethernet driver buffers size and count */
#define ETH_RX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for receive */
#define ETH_TX_BUF_SIZE ETH_MAX_PACKET_SIZE /* buffer size for transmit */
#define ETH_RXBUFNB 4U /* 4 Rx buffers of size ETH_RX_BUF_SIZE */
#define ETH_TXBUFNB 4U /* 4 Tx buffers of size ETH_TX_BUF_SIZE */
/* Section 2: PHY configuration section */
/* DP83848_PHY_ADDRESS Address*/
#define DP83848_PHY_ADDRESS
/* PHY Reset delay these values are based on a 1 ms Systick interrupt*/
#define PHY_RESET_DELAY 0x000000FFU
/* PHY Configuration delay */
#define PHY_CONFIG_DELAY 0x00000FFFU
#define PHY_READ_TO 0x0000FFFFU
#define PHY_WRITE_TO 0x0000FFFFU
/* Section 3: Common PHY Registers */
#define PHY_BCR ((uint16_t)0x0000U) /*!< Transceiver Basic Control Register */
#define PHY_BSR ((uint16_t)0x0001U) /*!< Transceiver Basic Status Register */
#define PHY_RESET ((uint16_t)0x8000U) /*!< PHY Reset */
#define PHY_LOOPBACK ((uint16_t)0x4000U) /*!< Select loop-back mode */
#define PHY_FULLDUPLEX_100M ((uint16_t)0x2100U) /*!< Set the full-duplex mode at 100 Mb/s */
#define PHY_HALFDUPLEX_100M ((uint16_t)0x2000U) /*!< Set the half-duplex mode at 100 Mb/s */
#define PHY_FULLDUPLEX_10M ((uint16_t)0x0100U) /*!< Set the full-duplex mode at 10 Mb/s */
#define PHY_HALFDUPLEX_10M ((uint16_t)0x0000U) /*!< Set the half-duplex mode at 10 Mb/s */
#define PHY_AUTONEGOTIATION ((uint16_t)0x1000U) /*!< Enable auto-negotiation function */
#define PHY_RESTART_AUTONEGOTIATION ((uint16_t)0x0200U) /*!< Restart auto-negotiation function */
#define PHY_POWERDOWN ((uint16_t)0x0800U) /*!< Select the power down mode */
#define PHY_ISOLATE ((uint16_t)0x0400U) /*!< Isolate PHY from MII */
#define PHY_AUTONEGO_COMPLETE ((uint16_t)0x0020U) /*!< Auto-Negotiation process completed */
#define PHY_LINKED_STATUS ((uint16_t)0x0004U) /*!< Valid link established */
#define PHY_JABBER_DETECTION ((uint16_t)0x0002U) /*!< Jabber condition detected */
/* Section 4: Extended PHY Registers */
#define PHY_SR ((uint16_t)) /*!< PHY status register Offset */
#define PHY_SPEED_STATUS ((uint16_t)) /*!< PHY Speed mask */
#define PHY_DUPLEX_STATUS ((uint16_t)) /*!< PHY Duplex mask */
/* ################## SPI peripheral configuration ########################## */
/* CRC FEATURE: Use to activate CRC feature inside HAL SPI Driver
* Activated: CRC code is present inside driver
* Deactivated: CRC code cleaned from driver
*/
#define USE_SPI_CRC 0U
/* Includes ------------------------------------------------------------------*/
/**
* @brief Include module's header file
*/
#ifdef HAL_RCC_MODULE_ENABLED
#include "stm32f4xx_hal_rcc.h"
#endif /* HAL_RCC_MODULE_ENABLED */
#ifdef HAL_GPIO_MODULE_ENABLED
#include "stm32f4xx_hal_gpio.h"
#endif /* HAL_GPIO_MODULE_ENABLED */
#ifdef HAL_EXTI_MODULE_ENABLED
#include "stm32f4xx_hal_exti.h"
#endif /* HAL_EXTI_MODULE_ENABLED */
#ifdef HAL_DMA_MODULE_ENABLED
#include "stm32f4xx_hal_dma.h"
#endif /* HAL_DMA_MODULE_ENABLED */
#ifdef HAL_CORTEX_MODULE_ENABLED
#include "stm32f4xx_hal_cortex.h"
#endif /* HAL_CORTEX_MODULE_ENABLED */
#ifdef HAL_ADC_MODULE_ENABLED
#include "stm32f4xx_hal_adc.h"
#endif /* HAL_ADC_MODULE_ENABLED */
#ifdef HAL_CAN_MODULE_ENABLED
#include "stm32f4xx_hal_can.h"
#endif /* HAL_CAN_MODULE_ENABLED */
#ifdef HAL_CAN_LEGACY_MODULE_ENABLED
#include "stm32f4xx_hal_can_legacy.h"
#endif /* HAL_CAN_LEGACY_MODULE_ENABLED */
#ifdef HAL_CRC_MODULE_ENABLED
#include "stm32f4xx_hal_crc.h"
#endif /* HAL_CRC_MODULE_ENABLED */
#ifdef HAL_CRYP_MODULE_ENABLED
#include "stm32f4xx_hal_cryp.h"
#endif /* HAL_CRYP_MODULE_ENABLED */
#ifdef HAL_DMA2D_MODULE_ENABLED
#include "stm32f4xx_hal_dma2d.h"
#endif /* HAL_DMA2D_MODULE_ENABLED */
#ifdef HAL_DAC_MODULE_ENABLED
#include "stm32f4xx_hal_dac.h"
#endif /* HAL_DAC_MODULE_ENABLED */
#ifdef HAL_DCMI_MODULE_ENABLED
#include "stm32f4xx_hal_dcmi.h"
#endif /* HAL_DCMI_MODULE_ENABLED */
#ifdef HAL_ETH_MODULE_ENABLED
#include "stm32f4xx_hal_eth.h"
#endif /* HAL_ETH_MODULE_ENABLED */
#ifdef HAL_ETH_LEGACY_MODULE_ENABLED
#include "stm32f4xx_hal_eth_legacy.h"
#endif /* HAL_ETH_LEGACY_MODULE_ENABLED */
#ifdef HAL_FLASH_MODULE_ENABLED
#include "stm32f4xx_hal_flash.h"
#endif /* HAL_FLASH_MODULE_ENABLED */
#ifdef HAL_SRAM_MODULE_ENABLED
#include "stm32f4xx_hal_sram.h"
#endif /* HAL_SRAM_MODULE_ENABLED */
#ifdef HAL_NOR_MODULE_ENABLED
#include "stm32f4xx_hal_nor.h"
#endif /* HAL_NOR_MODULE_ENABLED */
#ifdef HAL_NAND_MODULE_ENABLED
#include "stm32f4xx_hal_nand.h"
#endif /* HAL_NAND_MODULE_ENABLED */
#ifdef HAL_PCCARD_MODULE_ENABLED
#include "stm32f4xx_hal_pccard.h"
#endif /* HAL_PCCARD_MODULE_ENABLED */
#ifdef HAL_SDRAM_MODULE_ENABLED
#include "stm32f4xx_hal_sdram.h"
#endif /* HAL_SDRAM_MODULE_ENABLED */
#ifdef HAL_HASH_MODULE_ENABLED
#include "stm32f4xx_hal_hash.h"
#endif /* HAL_HASH_MODULE_ENABLED */
#ifdef HAL_I2C_MODULE_ENABLED
#include "stm32f4xx_hal_i2c.h"
#endif /* HAL_I2C_MODULE_ENABLED */
#ifdef HAL_SMBUS_MODULE_ENABLED
#include "stm32f4xx_hal_smbus.h"
#endif /* HAL_SMBUS_MODULE_ENABLED */
#ifdef HAL_I2S_MODULE_ENABLED
#include "stm32f4xx_hal_i2s.h"
#endif /* HAL_I2S_MODULE_ENABLED */
#ifdef HAL_IWDG_MODULE_ENABLED
#include "stm32f4xx_hal_iwdg.h"
#endif /* HAL_IWDG_MODULE_ENABLED */
#ifdef HAL_LTDC_MODULE_ENABLED
#include "stm32f4xx_hal_ltdc.h"
#endif /* HAL_LTDC_MODULE_ENABLED */
#ifdef HAL_PWR_MODULE_ENABLED
#include "stm32f4xx_hal_pwr.h"
#endif /* HAL_PWR_MODULE_ENABLED */
#ifdef HAL_RNG_MODULE_ENABLED
#include "stm32f4xx_hal_rng.h"
#endif /* HAL_RNG_MODULE_ENABLED */
#ifdef HAL_RTC_MODULE_ENABLED
#include "stm32f4xx_hal_rtc.h"
#endif /* HAL_RTC_MODULE_ENABLED */
#ifdef HAL_SAI_MODULE_ENABLED
#include "stm32f4xx_hal_sai.h"
#endif /* HAL_SAI_MODULE_ENABLED */
#ifdef HAL_SD_MODULE_ENABLED
#include "stm32f4xx_hal_sd.h"
#endif /* HAL_SD_MODULE_ENABLED */
#ifdef HAL_SPI_MODULE_ENABLED
#include "stm32f4xx_hal_spi.h"
#endif /* HAL_SPI_MODULE_ENABLED */
#ifdef HAL_TIM_MODULE_ENABLED
#include "stm32f4xx_hal_tim.h"
#endif /* HAL_TIM_MODULE_ENABLED */
#ifdef HAL_UART_MODULE_ENABLED
#include "stm32f4xx_hal_uart.h"
#endif /* HAL_UART_MODULE_ENABLED */
#ifdef HAL_USART_MODULE_ENABLED
#include "stm32f4xx_hal_usart.h"
#endif /* HAL_USART_MODULE_ENABLED */
#ifdef HAL_IRDA_MODULE_ENABLED
#include "stm32f4xx_hal_irda.h"
#endif /* HAL_IRDA_MODULE_ENABLED */
#ifdef HAL_SMARTCARD_MODULE_ENABLED
#include "stm32f4xx_hal_smartcard.h"
#endif /* HAL_SMARTCARD_MODULE_ENABLED */
#ifdef HAL_WWDG_MODULE_ENABLED
#include "stm32f4xx_hal_wwdg.h"
#endif /* HAL_WWDG_MODULE_ENABLED */
#ifdef HAL_PCD_MODULE_ENABLED
#include "stm32f4xx_hal_pcd.h"
#endif /* HAL_PCD_MODULE_ENABLED */
#ifdef HAL_HCD_MODULE_ENABLED
#include "stm32f4xx_hal_hcd.h"
#endif /* HAL_HCD_MODULE_ENABLED */
#ifdef HAL_DSI_MODULE_ENABLED
#include "stm32f4xx_hal_dsi.h"
#endif /* HAL_DSI_MODULE_ENABLED */
#ifdef HAL_QSPI_MODULE_ENABLED
#include "stm32f4xx_hal_qspi.h"
#endif /* HAL_QSPI_MODULE_ENABLED */
#ifdef HAL_CEC_MODULE_ENABLED
#include "stm32f4xx_hal_cec.h"
#endif /* HAL_CEC_MODULE_ENABLED */
#ifdef HAL_FMPI2C_MODULE_ENABLED
#include "stm32f4xx_hal_fmpi2c.h"
#endif /* HAL_FMPI2C_MODULE_ENABLED */
#ifdef HAL_FMPSMBUS_MODULE_ENABLED
#include "stm32f4xx_hal_fmpsmbus.h"
#endif /* HAL_FMPSMBUS_MODULE_ENABLED */
#ifdef HAL_SPDIFRX_MODULE_ENABLED
#include "stm32f4xx_hal_spdifrx.h"
#endif /* HAL_SPDIFRX_MODULE_ENABLED */
#ifdef HAL_DFSDM_MODULE_ENABLED
#include "stm32f4xx_hal_dfsdm.h"
#endif /* HAL_DFSDM_MODULE_ENABLED */
#ifdef HAL_LPTIM_MODULE_ENABLED
#include "stm32f4xx_hal_lptim.h"
#endif /* HAL_LPTIM_MODULE_ENABLED */
#ifdef HAL_MMC_MODULE_ENABLED
#include "stm32f4xx_hal_mmc.h"
#endif /* HAL_MMC_MODULE_ENABLED */
/* Exported macro ------------------------------------------------------------*/
#ifdef USE_FULL_ASSERT
/**
* @brief The assert_param macro is used for function's parameters check.
* @param expr If expr is false, it calls assert_failed function
* which reports the name of the source file and the source
* line number of the call that failed.
* If expr is true, it returns no value.
* @retval None
*/
#define assert_param(expr) ((expr) ? (void)0U : assert_failed((uint8_t *)__FILE__, __LINE__))
/* Exported functions ------------------------------------------------------- */
void assert_failed(uint8_t* file, uint32_t line);
#else
#define assert_param(expr) ((void)0U)
#endif /* USE_FULL_ASSERT */
#ifdef __cplusplus
}
#endif
#endif /* __STM32F4xx_HAL_CONF_H */

72
Core/Inc/stm32f4xx_it.h Normal file
View File

@ -0,0 +1,72 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file stm32f4xx_it.h
* @brief This file contains the headers of the interrupt handlers.
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __STM32F4xx_IT_H
#define __STM32F4xx_IT_H
#ifdef __cplusplus
extern "C" {
#endif
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* Exported types ------------------------------------------------------------*/
/* USER CODE BEGIN ET */
/* USER CODE END ET */
/* Exported constants --------------------------------------------------------*/
/* USER CODE BEGIN EC */
/* USER CODE END EC */
/* Exported macro ------------------------------------------------------------*/
/* USER CODE BEGIN EM */
/* USER CODE END EM */
/* Exported functions prototypes ---------------------------------------------*/
void NMI_Handler(void);
void HardFault_Handler(void);
void MemManage_Handler(void);
void BusFault_Handler(void);
void UsageFault_Handler(void);
void DebugMon_Handler(void);
void SysTick_Handler(void);
void DMA1_Stream1_IRQHandler(void);
void CAN1_TX_IRQHandler(void);
void CAN1_RX0_IRQHandler(void);
void CAN1_RX1_IRQHandler(void);
void TIM5_IRQHandler(void);
void CAN2_TX_IRQHandler(void);
void CAN2_RX0_IRQHandler(void);
void CAN2_RX1_IRQHandler(void);
/* USER CODE BEGIN EFP */
/* USER CODE END EFP */
#ifdef __cplusplus
}
#endif
#endif /* __STM32F4xx_IT_H */

54
Core/Inc/tim.h Normal file
View File

@ -0,0 +1,54 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file tim.h
* @brief This file contains all the function prototypes for
* the tim.c file
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __TIM_H__
#define __TIM_H__
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "main.h"
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
extern TIM_HandleTypeDef htim5;
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
void MX_TIM5_Init(void);
void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
/* USER CODE BEGIN Prototypes */
/* USER CODE END Prototypes */
#ifdef __cplusplus
}
#endif
#endif /* __TIM_H__ */

52
Core/Inc/usart.h Normal file
View File

@ -0,0 +1,52 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file usart.h
* @brief This file contains all the function prototypes for
* the usart.c file
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __USART_H__
#define __USART_H__
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "main.h"
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
extern UART_HandleTypeDef huart3;
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
void MX_USART3_UART_Init(void);
/* USER CODE BEGIN Prototypes */
/* USER CODE END Prototypes */
#ifdef __cplusplus
}
#endif
#endif /* __USART_H__ */

230
Core/Src/can.c Normal file
View File

@ -0,0 +1,230 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file can.c
* @brief This file provides code for the configuration
* of the CAN instances.
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "can.h"
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
CAN_HandleTypeDef hcan1;
CAN_HandleTypeDef hcan2;
/* CAN1 init function */
void MX_CAN1_Init(void)
{
/* USER CODE BEGIN CAN1_Init 0 */
/* USER CODE END CAN1_Init 0 */
/* USER CODE BEGIN CAN1_Init 1 */
/* USER CODE END CAN1_Init 1 */
hcan1.Instance = CAN1;
hcan1.Init.Prescaler = 3;
hcan1.Init.Mode = CAN_MODE_NORMAL;
hcan1.Init.SyncJumpWidth = CAN_SJW_1TQ;
hcan1.Init.TimeSeg1 = CAN_BS1_10TQ;
hcan1.Init.TimeSeg2 = CAN_BS2_3TQ;
hcan1.Init.TimeTriggeredMode = DISABLE;
hcan1.Init.AutoBusOff = DISABLE;
hcan1.Init.AutoWakeUp = DISABLE;
hcan1.Init.AutoRetransmission = ENABLE;
hcan1.Init.ReceiveFifoLocked = DISABLE;
hcan1.Init.TransmitFifoPriority = DISABLE;
if (HAL_CAN_Init(&hcan1) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN CAN1_Init 2 */
/* USER CODE END CAN1_Init 2 */
}
/* CAN2 init function */
void MX_CAN2_Init(void)
{
/* USER CODE BEGIN CAN2_Init 0 */
/* USER CODE END CAN2_Init 0 */
/* USER CODE BEGIN CAN2_Init 1 */
/* USER CODE END CAN2_Init 1 */
hcan2.Instance = CAN2;
hcan2.Init.Prescaler = 3;
hcan2.Init.Mode = CAN_MODE_NORMAL;
hcan2.Init.SyncJumpWidth = CAN_SJW_1TQ;
hcan2.Init.TimeSeg1 = CAN_BS1_10TQ;
hcan2.Init.TimeSeg2 = CAN_BS2_3TQ;
hcan2.Init.TimeTriggeredMode = DISABLE;
hcan2.Init.AutoBusOff = DISABLE;
hcan2.Init.AutoWakeUp = DISABLE;
hcan2.Init.AutoRetransmission = ENABLE;
hcan2.Init.ReceiveFifoLocked = DISABLE;
hcan2.Init.TransmitFifoPriority = DISABLE;
if (HAL_CAN_Init(&hcan2) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN CAN2_Init 2 */
/* USER CODE END CAN2_Init 2 */
}
static uint32_t HAL_RCC_CAN1_CLK_ENABLED=0;
void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if(canHandle->Instance==CAN1)
{
/* USER CODE BEGIN CAN1_MspInit 0 */
/* USER CODE END CAN1_MspInit 0 */
/* CAN1 clock enable */
HAL_RCC_CAN1_CLK_ENABLED++;
if(HAL_RCC_CAN1_CLK_ENABLED==1){
__HAL_RCC_CAN1_CLK_ENABLE();
}
__HAL_RCC_GPIOD_CLK_ENABLE();
/**CAN1 GPIO Configuration
PD0 ------> CAN1_RX
PD1 ------> CAN1_TX
*/
GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF9_CAN1;
HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
/* CAN1 interrupt Init */
HAL_NVIC_SetPriority(CAN1_TX_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(CAN1_TX_IRQn);
HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);
HAL_NVIC_SetPriority(CAN1_RX1_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(CAN1_RX1_IRQn);
/* USER CODE BEGIN CAN1_MspInit 1 */
/* USER CODE END CAN1_MspInit 1 */
}
else if(canHandle->Instance==CAN2)
{
/* USER CODE BEGIN CAN2_MspInit 0 */
/* USER CODE END CAN2_MspInit 0 */
/* CAN2 clock enable */
__HAL_RCC_CAN2_CLK_ENABLE();
HAL_RCC_CAN1_CLK_ENABLED++;
if(HAL_RCC_CAN1_CLK_ENABLED==1){
__HAL_RCC_CAN1_CLK_ENABLE();
}
__HAL_RCC_GPIOB_CLK_ENABLE();
/**CAN2 GPIO Configuration
PB5 ------> CAN2_RX
PB6 ------> CAN2_TX
*/
GPIO_InitStruct.Pin = GPIO_PIN_5|GPIO_PIN_6;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF9_CAN2;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* CAN2 interrupt Init */
HAL_NVIC_SetPriority(CAN2_TX_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(CAN2_TX_IRQn);
HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn);
HAL_NVIC_SetPriority(CAN2_RX1_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(CAN2_RX1_IRQn);
/* USER CODE BEGIN CAN2_MspInit 1 */
/* USER CODE END CAN2_MspInit 1 */
}
}
void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
{
if(canHandle->Instance==CAN1)
{
/* USER CODE BEGIN CAN1_MspDeInit 0 */
/* USER CODE END CAN1_MspDeInit 0 */
/* Peripheral clock disable */
HAL_RCC_CAN1_CLK_ENABLED--;
if(HAL_RCC_CAN1_CLK_ENABLED==0){
__HAL_RCC_CAN1_CLK_DISABLE();
}
/**CAN1 GPIO Configuration
PD0 ------> CAN1_RX
PD1 ------> CAN1_TX
*/
HAL_GPIO_DeInit(GPIOD, GPIO_PIN_0|GPIO_PIN_1);
/* CAN1 interrupt Deinit */
HAL_NVIC_DisableIRQ(CAN1_TX_IRQn);
HAL_NVIC_DisableIRQ(CAN1_RX0_IRQn);
HAL_NVIC_DisableIRQ(CAN1_RX1_IRQn);
/* USER CODE BEGIN CAN1_MspDeInit 1 */
/* USER CODE END CAN1_MspDeInit 1 */
}
else if(canHandle->Instance==CAN2)
{
/* USER CODE BEGIN CAN2_MspDeInit 0 */
/* USER CODE END CAN2_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_CAN2_CLK_DISABLE();
HAL_RCC_CAN1_CLK_ENABLED--;
if(HAL_RCC_CAN1_CLK_ENABLED==0){
__HAL_RCC_CAN1_CLK_DISABLE();
}
/**CAN2 GPIO Configuration
PB5 ------> CAN2_RX
PB6 ------> CAN2_TX
*/
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_5|GPIO_PIN_6);
/* CAN2 interrupt Deinit */
HAL_NVIC_DisableIRQ(CAN2_TX_IRQn);
HAL_NVIC_DisableIRQ(CAN2_RX0_IRQn);
HAL_NVIC_DisableIRQ(CAN2_RX1_IRQn);
/* USER CODE BEGIN CAN2_MspDeInit 1 */
/* USER CODE END CAN2_MspDeInit 1 */
}
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */

55
Core/Src/dma.c Normal file
View File

@ -0,0 +1,55 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file dma.c
* @brief This file provides code for the configuration
* of all the requested memory to memory DMA transfers.
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "dma.h"
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/*----------------------------------------------------------------------------*/
/* Configure DMA */
/*----------------------------------------------------------------------------*/
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/**
* Enable DMA controller clock
*/
void MX_DMA_Init(void)
{
/* DMA controller clock enable */
__HAL_RCC_DMA1_CLK_ENABLE();
/* DMA interrupt init */
/* DMA1_Stream1_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Stream1_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA1_Stream1_IRQn);
}
/* USER CODE BEGIN 2 */
/* USER CODE END 2 */

127
Core/Src/freertos.c Normal file
View File

@ -0,0 +1,127 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* File Name : freertos.c
* Description : Code for freertos applications
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h"
#include "task.h"
#include "main.h"
#include "cmsis_os.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "task/user_task.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
/* USER CODE END Variables */
/* Definitions for defaultTask */
osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = {
.name = "defaultTask",
.stack_size = 128 * 4,
.priority = (osPriority_t) osPriorityNormal,
};
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
/* USER CODE END FunctionPrototypes */
void StartDefaultTask(void *argument);
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
/**
* @brief FreeRTOS initialization
* @param None
* @retval None
*/
void MX_FREERTOS_Init(void) {
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* USER CODE BEGIN RTOS_MUTEX */
/* add mutexes, ... */
/* USER CODE END RTOS_MUTEX */
/* USER CODE BEGIN RTOS_SEMAPHORES */
/* add semaphores, ... */
/* USER CODE END RTOS_SEMAPHORES */
/* USER CODE BEGIN RTOS_TIMERS */
/* start timers, add new ones, ... */
/* USER CODE END RTOS_TIMERS */
/* USER CODE BEGIN RTOS_QUEUES */
/* add queues, ... */
/* USER CODE END RTOS_QUEUES */
/* Create the thread(s) */
/* creation of defaultTask */
defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);
/* USER CODE BEGIN RTOS_THREADS */
/* add threads, ... */
osThreadNew(Task_Init, NULL, &attr_init); // 创建初始化任务
/* USER CODE END RTOS_THREADS */
/* USER CODE BEGIN RTOS_EVENTS */
/* add events, ... */
/* USER CODE END RTOS_EVENTS */
}
/* USER CODE BEGIN Header_StartDefaultTask */
/**
* @brief Function implementing the defaultTask thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument)
{
/* USER CODE BEGIN StartDefaultTask */
osThreadTerminate(osThreadGetId());
/* USER CODE END StartDefaultTask */
}
/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
/* USER CODE END Application */

56
Core/Src/gpio.c Normal file
View File

@ -0,0 +1,56 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file gpio.c
* @brief This file provides code for the configuration
* of all used GPIO pins.
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "gpio.h"
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/*----------------------------------------------------------------------------*/
/* Configure GPIO */
/*----------------------------------------------------------------------------*/
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/** Configure pins as
* Analog
* Input
* Output
* EVENT_OUT
* EXTI
*/
void MX_GPIO_Init(void)
{
/* GPIO Ports Clock Enable */
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOD_CLK_ENABLE();
__HAL_RCC_GPIOC_CLK_ENABLE();
__HAL_RCC_GPIOH_CLK_ENABLE();
}
/* USER CODE BEGIN 2 */
/* USER CODE END 2 */

202
Core/Src/main.c Normal file
View File

@ -0,0 +1,202 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "cmsis_os.h"
#include "can.h"
#include "dma.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "retarget.h"
#include <stdio.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 PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
void MX_FREERTOS_Init(void);
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_DMA_Init();
MX_CAN1_Init();
MX_CAN2_Init();
MX_TIM5_Init();
MX_USART3_UART_Init();
/* USER CODE BEGIN 2 */
/* USER CODE END 2 */
/* Init scheduler */
osKernelInitialize(); /* Call init function for freertos objects (in cmsis_os2.c) */
MX_FREERTOS_Init();
/* Start scheduler */
osKernelStart();
/* We should never get here as control is now taken by the scheduler */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
/** Configure the main internal regulator output voltage
*/
__HAL_RCC_PWR_CLK_ENABLE();
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
RCC_OscInitStruct.HSEState = RCC_HSE_ON;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
RCC_OscInitStruct.PLL.PLLM = 6;
RCC_OscInitStruct.PLL.PLLN = 168;
RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
RCC_OscInitStruct.PLL.PLLQ = 4;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK)
{
Error_Handler();
}
}
/* USER CODE BEGIN 4 */
/* USER CODE END 4 */
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
}
/* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t *file, uint32_t line)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

111
Core/Src/retarget.c Normal file
View File

@ -0,0 +1,111 @@
/**
******************************************************************************
* @file retarget.c
* @brief Printf 重定向实现（支持 Ozone SWO/ITM 调试）
* @note 提供多种 printf 输出方式
******************************************************************************
*/
#include "stm32f4xx.h"
#include <stdio.h>
/* 配置选择：根据需求选择一种方式 */
#define USE_ITM_SWO 1 // 使用 ITM/SWO (Ozone Terminal 查看)
#define USE_UART 0 // 使用 UART 串口
#define USE_SEMIHOSTING 0 // 使用半主机模式
/* ========== 方案1: ITM/SWO 输出（Ozone 原生支持）========== */
#if USE_ITM_SWO
/**
* @brief 初始化 ITM（必须在 main 开始时调用）
* @note 在 main() 函数的 USER CODE BEGIN 2 中调用此函数
*/
void ITM_Init(void)
{
// 1. 启用 TRCENA (Trace Enable)
CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
// 2. 解锁 ITM 访问
ITM->LAR = 0xC5ACCE55;
// 3. 启用 ITM
ITM->TCR = ITM_TCR_ITMENA_Msk;
// 4. 启用 ITM 端口0（用于 printf）
ITM->TER = 0x01;
// 5. 设置 ITM 的 Trace Privilege
ITM->TPR = 0x00;
}
/**
* @brief 通过 ITM 端口0 发送字符
* @note Ozone 中需要配置：Tools -> Terminal -> Enable SWO
*/
int __io_putchar(int ch)
{
// 直接发送，不检查（因为已经初始化过了）
while (ITM->PORT[0].u32 == 0UL)
{
__NOP(); // 等待端口就绪
}
ITM->PORT[0].u8 = (uint8_t)ch;
return ch;
}
#endif // USE_ITM_SWO
/* ========== 方案2: UART 串口输出 ========== */
#if USE_UART
#include "usart.h" // 根据你的项目调整头文件
/**
* @brief 通过 UART 发送字符
* @note 需要先在 CubeMX 中配置好 UART
*/
int __io_putchar(int ch)
{
// 假设使用 UART1，根据实际情况修改
HAL_UART_Transmit(&huart1, (uint8_t *)&ch, 1, HAL_MAX_DELAY);
return ch;
}
#endif // USE_UART
/* ========== 方案3: 半主机模式（Semihosting）========== */
#if USE_SEMIHOSTING
/**
* @brief 半主机模式输出
* @note 需要在链接选项中添加 --specs=rdimon.specs
* 并在 main 函数前调用 initialise_monitor_handles();
*/
// 半主机模式使用系统默认实现，无需额外代码
// 但需要在 main() 中调用：
// extern void initialise_monitor_handles(void);
// initialise_monitor_handles();
#endif // USE_SEMIHOSTING
/* ========== getchar 实现（可选）========== */
#if USE_ITM_SWO || USE_UART
int __io_getchar(void)
{
// 简单实现：返回 EOF
return EOF;
/* 如果需要从 UART 读取，可以这样实现：
uint8_t ch;
HAL_UART_Receive(&huart1, &ch, 1, HAL_MAX_DELAY);
return ch;
*/
}
#endif

84
Core/Src/stm32f4xx_hal_msp.c Normal file
View File

@ -0,0 +1,84 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file stm32f4xx_hal_msp.c
* @brief This file provides code for the MSP Initialization
* and de-Initialization codes.
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN TD */
/* USER CODE END TD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN Define */
/* USER CODE END Define */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN Macro */
/* USER CODE END Macro */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* External functions --------------------------------------------------------*/
/* USER CODE BEGIN ExternalFunctions */
/* USER CODE END ExternalFunctions */
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/**
* Initializes the Global MSP.
*/
void HAL_MspInit(void)
{
/* USER CODE BEGIN MspInit 0 */
/* USER CODE END MspInit 0 */
__HAL_RCC_SYSCFG_CLK_ENABLE();
__HAL_RCC_PWR_CLK_ENABLE();
/* System interrupt init*/
/* PendSV_IRQn interrupt configuration */
HAL_NVIC_SetPriority(PendSV_IRQn, 15, 0);
/* USER CODE BEGIN MspInit 1 */
/* USER CODE END MspInit 1 */
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */

302
Core/Src/stm32f4xx_it.c Normal file
View File

@ -0,0 +1,302 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file stm32f4xx_it.c
* @brief Interrupt Service Routines.
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "stm32f4xx_it.h"
#include "FreeRTOS.h"
#include "task.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN TD */
/* USER CODE END TD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN PV */
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN PFP */
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/* External variables --------------------------------------------------------*/
extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;
extern TIM_HandleTypeDef htim5;
extern DMA_HandleTypeDef hdma_usart3_rx;
/* USER CODE BEGIN EV */
/* USER CODE END EV */
/******************************************************************************/
/* Cortex-M4 Processor Interruption and Exception Handlers */
/******************************************************************************/
/**
* @brief This function handles Non maskable interrupt.
*/
void NMI_Handler(void)
{
/* USER CODE BEGIN NonMaskableInt_IRQn 0 */
/* USER CODE END NonMaskableInt_IRQn 0 */
/* USER CODE BEGIN NonMaskableInt_IRQn 1 */
while (1)
{
}
/* USER CODE END NonMaskableInt_IRQn 1 */
}
/**
* @brief This function handles Hard fault interrupt.
*/
void HardFault_Handler(void)
{
/* USER CODE BEGIN HardFault_IRQn 0 */
/* USER CODE END HardFault_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_HardFault_IRQn 0 */
/* USER CODE END W1_HardFault_IRQn 0 */
}
}
/**
* @brief This function handles Memory management fault.
*/
void MemManage_Handler(void)
{
/* USER CODE BEGIN MemoryManagement_IRQn 0 */
/* USER CODE END MemoryManagement_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_MemoryManagement_IRQn 0 */
/* USER CODE END W1_MemoryManagement_IRQn 0 */
}
}
/**
* @brief This function handles Pre-fetch fault, memory access fault.
*/
void BusFault_Handler(void)
{
/* USER CODE BEGIN BusFault_IRQn 0 */
/* USER CODE END BusFault_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_BusFault_IRQn 0 */
/* USER CODE END W1_BusFault_IRQn 0 */
}
}
/**
* @brief This function handles Undefined instruction or illegal state.
*/
void UsageFault_Handler(void)
{
/* USER CODE BEGIN UsageFault_IRQn 0 */
/* USER CODE END UsageFault_IRQn 0 */
while (1)
{
/* USER CODE BEGIN W1_UsageFault_IRQn 0 */
/* USER CODE END W1_UsageFault_IRQn 0 */
}
}
/**
* @brief This function handles Debug monitor.
*/
void DebugMon_Handler(void)
{
/* USER CODE BEGIN DebugMonitor_IRQn 0 */
/* USER CODE END DebugMonitor_IRQn 0 */
/* USER CODE BEGIN DebugMonitor_IRQn 1 */
/* USER CODE END DebugMonitor_IRQn 1 */
}
/**
* @brief This function handles System tick timer.
*/
void SysTick_Handler(void)
{
/* USER CODE BEGIN SysTick_IRQn 0 */
/* USER CODE END SysTick_IRQn 0 */
HAL_IncTick();
#if (INCLUDE_xTaskGetSchedulerState == 1 )
if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED)
{
#endif /* INCLUDE_xTaskGetSchedulerState */
xPortSysTickHandler();
#if (INCLUDE_xTaskGetSchedulerState == 1 )
}
#endif /* INCLUDE_xTaskGetSchedulerState */
/* USER CODE BEGIN SysTick_IRQn 1 */
/* USER CODE END SysTick_IRQn 1 */
}
/******************************************************************************/
/* STM32F4xx Peripheral Interrupt Handlers */
/* Add here the Interrupt Handlers for the used peripherals. */
/* For the available peripheral interrupt handler names, */
/* please refer to the startup file (startup_stm32f4xx.s). */
/******************************************************************************/
/**
* @brief This function handles DMA1 stream1 global interrupt.
*/
void DMA1_Stream1_IRQHandler(void)
{
/* USER CODE BEGIN DMA1_Stream1_IRQn 0 */
/* USER CODE END DMA1_Stream1_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_usart3_rx);
/* USER CODE BEGIN DMA1_Stream1_IRQn 1 */
/* USER CODE END DMA1_Stream1_IRQn 1 */
}
/**
* @brief This function handles CAN1 TX interrupts.
*/
void CAN1_TX_IRQHandler(void)
{
/* USER CODE BEGIN CAN1_TX_IRQn 0 */
/* USER CODE END CAN1_TX_IRQn 0 */
HAL_CAN_IRQHandler(&hcan1);
/* USER CODE BEGIN CAN1_TX_IRQn 1 */
/* USER CODE END CAN1_TX_IRQn 1 */
}
/**
* @brief This function handles CAN1 RX0 interrupts.
*/
void CAN1_RX0_IRQHandler(void)
{
/* USER CODE BEGIN CAN1_RX0_IRQn 0 */
/* USER CODE END CAN1_RX0_IRQn 0 */
HAL_CAN_IRQHandler(&hcan1);
/* USER CODE BEGIN CAN1_RX0_IRQn 1 */
/* USER CODE END CAN1_RX0_IRQn 1 */
}
/**
* @brief This function handles CAN1 RX1 interrupt.
*/
void CAN1_RX1_IRQHandler(void)
{
/* USER CODE BEGIN CAN1_RX1_IRQn 0 */
/* USER CODE END CAN1_RX1_IRQn 0 */
HAL_CAN_IRQHandler(&hcan1);
/* USER CODE BEGIN CAN1_RX1_IRQn 1 */
/* USER CODE END CAN1_RX1_IRQn 1 */
}
/**
* @brief This function handles TIM5 global interrupt.
*/
void TIM5_IRQHandler(void)
{
/* USER CODE BEGIN TIM5_IRQn 0 */
/* USER CODE END TIM5_IRQn 0 */
HAL_TIM_IRQHandler(&htim5);
/* USER CODE BEGIN TIM5_IRQn 1 */
/* USER CODE END TIM5_IRQn 1 */
}
/**
* @brief This function handles CAN2 TX interrupts.
*/
void CAN2_TX_IRQHandler(void)
{
/* USER CODE BEGIN CAN2_TX_IRQn 0 */
/* USER CODE END CAN2_TX_IRQn 0 */
HAL_CAN_IRQHandler(&hcan2);
/* USER CODE BEGIN CAN2_TX_IRQn 1 */
/* USER CODE END CAN2_TX_IRQn 1 */
}
/**
* @brief This function handles CAN2 RX0 interrupts.
*/
void CAN2_RX0_IRQHandler(void)
{
/* USER CODE BEGIN CAN2_RX0_IRQn 0 */
/* USER CODE END CAN2_RX0_IRQn 0 */
HAL_CAN_IRQHandler(&hcan2);
/* USER CODE BEGIN CAN2_RX0_IRQn 1 */
/* USER CODE END CAN2_RX0_IRQn 1 */
}
/**
* @brief This function handles CAN2 RX1 interrupt.
*/
void CAN2_RX1_IRQHandler(void)
{
/* USER CODE BEGIN CAN2_RX1_IRQn 0 */
/* USER CODE END CAN2_RX1_IRQn 0 */
HAL_CAN_IRQHandler(&hcan2);
/* USER CODE BEGIN CAN2_RX1_IRQn 1 */
/* USER CODE END CAN2_RX1_IRQn 1 */
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */

244
Core/Src/syscalls.c Normal file
View File

@ -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 <sys/stat.h>
#include <stdlib.h>
#include <errno.h>
#include <stdio.h>
#include <signal.h>
#include <time.h>
#include <sys/time.h>
#include <sys/times.h>
/* 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__

87
Core/Src/sysmem.c Normal file
View File

@ -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 <errno.h>
#include <stdint.h>
#include <stddef.h>
/**
* 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

747
Core/Src/system_stm32f4xx.c Normal file
View File

@ -0,0 +1,747 @@
/**
******************************************************************************
* @file system_stm32f4xx.c
* @author MCD Application Team
* @brief CMSIS Cortex-M4 Device Peripheral Access Layer System Source File.
*
* This file provides two functions and one global variable to be called from
* user application:
* - SystemInit(): This function is called at startup just after reset and
* before branch to main program. This call is made inside
* the "startup_stm32f4xx.s" file.
*
* - SystemCoreClock variable: Contains the core clock (HCLK), it can be used
* by the user application to setup the SysTick
* timer or configure other parameters.
*
* - SystemCoreClockUpdate(): Updates the variable SystemCoreClock and must
* be called whenever the core clock is changed
* during program execution.
*
*
******************************************************************************
* @attention
*
* Copyright (c) 2017 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/** @addtogroup CMSIS
* @{
*/
/** @addtogroup stm32f4xx_system
* @{
*/
/** @addtogroup STM32F4xx_System_Private_Includes
* @{
*/
#include "stm32f4xx.h"
#if !defined (HSE_VALUE)
#define HSE_VALUE ((uint32_t)25000000) /*!< Default value of the External oscillator in Hz */
#endif /* HSE_VALUE */
#if !defined (HSI_VALUE)
#define HSI_VALUE ((uint32_t)16000000) /*!< Value of the Internal oscillator in Hz*/
#endif /* HSI_VALUE */
/**
* @}
*/
/** @addtogroup STM32F4xx_System_Private_TypesDefinitions
* @{
*/
/**
* @}
*/
/** @addtogroup STM32F4xx_System_Private_Defines
* @{
*/
/************************* Miscellaneous Configuration ************************/
/*!< Uncomment the following line if you need to use external SRAM or SDRAM as data memory */
#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx) || defined(STM32F417xx)\
|| defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\
|| defined(STM32F469xx) || defined(STM32F479xx) || defined(STM32F412Zx) || defined(STM32F412Vx)
/* #define DATA_IN_ExtSRAM */
#endif /* STM32F40xxx || STM32F41xxx || STM32F42xxx || STM32F43xxx || STM32F469xx || STM32F479xx ||\
STM32F412Zx || STM32F412Vx */
#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\
|| defined(STM32F446xx) || defined(STM32F469xx) || defined(STM32F479xx)
/* #define DATA_IN_ExtSDRAM */
#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F446xx || STM32F469xx ||\
STM32F479xx */
/* Note: Following vector table addresses must be defined in line with linker
configuration. */
/*!< Uncomment the following line if you need to relocate the vector table
anywhere in Flash or Sram, else the vector table is kept at the automatic
remap of boot address selected */
/* #define USER_VECT_TAB_ADDRESS */
#if defined(USER_VECT_TAB_ADDRESS)
/*!< Uncomment the following line if you need to relocate your vector Table
in Sram else user remap will be done in Flash. */
/* #define VECT_TAB_SRAM */
#if defined(VECT_TAB_SRAM)
#define VECT_TAB_BASE_ADDRESS SRAM_BASE /*!< Vector Table base address field.
This value must be a multiple of 0x200. */
#else
#define VECT_TAB_BASE_ADDRESS FLASH_BASE /*!< Vector Table base address field.
This value must be a multiple of 0x200. */
#endif /* VECT_TAB_SRAM */
#if !defined(VECT_TAB_OFFSET)
#define VECT_TAB_OFFSET 0x00000000U /*!< Vector Table offset field.
This value must be a multiple of 0x200. */
#endif /* VECT_TAB_OFFSET */
#endif /* USER_VECT_TAB_ADDRESS */
/******************************************************************************/
/**
* @}
*/
/** @addtogroup STM32F4xx_System_Private_Macros
* @{
*/
/**
* @}
*/
/** @addtogroup STM32F4xx_System_Private_Variables
* @{
*/
/* This variable is updated in three ways:
1) by calling CMSIS function SystemCoreClockUpdate()
2) by calling HAL API function HAL_RCC_GetHCLKFreq()
3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency
Note: If you use this function to configure the system clock; then there
is no need to call the 2 first functions listed above, since SystemCoreClock
variable is updated automatically.
*/
uint32_t SystemCoreClock = 16000000;
const uint8_t AHBPrescTable[16] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 3, 4, 6, 7, 8, 9};
const uint8_t APBPrescTable[8] = {0, 0, 0, 0, 1, 2, 3, 4};
/**
* @}
*/
/** @addtogroup STM32F4xx_System_Private_FunctionPrototypes
* @{
*/
#if defined (DATA_IN_ExtSRAM) || defined (DATA_IN_ExtSDRAM)
static void SystemInit_ExtMemCtl(void);
#endif /* DATA_IN_ExtSRAM || DATA_IN_ExtSDRAM */
/**
* @}
*/
/** @addtogroup STM32F4xx_System_Private_Functions
* @{
*/
/**
* @brief Setup the microcontroller system
* Initialize the FPU setting, vector table location and External memory
* configuration.
* @param None
* @retval None
*/
void SystemInit(void)
{
/* FPU settings ------------------------------------------------------------*/
#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
SCB->CPACR |= ((3UL << 10*2)|(3UL << 11*2)); /* set CP10 and CP11 Full Access */
#endif
#if defined (DATA_IN_ExtSRAM) || defined (DATA_IN_ExtSDRAM)
SystemInit_ExtMemCtl();
#endif /* DATA_IN_ExtSRAM || DATA_IN_ExtSDRAM */
/* Configure the Vector Table location -------------------------------------*/
#if defined(USER_VECT_TAB_ADDRESS)
SCB->VTOR = VECT_TAB_BASE_ADDRESS | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */
#endif /* USER_VECT_TAB_ADDRESS */
}
/**
* @brief Update SystemCoreClock variable according to Clock Register Values.
* The SystemCoreClock variable contains the core clock (HCLK), it can
* be used by the user application to setup the SysTick timer or configure
* other parameters.
*
* @note Each time the core clock (HCLK) changes, this function must be called
* to update SystemCoreClock variable value. Otherwise, any configuration
* based on this variable will be incorrect.
*
* @note - The system frequency computed by this function is not the real
* frequency in the chip. It is calculated based on the predefined
* constant and the selected clock source:
*
* - If SYSCLK source is HSI, SystemCoreClock will contain the HSI_VALUE(*)
*
* - If SYSCLK source is HSE, SystemCoreClock will contain the HSE_VALUE(**)
*
* - If SYSCLK source is PLL, SystemCoreClock will contain the HSE_VALUE(**)
* or HSI_VALUE(*) multiplied/divided by the PLL factors.
*
* (*) HSI_VALUE is a constant defined in stm32f4xx_hal_conf.h file (default value
* 16 MHz) but the real value may vary depending on the variations
* in voltage and temperature.
*
* (**) HSE_VALUE is a constant defined in stm32f4xx_hal_conf.h file (its value
* depends on the application requirements), user has to ensure that HSE_VALUE
* is same as the real frequency of the crystal used. Otherwise, this function
* may have wrong result.
*
* - The result of this function could be not correct when using fractional
* value for HSE crystal.
*
* @param None
* @retval None
*/
void SystemCoreClockUpdate(void)
{
uint32_t tmp, pllvco, pllp, pllsource, pllm;
/* Get SYSCLK source -------------------------------------------------------*/
tmp = RCC->CFGR & RCC_CFGR_SWS;
switch (tmp)
{
case 0x00: /* HSI used as system clock source */
SystemCoreClock = HSI_VALUE;
break;
case 0x04: /* HSE used as system clock source */
SystemCoreClock = HSE_VALUE;
break;
case 0x08: /* PLL used as system clock source */
/* PLL_VCO = (HSE_VALUE or HSI_VALUE / PLL_M) * PLL_N
SYSCLK = PLL_VCO / PLL_P
*/
pllsource = (RCC->PLLCFGR & RCC_PLLCFGR_PLLSRC) >> 22;
pllm = RCC->PLLCFGR & RCC_PLLCFGR_PLLM;
if (pllsource != 0)
{
/* HSE used as PLL clock source */
pllvco = (HSE_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
}
else
{
/* HSI used as PLL clock source */
pllvco = (HSI_VALUE / pllm) * ((RCC->PLLCFGR & RCC_PLLCFGR_PLLN) >> 6);
}
pllp = (((RCC->PLLCFGR & RCC_PLLCFGR_PLLP) >>16) + 1 ) *2;
SystemCoreClock = pllvco/pllp;
break;
default:
SystemCoreClock = HSI_VALUE;
break;
}
/* Compute HCLK frequency --------------------------------------------------*/
/* Get HCLK prescaler */
tmp = AHBPrescTable[((RCC->CFGR & RCC_CFGR_HPRE) >> 4)];
/* HCLK frequency */
SystemCoreClock >>= tmp;
}
#if defined (DATA_IN_ExtSRAM) && defined (DATA_IN_ExtSDRAM)
#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\
|| defined(STM32F469xx) || defined(STM32F479xx)
/**
* @brief Setup the external memory controller.
* Called in startup_stm32f4xx.s before jump to main.
* This function configures the external memories (SRAM/SDRAM)
* This SRAM/SDRAM will be used as program data memory (including heap and stack).
* @param None
* @retval None
*/
void SystemInit_ExtMemCtl(void)
{
__IO uint32_t tmp = 0x00;
register uint32_t tmpreg = 0, timeout = 0xFFFF;
register __IO uint32_t index;
/* Enable GPIOC, GPIOD, GPIOE, GPIOF, GPIOG, GPIOH and GPIOI interface clock */
RCC->AHB1ENR |= 0x000001F8;
/* Delay after an RCC peripheral clock enabling */
tmp = READ_BIT(RCC->AHB1ENR, RCC_AHB1ENR_GPIOCEN);
/* Connect PDx pins to FMC Alternate function */
GPIOD->AFR[0] = 0x00CCC0CC;
GPIOD->AFR[1] = 0xCCCCCCCC;
/* Configure PDx pins in Alternate function mode */
GPIOD->MODER = 0xAAAA0A8A;
/* Configure PDx pins speed to 100 MHz */
GPIOD->OSPEEDR = 0xFFFF0FCF;
/* Configure PDx pins Output type to push-pull */
GPIOD->OTYPER = 0x00000000;
/* No pull-up, pull-down for PDx pins */
GPIOD->PUPDR = 0x00000000;
/* Connect PEx pins to FMC Alternate function */
GPIOE->AFR[0] = 0xC00CC0CC;
GPIOE->AFR[1] = 0xCCCCCCCC;
/* Configure PEx pins in Alternate function mode */
GPIOE->MODER = 0xAAAA828A;
/* Configure PEx pins speed to 100 MHz */
GPIOE->OSPEEDR = 0xFFFFC3CF;
/* Configure PEx pins Output type to push-pull */
GPIOE->OTYPER = 0x00000000;
/* No pull-up, pull-down for PEx pins */
GPIOE->PUPDR = 0x00000000;
/* Connect PFx pins to FMC Alternate function */
GPIOF->AFR[0] = 0xCCCCCCCC;
GPIOF->AFR[1] = 0xCCCCCCCC;
/* Configure PFx pins in Alternate function mode */
GPIOF->MODER = 0xAA800AAA;
/* Configure PFx pins speed to 50 MHz */
GPIOF->OSPEEDR = 0xAA800AAA;
/* Configure PFx pins Output type to push-pull */
GPIOF->OTYPER = 0x00000000;
/* No pull-up, pull-down for PFx pins */
GPIOF->PUPDR = 0x00000000;
/* Connect PGx pins to FMC Alternate function */
GPIOG->AFR[0] = 0xCCCCCCCC;
GPIOG->AFR[1] = 0xCCCCCCCC;
/* Configure PGx pins in Alternate function mode */
GPIOG->MODER = 0xAAAAAAAA;
/* Configure PGx pins speed to 50 MHz */
GPIOG->OSPEEDR = 0xAAAAAAAA;
/* Configure PGx pins Output type to push-pull */
GPIOG->OTYPER = 0x00000000;
/* No pull-up, pull-down for PGx pins */
GPIOG->PUPDR = 0x00000000;
/* Connect PHx pins to FMC Alternate function */
GPIOH->AFR[0] = 0x00C0CC00;
GPIOH->AFR[1] = 0xCCCCCCCC;
/* Configure PHx pins in Alternate function mode */
GPIOH->MODER = 0xAAAA08A0;
/* Configure PHx pins speed to 50 MHz */
GPIOH->OSPEEDR = 0xAAAA08A0;
/* Configure PHx pins Output type to push-pull */
GPIOH->OTYPER = 0x00000000;
/* No pull-up, pull-down for PHx pins */
GPIOH->PUPDR = 0x00000000;
/* Connect PIx pins to FMC Alternate function */
GPIOI->AFR[0] = 0xCCCCCCCC;
GPIOI->AFR[1] = 0x00000CC0;
/* Configure PIx pins in Alternate function mode */
GPIOI->MODER = 0x0028AAAA;
/* Configure PIx pins speed to 50 MHz */
GPIOI->OSPEEDR = 0x0028AAAA;
/* Configure PIx pins Output type to push-pull */
GPIOI->OTYPER = 0x00000000;
/* No pull-up, pull-down for PIx pins */
GPIOI->PUPDR = 0x00000000;
/*-- FMC Configuration -------------------------------------------------------*/
/* Enable the FMC interface clock */
RCC->AHB3ENR |= 0x00000001;
/* Delay after an RCC peripheral clock enabling */
tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FMCEN);
FMC_Bank5_6->SDCR[0] = 0x000019E4;
FMC_Bank5_6->SDTR[0] = 0x01115351;
/* SDRAM initialization sequence */
/* Clock enable command */
FMC_Bank5_6->SDCMR = 0x00000011;
tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
while((tmpreg != 0) && (timeout-- > 0))
{
tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
}
/* Delay */
for (index = 0; index<1000; index++);
/* PALL command */
FMC_Bank5_6->SDCMR = 0x00000012;
tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
timeout = 0xFFFF;
while((tmpreg != 0) && (timeout-- > 0))
{
tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
}
/* Auto refresh command */
FMC_Bank5_6->SDCMR = 0x00000073;
tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
timeout = 0xFFFF;
while((tmpreg != 0) && (timeout-- > 0))
{
tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
}
/* MRD register program */
FMC_Bank5_6->SDCMR = 0x00046014;
tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
timeout = 0xFFFF;
while((tmpreg != 0) && (timeout-- > 0))
{
tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
}
/* Set refresh count */
tmpreg = FMC_Bank5_6->SDRTR;
FMC_Bank5_6->SDRTR = (tmpreg | (0x0000027C<<1));
/* Disable write protection */
tmpreg = FMC_Bank5_6->SDCR[0];
FMC_Bank5_6->SDCR[0] = (tmpreg & 0xFFFFFDFF);
#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)
/* Configure and enable Bank1_SRAM2 */
FMC_Bank1->BTCR[2] = 0x00001011;
FMC_Bank1->BTCR[3] = 0x00000201;
FMC_Bank1E->BWTR[2] = 0x0fffffff;
#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx */
#if defined(STM32F469xx) || defined(STM32F479xx)
/* Configure and enable Bank1_SRAM2 */
FMC_Bank1->BTCR[2] = 0x00001091;
FMC_Bank1->BTCR[3] = 0x00110212;
FMC_Bank1E->BWTR[2] = 0x0fffffff;
#endif /* STM32F469xx || STM32F479xx */
(void)(tmp);
}
#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F469xx || STM32F479xx */
#elif defined (DATA_IN_ExtSRAM) || defined (DATA_IN_ExtSDRAM)
/**
* @brief Setup the external memory controller.
* Called in startup_stm32f4xx.s before jump to main.
* This function configures the external memories (SRAM/SDRAM)
* This SRAM/SDRAM will be used as program data memory (including heap and stack).
* @param None
* @retval None
*/
void SystemInit_ExtMemCtl(void)
{
__IO uint32_t tmp = 0x00;
#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\
|| defined(STM32F446xx) || defined(STM32F469xx) || defined(STM32F479xx)
#if defined (DATA_IN_ExtSDRAM)
register uint32_t tmpreg = 0, timeout = 0xFFFF;
register __IO uint32_t index;
#if defined(STM32F446xx)
/* Enable GPIOA, GPIOC, GPIOD, GPIOE, GPIOF, GPIOG interface
clock */
RCC->AHB1ENR |= 0x0000007D;
#else
/* Enable GPIOC, GPIOD, GPIOE, GPIOF, GPIOG, GPIOH and GPIOI interface
clock */
RCC->AHB1ENR |= 0x000001F8;
#endif /* STM32F446xx */
/* Delay after an RCC peripheral clock enabling */
tmp = READ_BIT(RCC->AHB1ENR, RCC_AHB1ENR_GPIOCEN);
#if defined(STM32F446xx)
/* Connect PAx pins to FMC Alternate function */
GPIOA->AFR[0] |= 0xC0000000;
GPIOA->AFR[1] |= 0x00000000;
/* Configure PDx pins in Alternate function mode */
GPIOA->MODER |= 0x00008000;
/* Configure PDx pins speed to 50 MHz */
GPIOA->OSPEEDR |= 0x00008000;
/* Configure PDx pins Output type to push-pull */
GPIOA->OTYPER |= 0x00000000;
/* No pull-up, pull-down for PDx pins */
GPIOA->PUPDR |= 0x00000000;
/* Connect PCx pins to FMC Alternate function */
GPIOC->AFR[0] |= 0x00CC0000;
GPIOC->AFR[1] |= 0x00000000;
/* Configure PDx pins in Alternate function mode */
GPIOC->MODER |= 0x00000A00;
/* Configure PDx pins speed to 50 MHz */
GPIOC->OSPEEDR |= 0x00000A00;
/* Configure PDx pins Output type to push-pull */
GPIOC->OTYPER |= 0x00000000;
/* No pull-up, pull-down for PDx pins */
GPIOC->PUPDR |= 0x00000000;
#endif /* STM32F446xx */
/* Connect PDx pins to FMC Alternate function */
GPIOD->AFR[0] = 0x000000CC;
GPIOD->AFR[1] = 0xCC000CCC;
/* Configure PDx pins in Alternate function mode */
GPIOD->MODER = 0xA02A000A;
/* Configure PDx pins speed to 50 MHz */
GPIOD->OSPEEDR = 0xA02A000A;
/* Configure PDx pins Output type to push-pull */
GPIOD->OTYPER = 0x00000000;
/* No pull-up, pull-down for PDx pins */
GPIOD->PUPDR = 0x00000000;
/* Connect PEx pins to FMC Alternate function */
GPIOE->AFR[0] = 0xC00000CC;
GPIOE->AFR[1] = 0xCCCCCCCC;
/* Configure PEx pins in Alternate function mode */
GPIOE->MODER = 0xAAAA800A;
/* Configure PEx pins speed to 50 MHz */
GPIOE->OSPEEDR = 0xAAAA800A;
/* Configure PEx pins Output type to push-pull */
GPIOE->OTYPER = 0x00000000;
/* No pull-up, pull-down for PEx pins */
GPIOE->PUPDR = 0x00000000;
/* Connect PFx pins to FMC Alternate function */
GPIOF->AFR[0] = 0xCCCCCCCC;
GPIOF->AFR[1] = 0xCCCCCCCC;
/* Configure PFx pins in Alternate function mode */
GPIOF->MODER = 0xAA800AAA;
/* Configure PFx pins speed to 50 MHz */
GPIOF->OSPEEDR = 0xAA800AAA;
/* Configure PFx pins Output type to push-pull */
GPIOF->OTYPER = 0x00000000;
/* No pull-up, pull-down for PFx pins */
GPIOF->PUPDR = 0x00000000;
/* Connect PGx pins to FMC Alternate function */
GPIOG->AFR[0] = 0xCCCCCCCC;
GPIOG->AFR[1] = 0xCCCCCCCC;
/* Configure PGx pins in Alternate function mode */
GPIOG->MODER = 0xAAAAAAAA;
/* Configure PGx pins speed to 50 MHz */
GPIOG->OSPEEDR = 0xAAAAAAAA;
/* Configure PGx pins Output type to push-pull */
GPIOG->OTYPER = 0x00000000;
/* No pull-up, pull-down for PGx pins */
GPIOG->PUPDR = 0x00000000;
#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\
|| defined(STM32F469xx) || defined(STM32F479xx)
/* Connect PHx pins to FMC Alternate function */
GPIOH->AFR[0] = 0x00C0CC00;
GPIOH->AFR[1] = 0xCCCCCCCC;
/* Configure PHx pins in Alternate function mode */
GPIOH->MODER = 0xAAAA08A0;
/* Configure PHx pins speed to 50 MHz */
GPIOH->OSPEEDR = 0xAAAA08A0;
/* Configure PHx pins Output type to push-pull */
GPIOH->OTYPER = 0x00000000;
/* No pull-up, pull-down for PHx pins */
GPIOH->PUPDR = 0x00000000;
/* Connect PIx pins to FMC Alternate function */
GPIOI->AFR[0] = 0xCCCCCCCC;
GPIOI->AFR[1] = 0x00000CC0;
/* Configure PIx pins in Alternate function mode */
GPIOI->MODER = 0x0028AAAA;
/* Configure PIx pins speed to 50 MHz */
GPIOI->OSPEEDR = 0x0028AAAA;
/* Configure PIx pins Output type to push-pull */
GPIOI->OTYPER = 0x00000000;
/* No pull-up, pull-down for PIx pins */
GPIOI->PUPDR = 0x00000000;
#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F469xx || STM32F479xx */
/*-- FMC Configuration -------------------------------------------------------*/
/* Enable the FMC interface clock */
RCC->AHB3ENR |= 0x00000001;
/* Delay after an RCC peripheral clock enabling */
tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FMCEN);
/* Configure and enable SDRAM bank1 */
#if defined(STM32F446xx)
FMC_Bank5_6->SDCR[0] = 0x00001954;
#else
FMC_Bank5_6->SDCR[0] = 0x000019E4;
#endif /* STM32F446xx */
FMC_Bank5_6->SDTR[0] = 0x01115351;
/* SDRAM initialization sequence */
/* Clock enable command */
FMC_Bank5_6->SDCMR = 0x00000011;
tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
while((tmpreg != 0) && (timeout-- > 0))
{
tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
}
/* Delay */
for (index = 0; index<1000; index++);
/* PALL command */
FMC_Bank5_6->SDCMR = 0x00000012;
tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
timeout = 0xFFFF;
while((tmpreg != 0) && (timeout-- > 0))
{
tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
}
/* Auto refresh command */
#if defined(STM32F446xx)
FMC_Bank5_6->SDCMR = 0x000000F3;
#else
FMC_Bank5_6->SDCMR = 0x00000073;
#endif /* STM32F446xx */
tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
timeout = 0xFFFF;
while((tmpreg != 0) && (timeout-- > 0))
{
tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
}
/* MRD register program */
#if defined(STM32F446xx)
FMC_Bank5_6->SDCMR = 0x00044014;
#else
FMC_Bank5_6->SDCMR = 0x00046014;
#endif /* STM32F446xx */
tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
timeout = 0xFFFF;
while((tmpreg != 0) && (timeout-- > 0))
{
tmpreg = FMC_Bank5_6->SDSR & 0x00000020;
}
/* Set refresh count */
tmpreg = FMC_Bank5_6->SDRTR;
#if defined(STM32F446xx)
FMC_Bank5_6->SDRTR = (tmpreg | (0x0000050C<<1));
#else
FMC_Bank5_6->SDRTR = (tmpreg | (0x0000027C<<1));
#endif /* STM32F446xx */
/* Disable write protection */
tmpreg = FMC_Bank5_6->SDCR[0];
FMC_Bank5_6->SDCR[0] = (tmpreg & 0xFFFFFDFF);
#endif /* DATA_IN_ExtSDRAM */
#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx || STM32F446xx || STM32F469xx || STM32F479xx */
#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx) || defined(STM32F417xx)\
|| defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)\
|| defined(STM32F469xx) || defined(STM32F479xx) || defined(STM32F412Zx) || defined(STM32F412Vx)
#if defined(DATA_IN_ExtSRAM)
/*-- GPIOs Configuration -----------------------------------------------------*/
/* Enable GPIOD, GPIOE, GPIOF and GPIOG interface clock */
RCC->AHB1ENR |= 0x00000078;
/* Delay after an RCC peripheral clock enabling */
tmp = READ_BIT(RCC->AHB1ENR, RCC_AHB1ENR_GPIODEN);
/* Connect PDx pins to FMC Alternate function */
GPIOD->AFR[0] = 0x00CCC0CC;
GPIOD->AFR[1] = 0xCCCCCCCC;
/* Configure PDx pins in Alternate function mode */
GPIOD->MODER = 0xAAAA0A8A;
/* Configure PDx pins speed to 100 MHz */
GPIOD->OSPEEDR = 0xFFFF0FCF;
/* Configure PDx pins Output type to push-pull */
GPIOD->OTYPER = 0x00000000;
/* No pull-up, pull-down for PDx pins */
GPIOD->PUPDR = 0x00000000;
/* Connect PEx pins to FMC Alternate function */
GPIOE->AFR[0] = 0xC00CC0CC;
GPIOE->AFR[1] = 0xCCCCCCCC;
/* Configure PEx pins in Alternate function mode */
GPIOE->MODER = 0xAAAA828A;
/* Configure PEx pins speed to 100 MHz */
GPIOE->OSPEEDR = 0xFFFFC3CF;
/* Configure PEx pins Output type to push-pull */
GPIOE->OTYPER = 0x00000000;
/* No pull-up, pull-down for PEx pins */
GPIOE->PUPDR = 0x00000000;
/* Connect PFx pins to FMC Alternate function */
GPIOF->AFR[0] = 0x00CCCCCC;
GPIOF->AFR[1] = 0xCCCC0000;
/* Configure PFx pins in Alternate function mode */
GPIOF->MODER = 0xAA000AAA;
/* Configure PFx pins speed to 100 MHz */
GPIOF->OSPEEDR = 0xFF000FFF;
/* Configure PFx pins Output type to push-pull */
GPIOF->OTYPER = 0x00000000;
/* No pull-up, pull-down for PFx pins */
GPIOF->PUPDR = 0x00000000;
/* Connect PGx pins to FMC Alternate function */
GPIOG->AFR[0] = 0x00CCCCCC;
GPIOG->AFR[1] = 0x000000C0;
/* Configure PGx pins in Alternate function mode */
GPIOG->MODER = 0x00085AAA;
/* Configure PGx pins speed to 100 MHz */
GPIOG->OSPEEDR = 0x000CAFFF;
/* Configure PGx pins Output type to push-pull */
GPIOG->OTYPER = 0x00000000;
/* No pull-up, pull-down for PGx pins */
GPIOG->PUPDR = 0x00000000;
/*-- FMC/FSMC Configuration --------------------------------------------------*/
/* Enable the FMC/FSMC interface clock */
RCC->AHB3ENR |= 0x00000001;
#if defined(STM32F427xx) || defined(STM32F437xx) || defined(STM32F429xx) || defined(STM32F439xx)
/* Delay after an RCC peripheral clock enabling */
tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FMCEN);
/* Configure and enable Bank1_SRAM2 */
FMC_Bank1->BTCR[2] = 0x00001011;
FMC_Bank1->BTCR[3] = 0x00000201;
FMC_Bank1E->BWTR[2] = 0x0fffffff;
#endif /* STM32F427xx || STM32F437xx || STM32F429xx || STM32F439xx */
#if defined(STM32F469xx) || defined(STM32F479xx)
/* Delay after an RCC peripheral clock enabling */
tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FMCEN);
/* Configure and enable Bank1_SRAM2 */
FMC_Bank1->BTCR[2] = 0x00001091;
FMC_Bank1->BTCR[3] = 0x00110212;
FMC_Bank1E->BWTR[2] = 0x0fffffff;
#endif /* STM32F469xx || STM32F479xx */
#if defined(STM32F405xx) || defined(STM32F415xx) || defined(STM32F407xx)|| defined(STM32F417xx)\
|| defined(STM32F412Zx) || defined(STM32F412Vx)
/* Delay after an RCC peripheral clock enabling */
tmp = READ_BIT(RCC->AHB3ENR, RCC_AHB3ENR_FSMCEN);
/* Configure and enable Bank1_SRAM2 */
FSMC_Bank1->BTCR[2] = 0x00001011;
FSMC_Bank1->BTCR[3] = 0x00000201;
FSMC_Bank1E->BWTR[2] = 0x0FFFFFFF;
#endif /* STM32F405xx || STM32F415xx || STM32F407xx || STM32F417xx || STM32F412Zx || STM32F412Vx */
#endif /* DATA_IN_ExtSRAM */
#endif /* STM32F405xx || STM32F415xx || STM32F407xx || STM32F417xx || STM32F427xx || STM32F437xx ||\
STM32F429xx || STM32F439xx || STM32F469xx || STM32F479xx || STM32F412Zx || STM32F412Vx */
(void)(tmp);
}
#endif /* DATA_IN_ExtSRAM && DATA_IN_ExtSDRAM */
/**
* @}
*/
/**
* @}
*/
/**
* @}
*/

162
Core/Src/tim.c Normal file
View File

@ -0,0 +1,162 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file tim.c
* @brief This file provides code for the configuration
* of the TIM instances.
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "tim.h"
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
TIM_HandleTypeDef htim5;
/* TIM5 init function */
void MX_TIM5_Init(void)
{
/* USER CODE BEGIN TIM5_Init 0 */
/* USER CODE END TIM5_Init 0 */
TIM_ClockConfigTypeDef sClockSourceConfig = {0};
TIM_MasterConfigTypeDef sMasterConfig = {0};
TIM_OC_InitTypeDef sConfigOC = {0};
/* USER CODE BEGIN TIM5_Init 1 */
/* USER CODE END TIM5_Init 1 */
htim5.Instance = TIM5;
htim5.Init.Prescaler = 0;
htim5.Init.CounterMode = TIM_COUNTERMODE_UP;
htim5.Init.Period = 65535;
htim5.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim5.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim5) != HAL_OK)
{
Error_Handler();
}
sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
if (HAL_TIM_ConfigClockSource(&htim5, &sClockSourceConfig) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_Init(&htim5) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
sConfigOC.OCMode = TIM_OCMODE_PWM1;
sConfigOC.Pulse = 0;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
if (HAL_TIM_PWM_ConfigChannel(&htim5, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_ConfigChannel(&htim5, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_ConfigChannel(&htim5, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM5_Init 2 */
/* USER CODE END TIM5_Init 2 */
HAL_TIM_MspPostInit(&htim5);
}
void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
{
if(tim_baseHandle->Instance==TIM5)
{
/* USER CODE BEGIN TIM5_MspInit 0 */
/* USER CODE END TIM5_MspInit 0 */
/* TIM5 clock enable */
__HAL_RCC_TIM5_CLK_ENABLE();
/* TIM5 interrupt Init */
HAL_NVIC_SetPriority(TIM5_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(TIM5_IRQn);
/* USER CODE BEGIN TIM5_MspInit 1 */
/* USER CODE END TIM5_MspInit 1 */
}
}
void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if(timHandle->Instance==TIM5)
{
/* USER CODE BEGIN TIM5_MspPostInit 0 */
/* USER CODE END TIM5_MspPostInit 0 */
__HAL_RCC_GPIOH_CLK_ENABLE();
/**TIM5 GPIO Configuration
PH12 ------> TIM5_CH3
PH11 ------> TIM5_CH2
PH10 ------> TIM5_CH1
*/
GPIO_InitStruct.Pin = GPIO_PIN_12|GPIO_PIN_11|GPIO_PIN_10;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
GPIO_InitStruct.Alternate = GPIO_AF2_TIM5;
HAL_GPIO_Init(GPIOH, &GPIO_InitStruct);
/* USER CODE BEGIN TIM5_MspPostInit 1 */
/* USER CODE END TIM5_MspPostInit 1 */
}
}
void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle)
{
if(tim_baseHandle->Instance==TIM5)
{
/* USER CODE BEGIN TIM5_MspDeInit 0 */
/* USER CODE END TIM5_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_TIM5_CLK_DISABLE();
/* TIM5 interrupt Deinit */
HAL_NVIC_DisableIRQ(TIM5_IRQn);
/* USER CODE BEGIN TIM5_MspDeInit 1 */
/* USER CODE END TIM5_MspDeInit 1 */
}
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */

136
Core/Src/usart.c Normal file
View File

@ -0,0 +1,136 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file usart.c
* @brief This file provides code for the configuration
* of the USART instances.
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "usart.h"
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
UART_HandleTypeDef huart3;
DMA_HandleTypeDef hdma_usart3_rx;
/* USART3 init function */
void MX_USART3_UART_Init(void)
{
/* USER CODE BEGIN USART3_Init 0 */
/* USER CODE END USART3_Init 0 */
/* USER CODE BEGIN USART3_Init 1 */
/* USER CODE END USART3_Init 1 */
huart3.Instance = USART3;
huart3.Init.BaudRate = 115200;
huart3.Init.WordLength = UART_WORDLENGTH_8B;
huart3.Init.StopBits = UART_STOPBITS_1;
huart3.Init.Parity = UART_PARITY_NONE;
huart3.Init.Mode = UART_MODE_TX_RX;
huart3.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart3.Init.OverSampling = UART_OVERSAMPLING_16;
if (HAL_UART_Init(&huart3) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN USART3_Init 2 */
/* USER CODE END USART3_Init 2 */
}
void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if(uartHandle->Instance==USART3)
{
/* USER CODE BEGIN USART3_MspInit 0 */
/* USER CODE END USART3_MspInit 0 */
/* USART3 clock enable */
__HAL_RCC_USART3_CLK_ENABLE();
__HAL_RCC_GPIOC_CLK_ENABLE();
/**USART3 GPIO Configuration
PC11 ------> USART3_RX
PC10 ------> USART3_TX
*/
GPIO_InitStruct.Pin = GPIO_PIN_11|GPIO_PIN_10;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF7_USART3;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/* USART3 DMA Init */
/* USART3_RX Init */
hdma_usart3_rx.Instance = DMA1_Stream1;
hdma_usart3_rx.Init.Channel = DMA_CHANNEL_4;
hdma_usart3_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_usart3_rx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_usart3_rx.Init.MemInc = DMA_MINC_ENABLE;
hdma_usart3_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_usart3_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_usart3_rx.Init.Mode = DMA_NORMAL;
hdma_usart3_rx.Init.Priority = DMA_PRIORITY_LOW;
hdma_usart3_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
if (HAL_DMA_Init(&hdma_usart3_rx) != HAL_OK)
{
Error_Handler();
}
__HAL_LINKDMA(uartHandle,hdmarx,hdma_usart3_rx);
/* USER CODE BEGIN USART3_MspInit 1 */
/* USER CODE END USART3_MspInit 1 */
}
}
void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
{
if(uartHandle->Instance==USART3)
{
/* USER CODE BEGIN USART3_MspDeInit 0 */
/* USER CODE END USART3_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_USART3_CLK_DISABLE();
/**USART3 GPIO Configuration
PC11 ------> USART3_RX
PC10 ------> USART3_TX
*/
HAL_GPIO_DeInit(GPIOC, GPIO_PIN_11|GPIO_PIN_10);
/* USART3 DMA DeInit */
HAL_DMA_DeInit(uartHandle->hdmarx);
/* USER CODE BEGIN USART3_MspDeInit 1 */
/* USER CODE END USART3_MspDeInit 1 */
}
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */

0
ITM_TROUBLESHOOTING.md Normal file
View File

48
MDK-ARM/DebugConfig/arm_STM32F407IGHx.dbgconf Normal file
View File

@ -0,0 +1,48 @@
// File: STM32F405_415_407_417_427_437_429_439.dbgconf
// Version: 1.0.0
// Note: refer to STM32F405/415 STM32F407/417 STM32F427/437 STM32F429/439 reference manual (RM0090)
// refer to STM32F40x STM32F41x datasheets
// refer to STM32F42x STM32F43x datasheets
// <<< Use Configuration Wizard in Context Menu >>>
// <h> Debug MCU configuration register (DBGMCU_CR)
// <o.2> DBG_STANDBY <i> Debug Standby Mode
// <o.1> DBG_STOP <i> Debug Stop Mode
// <o.0> DBG_SLEEP <i> Debug Sleep Mode
// </h>
DbgMCU_CR = 0x00000007;
// <h> Debug MCU APB1 freeze register (DBGMCU_APB1_FZ)
// <i> Reserved bits must be kept at reset value
// <o.26> DBG_CAN2_STOP <i> CAN2 stopped when core is halted
// <o.25> DBG_CAN1_STOP <i> CAN2 stopped when core is halted
// <o.23> DBG_I2C3_SMBUS_TIMEOUT <i> I2C3 SMBUS timeout mode stopped when core is halted
// <o.22> DBG_I2C2_SMBUS_TIMEOUT <i> I2C2 SMBUS timeout mode stopped when core is halted
// <o.21> DBG_I2C1_SMBUS_TIMEOUT <i> I2C1 SMBUS timeout mode stopped when core is halted
// <o.12> DBG_IWDG_STOP <i> Independent watchdog stopped when core is halted
// <o.11> DBG_WWDG_STOP <i> Window watchdog stopped when core is halted
// <o.10> DBG_RTC_STOP <i> RTC stopped when core is halted
// <o.8> DBG_TIM14_STOP <i> TIM14 counter stopped when core is halted
// <o.7> DBG_TIM13_STOP <i> TIM13 counter stopped when core is halted
// <o.6> DBG_TIM12_STOP <i> TIM12 counter stopped when core is halted
// <o.5> DBG_TIM7_STOP <i> TIM7 counter stopped when core is halted
// <o.4> DBG_TIM6_STOP <i> TIM6 counter stopped when core is halted
// <o.3> DBG_TIM5_STOP <i> TIM5 counter stopped when core is halted
// <o.2> DBG_TIM4_STOP <i> TIM4 counter stopped when core is halted
// <o.1> DBG_TIM3_STOP <i> TIM3 counter stopped when core is halted
// <o.0> DBG_TIM2_STOP <i> TIM2 counter stopped when core is halted
// </h>
DbgMCU_APB1_Fz = 0x00000000;
// <h> Debug MCU APB2 freeze register (DBGMCU_APB2_FZ)
// <i> Reserved bits must be kept at reset value
// <o.18> DBG_TIM11_STOP <i> TIM11 counter stopped when core is halted
// <o.17> DBG_TIM10_STOP <i> TIM10 counter stopped when core is halted
// <o.16> DBG_TIM9_STOP <i> TIM9 counter stopped when core is halted
// <o.1> DBG_TIM8_STOP <i> TIM8 counter stopped when core is halted
// <o.0> DBG_TIM1_STOP <i> TIM1 counter stopped when core is halted
// </h>
DbgMCU_APB2_Fz = 0x00000000;
// <<< end of configuration section >>>

48
MDK-ARM/DebugConfig/arm_STM32F407IGHx_1.0.1.dbgconf Normal file
View File

@ -0,0 +1,48 @@
// File: STM32F405_415_407_417_427_437_429_439.dbgconf
// Version: 1.0.1
// Note: refer to STM32F405/415 STM32F407/417 STM32F427/437 STM32F429/439 reference manual (RM0090)
// refer to STM32F40x STM32F41x datasheets
// refer to STM32F42x STM32F43x datasheets
// <<< Use Configuration Wizard in Context Menu >>>
// <h> Debug MCU configuration register (DBGMCU_CR)
// <o.2> DBG_STANDBY <i> Debug Standby Mode
// <o.1> DBG_STOP <i> Debug Stop Mode
// <o.0> DBG_SLEEP <i> Debug Sleep Mode
// </h>
DbgMCU_CR = 0x00000007;
// <h> Debug MCU APB1 freeze register (DBGMCU_APB1_FZ)
// <i> Reserved bits must be kept at reset value
// <o.26> DBG_CAN2_STOP <i> CAN2 stopped when core is halted
// <o.25> DBG_CAN1_STOP <i> CAN2 stopped when core is halted
// <o.23> DBG_I2C3_SMBUS_TIMEOUT <i> I2C3 SMBUS timeout mode stopped when core is halted
// <o.22> DBG_I2C2_SMBUS_TIMEOUT <i> I2C2 SMBUS timeout mode stopped when core is halted
// <o.21> DBG_I2C1_SMBUS_TIMEOUT <i> I2C1 SMBUS timeout mode stopped when core is halted
// <o.12> DBG_IWDG_STOP <i> Independent watchdog stopped when core is halted
// <o.11> DBG_WWDG_STOP <i> Window watchdog stopped when core is halted
// <o.10> DBG_RTC_STOP <i> RTC stopped when core is halted
// <o.8> DBG_TIM14_STOP <i> TIM14 counter stopped when core is halted
// <o.7> DBG_TIM13_STOP <i> TIM13 counter stopped when core is halted
// <o.6> DBG_TIM12_STOP <i> TIM12 counter stopped when core is halted
// <o.5> DBG_TIM7_STOP <i> TIM7 counter stopped when core is halted
// <o.4> DBG_TIM6_STOP <i> TIM6 counter stopped when core is halted
// <o.3> DBG_TIM5_STOP <i> TIM5 counter stopped when core is halted
// <o.2> DBG_TIM4_STOP <i> TIM4 counter stopped when core is halted
// <o.1> DBG_TIM3_STOP <i> TIM3 counter stopped when core is halted
// <o.0> DBG_TIM2_STOP <i> TIM2 counter stopped when core is halted
// </h>
DbgMCU_APB1_Fz = 0x06E019FF;
// <h> Debug MCU APB2 freeze register (DBGMCU_APB2_FZ)
// <i> Reserved bits must be kept at reset value
// <o.18> DBG_TIM11_STOP <i> TIM11 counter stopped when core is halted
// <o.17> DBG_TIM10_STOP <i> TIM10 counter stopped when core is halted
// <o.16> DBG_TIM9_STOP <i> TIM9 counter stopped when core is halted
// <o.1> DBG_TIM8_STOP <i> TIM8 counter stopped when core is halted
// <o.0> DBG_TIM1_STOP <i> TIM1 counter stopped when core is halted
// </h>
DbgMCU_APB2_Fz = 0x00070003;
// <<< end of configuration section >>>

14
MDK-ARM/RTE/_arm/RTE_Components.h Normal file
View File

@ -0,0 +1,14 @@
/*
* UVISION generated file: DO NOT EDIT!
* Generated by: uVision version 5.40.0.0
*
* Project: 'arm'
* Target: 'arm'
*/
#ifndef RTE_COMPONENTS_H
#define RTE_COMPONENTS_H
#endif /* RTE_COMPONENTS_H */

1860
MDK-ARM/arm.uvguix.Lenovo Normal file
View File

File diff suppressed because one or more lines are too long

1860
MDK-ARM/arm.uvguix.yxm23 Normal file
View File

File diff suppressed because one or more lines are too long

672
MDK-ARM/arm.uvoptx Normal file
View File

@ -0,0 +1,672 @@
<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
<ProjectOpt xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="project_optx.xsd">
<SchemaVersion>1.0</SchemaVersion>
<Header>### uVision Project, (C) Keil Software</Header>
<Extensions>
<cExt>*.c</cExt>
<aExt>*.s*; *.src; *.a*</aExt>
<oExt>*.obj; *.o</oExt>
<lExt>*.lib</lExt>
<tExt>*.txt; *.h; *.inc; *.md</tExt>
<pExt>*.plm</pExt>
<CppX>*.cpp</CppX>
<nMigrate>0</nMigrate>
</Extensions>
<DaveTm>
<dwLowDateTime>0</dwLowDateTime>
<dwHighDateTime>0</dwHighDateTime>
</DaveTm>
<Target>
<TargetName>arm</TargetName>
<ToolsetNumber>0x4</ToolsetNumber>
<ToolsetName>ARM-ADS</ToolsetName>
<TargetOption>
<CLKADS>12000000</CLKADS>
<OPTTT>
<gFlags>1</gFlags>
<BeepAtEnd>1</BeepAtEnd>
<RunSim>0</RunSim>
<RunTarget>1</RunTarget>
<RunAbUc>0</RunAbUc>
</OPTTT>
<OPTHX>
<HexSelection>1</HexSelection>
<FlashByte>65535</FlashByte>
<HexRangeLowAddress>0</HexRangeLowAddress>
<HexRangeHighAddress>0</HexRangeHighAddress>
<HexOffset>0</HexOffset>
</OPTHX>
<OPTLEX>
<PageWidth>79</PageWidth>
<PageLength>66</PageLength>
<TabStop>8</TabStop>
<ListingPath></ListingPath>
</OPTLEX>
<ListingPage>
<CreateCListing>1</CreateCListing>
<CreateAListing>1</CreateAListing>
<CreateLListing>1</CreateLListing>
<CreateIListing>0</CreateIListing>
<AsmCond>1</AsmCond>
<AsmSymb>1</AsmSymb>
<AsmXref>0</AsmXref>
<CCond>1</CCond>
<CCode>0</CCode>
<CListInc>0</CListInc>
<CSymb>0</CSymb>
<LinkerCodeListing>0</LinkerCodeListing>
</ListingPage>
<OPTXL>
<LMap>1</LMap>
<LComments>1</LComments>
<LGenerateSymbols>1</LGenerateSymbols>
<LLibSym>1</LLibSym>
<LLines>1</LLines>
<LLocSym>1</LLocSym>
<LPubSym>1</LPubSym>
<LXref>0</LXref>
<LExpSel>0</LExpSel>
</OPTXL>
<OPTFL>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<IsCurrentTarget>1</IsCurrentTarget>
</OPTFL>
<CpuCode>18</CpuCode>
<DebugOpt>
<uSim>0</uSim>
<uTrg>1</uTrg>
<sLdApp>1</sLdApp>
<sGomain>1</sGomain>
<sRbreak>1</sRbreak>
<sRwatch>1</sRwatch>
<sRmem>1</sRmem>
<sRfunc>1</sRfunc>
<sRbox>1</sRbox>
<tLdApp>1</tLdApp>
<tGomain>1</tGomain>
<tRbreak>1</tRbreak>
<tRwatch>1</tRwatch>
<tRmem>1</tRmem>
<tRfunc>1</tRfunc>
<tRbox>1</tRbox>
<tRtrace>1</tRtrace>
<sRSysVw>1</sRSysVw>
<tRSysVw>1</tRSysVw>
<sRunDeb>0</sRunDeb>
<sLrtime>0</sLrtime>
<bEvRecOn>1</bEvRecOn>
<bSchkAxf>0</bSchkAxf>
<bTchkAxf>0</bTchkAxf>
<nTsel>6</nTsel>
<sDll></sDll>
<sDllPa></sDllPa>
<sDlgDll></sDlgDll>
<sDlgPa></sDlgPa>
<sIfile></sIfile>
<tDll></tDll>
<tDllPa></tDllPa>
<tDlgDll></tDlgDll>
<tDlgPa></tDlgPa>
<tIfile></tIfile>
<pMon>STLink\ST-LINKIII-KEIL_SWO.dll</pMon>
</DebugOpt>
<TargetDriverDllRegistry>
<SetRegEntry>
<Number>0</Number>
<Key>UL2CM3</Key>
<Name>UL2CM3(-S0 -C0 -P0 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024 -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM))</Name>
</SetRegEntry>
<SetRegEntry>
<Number>0</Number>
<Key>ST-LINKIII-KEIL_SWO</Key>
<Name>-U-O142 -O2254 -S0 -C0 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
</SetRegEntry>
</TargetDriverDllRegistry>
<Breakpoint/>
<Tracepoint>
<THDelay>0</THDelay>
</Tracepoint>
<DebugFlag>
<trace>0</trace>
<periodic>1</periodic>
<aLwin>1</aLwin>
<aCover>0</aCover>
<aSer1>0</aSer1>
<aSer2>0</aSer2>
<aPa>0</aPa>
<viewmode>1</viewmode>
<vrSel>0</vrSel>
<aSym>0</aSym>
<aTbox>0</aTbox>
<AscS1>0</AscS1>
<AscS2>0</AscS2>
<AscS3>0</AscS3>
<aSer3>0</aSer3>
<eProf>0</eProf>
<aLa>0</aLa>
<aPa1>0</aPa1>
<AscS4>0</AscS4>
<aSer4>0</aSer4>
<StkLoc>1</StkLoc>
<TrcWin>0</TrcWin>
<newCpu>0</newCpu>
<uProt>0</uProt>
</DebugFlag>
<LintExecutable></LintExecutable>
<LintConfigFile></LintConfigFile>
<bLintAuto>0</bLintAuto>
<bAutoGenD>0</bAutoGenD>
<LntExFlags>0</LntExFlags>
<pMisraName></pMisraName>
<pszMrule></pszMrule>
<pSingCmds></pSingCmds>
<pMultCmds></pMultCmds>
<pMisraNamep></pMisraNamep>
<pszMrulep></pszMrulep>
<pSingCmdsp></pSingCmdsp>
<pMultCmdsp></pMultCmdsp>
<DebugDescription>
<Enable>1</Enable>
<EnableFlashSeq>1</EnableFlashSeq>
<EnableLog>0</EnableLog>
<Protocol>2</Protocol>
<DbgClock>10000000</DbgClock>
</DebugDescription>
</TargetOption>
</Target>
<Group>
<GroupName>Application/MDK-ARM</GroupName>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
<File>
<GroupNumber>1</GroupNumber>
<FileNumber>1</FileNumber>
<FileType>2</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>startup_stm32f407xx.s</PathWithFileName>
<FilenameWithoutPath>startup_stm32f407xx.s</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>
<GroupName>Application/User/Core</GroupName>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
<File>
<GroupNumber>2</GroupNumber>
<FileNumber>2</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Core/Src/main.c</PathWithFileName>
<FilenameWithoutPath>main.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>2</GroupNumber>
<FileNumber>3</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Core/Src/gpio.c</PathWithFileName>
<FilenameWithoutPath>gpio.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>2</GroupNumber>
<FileNumber>4</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Core/Src/freertos.c</PathWithFileName>
<FilenameWithoutPath>freertos.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>2</GroupNumber>
<FileNumber>5</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Core/Src/can.c</PathWithFileName>
<FilenameWithoutPath>can.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>2</GroupNumber>
<FileNumber>6</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Core/Src/tim.c</PathWithFileName>
<FilenameWithoutPath>tim.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>2</GroupNumber>
<FileNumber>7</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Core/Src/stm32f4xx_it.c</PathWithFileName>
<FilenameWithoutPath>stm32f4xx_it.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>2</GroupNumber>
<FileNumber>8</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Core/Src/stm32f4xx_hal_msp.c</PathWithFileName>
<FilenameWithoutPath>stm32f4xx_hal_msp.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>
<GroupName>Drivers/STM32F4xx_HAL_Driver</GroupName>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>9</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c</PathWithFileName>
<FilenameWithoutPath>stm32f4xx_hal_can.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>10</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c</PathWithFileName>
<FilenameWithoutPath>stm32f4xx_hal_rcc.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>11</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c</PathWithFileName>
<FilenameWithoutPath>stm32f4xx_hal_rcc_ex.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>12</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c</PathWithFileName>
<FilenameWithoutPath>stm32f4xx_hal_flash.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>13</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c</PathWithFileName>
<FilenameWithoutPath>stm32f4xx_hal_flash_ex.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>14</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c</PathWithFileName>
<FilenameWithoutPath>stm32f4xx_hal_flash_ramfunc.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>15</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c</PathWithFileName>
<FilenameWithoutPath>stm32f4xx_hal_gpio.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>16</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c</PathWithFileName>
<FilenameWithoutPath>stm32f4xx_hal_dma_ex.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>17</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c</PathWithFileName>
<FilenameWithoutPath>stm32f4xx_hal_dma.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>18</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c</PathWithFileName>
<FilenameWithoutPath>stm32f4xx_hal_pwr.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>19</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c</PathWithFileName>
<FilenameWithoutPath>stm32f4xx_hal_pwr_ex.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>20</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c</PathWithFileName>
<FilenameWithoutPath>stm32f4xx_hal_cortex.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>21</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c</PathWithFileName>
<FilenameWithoutPath>stm32f4xx_hal.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>22</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c</PathWithFileName>
<FilenameWithoutPath>stm32f4xx_hal_exti.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>23</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c</PathWithFileName>
<FilenameWithoutPath>stm32f4xx_hal_tim.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>3</GroupNumber>
<FileNumber>24</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c</PathWithFileName>
<FilenameWithoutPath>stm32f4xx_hal_tim_ex.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>
<GroupName>Drivers/CMSIS</GroupName>
<tvExp>1</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
<File>
<GroupNumber>4</GroupNumber>
<FileNumber>25</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Core/Src/system_stm32f4xx.c</PathWithFileName>
<FilenameWithoutPath>system_stm32f4xx.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>
<GroupName>Middlewares/FreeRTOS</GroupName>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
<File>
<GroupNumber>5</GroupNumber>
<FileNumber>26</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Middlewares/Third_Party/FreeRTOS/Source/croutine.c</PathWithFileName>
<FilenameWithoutPath>croutine.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>5</GroupNumber>
<FileNumber>27</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Middlewares/Third_Party/FreeRTOS/Source/event_groups.c</PathWithFileName>
<FilenameWithoutPath>event_groups.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>5</GroupNumber>
<FileNumber>28</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Middlewares/Third_Party/FreeRTOS/Source/list.c</PathWithFileName>
<FilenameWithoutPath>list.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>5</GroupNumber>
<FileNumber>29</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Middlewares/Third_Party/FreeRTOS/Source/queue.c</PathWithFileName>
<FilenameWithoutPath>queue.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>5</GroupNumber>
<FileNumber>30</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c</PathWithFileName>
<FilenameWithoutPath>stream_buffer.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>5</GroupNumber>
<FileNumber>31</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Middlewares/Third_Party/FreeRTOS/Source/tasks.c</PathWithFileName>
<FilenameWithoutPath>tasks.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>5</GroupNumber>
<FileNumber>32</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Middlewares/Third_Party/FreeRTOS/Source/timers.c</PathWithFileName>
<FilenameWithoutPath>timers.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>5</GroupNumber>
<FileNumber>33</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c</PathWithFileName>
<FilenameWithoutPath>cmsis_os2.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>5</GroupNumber>
<FileNumber>34</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c</PathWithFileName>
<FilenameWithoutPath>heap_4.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>5</GroupNumber>
<FileNumber>35</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F/port.c</PathWithFileName>
<FilenameWithoutPath>port.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>
<GroupName>Middlewares/Library/DSP Library/DSP Library</GroupName>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>0</RteFlg>
<File>
<GroupNumber>6</GroupNumber>
<FileNumber>36</FileNumber>
<FileType>4</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>../Middlewares/ST/ARM/DSP/Lib/arm_cortexM4lf_math.lib</PathWithFileName>
<FilenameWithoutPath>arm_cortexM4lf_math.lib</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>
<GroupName>::CMSIS</GroupName>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel>
<RteFlg>1</RteFlg>
</Group>
</ProjectOpt>

710
MDK-ARM/arm.uvprojx Normal file
View File

@ -0,0 +1,710 @@
<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
<Project xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="project_projx.xsd">
<SchemaVersion>2.1</SchemaVersion>
<Header>### uVision Project, (C) Keil Software</Header>
<Targets>
<Target>
<TargetName>arm</TargetName>
<ToolsetNumber>0x4</ToolsetNumber>
<ToolsetName>ARM-ADS</ToolsetName>
<uAC6>0</uAC6>
<TargetOption>
<TargetCommonOption>
<Device>STM32F407IGHx</Device>
<Vendor>STMicroelectronics</Vendor>
<PackID>Keil.STM32F4xx_DFP.3.1.1</PackID>
<PackURL>https://www.keil.com/pack/</PackURL>
<Cpu>IRAM(0x20000000-0x2001BFFF) IRAM2(0x2001C000-0x2001FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(25000000) FPU2 CPUTYPE("Cortex-M4") TZ</Cpu>
<FlashUtilSpec></FlashUtilSpec>
<StartupFile></StartupFile>
<FlashDriverDll></FlashDriverDll>
<DeviceId>0</DeviceId>
<RegisterFile></RegisterFile>
<MemoryEnv></MemoryEnv>
<Cmp></Cmp>
<Asm></Asm>
<Linker></Linker>
<OHString></OHString>
<InfinionOptionDll></InfinionOptionDll>
<SLE66CMisc></SLE66CMisc>
<SLE66AMisc></SLE66AMisc>
<SLE66LinkerMisc></SLE66LinkerMisc>
<SFDFile>$$Device:STM32F407IGHx$CMSIS\SVD\STM32F40x.svd</SFDFile>
<bCustSvd>0</bCustSvd>
<UseEnv>0</UseEnv>
<BinPath></BinPath>
<IncludePath></IncludePath>
<LibPath></LibPath>
<RegisterFilePath></RegisterFilePath>
<DBRegisterFilePath></DBRegisterFilePath>
<TargetStatus>
<Error>0</Error>
<ExitCodeStop>0</ExitCodeStop>
<ButtonStop>0</ButtonStop>
<NotGenerated>0</NotGenerated>
<InvalidFlash>1</InvalidFlash>
</TargetStatus>
<OutputDirectory>arm\</OutputDirectory>
<OutputName>arm</OutputName>
<CreateExecutable>1</CreateExecutable>
<CreateLib>0</CreateLib>
<CreateHexFile>1</CreateHexFile>
<DebugInformation>1</DebugInformation>
<BrowseInformation>1</BrowseInformation>
<ListingPath></ListingPath>
<HexFormatSelection>1</HexFormatSelection>
<Merge32K>0</Merge32K>
<CreateBatchFile>0</CreateBatchFile>
<BeforeCompile>
<RunUserProg1>0</RunUserProg1>
<RunUserProg2>0</RunUserProg2>
<UserProg1Name></UserProg1Name>
<UserProg2Name></UserProg2Name>
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
<UserProg2Dos16Mode>0</UserProg2Dos16Mode>
<nStopU1X>0</nStopU1X>
<nStopU2X>0</nStopU2X>
</BeforeCompile>
<BeforeMake>
<RunUserProg1>0</RunUserProg1>
<RunUserProg2>0</RunUserProg2>
<UserProg1Name></UserProg1Name>
<UserProg2Name></UserProg2Name>
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
<UserProg2Dos16Mode>0</UserProg2Dos16Mode>
<nStopB1X>0</nStopB1X>
<nStopB2X>0</nStopB2X>
</BeforeMake>
<AfterMake>
<RunUserProg1>0</RunUserProg1>
<RunUserProg2>1</RunUserProg2>
<UserProg1Name></UserProg1Name>
<UserProg2Name></UserProg2Name>
<UserProg1Dos16Mode>0</UserProg1Dos16Mode>
<UserProg2Dos16Mode>0</UserProg2Dos16Mode>
<nStopA1X>0</nStopA1X>
<nStopA2X>0</nStopA2X>
</AfterMake>
<SelectedForBatchBuild>1</SelectedForBatchBuild>
<SVCSIdString></SVCSIdString>
</TargetCommonOption>
<CommonProperty>
<UseCPPCompiler>0</UseCPPCompiler>
<RVCTCodeConst>0</RVCTCodeConst>
<RVCTZI>0</RVCTZI>
<RVCTOtherData>0</RVCTOtherData>
<ModuleSelection>0</ModuleSelection>
<IncludeInBuild>1</IncludeInBuild>
<AlwaysBuild>0</AlwaysBuild>
<GenerateAssemblyFile>0</GenerateAssemblyFile>
<AssembleAssemblyFile>0</AssembleAssemblyFile>
<PublicsOnly>0</PublicsOnly>
<StopOnExitCode>3</StopOnExitCode>
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>0</ComprImg>
</CommonProperty>
<DllOption>
<SimDllName>SARMCM3.DLL</SimDllName>
<SimDllArguments>-REMAP -MPU</SimDllArguments>
<SimDlgDll>DCM.DLL</SimDlgDll>
<SimDlgDllArguments>-pCM4</SimDlgDllArguments>
<TargetDllName>SARMCM3.DLL</TargetDllName>
<TargetDllArguments>-MPU</TargetDllArguments>
<TargetDlgDll>TCM.DLL</TargetDlgDll>
<TargetDlgDllArguments>-pCM4</TargetDlgDllArguments>
</DllOption>
<DebugOption>
<OPTHX>
<HexSelection>1</HexSelection>
<HexRangeLowAddress>0</HexRangeLowAddress>
<HexRangeHighAddress>0</HexRangeHighAddress>
<HexOffset>0</HexOffset>
<Oh166RecLen>16</Oh166RecLen>
</OPTHX>
</DebugOption>
<Utilities>
<Flash1>
<UseTargetDll>1</UseTargetDll>
<UseExternalTool>0</UseExternalTool>
<RunIndependent>0</RunIndependent>
<UpdateFlashBeforeDebugging>1</UpdateFlashBeforeDebugging>
<Capability>1</Capability>
<DriverSelection>4101</DriverSelection>
</Flash1>
<bUseTDR>1</bUseTDR>
<Flash2>BIN\UL2V8M.DLL</Flash2>
<Flash3></Flash3>
<Flash4></Flash4>
<pFcarmOut></pFcarmOut>
<pFcarmGrp></pFcarmGrp>
<pFcArmRoot></pFcArmRoot>
<FcArmLst>0</FcArmLst>
</Utilities>
<TargetArmAds>
<ArmAdsMisc>
<GenerateListings>0</GenerateListings>
<asHll>1</asHll>
<asAsm>1</asAsm>
<asMacX>1</asMacX>
<asSyms>1</asSyms>
<asFals>1</asFals>
<asDbgD>1</asDbgD>
<asForm>1</asForm>
<ldLst>0</ldLst>
<ldmm>1</ldmm>
<ldXref>1</ldXref>
<BigEnd>0</BigEnd>
<AdsALst>1</AdsALst>
<AdsACrf>1</AdsACrf>
<AdsANop>0</AdsANop>
<AdsANot>0</AdsANot>
<AdsLLst>1</AdsLLst>
<AdsLmap>1</AdsLmap>
<AdsLcgr>1</AdsLcgr>
<AdsLsym>1</AdsLsym>
<AdsLszi>1</AdsLszi>
<AdsLtoi>1</AdsLtoi>
<AdsLsun>1</AdsLsun>
<AdsLven>1</AdsLven>
<AdsLsxf>1</AdsLsxf>
<RvctClst>0</RvctClst>
<GenPPlst>0</GenPPlst>
<AdsCpuType>"Cortex-M4"</AdsCpuType>
<RvctDeviceName></RvctDeviceName>
<mOS>0</mOS>
<uocRom>0</uocRom>
<uocRam>0</uocRam>
<hadIROM>1</hadIROM>
<hadIRAM>1</hadIRAM>
<hadXRAM>0</hadXRAM>
<uocXRam>0</uocXRam>
<RvdsVP>2</RvdsVP>
<RvdsMve>0</RvdsMve>
<RvdsCdeCp>0</RvdsCdeCp>
<nBranchProt>0</nBranchProt>
<hadIRAM2>1</hadIRAM2>
<hadIROM2>0</hadIROM2>
<StupSel>8</StupSel>
<useUlib>0</useUlib>
<EndSel>0</EndSel>
<uLtcg>0</uLtcg>
<nSecure>0</nSecure>
<RoSelD>3</RoSelD>
<RwSelD>4</RwSelD>
<CodeSel>0</CodeSel>
<OptFeed>0</OptFeed>
<NoZi1>0</NoZi1>
<NoZi2>0</NoZi2>
<NoZi3>0</NoZi3>
<NoZi4>0</NoZi4>
<NoZi5>0</NoZi5>
<Ro1Chk>0</Ro1Chk>
<Ro2Chk>0</Ro2Chk>
<Ro3Chk>0</Ro3Chk>
<Ir1Chk>1</Ir1Chk>
<Ir2Chk>1</Ir2Chk>
<Ra1Chk>0</Ra1Chk>
<Ra2Chk>0</Ra2Chk>
<Ra3Chk>0</Ra3Chk>
<Im1Chk>1</Im1Chk>
<Im2Chk>1</Im2Chk>
<OnChipMemories>
<Ocm1>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm1>
<Ocm2>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm2>
<Ocm3>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm3>
<Ocm4>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm4>
<Ocm5>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm5>
<Ocm6>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</Ocm6>
<IRAM>
<Type>0</Type>
<StartAddress>0x20000000</StartAddress>
<Size>0x1c000</Size>
</IRAM>
<IROM>
<Type>1</Type>
<StartAddress>0x8000000</StartAddress>
<Size>0x100000</Size>
</IROM>
<XRAM>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</XRAM>
<OCR_RVCT1>
<Type>1</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT1>
<OCR_RVCT2>
<Type>1</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT2>
<OCR_RVCT3>
<Type>1</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT3>
<OCR_RVCT4>
<Type>1</Type>
<StartAddress>0x8000000</StartAddress>
<Size>0x100000</Size>
</OCR_RVCT4>
<OCR_RVCT5>
<Type>1</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT5>
<OCR_RVCT6>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT6>
<OCR_RVCT7>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT7>
<OCR_RVCT8>
<Type>0</Type>
<StartAddress>0x0</StartAddress>
<Size>0x0</Size>
</OCR_RVCT8>
<OCR_RVCT9>
<Type>0</Type>
<StartAddress>0x20000000</StartAddress>
<Size>0x1c000</Size>
</OCR_RVCT9>
<OCR_RVCT10>
<Type>0</Type>
<StartAddress>0x2001c000</StartAddress>
<Size>0x4000</Size>
</OCR_RVCT10>
</OnChipMemories>
<RvctStartVector></RvctStartVector>
</ArmAdsMisc>
<Cads>
<interw>1</interw>
<Optim>4</Optim>
<oTime>0</oTime>
<SplitLS>0</SplitLS>
<OneElfS>1</OneElfS>
<Strict>0</Strict>
<EnumInt>0</EnumInt>
<PlainCh>0</PlainCh>
<Ropi>0</Ropi>
<Rwpi>0</Rwpi>
<wLevel>2</wLevel>
<uThumb>0</uThumb>
<uSurpInc>0</uSurpInc>
<uC99>1</uC99>
<uGnu>0</uGnu>
<useXO>0</useXO>
<v6Lang>5</v6Lang>
<v6LangP>3</v6LangP>
<vShortEn>1</vShortEn>
<vShortWch>1</vShortWch>
<v6Lto>0</v6Lto>
<v6WtE>0</v6WtE>
<v6Rtti>0</v6Rtti>
<VariousControls>
<MiscControls></MiscControls>
<Define>USE_HAL_DRIVER,STM32F407xx</Define>
<Undefine></Undefine>
<IncludePath>../Core/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include;../Middlewares/ST/ARM/DSP/Inc</IncludePath>
</VariousControls>
</Cads>
<Aads>
<interw>1</interw>
<Ropi>0</Ropi>
<Rwpi>0</Rwpi>
<thumb>0</thumb>
<SplitLS>0</SplitLS>
<SwStkChk>0</SwStkChk>
<NoWarn>0</NoWarn>
<uSurpInc>0</uSurpInc>
<useXO>0</useXO>
<ClangAsOpt>1</ClangAsOpt>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath>../Core/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include;../Middlewares/ST/ARM/DSP/Inc</IncludePath>
</VariousControls>
</Aads>
<LDads>
<umfTarg>1</umfTarg>
<Ropi>0</Ropi>
<Rwpi>0</Rwpi>
<noStLib>0</noStLib>
<RepFail>1</RepFail>
<useFile>0</useFile>
<TextAddressRange></TextAddressRange>
<DataAddressRange></DataAddressRange>
<pXoBase></pXoBase>
<ScatterFile></ScatterFile>
<IncludeLibs></IncludeLibs>
<IncludeLibsPath></IncludeLibsPath>
<Misc></Misc>
<LinkerInputFile></LinkerInputFile>
<DisabledWarnings></DisabledWarnings>
</LDads>
</TargetArmAds>
</TargetOption>
<Groups>
<Group>
<GroupName>Application/MDK-ARM</GroupName>
<Files>
<File>
<FileName>startup_stm32f407xx.s</FileName>
<FileType>2</FileType>
<FilePath>startup_stm32f407xx.s</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>Application/User/Core</GroupName>
<Files>
<File>
<FileName>main.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/main.c</FilePath>
</File>
<File>
<FileName>gpio.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/gpio.c</FilePath>
</File>
<File>
<FileName>freertos.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/freertos.c</FilePath>
</File>
<File>
<FileName>can.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/can.c</FilePath>
</File>
<File>
<FileName>tim.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/tim.c</FilePath>
</File>
<File>
<FileName>stm32f4xx_it.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/stm32f4xx_it.c</FilePath>
</File>
<File>
<FileName>stm32f4xx_hal_msp.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/stm32f4xx_hal_msp.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>Drivers/STM32F4xx_HAL_Driver</GroupName>
<Files>
<File>
<FileName>stm32f4xx_hal_can.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_can.c</FilePath>
</File>
<File>
<FileName>stm32f4xx_hal_rcc.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc.c</FilePath>
</File>
<File>
<FileName>stm32f4xx_hal_rcc_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_rcc_ex.c</FilePath>
</File>
<File>
<FileName>stm32f4xx_hal_flash.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash.c</FilePath>
</File>
<File>
<FileName>stm32f4xx_hal_flash_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ex.c</FilePath>
</File>
<File>
<FileName>stm32f4xx_hal_flash_ramfunc.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_flash_ramfunc.c</FilePath>
</File>
<File>
<FileName>stm32f4xx_hal_gpio.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_gpio.c</FilePath>
</File>
<File>
<FileName>stm32f4xx_hal_dma_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma_ex.c</FilePath>
</File>
<File>
<FileName>stm32f4xx_hal_dma.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_dma.c</FilePath>
</File>
<File>
<FileName>stm32f4xx_hal_pwr.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr.c</FilePath>
</File>
<File>
<FileName>stm32f4xx_hal_pwr_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_pwr_ex.c</FilePath>
</File>
<File>
<FileName>stm32f4xx_hal_cortex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_cortex.c</FilePath>
</File>
<File>
<FileName>stm32f4xx_hal.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal.c</FilePath>
</File>
<File>
<FileName>stm32f4xx_hal_exti.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_exti.c</FilePath>
</File>
<File>
<FileName>stm32f4xx_hal_tim.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim.c</FilePath>
</File>
<File>
<FileName>stm32f4xx_hal_tim_ex.c</FileName>
<FileType>1</FileType>
<FilePath>../Drivers/STM32F4xx_HAL_Driver/Src/stm32f4xx_hal_tim_ex.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>Drivers/CMSIS</GroupName>
<Files>
<File>
<FileName>system_stm32f4xx.c</FileName>
<FileType>1</FileType>
<FilePath>../Core/Src/system_stm32f4xx.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>Middlewares/FreeRTOS</GroupName>
<Files>
<File>
<FileName>croutine.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/croutine.c</FilePath>
</File>
<File>
<FileName>event_groups.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/event_groups.c</FilePath>
</File>
<File>
<FileName>list.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/list.c</FilePath>
</File>
<File>
<FileName>queue.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/queue.c</FilePath>
</File>
<File>
<FileName>stream_buffer.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/stream_buffer.c</FilePath>
</File>
<File>
<FileName>tasks.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/tasks.c</FilePath>
</File>
<File>
<FileName>timers.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/timers.c</FilePath>
</File>
<File>
<FileName>cmsis_os2.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.c</FilePath>
</File>
<File>
<FileName>heap_4.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/portable/MemMang/heap_4.c</FilePath>
</File>
<File>
<FileName>port.c</FileName>
<FileType>1</FileType>
<FilePath>../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F/port.c</FilePath>
</File>
</Files>
</Group>
<Group>
<GroupName>Middlewares/Library/DSP Library/DSP Library</GroupName>
<GroupOption>
<CommonProperty>
<UseCPPCompiler>0</UseCPPCompiler>
<RVCTCodeConst>0</RVCTCodeConst>
<RVCTZI>0</RVCTZI>
<RVCTOtherData>0</RVCTOtherData>
<ModuleSelection>0</ModuleSelection>
<IncludeInBuild>2</IncludeInBuild>
<AlwaysBuild>2</AlwaysBuild>
<GenerateAssemblyFile>2</GenerateAssemblyFile>
<AssembleAssemblyFile>2</AssembleAssemblyFile>
<PublicsOnly>2</PublicsOnly>
<StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty>
<GroupArmAds>
<Cads>
<interw>2</interw>
<Optim>0</Optim>
<oTime>2</oTime>
<SplitLS>2</SplitLS>
<OneElfS>2</OneElfS>
<Strict>2</Strict>
<EnumInt>2</EnumInt>
<PlainCh>2</PlainCh>
<Ropi>2</Ropi>
<Rwpi>2</Rwpi>
<wLevel>4</wLevel>
<uThumb>2</uThumb>
<uSurpInc>2</uSurpInc>
<uC99>2</uC99>
<uGnu>2</uGnu>
<useXO>2</useXO>
<v6Lang>0</v6Lang>
<v6LangP>0</v6LangP>
<vShortEn>2</vShortEn>
<vShortWch>2</vShortWch>
<v6Lto>2</v6Lto>
<v6WtE>2</v6WtE>
<v6Rtti>2</v6Rtti>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath></IncludePath>
</VariousControls>
</Cads>
<Aads>
<interw>2</interw>
<Ropi>2</Ropi>
<Rwpi>2</Rwpi>
<thumb>2</thumb>
<SplitLS>2</SplitLS>
<SwStkChk>2</SwStkChk>
<NoWarn>2</NoWarn>
<uSurpInc>2</uSurpInc>
<useXO>2</useXO>
<ClangAsOpt>1</ClangAsOpt>
<VariousControls>
<MiscControls></MiscControls>
<Define></Define>
<Undefine></Undefine>
<IncludePath></IncludePath>
</VariousControls>
</Aads>
</GroupArmAds>
</GroupOption>
<Files>
<File>
<FileName>arm_cortexM4lf_math.lib</FileName>
<FileType>4</FileType>
<FilePath>../Middlewares/ST/ARM/DSP/Lib/arm_cortexM4lf_math.lib</FilePath>
<FileOption>
<CommonProperty>
<UseCPPCompiler>2</UseCPPCompiler>
<RVCTCodeConst>0</RVCTCodeConst>
<RVCTZI>0</RVCTZI>
<RVCTOtherData>0</RVCTOtherData>
<ModuleSelection>0</ModuleSelection>
<IncludeInBuild>1</IncludeInBuild>
<AlwaysBuild>2</AlwaysBuild>
<GenerateAssemblyFile>2</GenerateAssemblyFile>
<AssembleAssemblyFile>2</AssembleAssemblyFile>
<PublicsOnly>2</PublicsOnly>
<StopOnExitCode>11</StopOnExitCode>
<CustomArgument></CustomArgument>
<IncludeLibraryModules></IncludeLibraryModules>
<ComprImg>1</ComprImg>
</CommonProperty>
<FileArmAds/>
</FileOption>
</File>
</Files>
</Group>
<Group>
<GroupName>::CMSIS</GroupName>
</Group>
</Groups>
</Target>
</Targets>
<RTE>
<apis/>
<components>
<component Cclass="CMSIS" Cgroup="CORE" Cvendor="ARM" Cversion="4.3.0" condition="CMSIS Core">
<package name="CMSIS" schemaVersion="1.3" url="http://www.keil.com/pack/" vendor="ARM" version="4.5.0"/>
<targetInfos>
<targetInfo name="arm"/>
</targetInfos>
</component>
</components>
<files/>
</RTE>
<LayerInfo>
<Layers>
<Layer>
<LayName>arm</LayName>
<LayPrjMark>1</LayPrjMark>
</Layer>
</Layers>
</LayerInfo>
</Project>

422
MDK-ARM/startup_stm32f407xx.s Normal file
View File

@ -0,0 +1,422 @@
;*******************************************************************************
;* File Name : startup_stm32f407xx.s
;* Author : MCD Application Team
;* Description : STM32F407xx devices vector table for MDK-ARM toolchain.
;* This module performs:
;* - Set the initial SP
;* - Set the initial PC == Reset_Handler
;* - Set the vector table entries with the exceptions ISR address
;* - Branches to __main in the C library (which eventually
;* calls main()).
;* After Reset the CortexM4 processor is in Thread mode,
;* priority is Privileged, and the Stack is set to Main.
;*******************************************************************************
;* @attention
;*
;* Copyright (c) 2017 STMicroelectronics.
;* All rights reserved.
;*
;* This software is licensed under terms that can be found in the LICENSE file
;* in the root directory of this software component.
;* If no LICENSE file comes with this software, it is provided AS-IS.
;*
;*******************************************************************************
;* <<< Use Configuration Wizard in Context Menu >>>
;
; Amount of memory (in bytes) allocated for Stack
; Tailor this value to your application needs
; <h> Stack Configuration
; <o> Stack Size (in Bytes) <0x0-0xFFFFFFFF:8>
; </h>
Stack_Size EQU 0x2000
AREA STACK, NOINIT, READWRITE, ALIGN=3
Stack_Mem SPACE Stack_Size
__initial_sp
; <h> Heap Configuration
; <o> Heap Size (in Bytes) <0x0-0xFFFFFFFF:8>
; </h>
Heap_Size EQU 0x2000
AREA HEAP, NOINIT, READWRITE, ALIGN=3
__heap_base
Heap_Mem SPACE Heap_Size
__heap_limit
PRESERVE8
THUMB
; Vector Table Mapped to Address 0 at Reset
AREA RESET, DATA, READONLY
EXPORT __Vectors
EXPORT __Vectors_End
EXPORT __Vectors_Size
__Vectors DCD __initial_sp ; Top of Stack
DCD Reset_Handler ; Reset Handler
DCD NMI_Handler ; NMI Handler
DCD HardFault_Handler ; Hard Fault Handler
DCD MemManage_Handler ; MPU Fault Handler
DCD BusFault_Handler ; Bus Fault Handler
DCD UsageFault_Handler ; Usage Fault Handler
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD 0 ; Reserved
DCD SVC_Handler ; SVCall Handler
DCD DebugMon_Handler ; Debug Monitor Handler
DCD 0 ; Reserved
DCD PendSV_Handler ; PendSV Handler
DCD SysTick_Handler ; SysTick Handler
; External Interrupts
DCD WWDG_IRQHandler ; Window WatchDog
DCD PVD_IRQHandler ; PVD through EXTI Line detection
DCD TAMP_STAMP_IRQHandler ; Tamper and TimeStamps through the EXTI line
DCD RTC_WKUP_IRQHandler ; RTC Wakeup through the EXTI line
DCD FLASH_IRQHandler ; FLASH
DCD RCC_IRQHandler ; RCC
DCD EXTI0_IRQHandler ; EXTI Line0
DCD EXTI1_IRQHandler ; EXTI Line1
DCD EXTI2_IRQHandler ; EXTI Line2
DCD EXTI3_IRQHandler ; EXTI Line3
DCD EXTI4_IRQHandler ; EXTI Line4
DCD DMA1_Stream0_IRQHandler ; DMA1 Stream 0
DCD DMA1_Stream1_IRQHandler ; DMA1 Stream 1
DCD DMA1_Stream2_IRQHandler ; DMA1 Stream 2
DCD DMA1_Stream3_IRQHandler ; DMA1 Stream 3
DCD DMA1_Stream4_IRQHandler ; DMA1 Stream 4
DCD DMA1_Stream5_IRQHandler ; DMA1 Stream 5
DCD DMA1_Stream6_IRQHandler ; DMA1 Stream 6
DCD ADC_IRQHandler ; ADC1, ADC2 and ADC3s
DCD CAN1_TX_IRQHandler ; CAN1 TX
DCD CAN1_RX0_IRQHandler ; CAN1 RX0
DCD CAN1_RX1_IRQHandler ; CAN1 RX1
DCD CAN1_SCE_IRQHandler ; CAN1 SCE
DCD EXTI9_5_IRQHandler ; External Line[9:5]s
DCD TIM1_BRK_TIM9_IRQHandler ; TIM1 Break and TIM9
DCD TIM1_UP_TIM10_IRQHandler ; TIM1 Update and TIM10
DCD TIM1_TRG_COM_TIM11_IRQHandler ; TIM1 Trigger and Commutation and TIM11
DCD TIM1_CC_IRQHandler ; TIM1 Capture Compare
DCD TIM2_IRQHandler ; TIM2
DCD TIM3_IRQHandler ; TIM3
DCD TIM4_IRQHandler ; TIM4
DCD I2C1_EV_IRQHandler ; I2C1 Event
DCD I2C1_ER_IRQHandler ; I2C1 Error
DCD I2C2_EV_IRQHandler ; I2C2 Event
DCD I2C2_ER_IRQHandler ; I2C2 Error
DCD SPI1_IRQHandler ; SPI1
DCD SPI2_IRQHandler ; SPI2
DCD USART1_IRQHandler ; USART1
DCD USART2_IRQHandler ; USART2
DCD USART3_IRQHandler ; USART3
DCD EXTI15_10_IRQHandler ; External Line[15:10]s
DCD RTC_Alarm_IRQHandler ; RTC Alarm (A and B) through EXTI Line
DCD OTG_FS_WKUP_IRQHandler ; USB OTG FS Wakeup through EXTI line
DCD TIM8_BRK_TIM12_IRQHandler ; TIM8 Break and TIM12
DCD TIM8_UP_TIM13_IRQHandler ; TIM8 Update and TIM13
DCD TIM8_TRG_COM_TIM14_IRQHandler ; TIM8 Trigger and Commutation and TIM14
DCD TIM8_CC_IRQHandler ; TIM8 Capture Compare
DCD DMA1_Stream7_IRQHandler ; DMA1 Stream7
DCD FMC_IRQHandler ; FMC
DCD SDIO_IRQHandler ; SDIO
DCD TIM5_IRQHandler ; TIM5
DCD SPI3_IRQHandler ; SPI3
DCD UART4_IRQHandler ; UART4
DCD UART5_IRQHandler ; UART5
DCD TIM6_DAC_IRQHandler ; TIM6 and DAC1&2 underrun errors
DCD TIM7_IRQHandler ; TIM7
DCD DMA2_Stream0_IRQHandler ; DMA2 Stream 0
DCD DMA2_Stream1_IRQHandler ; DMA2 Stream 1
DCD DMA2_Stream2_IRQHandler ; DMA2 Stream 2
DCD DMA2_Stream3_IRQHandler ; DMA2 Stream 3
DCD DMA2_Stream4_IRQHandler ; DMA2 Stream 4
DCD ETH_IRQHandler ; Ethernet
DCD ETH_WKUP_IRQHandler ; Ethernet Wakeup through EXTI line
DCD CAN2_TX_IRQHandler ; CAN2 TX
DCD CAN2_RX0_IRQHandler ; CAN2 RX0
DCD CAN2_RX1_IRQHandler ; CAN2 RX1
DCD CAN2_SCE_IRQHandler ; CAN2 SCE
DCD OTG_FS_IRQHandler ; USB OTG FS
DCD DMA2_Stream5_IRQHandler ; DMA2 Stream 5
DCD DMA2_Stream6_IRQHandler ; DMA2 Stream 6
DCD DMA2_Stream7_IRQHandler ; DMA2 Stream 7
DCD USART6_IRQHandler ; USART6
DCD I2C3_EV_IRQHandler ; I2C3 event
DCD I2C3_ER_IRQHandler ; I2C3 error
DCD OTG_HS_EP1_OUT_IRQHandler ; USB OTG HS End Point 1 Out
DCD OTG_HS_EP1_IN_IRQHandler ; USB OTG HS End Point 1 In
DCD OTG_HS_WKUP_IRQHandler ; USB OTG HS Wakeup through EXTI
DCD OTG_HS_IRQHandler ; USB OTG HS
DCD DCMI_IRQHandler ; DCMI
DCD 0 ; Reserved
DCD HASH_RNG_IRQHandler ; Hash and Rng
DCD FPU_IRQHandler ; FPU
__Vectors_End
__Vectors_Size EQU __Vectors_End - __Vectors
AREA |.text|, CODE, READONLY
; Reset handler
Reset_Handler PROC
EXPORT Reset_Handler [WEAK]
IMPORT SystemInit
IMPORT __main
LDR R0, =SystemInit
BLX R0
LDR R0, =__main
BX R0
ENDP
; Dummy Exception Handlers (infinite loops which can be modified)
NMI_Handler PROC
EXPORT NMI_Handler [WEAK]
B .
ENDP
HardFault_Handler\
PROC
EXPORT HardFault_Handler [WEAK]
B .
ENDP
MemManage_Handler\
PROC
EXPORT MemManage_Handler [WEAK]
B .
ENDP
BusFault_Handler\
PROC
EXPORT BusFault_Handler [WEAK]
B .
ENDP
UsageFault_Handler\
PROC
EXPORT UsageFault_Handler [WEAK]
B .
ENDP
SVC_Handler PROC
EXPORT SVC_Handler [WEAK]
B .
ENDP
DebugMon_Handler\
PROC
EXPORT DebugMon_Handler [WEAK]
B .
ENDP
PendSV_Handler PROC
EXPORT PendSV_Handler [WEAK]
B .
ENDP
SysTick_Handler PROC
EXPORT SysTick_Handler [WEAK]
B .
ENDP
Default_Handler PROC
EXPORT WWDG_IRQHandler [WEAK]
EXPORT PVD_IRQHandler [WEAK]
EXPORT TAMP_STAMP_IRQHandler [WEAK]
EXPORT RTC_WKUP_IRQHandler [WEAK]
EXPORT FLASH_IRQHandler [WEAK]
EXPORT RCC_IRQHandler [WEAK]
EXPORT EXTI0_IRQHandler [WEAK]
EXPORT EXTI1_IRQHandler [WEAK]
EXPORT EXTI2_IRQHandler [WEAK]
EXPORT EXTI3_IRQHandler [WEAK]
EXPORT EXTI4_IRQHandler [WEAK]
EXPORT DMA1_Stream0_IRQHandler [WEAK]
EXPORT DMA1_Stream1_IRQHandler [WEAK]
EXPORT DMA1_Stream2_IRQHandler [WEAK]
EXPORT DMA1_Stream3_IRQHandler [WEAK]
EXPORT DMA1_Stream4_IRQHandler [WEAK]
EXPORT DMA1_Stream5_IRQHandler [WEAK]
EXPORT DMA1_Stream6_IRQHandler [WEAK]
EXPORT ADC_IRQHandler [WEAK]
EXPORT CAN1_TX_IRQHandler [WEAK]
EXPORT CAN1_RX0_IRQHandler [WEAK]
EXPORT CAN1_RX1_IRQHandler [WEAK]
EXPORT CAN1_SCE_IRQHandler [WEAK]
EXPORT EXTI9_5_IRQHandler [WEAK]
EXPORT TIM1_BRK_TIM9_IRQHandler [WEAK]
EXPORT TIM1_UP_TIM10_IRQHandler [WEAK]
EXPORT TIM1_TRG_COM_TIM11_IRQHandler [WEAK]
EXPORT TIM1_CC_IRQHandler [WEAK]
EXPORT TIM2_IRQHandler [WEAK]
EXPORT TIM3_IRQHandler [WEAK]
EXPORT TIM4_IRQHandler [WEAK]
EXPORT I2C1_EV_IRQHandler [WEAK]
EXPORT I2C1_ER_IRQHandler [WEAK]
EXPORT I2C2_EV_IRQHandler [WEAK]
EXPORT I2C2_ER_IRQHandler [WEAK]
EXPORT SPI1_IRQHandler [WEAK]
EXPORT SPI2_IRQHandler [WEAK]
EXPORT USART1_IRQHandler [WEAK]
EXPORT USART2_IRQHandler [WEAK]
EXPORT USART3_IRQHandler [WEAK]
EXPORT EXTI15_10_IRQHandler [WEAK]
EXPORT RTC_Alarm_IRQHandler [WEAK]
EXPORT OTG_FS_WKUP_IRQHandler [WEAK]
EXPORT TIM8_BRK_TIM12_IRQHandler [WEAK]
EXPORT TIM8_UP_TIM13_IRQHandler [WEAK]
EXPORT TIM8_TRG_COM_TIM14_IRQHandler [WEAK]
EXPORT TIM8_CC_IRQHandler [WEAK]
EXPORT DMA1_Stream7_IRQHandler [WEAK]
EXPORT FMC_IRQHandler [WEAK]
EXPORT SDIO_IRQHandler [WEAK]
EXPORT TIM5_IRQHandler [WEAK]
EXPORT SPI3_IRQHandler [WEAK]
EXPORT UART4_IRQHandler [WEAK]
EXPORT UART5_IRQHandler [WEAK]
EXPORT TIM6_DAC_IRQHandler [WEAK]
EXPORT TIM7_IRQHandler [WEAK]
EXPORT DMA2_Stream0_IRQHandler [WEAK]
EXPORT DMA2_Stream1_IRQHandler [WEAK]
EXPORT DMA2_Stream2_IRQHandler [WEAK]
EXPORT DMA2_Stream3_IRQHandler [WEAK]
EXPORT DMA2_Stream4_IRQHandler [WEAK]
EXPORT ETH_IRQHandler [WEAK]
EXPORT ETH_WKUP_IRQHandler [WEAK]
EXPORT CAN2_TX_IRQHandler [WEAK]
EXPORT CAN2_RX0_IRQHandler [WEAK]
EXPORT CAN2_RX1_IRQHandler [WEAK]
EXPORT CAN2_SCE_IRQHandler [WEAK]
EXPORT OTG_FS_IRQHandler [WEAK]
EXPORT DMA2_Stream5_IRQHandler [WEAK]
EXPORT DMA2_Stream6_IRQHandler [WEAK]
EXPORT DMA2_Stream7_IRQHandler [WEAK]
EXPORT USART6_IRQHandler [WEAK]
EXPORT I2C3_EV_IRQHandler [WEAK]
EXPORT I2C3_ER_IRQHandler [WEAK]
EXPORT OTG_HS_EP1_OUT_IRQHandler [WEAK]
EXPORT OTG_HS_EP1_IN_IRQHandler [WEAK]
EXPORT OTG_HS_WKUP_IRQHandler [WEAK]
EXPORT OTG_HS_IRQHandler [WEAK]
EXPORT DCMI_IRQHandler [WEAK]
EXPORT HASH_RNG_IRQHandler [WEAK]
EXPORT FPU_IRQHandler [WEAK]
WWDG_IRQHandler
PVD_IRQHandler
TAMP_STAMP_IRQHandler
RTC_WKUP_IRQHandler
FLASH_IRQHandler
RCC_IRQHandler
EXTI0_IRQHandler
EXTI1_IRQHandler
EXTI2_IRQHandler
EXTI3_IRQHandler
EXTI4_IRQHandler
DMA1_Stream0_IRQHandler
DMA1_Stream1_IRQHandler
DMA1_Stream2_IRQHandler
DMA1_Stream3_IRQHandler
DMA1_Stream4_IRQHandler
DMA1_Stream5_IRQHandler
DMA1_Stream6_IRQHandler
ADC_IRQHandler
CAN1_TX_IRQHandler
CAN1_RX0_IRQHandler
CAN1_RX1_IRQHandler
CAN1_SCE_IRQHandler
EXTI9_5_IRQHandler
TIM1_BRK_TIM9_IRQHandler
TIM1_UP_TIM10_IRQHandler
TIM1_TRG_COM_TIM11_IRQHandler
TIM1_CC_IRQHandler
TIM2_IRQHandler
TIM3_IRQHandler
TIM4_IRQHandler
I2C1_EV_IRQHandler
I2C1_ER_IRQHandler
I2C2_EV_IRQHandler
I2C2_ER_IRQHandler
SPI1_IRQHandler
SPI2_IRQHandler
USART1_IRQHandler
USART2_IRQHandler
USART3_IRQHandler
EXTI15_10_IRQHandler
RTC_Alarm_IRQHandler
OTG_FS_WKUP_IRQHandler
TIM8_BRK_TIM12_IRQHandler
TIM8_UP_TIM13_IRQHandler
TIM8_TRG_COM_TIM14_IRQHandler
TIM8_CC_IRQHandler
DMA1_Stream7_IRQHandler
FMC_IRQHandler
SDIO_IRQHandler
TIM5_IRQHandler
SPI3_IRQHandler
UART4_IRQHandler
UART5_IRQHandler
TIM6_DAC_IRQHandler
TIM7_IRQHandler
DMA2_Stream0_IRQHandler
DMA2_Stream1_IRQHandler
DMA2_Stream2_IRQHandler
DMA2_Stream3_IRQHandler
DMA2_Stream4_IRQHandler
ETH_IRQHandler
ETH_WKUP_IRQHandler
CAN2_TX_IRQHandler
CAN2_RX0_IRQHandler
CAN2_RX1_IRQHandler
CAN2_SCE_IRQHandler
OTG_FS_IRQHandler
DMA2_Stream5_IRQHandler
DMA2_Stream6_IRQHandler
DMA2_Stream7_IRQHandler
USART6_IRQHandler
I2C3_EV_IRQHandler
I2C3_ER_IRQHandler
OTG_HS_EP1_OUT_IRQHandler
OTG_HS_EP1_IN_IRQHandler
OTG_HS_WKUP_IRQHandler
OTG_HS_IRQHandler
DCMI_IRQHandler
HASH_RNG_IRQHandler
FPU_IRQHandler
B .
ENDP
ALIGN
;*******************************************************************************
; User Stack and Heap initialization
;*******************************************************************************
IF :DEF:__MICROLIB
EXPORT __initial_sp
EXPORT __heap_base
EXPORT __heap_limit
ELSE
IMPORT __use_two_region_memory
EXPORT __user_initial_stackheap
__user_initial_stackheap
LDR R0, = Heap_Mem
LDR R1, =(Stack_Mem + Stack_Size)
LDR R2, = (Heap_Mem + Heap_Size)
LDR R3, = Stack_Mem
BX LR
ALIGN
ENDIF
END

176
PRINTF_OUTPUT_GUIDE.md Normal file
View File

@ -0,0 +1,176 @@
# Printf 输出配置指南（Ozone 调试）
## 📌 当前状态
你的项目 `printf` 通过 `__io_putchar()` 输出，但该函数是弱定义（未实现），导致：
- ❌ **Ozone 调试时看不到任何 printf 输出**
- ❌ 验证测试的结果无法查看
---
## ✅ 解决方案（3种方式）
### 🔹 **方案1: SWO/ITM 输出（推荐）**
**优点：**
- ✅ Ozone 原生支持，无需额外硬件
- ✅ 不占用任何外设（UART等）
- ✅ 实时性好，不影响程序执行
**配置步骤：**
#### 1. 编译设置
已创建 `Core/Src/retarget.c` 文件，确保：
```c
#define USE_ITM_SWO 1 // 启用 ITM
#define USE_UART 0
#define USE_SEMIHOSTING 0
```
#### 2. Ozone 配置
在 Ozone 中：
1. **Tools** → **Terminal**
2. 勾选 **"Enable SWO"**
3. 设置 **SWO Speed**（通常自动检测）
4. 选择 **ITM Port 0** 用于 printf
#### 3. 硬件连接
确保调试器的 **SWO** 引脚已连接：
- ST-Link V2: SWO 自动支持
- J-Link: 需要连接 SWO 线（PB3）
#### 4. 验证
运行程序后，在 Ozone 的 **Terminal** 窗口应该能看到：
```
========================================
正逆运动学验证测试
========================================
...
```
---
### 🔹 **方案2: UART 串口输出**
**优点：**
- ✅ 可以同时用串口助手查看
- ✅ 脱离调试器也能输出
**缺点：**
- ❌ 占用一个 UART 外设
- ❌ 需要额外的 USB-TTL 模块
**配置步骤：**
#### 1. CubeMX 配置 UART
在 `arm.ioc` 中：
- 启用 USART1（或其他）
- 波特率：115200
- 生成代码
#### 2. 修改 retarget.c
```c
#define USE_ITM_SWO 0
#define USE_UART 1 // 启用 UART
#define USE_SEMIHOSTING 0
```
并确保包含正确的头文件：
```c
#include "usart.h" // 根据你的 UART 外设调整
```
#### 3. 查看输出
- 使用串口助手（如 PuTTY、Tera Term）
- 波特率：115200
- 连接对应的 COM 口
---
### 🔹 **方案3: 半主机模式（不推荐）**
**缺点：**
- ❌ 严重降低程序运行速度
- ❌ 每个 printf 都会暂停 CPU
仅适用于简单调试，**不推荐**在实时控制系统中使用。
---
## 🎯 推荐配置（默认已配置）
**使用 ITM/SWO 方式**，因为：
1. 不占用外设资源
2. Ozone 原生支持，配置简单
3. 实时性好，适合机器人控制
---
## 🔧 Ozone 具体操作步骤
### 1. 打开 SWO 配置
```
Tools → Terminal
```
### 2. 配置 SWO
```
☑ Enable SWO
SWO Speed: [Auto] (或手动设置为系统时钟/2)
Prescaler: 1
```
### 3. 选择 ITM 通道
```
☑ ITM Port 0 (用于 printf)
```
### 4. 运行程序
点击 **Go** 或 **F5** 运行程序
### 5. 查看输出
在 Ozone 底部的 **Terminal** 窗口查看 printf 输出
---
## 🐛 故障排查
| 问题 | 原因 | 解决方法 |
|------|------|---------|
| Terminal 窗口无输出 | SWO 未启用 | 检查 Tools→Terminal 配置 |
| 乱码 | SWO 速度设置错误 | 调整 SWO Speed |
| 仍然无输出 | retarget.c 未编译 | 检查 CMakeLists.txt 是否包含 |
| 硬件错误 | SWO 引脚未连接 | 检查调试器连接 |
---
## 📝 CMakeLists.txt 配置
确保 `retarget.c` 被编译：
```cmake
# 在 CMakeLists.txt 中添加
set(SOURCES
# ... 其他文件 ...
Core/Src/retarget.c # 添加这一行
)
```
---
## 💡 验证是否工作
重新编译并运行后，应该在 Ozone Terminal 看到：
```
========================================
正逆运动学验证测试
========================================
测试1: 零位验证
原始关节角度: [0.000, 0.000, 0.000, 0.000, 0.000, 0.000] rad
正运动学位姿: x=0.00, y=0.00, z=530.00 mm
...
```
如果看不到输出，按照上述步骤逐一检查配置。

269
STM32F407XX_FLASH.ld Normal file
View File

@ -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
**
** <h2><center>&copy; COPYRIGHT(c) 2025 STMicroelectronics</center></h2>
**
** 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:* ( * )
}
}

28
User/bsp/bsp.h Normal file
View File

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

26
User/bsp/bsp_config.yaml Normal file
View File

@ -0,0 +1,26 @@
can:
devices:
- instance: CAN1
name: '1'
- instance: CAN2
name: '2'
enabled: true
mm:
enabled: true
pwm:
configs:
- channel: TIM_CHANNEL_1
custom_name: TIM5_CH1
label: TIM5_CH1
timer: TIM5
- channel: TIM_CHANNEL_2
custom_name: TIM5_CH2
label: TIM5_CH2
timer: TIM5
- channel: TIM_CHANNEL_3
custom_name: TIM5_CH3
label: TIM5_CH3
timer: TIM5
enabled: true
time:
enabled: true

708
User/bsp/can.c Normal file
View File

@ -0,0 +1,708 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp/can.h"
#include "bsp/bsp.h"
#include <can.h>
#include <cmsis_os2.h>
#include <string.h>
/* 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);
}

259
User/bsp/can.h Normal file
View File

@ -0,0 +1,259 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <can.h>
#include "bsp/bsp.h"
#include "bsp/mm.h"
#include <stdint.h>
#include <stdbool.h>
#include <cmsis_os.h>
/* 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

30
User/bsp/mm.c Normal file
View File

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

32
User/bsp/mm.h Normal file
View File

@ -0,0 +1,32 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stddef.h>
#include <stdint.h>
/* 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

112
User/bsp/pwm.c Normal file
View File

@ -0,0 +1,112 @@
/* Includes ----------------------------------------------------------------- */
#include "tim.h"
#include "bsp/pwm.h"
#include "bsp.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
typedef struct {
TIM_HandleTypeDef *tim;
uint16_t channel;
} BSP_PWM_Config_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */
static const BSP_PWM_Config_t PWM_Map[BSP_PWM_NUM] = {
{&htim5, TIM_CHANNEL_1},
{&htim5, TIM_CHANNEL_2},
{&htim5, TIM_CHANNEL_3},
};
/* Private function -------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch) {
if (ch >= BSP_PWM_NUM) return BSP_ERR;
HAL_TIM_PWM_Start(PWM_Map[ch].tim, PWM_Map[ch].channel);
return BSP_OK;
}
int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle) {
if (ch >= BSP_PWM_NUM) return BSP_ERR;
if (duty_cycle > 1.0f) {
duty_cycle = 1.0f;
}
if (duty_cycle < 0.0f) {
duty_cycle = 0.0f;
}
// 获取ARR值（周期值）
uint32_t arr = __HAL_TIM_GET_AUTORELOAD(PWM_Map[ch].tim);
// 计算比较值：CCR = duty_cycle * (ARR + 1)
uint32_t ccr = (uint32_t)(duty_cycle * (arr + 1));
__HAL_TIM_SET_COMPARE(PWM_Map[ch].tim, PWM_Map[ch].channel, ccr);
return BSP_OK;
}
int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq) {
if (ch >= BSP_PWM_NUM) return BSP_ERR;
uint32_t timer_clock = HAL_RCC_GetPCLK1Freq(); // Get the timer clock frequency
uint32_t prescaler = PWM_Map[ch].tim->Init.Prescaler;
uint32_t period = (timer_clock / (prescaler + 1)) / freq - 1;
if (period > UINT16_MAX) {
return BSP_ERR; // Frequency too low
}
__HAL_TIM_SET_AUTORELOAD(PWM_Map[ch].tim, period);
return BSP_OK;
}
int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch) {
if (ch >= BSP_PWM_NUM) return BSP_ERR;
HAL_TIM_PWM_Stop(PWM_Map[ch].tim, PWM_Map[ch].channel);
return BSP_OK;
}
uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch) {
if (ch >= BSP_PWM_NUM) return BSP_ERR;
return PWM_Map[ch].tim->Init.AutoReloadPreload;
}
TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch) {
return PWM_Map[ch].tim;
}
uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch) {
if (ch >= BSP_PWM_NUM) return BSP_ERR;
return PWM_Map[ch].channel;
}
int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length) {
if (ch >= BSP_PWM_NUM) return BSP_ERR;
HAL_TIM_PWM_Start_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel, pData, Length);
return BSP_OK;
}
int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch) {
if (ch >= BSP_PWM_NUM) return BSP_ERR;
HAL_TIM_PWM_Stop_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel);
return BSP_OK;
}

50
User/bsp/pwm.h Normal file
View File

@ -0,0 +1,50 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include "tim.h"
#include "bsp.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported types ----------------------------------------------------------- */
/* PWM通道 */
typedef enum {
BSP_PWM_TIM5_CH1,
BSP_PWM_TIM5_CH2,
BSP_PWM_TIM5_CH3,
BSP_PWM_NUM,
BSP_PWM_ERR,
} BSP_PWM_Channel_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch);
int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle);
int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq);
int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch);
uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch);
uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch);
TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch);
int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length);
int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

81
User/bsp/time.c Normal file
View File

@ -0,0 +1,81 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp/time.h"
#include "bsp.h"
#include <cmsis_os2.h>
#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 */

43
User/bsp/time.h Normal file
View File

@ -0,0 +1,43 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#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

155
User/bsp/uart.c Normal file
View File

@ -0,0 +1,155 @@
/* Includes ----------------------------------------------------------------- */
#include <usart.h>
#include "bsp/uart.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */
static void (*UART_Callback[BSP_UART_NUM][BSP_UART_CB_NUM])(void);
/* Private function -------------------------------------------------------- */
static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
if (huart->Instance == UART5)
return BSP_UART_RC;
else
return BSP_UART_ERR;
}
void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_TX_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_TX_CPLT_CB]();
}
}
}
void HAL_UART_TxHalfCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_TX_HALF_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_TX_HALF_CPLT_CB]();
}
}
}
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_RX_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_RX_CPLT_CB]();
}
}
}
void HAL_UART_RxHalfCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_RX_HALF_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_RX_HALF_CPLT_CB]();
}
}
}
void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_ERROR_CB]) {
UART_Callback[bsp_uart][BSP_UART_ERROR_CB]();
}
}
}
void HAL_UART_AbortCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_ABORT_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_ABORT_CPLT_CB]();
}
}
}
void HAL_UART_AbortTransmitCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_ABORT_TX_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_ABORT_TX_CPLT_CB]();
}
}
}
void HAL_UART_AbortReceiveCpltCallback(UART_HandleTypeDef *huart) {
BSP_UART_t bsp_uart = UART_Get(huart);
if (bsp_uart != BSP_UART_ERR) {
if (UART_Callback[bsp_uart][BSP_UART_ABORT_RX_CPLT_CB]) {
UART_Callback[bsp_uart][BSP_UART_ABORT_RX_CPLT_CB]();
}
}
}
/* Exported functions ------------------------------------------------------- */
void BSP_UART_IRQHandler(UART_HandleTypeDef *huart) {
if (__HAL_UART_GET_FLAG(huart, UART_FLAG_IDLE)) {
__HAL_UART_CLEAR_IDLEFLAG(huart);
if (UART_Callback[UART_Get(huart)][BSP_UART_IDLE_LINE_CB]) {
UART_Callback[UART_Get(huart)][BSP_UART_IDLE_LINE_CB]();
}
}
}
UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
switch (uart) {
case BSP_UART_RC:
return &huart3;
default:
return NULL;
}
}
int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type,
void (*callback)(void)) {
if (callback == NULL) return BSP_ERR_NULL;
if (uart >= BSP_UART_NUM || type >= BSP_UART_CB_NUM) return BSP_ERR;
UART_Callback[uart][type] = callback;
return BSP_OK;
}
int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma) {
if (uart >= BSP_UART_NUM) return BSP_ERR;
if (data == NULL || size == 0) return BSP_ERR_NULL;
if (dma) {
return HAL_UART_Transmit_DMA(BSP_UART_GetHandle(uart), data, size);
} else {
return HAL_UART_Transmit_IT(BSP_UART_GetHandle(uart), data, size);
}
}
int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma) {
if (uart >= BSP_UART_NUM) return BSP_ERR;
if (data == NULL || size == 0) return BSP_ERR_NULL;
if (dma) {
return HAL_UART_Receive_DMA(BSP_UART_GetHandle(uart), data, size);
} else {
return HAL_UART_Receive_IT(BSP_UART_GetHandle(uart), data, size);
}
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

68
User/bsp/uart.h Normal file
View File

@ -0,0 +1,68 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <usart.h>
#include <stdint.h>
#include <stdbool.h>
#include "bsp/bsp.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported types ----------------------------------------------------------- */
/* 要添加使用UART的新设备，需要先在此添加对应的枚举值 */
/* UART实体枚举，与设备对应 */
typedef enum {
BSP_UART_RC,
BSP_UART_NUM,
BSP_UART_ERR,
} BSP_UART_t;
/* UART支持的中断回调函数类型，具体参考HAL中定义 */
typedef enum {
BSP_UART_TX_HALF_CPLT_CB,
BSP_UART_TX_CPLT_CB,
BSP_UART_RX_HALF_CPLT_CB,
BSP_UART_RX_CPLT_CB,
BSP_UART_ERROR_CB,
BSP_UART_ABORT_CPLT_CB,
BSP_UART_ABORT_TX_CPLT_CB,
BSP_UART_ABORT_RX_CPLT_CB,
BSP_UART_IDLE_LINE_CB,
BSP_UART_CB_NUM,
} BSP_UART_Callback_t;
/* Exported functions prototypes -------------------------------------------- */
UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart);
void BSP_UART_IRQHandler(UART_HandleTypeDef *huart);
int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type,
void (*callback)(void));
int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma);
int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

184
User/component/ARM_USAGE.md Normal file
View File

@ -0,0 +1,184 @@
# 6自由度机械臂正运动学使用说明
## 📋 文件说明
- **arm.h** - 机械臂接口头文件
- **arm.cpp** - 机械臂实现文件（基于toolbox/robotics库）
- **arm_main.c** - 使用示例
## 🚀 快速开始
### 1. 初始化机械臂
```c
#include "component/arm.h"
// 定义机器人尺寸参数 (TODO: 替换为实际测量值)
ArmParams_t arm_params = {
.h0 = 100.0f, // 基座高度 (mm)
.L1 = 200.0f, // 大臂长度 (mm)
.L2 = 150.0f, // 小臂长度 (mm)
.L3 = 80.0f // 夹爪长度 (mm)
};
// 初始化
Arm_Init(&arm_params);
```
### 2. 正运动学计算
```c
// 输入：6个关节角度
JointAngles_t joint_angles;
joint_angles.q[0] = 0.0f; // J1: 基座Yaw (rad)
joint_angles.q[1] = M_PI/4; // J2: 大臂Pitch (rad)
joint_angles.q[2] = -M_PI/4; // J3: 大臂末端Pitch (rad)
joint_angles.q[3] = 0.0f; // J4: 小臂Roll (rad)
joint_angles.q[4] = M_PI/6; // J5: 小臂末端Pitch (rad)
joint_angles.q[5] = 0.0f; // J6: 夹爪Roll (rad)
// 输出：末端位姿
Pose_t end_pose;
// 执行正运动学
if (Arm_ForwardKinematics(&joint_angles, &end_pose) == 0) {
// 成功！
printf("末端位置: x=%.2f, y=%.2f, z=%.2f (mm)\n",
end_pose.x, end_pose.y, end_pose.z);
printf("末端姿态: roll=%.2f, pitch=%.2f, yaw=%.2f (rad)\n",
end_pose.roll, end_pose.pitch, end_pose.yaw);
}
```
### 3. 获取中间关节位姿（可选）
```c
Pose_t joint3_pose;
// 获取第3个关节的位姿
if (Arm_GetJointPose(&joint_angles, 3, &joint3_pose) == 0) {
printf("J3位置: x=%.2f, y=%.2f, z=%.2f\n",
joint3_pose.x, joint3_pose.y, joint3_pose.z);
}
```
## 📐 关节配置说明
| 关节 | 名称 | 类型 | 旋转轴 | 说明 |
|-----|------|------|--------|------|
| J1 | 基座 | Yaw | Z轴 | 基座旋转 |
| J2 | 大臂起点 | Pitch | Y轴 | 大臂俯仰 |
| J3 | 大臂末端 | Pitch | Y轴 | 控制小臂 |
| J4 | 小臂起点 | Roll | X轴 | 小臂旋转 |
| J5 | 小臂末端 | Pitch | Y轴 | 控制夹爪段 |
| J6 | 夹爪 | Roll | X轴 | 夹爪旋转 |
## ⚙️ DH参数配置
当前使用的是**标准DH参数**，定义在 `arm.cpp` 的 `CreateDHLinks()` 函数中：
```cpp
// J1: 基座 Yaw
s_links[0] = robotics::Link(0, params->h0, 0, 0, robotics::R);
// J2: 大臂 Pitch
s_links[1] = robotics::Link(0, 0, 0, M_PI_2, robotics::R);
// J3: 大臂末端 Pitch
s_links[2] = robotics::Link(0, 0, params->L1, 0, robotics::R);
// J4: 小臂起点 Roll
s_links[3] = robotics::Link(0, 0, 0, M_PI_2, robotics::R);
// J5: 小臂末端 Pitch
s_links[4] = robotics::Link(0, 0, params->L2, M_PI_2, robotics::R);
// J6: 夹爪 Roll
s_links[5] = robotics::Link(0, params->L3, 0, M_PI_2, robotics::R);
```
⚠️ **需要根据实际机器人测量结果调整！**
## 📊 数据结构
### ArmParams_t - 机器人尺寸参数
```c
typedef struct {
float h0; // 基座高度 (基座到J2的高度, mm)
float L1; // 大臂长度 (J2到J3的距离, mm)
float L2; // 小臂长度 (J4到J5的距离, mm)
float L3; // 夹爪长度 (J6到末端的距离, mm)
} ArmParams_t;
```
### JointAngles_t - 关节角度
```c
typedef struct {
float q[6]; // 6个关节角度 (rad)
} JointAngles_t;
```
### Pose_t - 位姿
```c
typedef struct {
float x, y, z; // 位置 (mm)
float roll, pitch, yaw; // 姿态 (rad) - RPY欧拉角
} Pose_t;
```
## 🔧 待办事项
- [ ] **测量实际机器人尺寸**：h0, L1, L2, L3
- [ ] **拍摄零位照片**：确认零位时各关节的姿态
- [ ] **验证DH参数**：根据实际机器人调整 `CreateDHLinks()` 函数
- [ ] **集成电机反馈**：将实际电机角度输入到正运动学
- [ ] **添加逆运动学**：实现末端位姿到关节角度的计算
## 📚 依赖库
- **toolbox/robotics.h** - 机器人学工具箱
- **toolbox/matrix.h** - 矩阵运算库
- **ARM CMSIS DSP** - 数学计算库
## 🎯 使用示例（完整）
参考 `User/task/arm_main.c` 中的实现：
```c
void Task_arm_main(void *argument) {
// 1. 初始化
ArmParams_t params = {100.0f, 200.0f, 150.0f, 80.0f};
Arm_Init(&params);
// 2. 设置关节角度
JointAngles_t q = {0};
while (1) {
// 3. 读取电机角度（示例）
q.q[0] = get_motor_angle(0);
q.q[1] = get_motor_angle(1);
// ...
// 4. 计算正运动学
Pose_t pose;
Arm_ForwardKinematics(&q, &pose);
// 5. 使用末端位姿
// ...
osDelay(10);
}
}
```
## 🐛 调试提示
如果计算结果不符合预期：
1. 检查DH参数是否正确
2. 确认角度单位是弧度（不是角度）
3. 验证关节零位的定义
4. 检查关节旋转方向（正负）
---
**下一步**：提供实际机器人的尺寸数据，我将帮您完善DH参数！

88
User/component/PowerControl.c Normal file
View File

@ -0,0 +1,88 @@
#include "PowerControl.h"
#include "stdint.h"
#include "math.h"
void power_calu(power_model_t* param,float* in_array,float* rpm_array)
{
float InitialGivePower[param->motor_num];
float totalpower=0;
float in[4];
float ve[4];
for (uint8_t i = 0; i < param->motor_num; i++) // first get all the initial motor power and total motor power
{
in[i] = in_array[i];
ve[i] = rpm_array[i];
InitialGivePower[i] = in[i] * param->toque_coefficient * ve[i] +
param->k2 * ve[i] * ve[i]+ param->k1 * in[i]*in[i] + param->constant;
param->power[i] = InitialGivePower[i];
if (InitialGivePower[i] < 0) // negative power not included (transitory)
continue;
totalpower += InitialGivePower[i];
}
param->total_power = totalpower;
}
float power_scale_calu(power_model_t** param_array,uint8_t num,float max_power)
{
float total_power=0;
for(uint8_t i=0;i<num;i++)
{
total_power+=param_array[i]->total_power;
}
if(total_power>max_power)
{return max_power/total_power;}
else
{return 2;}
}
void power_out_calu(power_model_t* param,float scale,float* in_array,float* rpm_array,float* out_array)
{
float in[4];
float ve[4];
if(scale<1)
{
float ScaledGivePower[4];
for (uint8_t i = 0; i < param->motor_num; i++)
{
in[i] = in_array[i];
ve[i] = rpm_array[i];
ScaledGivePower[i] = param->power[i] * scale; // get scaled power
if (ScaledGivePower[i] < 0)
{
continue;
}
float b = param->toque_coefficient * ve[i];
float c = param->k2 * ve[i]*ve[i] - ScaledGivePower[i] + param->constant;
float inside = b * b - 4 * param->k1 * c;
if (inside < 0)
{
continue;
}
else if (in[i] > 0) // Selection of the calculation formula according to the direction of the original moment
{
float temp = (-b + sqrt(inside)) / (2 * param->k1);
if (temp > 16000)
{
out_array[i] = 16000;
}
else
out_array[i] = temp;
}
else
{
float temp = (-b - sqrt(inside)) / (2 * param->k1);
if (temp < -16000)
{
out_array[i] = -16000;
}
else
out_array[i] = temp;
}
}
}
}

29
User/component/PowerControl.h Normal file
View File

@ -0,0 +1,29 @@
#ifndef CHASSIS_POWER_CONTROL_WITH_SUPERCAP_H
#define CHASSIS_POWER_CONTROL_WITH_SUPERCAP_H
#ifdef __cplusplus
extern "C" {
#endif
#include "main.h"
typedef struct power_model_t
{
uint8_t motor_num;
float total_power;
float* power;
float toque_coefficient;
float k1;
float k2 ;
float constant;
}power_model_t;
void power_calu(power_model_t* param,float* in_array,float* rpm_array);
float power_scale_calu(power_model_t** param_array,uint8_t num,float max_power);
void power_out_calu(power_model_t* param,float scale,float* in_array,float* rpm_array,float* out_array);
#ifdef __cplusplus
}
#endif
#endif

417
User/component/ahrs.c Normal file
View File

@ -0,0 +1,417 @@
/*
开源的AHRS算法。
MadgwickAHRS
*/
#include "ahrs.h"
#include <string.h>
#include "component/user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
#define BETA_IMU (0.033f)
#define BETA_AHRS (0.041f)
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* 2 * proportional gain (Kp) */
static float beta = BETA_IMU;
/**
* @brief 不使用磁力计计算姿态
*
* @param ahrs 姿态解算主结构体
* @param accl 加速度计数据
* @param gyro 陀螺仪数据
* @return int8_t 0对应没有错误
*/
static int8_t AHRS_UpdateIMU(AHRS_t *ahrs, const AHRS_Accl_t *accl,
const AHRS_Gyro_t *gyro) {
if (ahrs == NULL) return -1;
if (accl == NULL) return -1;
if (gyro == NULL) return -1;
beta = BETA_IMU;
float ax = accl->x;
float ay = accl->y;
float az = accl->z;
float gx = gyro->x;
float gy = gyro->y;
float gz = gyro->z;
float recip_norm;
float s0, s1, s2, s3;
float q_dot1, q_dot2, q_dot3, q_dot4;
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2,
q3q3;
/* Rate of change of quaternion from gyroscope */
q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy -
ahrs->quat.q3 * gz);
q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz -
ahrs->quat.q3 * gy);
q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz +
ahrs->quat.q3 * gx);
q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy -
ahrs->quat.q2 * gx);
/* Compute feedback only if accelerometer measurement valid (avoids NaN in
* accelerometer normalisation) */
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
/* Normalise accelerometer measurement */
recip_norm = InvSqrt(ax * ax + ay * ay + az * az);
ax *= recip_norm;
ay *= recip_norm;
az *= recip_norm;
/* Auxiliary variables to avoid repeated arithmetic */
_2q0 = 2.0f * ahrs->quat.q0;
_2q1 = 2.0f * ahrs->quat.q1;
_2q2 = 2.0f * ahrs->quat.q2;
_2q3 = 2.0f * ahrs->quat.q3;
_4q0 = 4.0f * ahrs->quat.q0;
_4q1 = 4.0f * ahrs->quat.q1;
_4q2 = 4.0f * ahrs->quat.q2;
_8q1 = 8.0f * ahrs->quat.q1;
_8q2 = 8.0f * ahrs->quat.q2;
q0q0 = ahrs->quat.q0 * ahrs->quat.q0;
q1q1 = ahrs->quat.q1 * ahrs->quat.q1;
q2q2 = ahrs->quat.q2 * ahrs->quat.q2;
q3q3 = ahrs->quat.q3 * ahrs->quat.q3;
/* Gradient decent algorithm corrective step */
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * ahrs->quat.q1 -
_2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
s2 = 4.0f * q0q0 * ahrs->quat.q2 + _2q0 * ax + _4q2 * q3q3 -
_2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
s3 = 4.0f * q1q1 * ahrs->quat.q3 - _2q1 * ax +
4.0f * q2q2 * ahrs->quat.q3 - _2q2 * ay;
/* normalise step magnitude */
recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
s0 *= recip_norm;
s1 *= recip_norm;
s2 *= recip_norm;
s3 *= recip_norm;
/* Apply feedback step */
q_dot1 -= beta * s0;
q_dot2 -= beta * s1;
q_dot3 -= beta * s2;
q_dot4 -= beta * s3;
}
/* Integrate rate of change of quaternion to yield quaternion */
ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq;
ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq;
ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq;
ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq;
/* Normalise quaternion */
recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 +
ahrs->quat.q1 * ahrs->quat.q1 +
ahrs->quat.q2 * ahrs->quat.q2 +
ahrs->quat.q3 * ahrs->quat.q3);
ahrs->quat.q0 *= recip_norm;
ahrs->quat.q1 *= recip_norm;
ahrs->quat.q2 *= recip_norm;
ahrs->quat.q3 *= recip_norm;
return 0;
}
/**
* @brief 初始化姿态解算
*
* @param ahrs 姿态解算主结构体
* @param magn 磁力计数据
* @param sample_freq 采样频率
* @return int8_t 0对应没有错误
*/
int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq) {
if (ahrs == NULL) return -1;
ahrs->inv_sample_freq = 1.0f / sample_freq;
ahrs->quat.q0 = 1.0f;
ahrs->quat.q1 = 0.0f;
ahrs->quat.q2 = 0.0f;
ahrs->quat.q3 = 0.0f;
if (magn) {
float yaw = -atan2(magn->y, magn->x);
if ((magn->x == 0.0f) && (magn->y == 0.0f) && (magn->z == 0.0f)) {
ahrs->quat.q0 = 0.800884545f;
ahrs->quat.q1 = 0.00862364192f;
ahrs->quat.q2 = -0.00283267116f;
ahrs->quat.q3 = 0.598749936f;
} else if ((yaw < (M_PI / 2.0f)) || (yaw > 0.0f)) {
ahrs->quat.q0 = 0.997458339f;
ahrs->quat.q1 = 0.000336312107f;
ahrs->quat.q2 = -0.0057230792f;
ahrs->quat.q3 = 0.0740156546;
} else if ((yaw < M_PI) || (yaw > (M_PI / 2.0f))) {
ahrs->quat.q0 = 0.800884545f;
ahrs->quat.q1 = 0.00862364192f;
ahrs->quat.q2 = -0.00283267116f;
ahrs->quat.q3 = 0.598749936f;
} else if ((yaw < 90.0f) || (yaw > M_PI)) {
ahrs->quat.q0 = 0.800884545f;
ahrs->quat.q1 = 0.00862364192f;
ahrs->quat.q2 = -0.00283267116f;
ahrs->quat.q3 = 0.598749936f;
} else if ((yaw < 90.0f) || (yaw > 0.0f)) {
ahrs->quat.q0 = 0.800884545f;
ahrs->quat.q1 = 0.00862364192f;
ahrs->quat.q2 = -0.00283267116f;
ahrs->quat.q3 = 0.598749936f;
}
}
return 0;
}
/**
* @brief 姿态运算更新一次
* @note 输入数据必须是NED(North East Down) 参考坐标系
*
* @param ahrs 姿态解算主结构体
* @param accl 加速度计数据
* @param gyro 陀螺仪数据
* @param magn 磁力计数据
* @return int8_t 0对应没有错误
*/
int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl,
const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn) {
if (ahrs == NULL) return -1;
if (accl == NULL) return -1;
if (gyro == NULL) return -1;
beta = BETA_AHRS;
float recip_norm;
float s0, s1, s2, s3;
float q_dot1, q_dot2, q_dot3, q_dot4;
float hx, hy;
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1,
_2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3,
q2q2, q2q3, q3q3;
if (magn == NULL) return AHRS_UpdateIMU(ahrs, accl, gyro);
float mx = magn->x;
float my = magn->y;
float mz = magn->z;
/* Use IMU algorithm if magnetometer measurement invalid (avoids NaN in */
/* magnetometer normalisation) */
if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
return AHRS_UpdateIMU(ahrs, accl, gyro);
}
float ax = accl->x;
float ay = accl->y;
float az = accl->z;
float gx = gyro->x;
float gy = gyro->y;
float gz = gyro->z;
/* Rate of change of quaternion from gyroscope */
q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy -
ahrs->quat.q3 * gz);
q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz -
ahrs->quat.q3 * gy);
q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz +
ahrs->quat.q3 * gx);
q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy -
ahrs->quat.q2 * gx);
/* Compute feedback only if accelerometer measurement valid (avoids NaN in
* accelerometer normalisation) */
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
/* Normalise accelerometer measurement */
recip_norm = InvSqrt(ax * ax + ay * ay + az * az);
ax *= recip_norm;
ay *= recip_norm;
az *= recip_norm;
/* Normalise magnetometer measurement */
recip_norm = InvSqrt(mx * mx + my * my + mz * mz);
mx *= recip_norm;
my *= recip_norm;
mz *= recip_norm;
/* Auxiliary variables to avoid repeated arithmetic */
_2q0mx = 2.0f * ahrs->quat.q0 * mx;
_2q0my = 2.0f * ahrs->quat.q0 * my;
_2q0mz = 2.0f * ahrs->quat.q0 * mz;
_2q1mx = 2.0f * ahrs->quat.q1 * mx;
_2q0 = 2.0f * ahrs->quat.q0;
_2q1 = 2.0f * ahrs->quat.q1;
_2q2 = 2.0f * ahrs->quat.q2;
_2q3 = 2.0f * ahrs->quat.q3;
_2q0q2 = 2.0f * ahrs->quat.q0 * ahrs->quat.q2;
_2q2q3 = 2.0f * ahrs->quat.q2 * ahrs->quat.q3;
q0q0 = ahrs->quat.q0 * ahrs->quat.q0;
q0q1 = ahrs->quat.q0 * ahrs->quat.q1;
q0q2 = ahrs->quat.q0 * ahrs->quat.q2;
q0q3 = ahrs->quat.q0 * ahrs->quat.q3;
q1q1 = ahrs->quat.q1 * ahrs->quat.q1;
q1q2 = ahrs->quat.q1 * ahrs->quat.q2;
q1q3 = ahrs->quat.q1 * ahrs->quat.q3;
q2q2 = ahrs->quat.q2 * ahrs->quat.q2;
q2q3 = ahrs->quat.q2 * ahrs->quat.q3;
q3q3 = ahrs->quat.q3 * ahrs->quat.q3;
/* Reference direction of Earth's magnetic field */
hx = mx * q0q0 - _2q0my * ahrs->quat.q3 +
_2q0mz * ahrs->quat.q2 + mx * q1q1 +
_2q1 * my * ahrs->quat.q2 + _2q1 * mz * ahrs->quat.q3 -
mx * q2q2 - mx * q3q3;
hy = _2q0mx * ahrs->quat.q3 + my * q0q0 -
_2q0mz * ahrs->quat.q1 + _2q1mx * ahrs->quat.q2 -
my * q1q1 + my * q2q2 + _2q2 * mz * ahrs->quat.q3 - my * q3q3;
// _2bx = sqrtf(hx * hx + hy * hy);
// 改为invsqrt
_2bx = 1.f / InvSqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * ahrs->quat.q2 + _2q0my * ahrs->quat.q1 +
mz * q0q0 + _2q1mx * ahrs->quat.q3 - mz * q1q1 +
_2q2 * my * ahrs->quat.q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
/* Gradient decent algorithm corrective step */
s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) +
_2q1 * (2.0f * q0q1 + _2q2q3 - ay) -
_2bz * ahrs->quat.q2 *
(_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
(-_2bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) *
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
_2bx * ahrs->quat.q2 *
(_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) +
_2q0 * (2.0f * q0q1 + _2q2q3 - ay) -
4.0f * ahrs->quat.q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
_2bz * ahrs->quat.q3 *
(_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
(_2bx * ahrs->quat.q2 + _2bz * ahrs->quat.q0) *
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
(_2bx * ahrs->quat.q3 - _4bz * ahrs->quat.q1) *
(_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) +
_2q3 * (2.0f * q0q1 + _2q2q3 - ay) -
4.0f * ahrs->quat.q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
(-_4bx * ahrs->quat.q2 - _2bz * ahrs->quat.q0) *
(_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
(_2bx * ahrs->quat.q1 + _2bz * ahrs->quat.q3) *
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
(_2bx * ahrs->quat.q0 - _4bz * ahrs->quat.q2) *
(_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) +
_2q2 * (2.0f * q0q1 + _2q2q3 - ay) +
(-_4bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) *
(_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
(-_2bx * ahrs->quat.q0 + _2bz * ahrs->quat.q2) *
(_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
_2bx * ahrs->quat.q1 *
(_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
/* normalise step magnitude */
recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
s0 *= recip_norm;
s1 *= recip_norm;
s2 *= recip_norm;
s3 *= recip_norm;
/* Apply feedback step */
q_dot1 -= beta * s0;
q_dot2 -= beta * s1;
q_dot3 -= beta * s2;
q_dot4 -= beta * s3;
}
/* Integrate rate of change of quaternion to yield quaternion */
ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq;
ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq;
ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq;
ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq;
/* Normalise quaternion */
recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 +
ahrs->quat.q1 * ahrs->quat.q1 +
ahrs->quat.q2 * ahrs->quat.q2 +
ahrs->quat.q3 * ahrs->quat.q3);
ahrs->quat.q0 *= recip_norm;
ahrs->quat.q1 *= recip_norm;
ahrs->quat.q2 *= recip_norm;
ahrs->quat.q3 *= recip_norm;
return 0;
}
/**
* @brief 通过姿态解算主结构体中的四元数计算欧拉角
*
* @param eulr 欧拉角
* @param ahrs 姿态解算主结构体
* @return int8_t 0对应没有错误
*/
int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs) {
if (eulr == NULL) return -1;
if (ahrs == NULL) return -1;
const float sinr_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q1 +
ahrs->quat.q2 * ahrs->quat.q3);
const float cosr_cosp =
1.0f - 2.0f * (ahrs->quat.q1 * ahrs->quat.q1 +
ahrs->quat.q2 * ahrs->quat.q2);
eulr->pit = atan2f(sinr_cosp, cosr_cosp);
const float sinp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q2 -
ahrs->quat.q3 * ahrs->quat.q1);
if (fabsf(sinp) >= 1.0f)
eulr->rol = copysignf(M_PI / 2.0f, sinp);
else
eulr->rol = asinf(sinp);
const float siny_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q3 +
ahrs->quat.q1 * ahrs->quat.q2);
const float cosy_cosp =
1.0f - 2.0f * (ahrs->quat.q2 * ahrs->quat.q2 +
ahrs->quat.q3 * ahrs->quat.q3);
eulr->yaw = atan2f(siny_cosp, cosy_cosp);
#if 0
eulr->yaw *= M_RAD2DEG_MULT;
eulr->rol *= M_RAD2DEG_MULT;
eulr->pit *= M_RAD2DEG_MULT;
#endif
return 0;
}
/**
* \brief 将对应数据置零
*
* \param eulr 被操作的数据
*/
void AHRS_ResetEulr(AHRS_Eulr_t *eulr) { memset(eulr, 0, sizeof(*eulr)); }
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

114
User/component/ahrs.h Normal file
View File

@ -0,0 +1,114 @@
/*
开源的AHRS算法。
MadgwickAHRS
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "component/user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* 欧拉角（Euler angle） */
typedef struct {
float yaw; /* 偏航角（Yaw angle） */
float pit; /* 俯仰角（Pitch angle） */
float rol; /* 翻滚角（Roll angle） */
} AHRS_Eulr_t;
/* 加速度计 Accelerometer */
typedef struct {
float x;
float y;
float z;
} AHRS_Accl_t;
/* 陀螺仪 Gyroscope */
typedef struct {
float x;
float y;
float z;
} AHRS_Gyro_t;
/* 磁力计 Magnetometer */
typedef struct {
float x;
float y;
float z;
} AHRS_Magn_t;
/* 四元数 */
typedef struct {
float q0;
float q1;
float q2;
float q3;
} AHRS_Quaternion_t;
/* 姿态解算算法主结构体 */
typedef struct {
/* 四元数 */
AHRS_Quaternion_t quat;
float inv_sample_freq; /* 采样频率的的倒数 */
} AHRS_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/**
* @brief 初始化姿态解算
*
* @param ahrs 姿态解算主结构体
* @param magn 磁力计数据
* @param sample_freq 采样频率
* @return int8_t 0对应没有错误
*/
int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq);
/**
* @brief 姿态运算更新一次
*
* @param ahrs 姿态解算主结构体
* @param accl 加速度计数据
* @param gyro 陀螺仪数据
* @param magn 磁力计数据
* @return int8_t 0对应没有错误
*/
int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl,
const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn);
/**
* @brief 通过姿态解算主结构体中的四元数计算欧拉角
*
* @param eulr 欧拉角
* @param ahrs 姿态解算主结构体
* @return int8_t 0对应没有错误
*/
int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs);
/**
* \brief 将对应数据置零
*
* \param eulr 被操作的数据
*/
void AHRS_ResetEulr(AHRS_Eulr_t *eulr);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

250
User/component/arm_kinematics/arm6dof.cpp Normal file
View File

@ -0,0 +1,250 @@
/**
******************************************************************************
* @file arm.cpp
* @brief 6-DOF robotic arm kinematics control implementation
******************************************************************************
*/
#include "arm6dof.h"
#include "toolbox/robotics.h"
#include "toolbox/matrix.h"
#include "user_math.h"
#include <string.h>
/* 静态变量 */
static robotics::Link* s_links = nullptr;
static robotics::Serial_Link<6>* s_robot = nullptr;
static bool s_initialized = false;
/**
* dh_params = [
{"a": 0, "alpha": pi / 2, "d": 0, "theta_offset": 0}, # Joint 1
{"a": 0.404, "alpha": 0.0, "d": 0, "theta_offset": 0}, # Joint 2
{"a": 0.091579, "alpha": -pi / 2, "d": 0, "theta_offset": pi / 2}, # Joint 3
{"a": 0, "alpha": pi / 2, "d": 0.2411, "theta_offset": 0},
{"a": 0, "alpha": -pi / 2, "d": 0, "theta_offset": 0},
{"a": 0, "alpha": 0, "d": 0.1094, "theta_offset": 0},
{"a": 0.0577, "alpha": pi / 2, "d": 0.033001, "theta_offset": 0},
{"a": 0.05, "alpha": 0, "d": 0, "theta_offset": 0},
]
* @brief 根据参数创建DH参数链表
* @note 标准DH参数，参考 DH_PARAMETERS.md 文档
*
* DH参数表（标准DH）：
* 关节 | θᵢ(变量) | dᵢ | aᵢ | αᵢ | 说明
* -----|----------|-------|------|--------|------------------
* 1 | q1 | h0 | 0 | π/2 | 底座旋转（Yaw）
* 2 | q2 | 0 | L1 | 0 | 肩部俯仰（Pitch）
* 3 | q3 | 0 | L2 | 0 | 肘部俯仰（Pitch）
* 4 | q4 | d4 | 0 | π/2 | 腕部滚转（Roll）
* 5 | q5 | 0 | 0 | -π/2 | 腕部俯仰（Pitch）
* 6 | q6 | d6 | 0 | 0 | 末端旋转（Roll）
/**
* @brief 初始化机械臂运动学模型
*/
void Arm6dof_Init(const Arm6dof_DHParams_t dh_params[6]) {
if (dh_params == nullptr) {
return;
}
// 创建DH链表
if (s_links != nullptr) {
delete[] s_links;
}
s_links = new robotics::Link[6];
for (int i = 0; i < 6; i++) {
// 构建质心向量（DH连杆坐标系下，由URDF经Rz(-theta_offset)旋转得到）
Matrixf<3, 1> rc_vec;
rc_vec[0][0] = dh_params[i].rc[0];
rc_vec[1][0] = dh_params[i].rc[1];
rc_vec[2][0] = dh_params[i].rc[2];
s_links[i] = robotics::Link(
dh_params[i].theta, // 初始theta值（通常为0）
dh_params[i].d, // 连杆偏移 (m)
dh_params[i].a, // 连杆长度 (m)
dh_params[i].alpha, // 扭转角 (rad)
robotics::R, // 旋转关节
dh_params[i].theta_offset, // theta偏移 (rad)
dh_params[i].qmin, // 最小角度 (rad)
dh_params[i].qmax, // 最大角度 (rad)
dh_params[i].m, // 质量 (kg)
rc_vec // 质心坐标（DH连杆坐标系，m）
);
}
// 创建机器人模型
if (s_robot != nullptr) {
delete s_robot;
}
s_robot = new robotics::Serial_Link<6>(s_links);
s_initialized = true;
}
/**
* @brief 正运动学
*/
int Arm6dof_ForwardKinematics(const Arm6dof_JointAngles_t* q, Arm6dof_Pose_t* pose) {
if (!s_initialized || q == nullptr || pose == nullptr) {
return -1;
}
// 将关节角度转换为矩阵格式
Matrixf<6, 1> q_mat;
for (int i = 0; i < 6; i++) {
q_mat[i][0] = q->q[i];
}
// 计算正运动学
Matrixf<4, 4> T = s_robot->fkine(q_mat);
// 提取位置 (x, y, z)
Matrixf<3, 1> pos = robotics::t2p(T);
pose->x = pos[0][0];
pose->y = pos[1][0];
pose->z = pos[2][0];
// 提取姿态 (RPY欧拉角)
Matrixf<3, 1> rpy = robotics::t2rpy(T);
pose->yaw = rpy[0][0];
pose->pitch = rpy[1][0];
pose->roll = rpy[2][0];
return 0;
}
/**
* @brief 获取指定关节的位姿
*/
int Arm6dof_GetJointPose(const Arm6dof_JointAngles_t* q, uint8_t joint_num, Arm6dof_Pose_t* pose) {
if (!s_initialized || q == nullptr || pose == nullptr) {
return -1;
}
if (joint_num < 1 || joint_num > 6) {
return -1;
}
// 将关节角度转换为矩阵格式
Matrixf<6, 1> q_mat;
for (int i = 0; i < 6; i++) {
q_mat[i][0] = q->q[i];
}
// 计算到第k个关节的变换矩阵
Matrixf<4, 4> T = s_robot->fkine(q_mat, joint_num);
// 提取位置和姿态
Matrixf<3, 1> pos = robotics::t2p(T);
pose->x = pos[0][0];
pose->y = pos[1][0];
pose->z = pos[2][0];
Matrixf<3, 1> rpy = robotics::t2rpy(T);
pose->yaw = rpy[0][0];
pose->pitch = rpy[1][0];
pose->roll = rpy[2][0];
return 0;
}
/**
* @brief 逆运动学
*/
int Arm6dof_InverseKinematics(const Arm6dof_Pose_t* pose, const Arm6dof_JointAngles_t* q_init,
Arm6dof_JointAngles_t* q_result, float tol, uint16_t max_iter) {
if (!s_initialized || pose == nullptr || q_result == nullptr) {
return -1;
}
// 构造目标变换矩阵
Matrixf<3, 1> rpy_vec;
rpy_vec[0][0] = pose->yaw;
rpy_vec[1][0] = pose->pitch;
rpy_vec[2][0] = pose->roll;
Matrixf<3, 3> R_target = robotics::rpy2r(rpy_vec);
Matrixf<3, 1> p_target;
p_target[0][0] = pose->x;
p_target[1][0] = pose->y;
p_target[2][0] = pose->z;
Matrixf<4, 4> T_target = robotics::rp2t(R_target, p_target);
// 初始猜测值
Matrixf<6, 1> q_guess = matrixf::zeros<6, 1>();
if (q_init != nullptr) {
for (int i = 0; i < 6; i++) {
q_guess[i][0] = q_init->q[i];
}
}
// 调用toolbox的逆运动学求解器（牛顿法）
Matrixf<6, 1> q_solved = s_robot->ikine(T_target, q_guess, tol, max_iter);
// 将结果复制到输出
for (int i = 0; i < 6; i++) {
q_result->q[i] = q_solved[i][0];
}
// 验证解的精度：位置误差 + 姿态误差双y重校验
Matrixf<4, 4> T_verify = s_robot->fkine(q_solved);
Matrixf<3, 1> p_verify = robotics::t2p(T_verify);
float pos_error = (p_verify - p_target).norm();
if (pos_error > tol * 10.0f) { // 位置误差过大，未收敛
return -1;
}
// // 姿态误差：计算两旋转矩阵的测地距离 theta = arccos((trace(R_v * R_t^T) - 1) / 2)
// // 使用旋转矩阵方法避免 RPY 奇异点和角度折叠问题
// Matrixf<3, 3> R_verify = robotics::t2r(T_verify);
// Matrixf<3, 3> R_diff = R_verify * R_target.trans();
// float trace_val = R_diff[0][0] + R_diff[1][1] + R_diff[2][2];
// float cos_theta = (trace_val - 1.0f) * 0.5f;
// // 数值钳位防止 acosf 域溢出
// if (cos_theta > 1.0f) cos_theta = 1.0f;
// if (cos_theta < -1.0f) cos_theta = -1.0f;
// float ang_error = acosf(cos_theta); // [0, π]
// const float ang_tol = 0.1f; // 姿态收敛阈值 (rad ≈ 5.7°)
// if (ang_error > ang_tol) { // 姿态未收敛（收敛到错误分支）
// return -1;
// }
return 0;
}
/**
* @brief 计算重力补偿力矩
* @details 利用牛顿-欧拉逆动力学(rne)，设速度qv=0、加速度qa=0，
* 计算的关节力矩即为平衡重力所需的力矩。
*/
int Arm6dof_GravityCompensation(const Arm6dof_JointAngles_t* q, float gravity_torques[6]) {
if (!s_initialized || q == nullptr || gravity_torques == nullptr) {
return -1;
}
// 关节角度
Matrixf<6, 1> q_mat;
for (int i = 0; i < 6; i++) {
q_mat[i][0] = q->q[i];
}
// 速度和加速度均为零 —— 只计算重力项
Matrixf<6, 1> qv = matrixf::zeros<6, 1>();
Matrixf<6, 1> qa = matrixf::zeros<6, 1>();
// 调用牛顿-欧拉逆动力学
Matrixf<6, 1> torques = s_robot->rne(q_mat, qv, qa);
for (int i = 0; i < 6; i++) {
gravity_torques[i] = torques[i][0];
}
return 0;
}

107
User/component/arm_kinematics/arm6dof.h Normal file
View File

@ -0,0 +1,107 @@
/**
******************************************************************************
* @file arm.h
* @brief 6-DOF robotic arm kinematics control
******************************************************************************
*/
#ifndef ARM_H
#define ARM_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
/* 机器人尺寸参数结构体 */
typedef struct {
// 6个运动关节的DH参数
float theta;
float d;
float a;
float alpha;
float theta_offset;
float qmin;
float qmax;
float m; // 连杆质量 (kg)
float rc[3]; // 质心在 DH 连杆坐标系下的坐标 (m): {x, y, z}
// 由 URDF inertial/origin 经 Rz(-theta_offset) 旋转得到
// MIT控制参数（可选，如不设置则使用默认值）
float kp; // 位置刚度 (N·m/rad)，默认值见Joint类
float kd; // 阻尼系数 (N·m·s/rad)，默认值见Joint类
} Arm6dof_DHParams_t;
typedef struct {
Arm6dof_DHParams_t DH_params[6];
struct {
float x, y, z; // 工具中心偏移 (m)
float roll, pitch, yaw; // 工具姿态偏移 (rad)
} tool_offset;
} Arm6dof_Params_t;
/* 末端位姿结构体 */
typedef struct {
float x, y, z; // 位置 (m)
float roll, pitch, yaw; // 姿态 (rad) - RPY欧拉角
} Arm6dof_Pose_t;
/* 关节角度结构体 */
typedef struct {
float q[6]; // 6个关节角度 (rad)
} Arm6dof_JointAngles_t;
/**
* @brief 初始化机械臂运动学模型
* @param dh_params 6个关节的DH参数数组
* @note 必须先调用此函数再使用正/逆运动学
*/
void Arm6dof_Init(const Arm6dof_DHParams_t dh_params[6]);
/**
* @brief 正运动学：根据关节角度计算末端位姿
* @param q 输入的6个关节角度 (rad)
* @param pose 输出的末端位姿 (位置m, 姿态rad)
* @retval 0: 成功, -1: 失败(未初始化)
*/
int Arm6dof_ForwardKinematics(const Arm6dof_JointAngles_t* q, Arm6dof_Pose_t* pose);
/**
* @brief 逆运动学：根据末端位姿计算关节角度
* @param pose 输入的目标末端位姿 (位置m, 姿态rad)
* @param q_init 初始关节角度猜测值 (用于数值解迭代)
* @param q_result 输出的关节角度解 (rad)
* @param tol 收敛误差容限，位置分量单位为(m)，建议0.001f(即1mm精度)
* @param max_iter 最大迭代次数，默认10
* @retval 0: 成功, -1: 失败(未初始化或无解)
* @note 使用牛顿法数值求解，需要提供合理的初始猜测值
*/
int Arm6dof_InverseKinematics(const Arm6dof_Pose_t* pose, const Arm6dof_JointAngles_t* q_init,
Arm6dof_JointAngles_t* q_result, float tol, uint16_t max_iter);
/**
* @brief 获取指定关节的位姿 (用于可视化或调试)
* @param q 输入的6个关节角度 (rad)
* @param joint_num 关节编号 (1-6)
* @param pose 输出的该关节位姿
* @retval 0: 成功, -1: 失败
*/
int Arm6dof_GetJointPose(const Arm6dof_JointAngles_t* q, uint8_t joint_num, Arm6dof_Pose_t* pose);
/**
* @brief 设置机器人参数 (如果需要动态修改)
* @param params 新的机器人尺寸参数
*/
void Arm6dof_SetParams(const Arm6dof_Params_t* params);
/**
* @brief 计算重力补偿力矩（基于牛顿-欧拉逆动力学，速度和加速度为零）
* @param q 当前6个关节角度 (rad)
* @param gravity_torques 输出的6个关节重力补偿力矩 (N·m，DH参数单位为m时)
* @retval 0: 成功, -1: 失败(未初始化)
*/
int Arm6dof_GravityCompensation(const Arm6dof_JointAngles_t* q, float gravity_torques[6]);
#ifdef __cplusplus
}
#endif
#endif // ARM_H

7
User/component/component_config.yaml Normal file
View File

@ -0,0 +1,7 @@
pid:
dependencies:
- component/filter
enabled: true
user_math:
dependencies: []
enabled: true

185
User/component/filter.c Normal file
View File

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

120
User/component/filter.h Normal file
View File

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

94
User/component/mixer.c Normal file
View File

@ -0,0 +1,94 @@
/*
混合器
*/
#include "mixer.h"
#include "math.h"
/**
* @brief 初始化混合器
*
* @param mixer 混合器
* @param mode 混合器模式
* @return int8_t 0对应没有错误
*/
int8_t Mixer_Init(Mixer_t *mixer, Mixer_Mode_t mode) {
if (mixer == NULL) return -1;
mixer->mode = mode;
return 0;
}
/**
* @brief 计算输出
*
* @param mixer 混合器
* @param move_vec 运动向量
* @param out 输出数组
* @param len 输出数组长短
* @param scale 输出放大因子
* @return int8_t 0对应没有错误
*/
int8_t Mixer_Apply(Mixer_t *mixer, MoveVector_t *move_vec, float *out,
int8_t len, float scale) {
if (mixer == NULL) return -1;
switch (mixer->mode) {
case MIXER_MECANUM:
if (len == 4) {
out[0] = move_vec->vx - move_vec->vy + move_vec->wz;
out[1] = move_vec->vx + move_vec->vy + move_vec->wz;
out[2] = -move_vec->vx + move_vec->vy + move_vec->wz;
out[3] = -move_vec->vx - move_vec->vy + move_vec->wz;
} else {
goto error;
}
break;
case MIXER_PARLFIX4:
if (len == 4) {
out[0] = -move_vec->vx;
out[1] = move_vec->vx;
out[2] = move_vec->vx;
out[3] = -move_vec->vx;
} else {
goto error;
}
case MIXER_PARLFIX2:
if (len == 2) {
out[0] = -move_vec->vx;
out[1] = move_vec->vx;
} else {
goto error;
}
case MIXER_SINGLE:
if (len == 1) {
out[0] = move_vec->vx;
} else {
goto error;
}
case MIXER_OMNICROSS:
case MIXER_OMNIPLUS:
goto error;
}
float abs_max = 0.f;
for (int8_t i = 0; i < len; i++) {
const float abs_val = fabsf(out[i]);
abs_max = (abs_val > abs_max) ? abs_val : abs_max;
}
if (abs_max > 1.f) {
for (int8_t i = 0; i < len; i++) {
out[i] /= abs_max;
}
}
for (int8_t i = 0; i < len; i++) {
out[i] *= scale;
}
return 0;
error:
for (uint8_t i = 0; i < len; i++) out[i] = 0;
return -1;
}

76
User/component/mixer.h Normal file
View File

@ -0,0 +1,76 @@
/*
混合器
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/** 四轮布局 */
/* 前 */
/* 2 1 */
/* 3 4 */
/* 两轮布局 */
/* 前 */
/* 2 1 */
/* 混合器模式 */
typedef enum {
MIXER_MECANUM, /* 麦克纳姆轮 */
MIXER_PARLFIX4, /* 平行四驱动轮 */
MIXER_PARLFIX2, /* 平行对侧两驱动轮 */
MIXER_OMNICROSS, /* 叉形全向轮 */
MIXER_OMNIPLUS, /* 十字全向轮 */
MIXER_SINGLE, /* 单个摩擦轮 */
} Mixer_Mode_t;
typedef struct {
Mixer_Mode_t mode;
} Mixer_t; /* 混合器主结构体 */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/**
* @brief 初始化混合器
*
* @param mixer 混合器
* @param mode 混合器模式
* @return int8_t 0对应没有错误
*/
int8_t Mixer_Init(Mixer_t *mixer, Mixer_Mode_t mode);
/**
* @brief 计算输出
*
* @param mixer 混合器
* @param move_vec 运动向量
* @param out 输出数组
* @param len 输出数组长短
* @param scale 输出放大因子
* @return int8_t 0对应没有错误
*/
int8_t Mixer_Apply(Mixer_t *mixer, MoveVector_t *move_vec, float *out,
int8_t len, float scale);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

158
User/component/pid.c Normal file
View File

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

107
User/component/pid.h Normal file
View File

@ -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 <stdint.h>
#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

24
User/component/toolbox/matrix.cpp Normal file
View File

@ -0,0 +1,24 @@
/**
******************************************************************************
* @file matrix.cpp/h
* @brief Matrix/vector calculation. 矩阵/向量运算
* @author Spoon Guan
******************************************************************************
* Copyright (c) 2023 Team JiaoLong-SJTU
* All rights reserved.
******************************************************************************
*/
#include "matrix.h"
// hat of vector
Matrixf<3, 3> vector3f::hat(Matrixf<3, 1> vec) {
float hat[9] = {0, -vec[2][0], vec[1][0], vec[2][0], 0,
-vec[0][0], -vec[1][0], vec[0][0], 0};
return Matrixf<3, 3>(hat);
}
// cross product
Matrixf<3, 1> vector3f::cross(Matrixf<3, 1> vec1, Matrixf<3, 1> vec2) {
return vector3f::hat(vec1) * vec2;
}

262
User/component/toolbox/matrix.h Normal file
View File

@ -0,0 +1,262 @@
/**
******************************************************************************
* @file matrix.cpp/h
* @brief Matrix/vector calculation. 矩阵/向量运算
* @author Spoon Guan
******************************************************************************
* Copyright (c) 2023 Team JiaoLong-SJTU
* All rights reserved.
******************************************************************************
*/
#ifndef MATRIX_H
#define MATRIX_H
#include "arm_math.h"
// Matrix class
template <int _rows, int _cols>
class
Matrixf {
public:
// Constructor without input data
Matrixf(void) : rows_(_rows), cols_(_cols) {
arm_mat_init_f32(&arm_mat_, _rows, _cols, this->data_);
}
// Constructor with input data
Matrixf(float data[_rows * _cols]) : Matrixf() {
memcpy(this->data_, data, _rows * _cols * sizeof(float));
}
// Copy constructor
Matrixf(const Matrixf<_rows, _cols>& mat) : Matrixf() {
memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float));
}
// Destructor
~Matrixf(void) {}
// Row size
int rows(void) { return _rows; }
// Column size
int cols(void) { return _cols; }
// Element
float* operator[](const int& row) { return &this->data_[row * _cols]; }
// Operators
Matrixf<_rows, _cols>& operator=(const Matrixf<_rows, _cols> mat) {
memcpy(this->data_, mat.data_, _rows * _cols * sizeof(float));
return *this;
}
Matrixf<_rows, _cols>& operator+=(const Matrixf<_rows, _cols> mat) {
arm_status s;
s = arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_);
return *this;
}
Matrixf<_rows, _cols>& operator-=(const Matrixf<_rows, _cols> mat) {
arm_status s;
s = arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &this->arm_mat_);
return *this;
}
Matrixf<_rows, _cols>& operator*=(const float& val) {
arm_status s;
s = arm_mat_scale_f32(&this->arm_mat_, val, &this->arm_mat_);
return *this;
}
Matrixf<_rows, _cols>& operator/=(const float& val) {
arm_status s;
s = arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &this->arm_mat_);
return *this;
}
Matrixf<_rows, _cols> operator+(const Matrixf<_rows, _cols>& mat) {
arm_status s;
Matrixf<_rows, _cols> res;
s = arm_mat_add_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_);
return res;
}
Matrixf<_rows, _cols> operator-(const Matrixf<_rows, _cols>& mat) {
arm_status s;
Matrixf<_rows, _cols> res;
s = arm_mat_sub_f32(&this->arm_mat_, &mat.arm_mat_, &res.arm_mat_);
return res;
}
Matrixf<_rows, _cols> operator*(const float& val) {
arm_status s;
Matrixf<_rows, _cols> res;
s = arm_mat_scale_f32(&this->arm_mat_, val, &res.arm_mat_);
return res;
}
friend Matrixf<_rows, _cols> operator*(const float& val,
const Matrixf<_rows, _cols>& mat) {
arm_status s;
Matrixf<_rows, _cols> res;
s = arm_mat_scale_f32(&mat.arm_mat_, val, &res.arm_mat_);
return res;
}
Matrixf<_rows, _cols> operator/(const float& val) {
arm_status s;
Matrixf<_rows, _cols> res;
s = arm_mat_scale_f32(&this->arm_mat_, 1.f / val, &res.arm_mat_);
return res;
}
// Matrix multiplication
template <int cols>
friend Matrixf<_rows, cols> operator*(const Matrixf<_rows, _cols>& mat1,
const Matrixf<_cols, cols>& mat2) {
arm_status s;
Matrixf<_rows, cols> res;
s = arm_mat_mult_f32(&mat1.arm_mat_, &mat2.arm_mat_, &res.arm_mat_);
return res;
}
// Submatrix
template <int rows, int cols>
Matrixf<rows, cols> block(const int& start_row, const int& start_col) {
Matrixf<rows, cols> res;
for (int row = start_row; row < start_row + rows; row++) {
memcpy((float*)res[0] + (row - start_row) * cols,
(float*)this->data_ + row * _cols + start_col,
cols * sizeof(float));
}
return res;
}
// Specific row
Matrixf<1, _cols> row(const int& row) { return block<1, _cols>(row, 0); }
// Specific column
Matrixf<_rows, 1> col(const int& col) { return block<_rows, 1>(0, col); }
// Transpose
Matrixf<_cols, _rows> trans(void) {
Matrixf<_cols, _rows> res;
arm_mat_trans_f32(&arm_mat_, &res.arm_mat_);
return res;
}
// Trace
float trace(void) {
float res = 0;
for (int i = 0; i < fmin(_rows, _cols); i++) {
res += (*this)[i][i];
}
return res;
}
// Norm
float norm(void) { return sqrtf((this->trans() * *this)[0][0]); }
public:
// arm matrix instance
arm_matrix_instance_f32 arm_mat_;
protected:
// size
int rows_, cols_;
// data
float data_[_rows * _cols];
};
// Matrix funtions
namespace matrixf {
// Special Matrices
// Zero matrix
template <int _rows, int _cols>
Matrixf<_rows, _cols> zeros(void) {
float data[_rows * _cols] = {0};
return Matrixf<_rows, _cols>(data);
}
// Ones matrix
template <int _rows, int _cols>
Matrixf<_rows, _cols> ones(void) {
float data[_rows * _cols] = {0};
for (int i = 0; i < _rows * _cols; i++) {
data[i] = 1;
}
return Matrixf<_rows, _cols>(data);
}
// Identity matrix
template <int _rows, int _cols>
Matrixf<_rows, _cols> eye(void) {
float data[_rows * _cols] = {0};
for (int i = 0; i < fmin(_rows, _cols); i++) {
data[i * _cols + i] = 1;
}
return Matrixf<_rows, _cols>(data);
}
// Diagonal matrix
template <int _rows, int _cols>
Matrixf<_rows, _cols> diag(Matrixf<_rows, 1> vec) {
Matrixf<_rows, _cols> res = matrixf::zeros<_rows, _cols>();
for (int i = 0; i < fmin(_rows, _cols); i++) {
res[i][i] = vec[i][0];
}
return res;
}
// Inverse
template <int _dim>
Matrixf<_dim, _dim> inv(Matrixf<_dim, _dim> mat) {
arm_status s;
// extended matrix [A|I]
Matrixf<_dim, 2 * _dim> ext_mat = matrixf::zeros<_dim, 2 * _dim>();
for (int i = 0; i < _dim; i++) {
memcpy(ext_mat[i], mat[i], _dim * sizeof(float));
ext_mat[i][_dim + i] = 1;
}
// elimination
for (int i = 0; i < _dim; i++) {
// find maximum absolute value in the first column in lower right block
float abs_max = fabs(ext_mat[i][i]);
int abs_max_row = i;
for (int row = i; row < _dim; row++) {
if (abs_max < fabs(ext_mat[row][i])) {
abs_max = fabs(ext_mat[row][i]);
abs_max_row = row;
}
}
if (abs_max < 1e-12f) { // singular
return matrixf::zeros<_dim, _dim>();
s = ARM_MATH_SINGULAR;
}
if (abs_max_row != i) { // row exchange
float tmp;
Matrixf<1, 2 * _dim> row_i = ext_mat.row(i);
Matrixf<1, 2 * _dim> row_abs_max = ext_mat.row(abs_max_row);
memcpy(ext_mat[i], row_abs_max[0], 2 * _dim * sizeof(float));
memcpy(ext_mat[abs_max_row], row_i[0], 2 * _dim * sizeof(float));
}
float k = 1.f / ext_mat[i][i];
for (int col = i; col < 2 * _dim; col++) {
ext_mat[i][col] *= k;
}
for (int row = 0; row < _dim; row++) {
if (row == i) {
continue;
}
k = ext_mat[row][i];
for (int j = i; j < 2 * _dim; j++) {
ext_mat[row][j] -= k * ext_mat[i][j];
}
}
}
// inv = ext_mat(:,n+1:2n)
s = ARM_MATH_SUCCESS;
Matrixf<_dim, _dim> res;
for (int i = 0; i < _dim; i++) {
memcpy(res[i], &ext_mat[i][_dim], _dim * sizeof(float));
}
return res;
}
} // namespace matrixf
namespace vector3f {
// hat of vector
Matrixf<3, 3> hat(Matrixf<3, 1> vec);
// cross product
Matrixf<3, 1> cross(Matrixf<3, 1> vec1, Matrixf<3, 1> vec2);
} // namespace vector3f
#endif // MATRIX_H

342
User/component/toolbox/robotics.cpp Normal file
View File

@ -0,0 +1,342 @@
/**
******************************************************************************
* @file robotics.cpp/h
* @brief Robotic toolbox on STM32. STM32机器人学库
* @author Spoon Guan
* @ref [1] SJTU ME385-2, Robotics, Y.Ding
* [2] Bruno Siciliano, et al., Robotics: Modelling, Planning and
* Control, Springer, 2010.
* [3] R.Murry, Z.X.Li, and S.Sastry, A Mathematical Introduction
* to Robotic Manipulation, CRC Press, 1994.
******************************************************************************
* Copyright (c) 2023 Team JiaoLong-SJTU
* All rights reserved.
******************************************************************************
*/
#include "robotics.h"
Matrixf<3, 1> robotics::r2rpy(Matrixf<3, 3> R) {
float rpy[3] = {
atan2f(R[1][0], R[0][0]), // yaw
atan2f(-R[2][0], sqrtf(R[2][1] * R[2][1] + R[2][2] * R[2][2])), // pitch
atan2f(R[2][1], R[2][2]) // roll
};
return Matrixf<3, 1>(rpy);
}
Matrixf<3, 3> robotics::rpy2r(Matrixf<3, 1> rpy) {
float c[3] = {cosf(rpy[0][0]), cosf(rpy[1][0]), cosf(rpy[2][0])};
float s[3] = {sinf(rpy[0][0]), sinf(rpy[1][0]), sinf(rpy[2][0])};
float R[9] = {
c[0] * c[1], // R11
c[0] * s[1] * s[2] - s[0] * c[2], // R12
c[0] * s[1] * c[2] + s[0] * s[2], // R13
s[0] * c[1], // R21
s[0] * s[1] * s[2] + c[0] * c[2], // R22
s[0] * s[1] * c[2] - c[0] * s[2], // R23
-s[1], // R31
c[1] * s[2], // R32
c[1] * c[2] // R33
};
return Matrixf<3, 3>(R);
}
Matrixf<4, 1> robotics::r2angvec(Matrixf<3, 3> R) {
float theta = acosf(math::limit(0.5f * (R.trace() - 1), -1, 1));
if (theta == 0 || theta == PI) {
float angvec[4] = {
(1 + R[0][0] - R[1][1] - R[2][2]) / 4.0f, // rx=(1+R11-R22-R33)/4
(1 - R[0][0] + R[1][1] - R[2][2]) / 4.0f, // ry=(1-R11+R22-R33)/4
(1 - R[0][0] - R[1][1] + R[2][2]) / 4.0f, // rz=(1-R11-R22+R33)/4
theta // theta
};
return Matrixf<4, 1>(angvec);
}
float angvec[4] = {
(R[2][1] - R[1][2]) / (2.0f * sinf(theta)), // rx=(R32-R23)/2sinθ
(R[0][2] - R[2][0]) / (2.0f * sinf(theta)), // ry=(R13-R31)/2sinθ
(R[1][0] - R[0][1]) / (2.0f * sinf(theta)), // rz=(R21-R12)/2sinθ
theta // theta
};
return Matrixf<4, 1>(angvec);
}
Matrixf<3, 3> robotics::angvec2r(Matrixf<4, 1> angvec) {
float theta = angvec[3][0];
Matrixf<3, 1> r = angvec.block<3, 1>(0, 0);
Matrixf<3, 3> R;
// Rodrigues formula: R=I+ω^sinθ+ω^^2(1-cosθ)
R = matrixf::eye<3, 3>() + vector3f::hat(r) * sinf(theta) +
vector3f::hat(r) * vector3f::hat(r) * (1 - cosf(theta));
return R;
}
Matrixf<4, 1> robotics::r2quat(Matrixf<3, 3> R) {
float q[4] = {
0.5f * sqrtf(math::limit(R.trace(), -1, 1) + 1), // q0=cos(θ/2)
math::sign(R[2][1] - R[1][2]) * 0.5f *
sqrtf(math::limit(R[0][0] - R[1][1] - R[2][2], -1, 1) +
1), // q1=rx*sin(θ/2)
math::sign(R[0][2] - R[2][0]) * 0.5f *
sqrtf(math::limit(-R[0][0] + R[1][1] - R[2][2], -1, 1) +
1), // q2=ry*sin(θ/2)
math::sign(R[1][0] - R[0][1]) * 0.5f *
sqrtf(math::limit(-R[0][0] - R[1][1] + R[2][2], -1, 1) +
1), // q3=rz*sin(θ/2)
};
return (Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm());
}
Matrixf<3, 3> robotics::quat2r(Matrixf<4, 1> q) {
float R[9] = {
1 - 2.0f * (q[2][0] * q[2][0] + q[3][0] * q[3][0]), // R11
2.0f * (q[1][0] * q[2][0] - q[0][0] * q[3][0]), // R12
2.0f * (q[1][0] * q[3][0] + q[0][0] * q[2][0]), // R13
2.0f * (q[1][0] * q[2][0] + q[0][0] * q[3][0]), // R21
1 - 2.0f * (q[1][0] * q[1][0] + q[3][0] * q[3][0]), // R22
2.0f * (q[2][0] * q[3][0] - q[0][0] * q[1][0]), // R23
2.0f * (q[1][0] * q[3][0] - q[0][0] * q[2][0]), // R31
2.0f * (q[2][0] * q[3][0] + q[0][0] * q[1][0]), // R32
1 - 2.0f * (q[1][0] * q[1][0] + q[2][0] * q[2][0]) // R33
};
return Matrixf<3, 3>(R);
}
Matrixf<3, 1> robotics::quat2rpy(Matrixf<4, 1> q) {
float rpy[3] = {
atan2f(2.0f * (q[1][0] * q[2][0] + q[0][0] * q[3][0]),
1 - 2.0f * (q[2][0] * q[2][0] + q[3][0] * q[3][0])), // yaw
asinf(2.0f * (q[0][0] * q[2][0] - q[1][0] * q[3][0])), // pitch
atan2f(2.0f * (q[2][0] * q[3][0] + q[0][0] * q[1][0]),
1 - 2.0f * (q[1][0] * q[1][0] + q[2][0] * q[2][0])) // rol
};
return Matrixf<3, 1>(rpy);
}
Matrixf<4, 1> robotics::rpy2quat(Matrixf<3, 1> rpy) {
float c[3] = {cosf(0.5f * rpy[0][0]), cosf(0.5f * rpy[1][0]),
cosf(0.5f * rpy[2][0])}; // cos(*/2)
float s[3] = {sinf(0.5f * rpy[0][0]), sinf(0.5f * rpy[1][0]),
sinf(0.5f * rpy[2][0])}; // sin(*/2)
float q[4] = {
c[0] * c[1] * c[2] + s[0] * s[1] * s[2], // q0=cos(θ/2)
c[0] * c[1] * s[2] - s[0] * s[1] * c[2], // q1=rx*sin(θ/2)
c[0] * s[1] * c[2] + s[0] * c[1] * s[2], // q2=ry*sin(θ/2)
s[0] * c[1] * c[2] - c[0] * s[1] * s[2] // q3=rz*sin(θ/2)
};
return (Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm());
}
Matrixf<4, 1> robotics::quat2angvec(Matrixf<4, 1> q) {
float cosq0;
float theta = 2.0f * acosf(math::limit(q[0][0], -1, 1));
if (theta == 0 || theta == PI) {
float angvec[4] = {0, 0, 0, theta}; // θ=0||PI, return[0;θ]
return Matrixf<4, 1>(angvec);
}
Matrixf<3, 1> vec = q.block<3, 1>(1, 0);
float angvec[4] = {
vec[0][0] / vec.norm(), // rx
vec[1][0] / vec.norm(), // ry
vec[2][0] / vec.norm(), // rz
theta // theta
};
return Matrixf<4, 1>(angvec);
}
Matrixf<4, 1> robotics::angvec2quat(Matrixf<4, 1> angvec) {
float c = cosf(0.5f * angvec[3][0]), s = sinf(0.5f * angvec[3][0]);
float q[4] = {
c, // q0=cos(θ/2)
s * angvec[0][0], // q1=rx*sin(θ/2)
s * angvec[1][0], // q2=ry*sin(θ/2)
s * angvec[2][0] // q3=rz*sin(θ/2)
};
return Matrixf<4, 1>(q) / Matrixf<4, 1>(q).norm();
}
Matrixf<3, 3> robotics::t2r(Matrixf<4, 4> T) {
return T.block<3, 3>(0, 0); // R=T(1:3,1:3)
}
Matrixf<4, 4> robotics::r2t(Matrixf<3, 3> R) {
// T=[R,0;0,1]
float T[16] = {R[0][0], R[0][1], R[0][2], 0, R[1][0], R[1][1], R[1][2], 0,
R[2][0], R[2][1], R[2][2], 0, 0, 0, 0, 1};
return Matrixf<4, 4>(T);
}
Matrixf<3, 1> robotics::t2p(Matrixf<4, 4> T) {
return T.block<3, 1>(0, 3); // p=T(1:3,4)
}
Matrixf<4, 4> robotics::p2t(Matrixf<3, 1> p) {
// T=[I,P;0,1]
float T[16] = {1, 0, 0, p[0][0], 0, 1, 0, p[1][0],
0, 0, 1, p[2][0], 0, 0, 0, 1};
return Matrixf<4, 4>(T);
}
Matrixf<4, 4> robotics::rp2t(Matrixf<3, 3> R, Matrixf<3, 1> p) {
// T=[R,P;0,1]
float T[16] = {R[0][0], R[0][1], R[0][2], p[0][0], R[1][0], R[1][1],
R[1][2], p[1][0], R[2][0], R[2][1], R[2][2], p[2][0],
0, 0, 0, 1};
return Matrixf<4, 4>(T);
}
Matrixf<4, 4> robotics::invT(Matrixf<4, 4> T) {
Matrixf<3, 3> RT = t2r(T).trans();
Matrixf<3, 1> p_ = -1.0f * RT * t2p(T);
float invT[16] = {RT[0][0], RT[0][1], RT[0][2], p_[0][0], RT[1][0], RT[1][1],
RT[1][2], p_[1][0], RT[2][0], RT[2][1], RT[2][2], p_[2][0],
0, 0, 0, 1};
return Matrixf<4, 4>(invT);
}
Matrixf<3, 1> robotics::t2rpy(Matrixf<4, 4> T) {
return r2rpy(t2r(T));
}
Matrixf<4, 4> robotics::rpy2t(Matrixf<3, 1> rpy) {
return r2t(rpy2r(rpy));
}
Matrixf<4, 1> robotics::t2angvec(Matrixf<4, 4> T) {
return r2angvec(t2r(T));
}
Matrixf<4, 4> robotics::angvec2t(Matrixf<4, 1> angvec) {
return r2t(angvec2r(angvec));
}
Matrixf<4, 1> robotics::t2quat(Matrixf<4, 4> T) {
return r2quat(t2r(T));
}
Matrixf<4, 4> robotics::quat2t(Matrixf<4, 1> quat) {
return r2t(quat2r(quat));
}
Matrixf<6, 1> robotics::t2twist(Matrixf<4, 4> T) {
Matrixf<3, 1> p = t2p(T);
Matrixf<4, 1> angvec = t2angvec(T);
Matrixf<3, 1> phi = angvec.block<3, 1>(0, 0) * angvec[3][0];
float twist[6] = {p[0][0], p[1][0], p[2][0], phi[0][0], phi[1][0], phi[2][0]};
return Matrixf<6, 1>(twist);
}
Matrixf<4, 4> robotics::twist2t(Matrixf<6, 1> twist) {
Matrixf<3, 1> p = twist.block<3, 1>(0, 0);
float theta = twist.block<3, 1>(3, 0).norm();
float angvec[4] = {0, 0, 0, theta};
if (theta != 0) {
angvec[0] = twist[3][0] / theta;
angvec[1] = twist[4][0] / theta;
angvec[2] = twist[5][0] / theta;
}
return rp2t(angvec2r(angvec), p);
}
Matrixf<4, 4> robotics::DH_t::fkine() {
float ct = cosf(theta), st = sinf(theta); // cosθ, sinθ
float ca = cosf(alpha), sa = sinf(alpha); // cosα, sinα
// T =
// | cθ -sθcα sθsα acθ |
// | sθ cθcα -cθsα asθ |
// | 0 sα cα d |
// | 0 0 0 1 |
T[0][0] = ct;
T[0][1] = -st * ca;
T[0][2] = st * sa;
T[0][3] = a * ct;
T[1][0] = st;
T[1][1] = ct * ca;
T[1][2] = -ct * sa;
T[1][3] = a * st;
T[2][0] = 0;
T[2][1] = sa;
T[2][2] = ca;
T[2][3] = d;
T[3][0] = 0;
T[3][1] = 0;
T[3][2] = 0;
T[3][3] = 1;
return T;
}
robotics::Link::Link(float theta,
float d,
float a,
float alpha,
robotics::Joint_Type_e type,
float offset,
float qmin,
float qmax,
float m,
Matrixf<3, 1> rc,
Matrixf<3, 3> I) {
dh_.theta = theta;
dh_.d = d;
dh_.alpha = alpha;
dh_.a = a;
type_ = type;
offset_ = offset;
qmin_ = qmin;
qmax_ = qmax;
m_ = m;
rc_ = rc;
I_ = I;
}
robotics::Link::Link(const Link& link) {
dh_.theta = link.dh_.theta;
dh_.d = link.dh_.d;
dh_.alpha = link.dh_.alpha;
dh_.a = link.dh_.a;
type_ = link.type_;
offset_ = link.offset_;
qmin_ = link.qmin_;
qmax_ = link.qmax_;
m_ = link.m_;
rc_ = link.rc_;
I_ = link.I_;
}
robotics::Link& robotics::Link::operator=(Link link) {
dh_.theta = link.dh_.theta;
dh_.d = link.dh_.d;
dh_.alpha = link.dh_.alpha;
dh_.a = link.dh_.a;
type_ = link.type_;
offset_ = link.offset_;
qmin_ = link.qmin_;
qmax_ = link.qmax_;
m_ = link.m_;
rc_ = link.rc_;
I_ = link.I_;
return *this;
}
Matrixf<4, 4> robotics::Link::T(float q) {
// 先对关节变量q进行限幅
float q_limited = q;
if (qmin_ < qmax_) { // 有效的限幅范围
q_limited = math::limit(q, qmin_, qmax_);
}
// 再计算实际的DH参数
if (type_ == R) {
dh_.theta = q_limited + offset_;
} else {
dh_.d = q_limited + offset_;
}
return dh_.fkine();
}

407
User/component/toolbox/robotics.h Normal file
View File

@ -0,0 +1,407 @@
/**
******************************************************************************
* @file robotics.cpp/h
* @brief Robotic toolbox on STM32. STM32机器人学库
* @author Spoon Guan
* @ref [1] SJTU ME385-2, Robotics, Y.Ding
* [2] Bruno Siciliano, et al., Robotics: Modelling, Planning and
* Control, Springer, 2010.
* [3] R.Murry, Z.X.Li, and S.Sastry, A Mathematical Introduction
* to Robotic Manipulation, CRC Press, 1994.
******************************************************************************
* Copyright (c) 2023 Team JiaoLong-SJTU
* All rights reserved.
******************************************************************************
*/
#ifndef ROBOTICS_H
#define ROBOTICS_H
#include "utils.h"
#include "matrix.h"
namespace robotics {
// rotation matrix(R) -> RPY([yaw;pitch;roll])
Matrixf<3, 1> r2rpy(Matrixf<3, 3> R);
// RPY([yaw;pitch;roll]) -> rotation matrix(R)
Matrixf<3, 3> rpy2r(Matrixf<3, 1> rpy);
// rotation matrix(R) -> angle vector([r;θ])
Matrixf<4, 1> r2angvec(Matrixf<3, 3> R);
// angle vector([r;θ]) -> rotation matrix(R)
Matrixf<3, 3> angvec2r(Matrixf<4, 1> angvec);
// rotation matrix(R) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)]
Matrixf<4, 1> r2quat(Matrixf<3, 3> R);
// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> rotation matrix(R)
Matrixf<3, 3> quat2r(Matrixf<4, 1> quat);
// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> RPY([yaw;pitch;roll])
Matrixf<3, 1> quat2rpy(Matrixf<4, 1> q);
// RPY([yaw;pitch;roll]) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)]
Matrixf<4, 1> rpy2quat(Matrixf<3, 1> rpy);
// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> angle vector([r;θ])
Matrixf<4, 1> quat2angvec(Matrixf<4, 1> q);
// angle vector([r;θ]) -> quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)]
Matrixf<4, 1> angvec2quat(Matrixf<4, 1> angvec);
// homogeneous transformation matrix(T) -> rotation matrix(R)
Matrixf<3, 3> t2r(Matrixf<4, 4> T);
// rotation matrix(R) -> homogeneous transformation matrix(T)
Matrixf<4, 4> r2t(Matrixf<3, 3> R);
// homogeneous transformation matrix(T) -> translation vector(p)
Matrixf<3, 1> t2p(Matrixf<4, 4> T);
// translation vector(p) -> homogeneous transformation matrix(T)
Matrixf<4, 4> p2t(Matrixf<3, 1> p);
// rotation matrix(R) & translation vector(p) -> homogeneous transformation
// matrix(T)
Matrixf<4, 4> rp2t(Matrixf<3, 3> R, Matrixf<3, 1> p);
// homogeneous transformation matrix(T) -> RPY([yaw;pitch;roll])
Matrixf<3, 1> t2rpy(Matrixf<4, 4> T);
// inverse of homogeneous transformation matrix(T^-1=[R',-R'P;0,1])
Matrixf<4, 4> invT(Matrixf<4, 4> T);
// RPY([yaw;pitch;roll]) -> homogeneous transformation matrix(T)
Matrixf<4, 4> rpy2t(Matrixf<3, 1> rpy);
// homogeneous transformation matrix(T) -> angle vector([r;θ])
Matrixf<4, 1> t2angvec(Matrixf<4, 4> T);
// angle vector([r;θ]) -> homogeneous transformation matrix(T)
Matrixf<4, 4> angvec2t(Matrixf<4, 1> angvec);
// homogeneous transformation matrix(T) -> quaternion,
// [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)]
Matrixf<4, 1> t2quat(Matrixf<4, 4> T);
// quaternion, [q0;q1;q2;q3]=[cos(θ/2);rsin(θ/2)] -> homogeneous transformation
// matrix(T)
Matrixf<4, 4> quat2t(Matrixf<4, 1> quat);
// homogeneous transformation matrix(T) -> twist coordinates vector ([p;rθ])
Matrixf<6, 1> t2twist(Matrixf<4, 4> T);
// twist coordinates vector ([p;rθ]) -> homogeneous transformation matrix(T)
Matrixf<4, 4> twist2t(Matrixf<6, 1> twist);
// joint type: R-revolute joint, P-prismatic joint
typedef enum joint_type {
R = 0,
P = 1,
} Joint_Type_e;
// Denavit–Hartenberg(DH) method
struct DH_t {
// forward kinematic
Matrixf<4, 4> fkine();
// DH parameter
float theta;
float d;
float a;
float alpha;
Matrixf<4, 4> T;
};
class Link {
public:
Link(){};
Link(float theta, float d, float a, float alpha, Joint_Type_e type = R,
float offset = 0, float qmin = 0, float qmax = 0, float m = 1,
Matrixf<3, 1> rc = matrixf::zeros<3, 1>(),
Matrixf<3, 3> I = matrixf::zeros<3, 3>());
Link(const Link& link);
Link& operator=(Link link);
float qmin() { return qmin_; }
float qmax() { return qmax_; }
Joint_Type_e type() { return type_; }
float m() { return m_; }
Matrixf<3, 1> rc() { return rc_; }
Matrixf<3, 3> I() { return I_; }
Matrixf<4, 4> T(float q); // forward kinematic
public:
// kinematic parameter
DH_t dh_;
float offset_;
// limit(qmin,qmax), no limit if qmin<=qmax
float qmin_;
float qmax_;
// joint type
Joint_Type_e type_;
// dynamic parameter
float m_; // mass
Matrixf<3, 1> rc_; // centroid(link coordinate)
Matrixf<3, 3> I_; // inertia tensor(3*3)
};
template <uint16_t _n = 1>
class Serial_Link {
public:
Serial_Link(Link links[_n]) {
for (int i = 0; i < _n; i++)
links_[i] = links[i];
gravity_ = matrixf::zeros<3, 1>();
gravity_[2][0] = -9.81f;
}
Serial_Link(Link links[_n], Matrixf<3, 1> gravity) {
for (int i = 0; i < _n; i++)
links_[i] = links[i];
gravity_ = gravity;
}
// forward kinematic: T_n^0
// param[in] q: joint variable vector
// param[out] T_n^0
Matrixf<4, 4> fkine(Matrixf<_n, 1> q) {
T_ = matrixf::eye<4, 4>();
for (int iminus1 = 0; iminus1 < _n; iminus1++)
T_ = T_ * links_[iminus1].T(q[iminus1][0]);
return T_;
}
// forward kinematic: T_k^0
// param[in] q: joint variable vector
// param[in] k: joint number
// param[out] T_k^0
Matrixf<4, 4> fkine(Matrixf<_n, 1> q, uint16_t k) {
if (k > _n)
k = _n;
Matrixf<4, 4> T = matrixf::eye<4, 4>();
for (int iminus1 = 0; iminus1 < k; iminus1++)
T = T * links_[iminus1].T(q[iminus1][0]);
return T;
}
// T_k^k-1: homogeneous transformation matrix of link k
// param[in] q: joint variable vector
// param[in] kminus: joint number k, input k-1
// param[out] T_k^k-1
Matrixf<4, 4> T(Matrixf<_n, 1> q, uint16_t kminus1) {
if (kminus1 >= _n)
kminus1 = _n - 1;
return links_[kminus1].T(q[kminus1][0]);
}
// jacobian matrix, J_i = [J_pi;j_oi]
// param[in] q: joint variable vector
// param[out] jacobian matix J_6*n
Matrixf<6, _n> jacob(Matrixf<_n, 1> q) {
Matrixf<3, 1> p_e = t2p(fkine(q)); // p_e
Matrixf<4, 4> T_iminus1 = matrixf::eye<4, 4>(); // T_i-1^0
Matrixf<3, 1> z_iminus1; // z_i-1^0
Matrixf<3, 1> p_iminus1; // p_i-1^0
Matrixf<3, 1> J_pi;
Matrixf<3, 1> J_oi;
for (int iminus1 = 0; iminus1 < _n; iminus1++) {
// revolute joint: J_pi = z_i-1x(p_e-p_i-1), J_oi = z_i-1
if (links_[iminus1].type() == R) {
z_iminus1 = T_iminus1.block<3, 1>(0, 2);
p_iminus1 = t2p(T_iminus1);
T_iminus1 = T_iminus1 * links_[iminus1].T(q[iminus1][0]);
J_pi = vector3f::cross(z_iminus1, p_e - p_iminus1);
J_oi = z_iminus1;
}
// prismatic joint: J_pi = z_i-1, J_oi = 0
else {
z_iminus1 = T_iminus1.block<3, 1>(0, 2);
T_iminus1 = T_iminus1 * links_[iminus1].T(q[iminus1][0]);
J_pi = z_iminus1;
J_oi = matrixf::zeros<3, 1>();
}
J_[0][iminus1] = J_pi[0][0];
J_[1][iminus1] = J_pi[1][0];
J_[2][iminus1] = J_pi[2][0];
J_[3][iminus1] = J_oi[0][0];
J_[4][iminus1] = J_oi[1][0];
J_[5][iminus1] = J_oi[2][0];
}
return J_;
}
// inverse kinematic, numerical solution(Newton method)
// param[in] T: homogeneous transformation matrix of end effector
// param[in] q: initial joint variable vector(q0) for Newton method's
// iteration
// param[in] tol: tolerance of error (norm(error of twist vector))
// param[in] max_iter: maximum iterations, default 30
// param[out] q: joint variable vector
Matrixf<_n, 1> ikine(Matrixf<4, 4> Td,
Matrixf<_n, 1> q = matrixf::zeros<_n, 1>(),
float tol = 1e-4f, uint16_t max_iter = 50) {
Matrixf<4, 4> T;
Matrixf<3, 1> pe, we;
Matrixf<6, 1> err, new_err;
Matrixf<_n, 1> dq;
float step = 1;
for (int i = 0; i < max_iter; i++) {
T = fkine(q);
pe = t2p(Td) - t2p(T);
// angvec(Td*T^-1), transform angular vector(T->Td) in world coordinate
we = t2twist(Td * invT(T)).block<3, 1>(3, 0);
for (int i = 0; i < 3; i++) {
err[i][0] = pe[i][0];
err[i + 3][0] = we[i][0];
}
if (err.norm() < tol)
return q;
// adjust iteration step
Matrixf<6, _n> J = jacob(q);
for (int j = 0; j < 5; j++) {
dq = matrixf::inv(J.trans() * J) * (J.trans() * err) * step;
if (dq[0][0] == INFINITY) // J'*J singular
{
dq = matrixf::inv(J.trans() * J + 0.1f * matrixf::eye<_n, _n>()) *
J.trans() * err * step;
// SVD<6, _n> JTJ_svd(J.trans() * J);
// dq = JTJ_svd.solve(err) * step * 5e-2f;
q += dq;
for (int i = 0; i < _n; i++) {
if (links_[i].type() == R)
q[i][0] = math::loopLimit(q[i][0], -PI, PI);
}
break;
}
T = fkine(q + dq);
pe = t2p(Td) - t2p(T);
we = t2twist(Td * invT(T)).block<3, 1>(3, 0);
for (int i = 0; i < 3; i++) {
new_err[i][0] = pe[i][0];
new_err[i + 3][0] = we[i][0];
}
if (new_err.norm() < err.norm()) {
q += dq;
for (int i = 0; i < _n; i++) {
if (links_[i].type() == robotics::Joint_Type_e::R) {
q[i][0] = math::loopLimit(q[i][0], -PI, PI);
}
}
break;
} else {
step /= 2.0f;
}
}
if (step < 1e-3f)
return q;
}
return q;
}
// (Reserved function) inverse kinematic, analytic solution(geometric method)
Matrixf<_n, 1> (*ikine_analytic)(Matrixf<4, 4> T);
// inverse dynamic, Newton-Euler method
// param[in] q: joint variable vector
// param[in] qv: dq/dt
// param[in] qa: d^2q/dt^2
// param[in] he: load on end effector [f;μ], default 0
Matrixf<_n, 1> rne(Matrixf<_n, 1> q,
Matrixf<_n, 1> qv = matrixf::zeros<_n, 1>(),
Matrixf<_n, 1> qa = matrixf::zeros<_n, 1>(),
Matrixf<6, 1> he = matrixf::zeros<6, 1>()) {
// forward propagation
// record each links' motion state in matrices
// [ωi] angular velocity
Matrixf<3, _n + 1> w = matrixf::zeros<3, _n + 1>();
// [βi] angular acceleration
Matrixf<3, _n + 1> b = matrixf::zeros<3, _n + 1>();
// [pi] position of joint
Matrixf<3, _n + 1> p = matrixf::zeros<3, _n + 1>();
// [vi] velocity of joint
Matrixf<3, _n + 1> v = matrixf::zeros<3, _n + 1>();
// [ai] acceleration of joint
Matrixf<3, _n + 1> a = matrixf::zeros<3, _n + 1>();
// [aci] acceleration of mass center
Matrixf<3, _n + 1> ac = matrixf::zeros<3, _n + 1>();
// temperary vectors
Matrixf<3, 1> w_i, b_i, p_i, v_i, ai, ac_i;
// i & i-1 coordinate convert to 0 coordinate
Matrixf<4, 4> T_0i = matrixf::eye<4, 4>();
Matrixf<4, 4> T_0iminus1 = matrixf::eye<4, 4>();
Matrixf<3, 3> R_0i = matrixf::eye<3, 3>();
Matrixf<3, 3> R_0iminus1 = matrixf::eye<3, 3>();
// unit vector of z-axis
Matrixf<3, 1> ez = matrixf::zeros<3, 1>();
ez[2][0] = 1;
for (int i = 1; i <= _n; i++) {
T_0i = T_0i * T(q, i - 1); // T_i^0
R_0i = t2r(T_0i); // R_i^0
R_0iminus1 = t2r(T_0iminus1); // R_i-1^0
// ω_i = ω_i-1+qv_i*R_i-1^0*ez
w_i = w.col(i - 1) + qv[i - 1][0] * R_0iminus1 * ez;
// β_i = β_i-1+ω_i-1x(qv_i*R_i-1^0*ez)+qa_i*R_i-1^0*ez
b_i = b.col(i - 1) +
vector3f::cross(w.col(i - 1), qv[i - 1][0] * R_0iminus1 * ez) +
qa[i - 1][0] * R_0iminus1 * ez;
p_i = t2p(T_0i); // p_i = T_i^0(1:3,4)
// v_i = v_i-1+ω_ix(p_i-p_i-1)
v_i = v.col(i - 1) + vector3f::cross(w_i, p_i - p.col(i - 1));
// a_i = a_i-1+β_ix(p_i-p_i-1)+ω_ix(ω_ix(p_i-p_i-1))
ai = a.col(i - 1) + vector3f::cross(b_i, p_i - p.col(i - 1)) +
vector3f::cross(w_i, vector3f::cross(w_i, p_i - p.col(i - 1)));
// ac_i = a_i+β_ix(R_0^i*rc_i^i)+ω_ix(ω_ix(R_0^i*rc_i^i))
ac_i =
ai + vector3f::cross(b_i, R_0i * links_[i - 1].rc()) +
vector3f::cross(w_i, vector3f::cross(w_i, R_0i * links_[i - 1].rc()));
for (int row = 0; row < 3; row++) {
w[row][i] = w_i[row][0];
b[row][i] = b_i[row][0];
p[row][i] = p_i[row][0];
v[row][i] = v_i[row][0];
a[row][i] = ai[row][0];
ac[row][i] = ac_i[row][0];
}
T_0iminus1 = T_0i; // T_i-1^0
}
// backward propagation
// record each links' force
Matrixf<3, _n + 1> f = matrixf::zeros<3, _n + 1>(); // joint force
Matrixf<3, _n + 1> mu = matrixf::zeros<3, _n + 1>(); // joint moment
// temperary vector
Matrixf<3, 1> f_iminus1, mu_iminus1;
// {T,R',P}_i^i-1
Matrixf<4, 4> T_iminus1i;
Matrixf<3, 3> RT_iminus1i;
Matrixf<3, 1> P_iminus1i;
// I_i-1(in 0 coordinate)
Matrixf<3, 3> I_i;
// joint torque
Matrixf<_n, 1> torq;
// load on end effector
for (int row = 0; row < 3; row++) {
f[row][_n] = he.block<3, 1>(0, 0)[row][0];
mu[row][_n] = he.block<3, 1>(3, 0)[row][0];
}
for (int i = _n; i > 0; i--) {
T_iminus1i = T(q, i - 1); // T_i^i-1
P_iminus1i = t2p(T_iminus1i); // P_i^i-1
RT_iminus1i = t2r(T_iminus1i).trans(); // R_i^i-1'
R_0iminus1 = R_0i * RT_iminus1i; // R_i-1^0
// I_i^0 = R_i^0*I_i^i*(R_i^0)'
I_i = R_0i * links_[i - 1].I() * R_0i.trans();
// f_i-1 = f_i+m_i*ac_i-m_i*g
f_iminus1 = f.col(i) + links_[i - 1].m() * ac.col(i) -
links_[i - 1].m() * gravity_;
// μ_i-1 = μ_i+f_ixrc_i-f_i-1xrc_i-1->ci+I_i*b_i+ω_ix(I_i*ω_i)
mu_iminus1 = mu.col(i) +
vector3f::cross(f.col(i), R_0i * links_[i - 1].rc()) -
vector3f::cross(f_iminus1, R_0i * (RT_iminus1i * P_iminus1i +
links_[i - 1].rc())) +
I_i * b.col(i) + vector3f::cross(w.col(i), I_i * w.col(i));
// τ_i = μ_i-1'*(R_i-1^0*ez)
torq[i - 1][0] = (mu_iminus1.trans() * R_0iminus1 * ez)[0][0];
for (int row = 0; row < 3; row++) {
f[row][i - 1] = f_iminus1[row][0];
mu[row][i - 1] = mu_iminus1[row][0];
}
R_0i = R_0iminus1;
}
return torq;
}
private:
Link links_[_n];
Matrixf<3, 1> gravity_;
Matrixf<4, 4> T_;
Matrixf<6, _n> J_;
};
}; // namespace robotics
#endif // ROBOTICS_H

56
User/component/toolbox/utils.cpp Normal file
View File

@ -0,0 +1,56 @@
/**
******************************************************************************
* @file utils.cpp/h
* @brief General math utils. 常用数学工具函数
* @author Spoon Guan
******************************************************************************
* Copyright (c) 2023 Team JiaoLong-SJTU
* All rights reserved.
******************************************************************************
*/
#include "utils.h"
float math::limit(float val, const float& min, const float& max) {
if (min > max)
return val;
else if (val < min)
val = min;
else if (val > max)
val = max;
return val;
}
float math::limitMin(float val, const float& min) {
if (val < min)
val = min;
return val;
}
float math::limitMax(float val, const float& max) {
if (val > max)
val = max;
return val;
}
float math::loopLimit(float val, const float& min, const float& max) {
if (min >= max)
return val;
if (val > max) {
while (val > max)
val -= (max - min);
} else if (val < min) {
while (val < min)
val += (max - min);
}
return val;
}
float math::sign(const float& val) {
if (val > 0)
return 1;
else if (val < 0)
return -1;
return 0;
}

27
User/component/toolbox/utils.h Normal file
View File

@ -0,0 +1,27 @@
/**
******************************************************************************
* @file utils.cpp/h
* @brief General math utils. 常用数学工具函数
* @author Spoon Guan
******************************************************************************
* Copyright (c) 2023 Team JiaoLong-SJTU
* All rights reserved.
******************************************************************************
*/
#ifndef UTILS_H
#define UTILS_H
#include <stdint.h>
namespace math {
float limit(float val, const float& min, const float& max);
float limitMin(float val, const float& min);
float limitMax(float val, const float& max);
float loopLimit(float val, const float& min, const float& max);
float sign(const float& val);
} // namespace math
#endif // UTILS_H

134
User/component/user_math.c Normal file
View File

@ -0,0 +1,134 @@
/*
自定义的数学运算。
*/
#include "user_math.h"
#include <string.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
inline float InvSqrt(float x) {
//#if 0
/* Fast inverse square-root */
/* See: http://en.wikipedia.org/wiki/Fast_inverse_square_root */
float halfx = 0.5f * x;
float y = x;
long i = *(long*)&y;
i = 0x5f3759df - (i>>1);
y = *(float*)&i;
y = y * (1.5f - (halfx * y * y));
y = y * (1.5f - (halfx * y * y));
return y;
//#else
// return 1.0f / sqrtf(x);
//#endif
}
inline float AbsClip(float in, float limit) {
return (in < -limit) ? -limit : ((in > limit) ? limit : in);
}
float fAbs(float in){
return (in > 0) ? in : -in;
}
inline void Clip(float *origin, float min, float max) {
if (*origin > max) *origin = max;
if (*origin < min) *origin = min;
}
inline float Sign(float in) { return (in > 0) ? 1.0f : 0.0f; }
/**
* \brief 将运动向量置零
*
* \param mv 被操作的值
*/
inline void ResetMoveVector(MoveVector_t *mv) { memset(mv, 0, sizeof(*mv)); }
/**
* \brief 计算循环值的误差，适用于设定值与反馈值均在（x,y）范围内循环的情况，range应设定为y-x
* 例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b;
* \param sp 设定值
* \param fb 反馈值
* \param range 被操作的值变化范围，正数时起效
* \return 函数运行结果
*/
inline float CircleError(float sp, float fb, float range) {
float error = sp - fb;
if (range > 0.0f) {
float half_range = range / 2.0f;
if (error > half_range)
error -= range;
else if (error < -half_range)
error += range;
}
return error;
}
/**
* \brief 循环加法，适用于被操作的值在（0,range）范围内循环的情况
* \param origin 被操作的值
* \param delta 变化量
* \param range 被操作的值变化范围，正数时起效
*/
inline void CircleAdd(float *origin, float delta, float range) {
float out = *origin + delta;
if (range > 0.0f) {
if (out >= range)
out -= range;
else if (out < 0.0f)
out += range;
}
*origin = out;
}
/**
* @brief 循环值取反
*
* @param origin 被操作的值
*/
inline void CircleReverse(float *origin) { *origin = -(*origin) + M_2PI; }
/**
* @brief 根据目标弹丸速度计算摩擦轮转速
*
* @param bullet_speed 弹丸速度
* @param fric_radius 摩擦轮半径
* @param is17mm 是否为17mm
* @return 摩擦轮转速
*/
inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) {
if (bullet_speed == 0.0f) return 0.f;
if (is17mm) {
if (bullet_speed == 15.0f) return 4670.f;
if (bullet_speed == 18.0f) return 5200.f;
if (bullet_speed == 30.0f) return 7350.f;
} else {
if (bullet_speed == 10.0f) return 4450.f;
if (bullet_speed == 16.0f) return 5800.f;
}
/* 不为裁判系统设定值时,计算转速 */
return 60.0f * (float)bullet_speed / (M_2PI * fric_radius);
}
// /**
// * @brief 断言失败处理
// *
// * @param file 文件名
// * @param line 行号
// */
// void VerifyFailed(const char *file, uint32_t line) {
// UNUSED(file);
// UNUSED(line);
// while (1) {
// __NOP();
// }
// }
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

183
User/component/user_math.h Normal file
View File

@ -0,0 +1,183 @@
/*
自定义的数学运算。
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <float.h>
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stddef.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
#define M_DEG2RAD_MULT (0.01745329251f)
#define M_RAD2DEG_MULT (57.2957795131f)
// 角度与弧度转换宏
#define DEG_TO_RAD (0.01745329251f) // π/180
#define RAD_TO_DEG (57.2957795131f) // 180/π
#ifndef M_PI_2
#define M_PI_2 1.57079632679f
#endif
#ifndef M_PI
#define M_PI 3.14159265358979323846f
#endif
#ifndef M_2PI
#define M_2PI 6.28318530717958647692f
#endif
#ifndef __packed
#define __packed __attribute__((__packed__))
#endif /* __packed */
#define max(a, b) \
({ \
__typeof__(a) _a = (a); \
__typeof__(b) _b = (b); \
_a > _b ? _a : _b; \
})
#define min(a, b) \
({ \
__typeof__(a) _a = (a); \
__typeof__(b) _b = (b); \
_a < _b ? _a : _b; \
})
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* 移动向量 */
typedef struct {
float vx; /* 前后平移 */
float vy; /* 左右平移 */
float wz; /* 转动 */
} MoveVector_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
float InvSqrt(float x);
float AbsClip(float in, float limit);
float fAbs(float in);
void Clip(float *origin, float min, float max);
float Sign(float in);
/**
* \brief 将运动向量置零
*
* \param mv 被操作的值
*/
void ResetMoveVector(MoveVector_t *mv);
/**
* \brief 计算循环值的误差，适用于设定值与反馈值均在（x,y）范围内循环的情况，range应设定为y-x
* 例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b;
* \param sp 设定值
* \param fb 反馈值
* \param range 被操作的值变化范围，正数时起效
* \return 函数运行结果
*/
float CircleError(float sp, float fb, float range);
/**
* \brief 循环加法，适用于被操作的值在（0,range）范围内循环的情况
* \param origin 被操作的值
* \param delta 变化量
* \param range 被操作的值变化范围，正数时起效
*/
void CircleAdd(float *origin, float delta, float range);
/**
* @brief 循环值取反
*
* @param origin 被操作的值
*/
void CircleReverse(float *origin);
/**
* @brief 根据目标弹丸速度计算摩擦轮转速
*
* @param bullet_speed 弹丸速度
* @param fric_radius 摩擦轮半径
* @param is17mm 是否为17mm
* @return 摩擦轮转速
*/
float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm);
#ifdef __cplusplus
}
#endif
#ifdef DEBUG
/**
* @brief 如果表达式的值为假则运行处理函数
*
*/
#define ASSERT(expr) \
do { \
if (!(expr)) { \
VerifyFailed(__FILE__, __LINE__); \
} \
} while (0)
#else
/**
* @brief 未定DEBUG，表达式不会运行，断言被忽略
*
*/
#define ASSERT(expr) ((void)(0))
#endif
#ifdef DEBUG
/**
* @brief 如果表达式的值为假则运行处理函数
*
*/
#define VERIFY(expr) \
do { \
if (!(expr)) { \
VerifyFailed(__FILE__, __LINE__); \
} \
} while (0)
#else
/**
* @brief 表达式会运行，忽略表达式结果
*
*/
#define VERIFY(expr) ((void)(expr))
#endif
// /**
// * @brief 断言失败处理
// *
// * @param file 文件名
// * @param line 行号
// */
// void VerifyFailed(const char *file, uint32_t line);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

121
User/device/at9s.c Normal file
View File

@ -0,0 +1,121 @@
/* Includes ----------------------------------------------------------------- */
#include "at9s.h"
#include <string.h>
#include "bsp/uart.h"
#include "device/device.h"
/* Private define ----------------------------------------------------------- */
#define DEAD_AREA 0.025f
/* Private variables -------------------------------------------------------- */
static osThreadId_t thread_alert;
static bool inited = false;
/* Private function --------------------------------------------------------- */
static void AT9S_RxCpltCallback(void)
{
osThreadFlagsSet(thread_alert, SIGNAL_AT9S_RAW_REDY);
}
/* Exported functions ------------------------------------------------------- */
int8_t AT9S_Init(AT9S_t *at9s)
{
if (at9s == NULL) return DEVICE_ERR_NULL;
if (inited) return DEVICE_ERR_INITED;
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
BSP_UART_RegisterCallback(BSP_UART_RC, BSP_UART_RX_CPLT_CB,
AT9S_RxCpltCallback);
inited = true;
return DEVICE_OK;
}
int8_t AT9S_Restart(void)
{
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_RC));
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_RC));
return DEVICE_OK;
}
int8_t AT9S_StartDmaRecv(uint8_t *cmd_buffer)
{
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_RC),
cmd_buffer,
AT9S_FRAME_LEN) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
bool AT9S_WaitDmaCplt(uint32_t timeout)
{
return (osThreadFlagsWait(SIGNAL_AT9S_RAW_REDY,
osFlagsWaitAll, timeout) == SIGNAL_AT9S_RAW_REDY);
}
AT9S_Raw_t r;
/* 解析 25 字节原始帧并填充结构体 */
void AT9S_ParseRaw(const uint8_t raw[AT9S_FRAME_LEN], AT9S_t *out)
{
// AT9S_Raw_t r;
/* 摇杆 */
r.ch[0] = ((raw[1] | raw[2] << 8) & 0x07FF);
r.ch[1] = ((raw[2] >> 3 | raw[3] << 5) & 0x07FF);
r.ch[2] = ((raw[3] >> 6 | raw[4] << 2 | raw[5] << 10) & 0x07FF);
r.ch[3] = ((raw[5] >> 1 | raw[6] << 7) & 0x07FF);
/* 开关/旋钮 */
r.sw[0] = ((raw[6] >> 4 | raw[7] << 4) & 0x07FF);
r.sw[1] = ((raw[7] >> 7 | raw[8] << 1 | raw[9] << 9) & 0x07FF);
r.sw[2] = ((raw[9] >> 2 | raw[10]<< 6) & 0x07FF);
r.sw[3] = ((raw[10]>> 5 | raw[11]<< 3) & 0x07FF);
r.sw[4] = ((raw[12] | raw[13]<< 8) & 0x07FF);
r.sw[5] = ((raw[13]>> 3 | raw[14]<< 5) & 0x07FF);
r.sw[6] = ((raw[14]>> 6 | raw[15]<< 2 | raw[16]<< 10) & 0x07FF);
r.sw[7] = ((raw[16]>> 1 | raw[17]<< 7) & 0x07FF);
/* 开关映射 */
#define MAP_SWITCH(v) \
((v) > 300 && (v) < 500) ? AT9S_CMD_SW_DOWN : \
((v) >= 500 && (v) < 1500) ? AT9S_CMD_SW_MID : \
((v) >= 1500 && (v) < 1700) ? AT9S_CMD_SW_UP : AT9S_CMD_SW_ERR
#define MAP_SWITCH_RESERVE(v) \
((v) > 300 && (v) < 500) ? AT9S_CMD_SW_UP : \
((v) >= 500 && (v) < 1500) ? AT9S_CMD_SW_MID : \
((v) >= 1500 && (v) < 1700) ? AT9S_CMD_SW_DOWN : AT9S_CMD_SW_ERR
out->data.rc.ch_l_x = 2.0f*(r.ch[3]-209.0f)/(1596.0f-209.0f)-1.0f;
out->data.rc.ch_l_y = 2.0f*(r.ch[2]-178.0f)/(1575.0f-198.0f)-1.0f;
out->data.rc.ch_r_x = 2.0f*(r.ch[0]-210.0f)/(1596.0f-210.0f)-1.0f;
out->data.rc.ch_r_y = 2.0f*(r.ch[1]-221.0f)/(1604.0f-217.0f)-1.0f;
if(fabs(out->data.rc.ch_l_x)<=DEAD_AREA)out->data.rc.ch_l_x = 0;
if(fabs(out->data.rc.ch_l_y)<=4*DEAD_AREA)out->data.rc.ch_l_y = 0;
if(fabs(out->data.rc.ch_r_x)<=DEAD_AREA)out->data.rc.ch_r_x = 0;
if(fabs(out->data.rc.ch_r_y)<=DEAD_AREA)out->data.rc.ch_r_y = 0;
if(fabs(out->data.rc.ch_l_x)>=1.0f)out->data.rc.ch_l_x= out->data.rc.ch_l_x>0?1.0f:-1.0f;
if(fabs(out->data.rc.ch_l_y)>=1.0f)out->data.rc.ch_l_y= out->data.rc.ch_l_y>0?1.0f:-1.0f;
if(fabs(out->data.rc.ch_r_x)>=1.0f)out->data.rc.ch_r_x= out->data.rc.ch_r_x>0?1.0f:-1.0f;
if(fabs(out->data.rc.ch_r_y)>=1.0f)out->data.rc.ch_r_y= out->data.rc.ch_r_y>0?1.0f:-1.0f;
// out->data.ch_r_x = r.ch[0];
// out->data.ch_r_y = r.ch[1];
// out->data.ch_l_y = r.ch[2];
// out->data.ch_l_x = r.ch[3];
out->data.rc.key_A = MAP_SWITCH_RESERVE(r.sw[0]);
out->data.rc.key_B = MAP_SWITCH_RESERVE(r.sw[1]);
out->data.rc.key_C = MAP_SWITCH_RESERVE(r.sw[2]);
out->data.rc.key_D = MAP_SWITCH(r.sw[3]);
out->data.rc.key_E = MAP_SWITCH(r.sw[4]);
out->data.rc.key_F = MAP_SWITCH(r.sw[5]);
out->data.rc.key_G = MAP_SWITCH_RESERVE(r.sw[6]);
out->data.rc.key_H = MAP_SWITCH(r.sw[7]);
// out->knob_left = MAP_SWITCH(r.sw[6]);
// out->knob_right = MAP_SWITCH(r.sw[7]);
#undef MAP_SWITCH
//check online.
out->header.online = (r.sw[7] != 300&&r.ch[0] != 1000);
}

70
User/device/at9s.h Normal file
View File

@ -0,0 +1,70 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include "component/user_math.h"
#include "device.h"
/* Exported constants ------------------------------------------------------- */
#define AT9S_FRAME_LEN 25u
/* Exported types ----------------------------------------------------------- */
typedef struct __attribute__((packed))
{
int16_t ch[4]; /* 摇杆原始值 */
int16_t sw[8]; /* 开关/旋钮原始值 */
} AT9S_Raw_t;
/* 拨杆位置 */
typedef enum {
AT9S_CMD_SW_ERR = 0,
AT9S_CMD_SW_UP = 1,
AT9S_CMD_SW_MID = 3,
AT9S_CMD_SW_DOWN = 2,
} AT9S_CMD_SwitchPos_t;
typedef struct
{
float ch_l_x; /* 左摇杆 X */
float ch_l_y; /* 左摇杆 Y（油门） */
float ch_r_x; /* 右摇杆 X */
float ch_r_y; /* 右摇杆 Y */
/* 开关/旋钮离散化后状态 */
AT9S_CMD_SwitchPos_t key_A;
AT9S_CMD_SwitchPos_t key_B;
AT9S_CMD_SwitchPos_t key_C;
AT9S_CMD_SwitchPos_t key_D;
AT9S_CMD_SwitchPos_t key_E;
AT9S_CMD_SwitchPos_t key_F;
AT9S_CMD_SwitchPos_t key_G;
AT9S_CMD_SwitchPos_t key_H;
float knob_left;
float knob_right;
float back_left;
float back_right;
} AT9S_DataRC_t;
typedef struct
{
AT9S_DataRC_t rc;
} AT9S_Data_t;
typedef struct{
DEVICE_Header_t header;
AT9S_Data_t data;
} AT9S_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t AT9S_Init(AT9S_t *at9s);
int8_t AT9S_Restart(void);
int8_t AT9S_StartDmaRecv(uint8_t *cmd_buffer);
bool AT9S_WaitDmaCplt(uint32_t timeout);
void AT9S_ParseRaw(const uint8_t raw[AT9S_FRAME_LEN], AT9S_t *out);
#ifdef __cplusplus
}
#endif

49
User/device/device.h Normal file
View File

@ -0,0 +1,49 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stdint.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
#define DEVICE_OK (0)
#define DEVICE_ERR (-1)
#define DEVICE_ERR_NULL (-2)
#define DEVICE_ERR_INITED (-3)
#define DEVICE_ERR_NO_DEV (-4)
/* AUTO GENERATED SIGNALS BEGIN */
#define SIGNAL_DR16_RAW_REDY (1u << 0)
#define SIGNAL_AT9S_RAW_REDY (1u << 7)
#define SIGNAL_VT13_RAW_REDY (1u << 8)
/* AUTO GENERATED SIGNALS END */
/* USER SIGNALS BEGIN */
/* USER SIGNALS END */
/*设备层通用Header*/
typedef struct {
bool online;
uint64_t last_online_time;
} DEVICE_Header_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

6
User/device/device_config.yaml Normal file
View File

@ -0,0 +1,6 @@
motor:
bsp_config: {}
enabled: true
motor_dm:
bsp_config: {}
enabled: true

221
User/device/dr16.c Normal file
View File

@ -0,0 +1,221 @@
/*
DR16接收机
Example：
DR16_Init(&dr16);
while (1) {
DR16_StartDmaRecv(&dr16);
if (DR16_WaitDmaCplt(20)) {
DR16_ParseData(&dr16);
} else {
DR16_Offline(&dr16);
}
}
*/
/* Includes ----------------------------------------------------------------- */
#include "dr16.h"
#include "bsp/uart.h"
#include "bsp/time.h"
#include "device.h"
#include <string.h>
#include <stdbool.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */
#define DR16_CH_VALUE_MIN (364u)
#define DR16_CH_VALUE_MID (1024u)
#define DR16_CH_VALUE_MAX (1684u)
//#define DR16_FRAME_SIZE (18u) // DR16数据帧固定18字节
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static osThreadId_t thread_alert;
static bool inited = false;
/* Private function -------------------------------------------------------- */
static void DR16_RxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY);
}
//static void DR16_ErrorCallback(void) {
// UART_HandleTypeDef *huart = BSP_UART_GetHandle(BSP_UART_RC);
//
// // 清除所有错误标志（包括FIFO错误）
// __HAL_UART_CLEAR_OREFLAG(huart);
// __HAL_UART_CLEAR_NEFLAG(huart);
// __HAL_UART_CLEAR_FEFLAG(huart);
// __HAL_UART_CLEAR_PEFLAG(huart);
//
// // 清除FIFO超时标志（STM32H7特有）
// if (__HAL_UART_GET_FLAG(huart, UART_FLAG_RTOF)) {
// __HAL_UART_CLEAR_FLAG(huart, UART_CLEAR_RTOF);
// }
//
// // 中止并清除当前DMA传输
// HAL_UART_AbortReceive(huart);
//}
static bool DR16_DataCorrupted(const DR16_t *dr16) {
if (dr16 == NULL) return DEVICE_ERR_NULL;
if ((dr16->raw_data.ch_r_x < DR16_CH_VALUE_MIN) ||
(dr16->raw_data.ch_r_x > DR16_CH_VALUE_MAX))
return DEVICE_ERR;
if ((dr16->raw_data.ch_r_y < DR16_CH_VALUE_MIN) ||
(dr16->raw_data.ch_r_y > DR16_CH_VALUE_MAX))
return DEVICE_ERR;
if ((dr16->raw_data.ch_l_x < DR16_CH_VALUE_MIN) ||
(dr16->raw_data.ch_l_x > DR16_CH_VALUE_MAX))
return DEVICE_ERR;
if ((dr16->raw_data.ch_l_y < DR16_CH_VALUE_MIN) ||
(dr16->raw_data.ch_l_y > DR16_CH_VALUE_MAX))
return DEVICE_ERR;
if (dr16->raw_data.sw_l == 0) return DEVICE_ERR;
if (dr16->raw_data.sw_r == 0) return DEVICE_ERR;
return DEVICE_OK;
}
/* Exported functions ------------------------------------------------------- */
int8_t DR16_Init(DR16_t *dr16) {
if (dr16 == NULL) return DEVICE_ERR_NULL;
if (inited) return DEVICE_ERR_INITED;
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
// // 清零结构体
// memset(dr16, 0, sizeof(DR16_t));
BSP_UART_RegisterCallback(BSP_UART_RC, BSP_UART_RX_CPLT_CB,
DR16_RxCpltCallback);
// BSP_UART_RegisterCallback(BSP_UART_RC, BSP_UART_ERROR_CB,
// DR16_ErrorCallback);
inited = true;
return DEVICE_OK;
}
int8_t DR16_Restart(void) {
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_RC));
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_RC));
return DEVICE_OK;
}
int8_t DR16_StartDmaRecv(DR16_t *dr16) {
UART_HandleTypeDef *huart = BSP_UART_GetHandle(BSP_UART_RC);
// // 确保之前的DMA接收已停止
// if (huart->RxState != HAL_UART_STATE_READY) {
// HAL_UART_AbortReceive(huart);
// }
//
// // 清除所有可能的错误标志和IDLE标志
// __HAL_UART_CLEAR_OREFLAG(huart);
// __HAL_UART_CLEAR_NEFLAG(huart);
// __HAL_UART_CLEAR_FEFLAG(huart);
// __HAL_UART_CLEAR_PEFLAG(huart);
// __HAL_UART_CLEAR_IDLEFLAG(huart);
//
// // 清除FIFO超时标志
// if (__HAL_UART_GET_FLAG(huart, UART_FLAG_RTOF)) {
// __HAL_UART_CLEAR_FLAG(huart, UART_CLEAR_RTOF);
// }
if (HAL_UART_Receive_DMA(huart, (uint8_t *)&(dr16->raw_data),
sizeof(dr16->raw_data)) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
bool DR16_WaitDmaCplt(uint32_t timeout) {
return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) ==
SIGNAL_DR16_RAW_REDY);
}
int8_t DR16_ParseData(DR16_t *dr16){
if (dr16 == NULL) return DEVICE_ERR_NULL;
// // STM32H7 D-Cache一致性处理：使DMA接收的数据对CPU可见
// // D-Cache按32字节对齐，需要对齐处理
// uint32_t addr = (uint32_t)&(dr16->raw_data);
// uint32_t size = sizeof(dr16->raw_data);
// // 向下对齐到32字节边界
// uint32_t aligned_addr = addr & ~0x1FU;
// // 计算对齐后的大小
// uint32_t aligned_size = ((size + (addr - aligned_addr) + 31) & ~0x1FU);
// SCB_InvalidateDCache_by_Addr((uint32_t *)aligned_addr, aligned_size);
if (DR16_DataCorrupted(dr16)) {
return DEVICE_ERR;
}
dr16->header.online = true;
dr16->header.last_online_time = BSP_TIME_Get_us();
memset(&(dr16->data), 0, sizeof(dr16->data));
float full_range = (float)(DR16_CH_VALUE_MAX - DR16_CH_VALUE_MIN);
// 解析摇杆数据
dr16->data.ch_r_x = 2.0f * ((float)dr16->raw_data.ch_r_x - DR16_CH_VALUE_MID) / full_range;
dr16->data.ch_r_y = 2.0f * ((float)dr16->raw_data.ch_r_y - DR16_CH_VALUE_MID) / full_range;
dr16->data.ch_l_x = 2.0f * ((float)dr16->raw_data.ch_l_x - DR16_CH_VALUE_MID) / full_range;
dr16->data.ch_l_y = 2.0f * ((float)dr16->raw_data.ch_l_y - DR16_CH_VALUE_MID) / full_range;
// 解析拨杆位置
dr16->data.sw_l = (DR16_SwitchPos_t)dr16->raw_data.sw_l;
dr16->data.sw_r = (DR16_SwitchPos_t)dr16->raw_data.sw_r;
// 解析鼠标数据
dr16->data.mouse.x = dr16->raw_data.x;
dr16->data.mouse.y = dr16->raw_data.y;
dr16->data.mouse.z = dr16->raw_data.z;
dr16->data.mouse.l_click = dr16->raw_data.press_l;
dr16->data.mouse.r_click = dr16->raw_data.press_r;
// 解析键盘按键 - 使用union简化代码
uint16_t key_value = dr16->raw_data.key;
// 解析键盘位映射（W-B键，位0-15）
for (int i = DR16_KEY_W; i <= DR16_KEY_B; i++) {
dr16->data.keyboard.key[i] = (key_value & (1 << i)) != 0;
}
// 解析鼠标点击
dr16->data.keyboard.key[DR16_L_CLICK] = dr16->data.mouse.l_click;
dr16->data.keyboard.key[DR16_R_CLICK] = dr16->data.mouse.r_click;
// 解析第五通道
dr16->data.ch_res = 2.0f * ((float)dr16->raw_data.res - DR16_CH_VALUE_MID) / full_range;
return DEVICE_OK;
}
int8_t DR16_Offline(DR16_t *dr16){
if (dr16 == NULL) return DEVICE_ERR_NULL;
dr16->header.online = false;
memset(&(dr16->data), 0, sizeof(dr16->data));
return DEVICE_OK;
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

119
User/device/dr16.h Normal file
View File

@ -0,0 +1,119 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include "component/user_math.h"
#include "device.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct __packed {
uint16_t ch_r_x : 11;
uint16_t ch_r_y : 11;
uint16_t ch_l_x : 11;
uint16_t ch_l_y : 11;
uint8_t sw_r : 2;
uint8_t sw_l : 2;
int16_t x;
int16_t y;
int16_t z;
uint8_t press_l;
uint8_t press_r;
uint16_t key;
uint16_t res;
} DR16_RawData_t;
// typedef struct __packed {
// uint8_t buff[18]; // 原始接收缓冲区
// } DR16_RawData_t;
typedef enum {
DR16_SW_ERR = 0,
DR16_SW_UP = 1,
DR16_SW_MID = 3,
DR16_SW_DOWN = 2,
} DR16_SwitchPos_t;
/* 键盘按键值 */
typedef enum {
DR16_KEY_W = 0,
DR16_KEY_S,
DR16_KEY_A,
DR16_KEY_D,
DR16_KEY_SHIFT,
DR16_KEY_CTRL,
DR16_KEY_Q,
DR16_KEY_E,
DR16_KEY_R,
DR16_KEY_F,
DR16_KEY_G,
DR16_KEY_Z,
DR16_KEY_X,
DR16_KEY_C,
DR16_KEY_V,
DR16_KEY_B,
DR16_L_CLICK,
DR16_R_CLICK,
DR16_KEY_NUM,
} DR16_Key_t;
typedef struct {
float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
float ch_res; /* 第五通道值 */
DR16_SwitchPos_t sw_r; /* 右侧拨杆位置 */
DR16_SwitchPos_t sw_l; /* 左侧拨杆位置 */
struct {
int16_t x;
int16_t y;
int16_t z;
bool l_click; /* 左键 */
bool r_click; /* 右键 */
} mouse; /* 鼠标值 */
struct {
bool key[DR16_KEY_NUM]; /* 键盘按键值 */
} keyboard;
uint16_t res; /* 保留，未启用 */
} DR16_Data_t;
typedef struct {
DEVICE_Header_t header;
DR16_RawData_t raw_data;
DR16_Data_t data;
} DR16_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t DR16_Init(DR16_t *dr16);
int8_t DR16_Restart(void);
int8_t DR16_StartDmaRecv(DR16_t *dr16);
bool DR16_WaitDmaCplt(uint32_t timeout);
int8_t DR16_ParseData(DR16_t *dr16);
int8_t DR16_Offline(DR16_t *dr16);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

52
User/device/motor.c Normal file
View File

@ -0,0 +1,52 @@
/*
电机通用函数
*/
/* Includes ----------------------------------------------------------------- */
#include "motor.h"
#include <string.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 -------------------------------------------------------- */
/* 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;
}

68
User/device/motor.h Normal file
View File

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

522
User/device/motor_dm.c Normal file
View File

@ -0,0 +1,522 @@
#define MOTOR_DM_FLOAT_TO_INT_SIGNED(x, x_min, x_max, bits) \
((int32_t)roundf(((x) / ((x_max) - (x_min))) * (1 << (bits)) + (1 << ((bits) - 1))))
#define MOTOR_DM_INT_TO_FLOAT_SIGNED(x_int, x_min, x_max, bits) \
(((float)((int32_t)(x_int) - (1 << ((bits) - 1))) * ((x_max) - (x_min)) / (float)(1 << (bits))))
/* Includes ----------------------------------------------------------------- */
#include "device/motor_dm.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/user_math.h"
#include "string.h"
#include "math.h"
/* Private constants -------------------------------------------------------- */
/* DM电机数据映射范围 */
#define DM_P_MIN (-12.56637f)
#define DM_P_MAX (12.56637f)
#define DM_V_MIN (-30.0f)
#define DM_V_MAX (30.0f)
#define DM_T_MIN (-12.0f)
#define DM_T_MAX (12.0f)
#define DM_KP_MIN (0.0f)
#define DM_KP_MAX (500.0f)
#define DM_KD_MIN (0.0f)
#define DM_KD_MAX (5.0f)
/* CAN ID偏移量 */
#define DM_CAN_ID_OFFSET_POS_VEL 0x100
#define DM_CAN_ID_OFFSET_VEL 0x200
/* Private macro ------------------------------------------------------------ */
#define FLOAT_TO_UINT(x, x_min, x_max, bits) \
(uint32_t)((x - x_min) * ((1 << bits) - 1) / (x_max - x_min))
#define UINT_TO_FLOAT(x_int, x_min, x_max, bits) \
((float)(x_int) * (x_max - x_min) / ((1 << bits) - 1) + x_min)
/* Private variables -------------------------------------------------------- */
static MOTOR_DM_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
static int float_to_uint(float x_float, float x_min, float x_max, int bits)
{
/* Converts a float to an unsigned int, given range and number of bits */
float span = x_max - x_min;
float offset = x_min;
return (int) ((x_float-offset)*((float)((1<<bits)-1))/span);
}
static float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
/* converts unsigned int to float, given range and number of bits */
float span = x_max - x_min;
float offset = x_min;
return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
}
/* Private function prototypes ---------------------------------------------- */
static int8_t MOTOR_DM_ParseFeedbackFrame(MOTOR_DM_t *motor, const uint8_t *data);
static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output);
static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel);
static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel);
static MOTOR_DM_CANManager_t* MOTOR_DM_GetCANManager(BSP_CAN_t can);
/* Private functions -------------------------------------------------------- */
/**
* @brief 解析DM电机反馈帧
* @param motor 电机实例
* @param data CAN数据
* @return 解析结果
*/
static int8_t MOTOR_DM_ParseFeedbackFrame(MOTOR_DM_t *motor, const uint8_t *data) {
if (motor == NULL || data == NULL) {
return DEVICE_ERR_NULL;
}
uint16_t p_int=(data[1]<<8)|data[2];
motor->motor.feedback.rotor_abs_angle = uint_to_float(p_int, DM_P_MIN, DM_P_MAX, 16); // (-12.5,12.5)
uint16_t v_int=(data[3]<<4)|(data[4]>>4);
motor->motor.feedback.rotor_speed = uint_to_float(v_int, DM_V_MIN, DM_V_MAX, 12); // (-30.0,30.0)
uint16_t t_int=((data[4]&0xF)<<8)|data[5];
motor->motor.feedback.torque_current = uint_to_float(t_int, DM_T_MIN, DM_T_MAX, 12); // (-12.0,12.0)
motor->motor.feedback.temp = (float)(data[6]);
while (motor->motor.feedback.rotor_abs_angle < 0) {
motor->motor.feedback.rotor_abs_angle += M_2PI;
}
while (motor->motor.feedback.rotor_abs_angle >= M_2PI) {
motor->motor.feedback.rotor_abs_angle -= M_2PI;
}
if (motor->param.reverse) {
motor->motor.feedback.rotor_abs_angle = M_2PI - motor->motor.feedback.rotor_abs_angle;
motor->motor.feedback.rotor_speed = -motor->motor.feedback.rotor_speed;
motor->motor.feedback.torque_current = -motor->motor.feedback.torque_current;
}
return DEVICE_OK;
}
/**
* @brief 发送MIT模式控制命令
* @param motor 电机实例
* @param output MIT控制参数
* @return 发送结果
*/
static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output) {
if (motor == NULL || output == NULL) {
return DEVICE_ERR_NULL;
}
uint8_t data[8];
uint16_t pos_tmp,vel_tmp,kp_tmp,kd_tmp,tor_tmp;
uint16_t id = motor->param.can_id;
pos_tmp = float_to_uint(output->angle, DM_P_MIN , DM_P_MAX, 16);
vel_tmp = float_to_uint(output->velocity, DM_V_MIN , DM_V_MAX, 12);
kp_tmp = float_to_uint(output->kp, DM_KP_MIN, DM_KP_MAX, 12);
kd_tmp = float_to_uint(output->kd, DM_KD_MIN, DM_KD_MAX, 12);
tor_tmp = float_to_uint(output->torque, DM_T_MIN , DM_T_MAX, 12);
/* 打包数据 */
data[0] = (pos_tmp >> 8);
data[1] = pos_tmp;
data[2] = (vel_tmp >> 4);
data[3] = ((vel_tmp&0xF)<<4)|(kp_tmp>>8);
data[4] = kp_tmp;
data[5] = (kd_tmp >> 4);
data[6] = ((kd_tmp&0xF)<<4)|(tor_tmp>>8);
data[7] = tor_tmp;
/* 发送CAN消息 */
BSP_CAN_StdDataFrame_t frame;
frame.id = motor->param.can_id;
frame.dlc = 8;
memcpy(frame.data, data, 8);
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 发送位置速度模式控制命令
* @param motor 电机实例
* @param pos 目标位置
* @param vel 目标速度
* @return 发送结果
*/
static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel) {
if (motor == NULL) {
return DEVICE_ERR_NULL;
}
uint8_t data[8] = {0};
/* 直接发送浮点数数据 */
memcpy(&data[0], &pos, 4); // 位置，低位在前
memcpy(&data[4], &vel, 4); // 速度，低位在前
/* 发送CAN消息，ID为原ID+0x100 */
uint32_t can_id = DM_CAN_ID_OFFSET_POS_VEL + motor->param.can_id;
BSP_CAN_StdDataFrame_t frame;
frame.id = can_id;
frame.dlc = 8;
memcpy(frame.data, data, 8);
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 发送速度模式控制命令
* @param motor 电机实例
* @param vel 目标速度
* @return 发送结果
*/
static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel) {
if (motor == NULL) {
return DEVICE_ERR_NULL;
}
uint8_t data[4] = {0};
/* 直接发送浮点数数据 */
memcpy(&data[0], &vel, 4); // 速度，低位在前
/* 发送CAN消息，ID为原ID+0x200 */
uint32_t can_id = DM_CAN_ID_OFFSET_VEL + motor->param.can_id;
BSP_CAN_StdDataFrame_t frame;
frame.id = can_id;
frame.dlc = 4;
memcpy(frame.data, data, 4);
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 获取指定CAN总线的管理器
* @param can CAN总线
* @return CAN管理器指针
*/
static MOTOR_DM_CANManager_t* MOTOR_DM_GetCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) {
return NULL;
}
return can_managers[can];
}
/**
* @brief 创建CAN管理器
* @param can CAN总线
* @return 创建结果
*/
static int8_t MOTOR_DM_CreateCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
if (can_managers[can] != NULL) return DEVICE_OK;
can_managers[can] = (MOTOR_DM_CANManager_t*)BSP_Malloc(sizeof(MOTOR_DM_CANManager_t));
if (can_managers[can] == NULL) return DEVICE_ERR;
memset(can_managers[can], 0, sizeof(MOTOR_DM_CANManager_t));
can_managers[can]->can = can;
return DEVICE_OK;
}
/* Exported functions ------------------------------------------------------- */
/**
* @brief 注册一个DM电机
* @param param 电机参数
* @return 注册结果
*/
int8_t MOTOR_DM_Register(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
/* 创建CAN管理器 */
if (MOTOR_DM_CreateCANManager(param->can) != DEVICE_OK) {
return DEVICE_ERR;
}
/* 获取CAN管理器 */
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
if (manager == NULL) {
return DEVICE_ERR;
}
/* 检查是否已注册 */
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.master_id == param->master_id) {
return DEVICE_ERR_INITED;
}
}
/* 检查是否已达到最大数量 */
if (manager->motor_count >= MOTOR_DM_MAX_MOTORS) {
return DEVICE_ERR;
}
/* 分配内存 */
MOTOR_DM_t *motor = (MOTOR_DM_t *)BSP_Malloc(sizeof(MOTOR_DM_t));
if (motor == NULL) {
return DEVICE_ERR;
}
/* 初始化电机 */
memset(motor, 0, sizeof(MOTOR_DM_t));
memcpy(&motor->param, param, sizeof(MOTOR_DM_Param_t));
motor->motor.header.online = false;
motor->motor.reverse = param->reverse;
/* 注册CAN接收ID - DM电机使用Master ID接收反馈 */
uint16_t feedback_id = param->master_id;
if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) {
BSP_Free(motor);
return DEVICE_ERR;
}
/* 添加到管理器 */
manager->motors[manager->motor_count] = motor;
manager->motor_count++;
return DEVICE_OK;
}
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return 更新结果
*/
int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
if (manager == NULL) {
return DEVICE_ERR_NO_DEV;
}
/* 查找电机 */
MOTOR_DM_t *motor = NULL;
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.master_id == param->master_id) {
motor = manager->motors[i];
break;
}
}
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
/* 主动接收CAN消息 */
uint16_t feedback_id = param->master_id;
BSP_CAN_Message_t rx_msg;
if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
uint64_t now_time = BSP_TIME_Get();
if (now_time - motor->motor.header.last_online_time > 100000) { // 100ms超时，单位微秒
motor->motor.header.online = false;
}
return DEVICE_ERR;
}
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
MOTOR_DM_ParseFeedbackFrame(motor, rx_msg.data);
return DEVICE_OK;
}
/**
* @brief 更新所有电机数据
* @return 更新结果
*/
int8_t MOTOR_DM_UpdateAll(void) {
int8_t ret = DEVICE_OK;
for (int can = 0; can < BSP_CAN_NUM; can++) {
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager((BSP_CAN_t)can);
if (manager == NULL) continue;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_DM_t *motor = manager->motors[i];
if (motor != NULL) {
if (MOTOR_DM_Update(&motor->param) != DEVICE_OK) {
ret = DEVICE_ERR;
}
}
}
}
return ret;
}
/**
* @brief MIT模式控制
* @param param 电机参数
* @param output MIT控制参数
* @return 控制结果
*/
int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output) {
if (param == NULL || output == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
return MOTOR_DM_SendMITCmd(motor, output);
}
/**
* @brief 位置速度模式控制
* @param param 电机参数
* @param target_pos 目标位置
* @param target_vel 目标速度
* @return 控制结果
*/
int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
return MOTOR_DM_SendPosVelCmd(motor, target_pos, target_vel);
}
/**
* @brief 速度模式控制
* @param param 电机参数
* @param target_vel 目标速度
* @return 控制结果
*/
int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
return MOTOR_DM_SendVelCmd(motor, target_vel);
}
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return 电机实例指针
*/
MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return NULL;
}
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
if (manager == NULL) {
return NULL;
}
/* 查找对应的电机 */
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_DM_t *motor = manager->motors[i];
if (motor && motor->param.can == param->can &&
motor->param.master_id == param->master_id) {
return motor;
}
}
return NULL;
}
int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param){
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
BSP_CAN_StdDataFrame_t frame;
frame.id = motor->param.can_id;
frame.dlc = 8;
frame.data[0] = 0XFF;
frame.data[1] = 0xFF;
frame.data[2] = 0xFF;
frame.data[3] = 0xFF;
frame.data[4] = 0xFF;
frame.data[5] = 0xFF;
frame.data[6] = 0xFF;
frame.data[7] = 0xFC;
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
//重新设置角度零点
int8_t MOTOR_DM_SetZero(MOTOR_DM_Param_t *param){
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
BSP_CAN_StdDataFrame_t frame;
frame.id = motor->param.can_id;
frame.dlc = 8;
frame.data[0] = 0XFF;
frame.data[1] = 0xFF;
frame.data[2] = 0xFF;
frame.data[3] = 0xFF;
frame.data[4] = 0xFF;
frame.data[5] = 0xFF;
frame.data[6] = 0xFF;
frame.data[7] = 0xFE;
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 使电机松弛（设置输出为0）
* @param param 电机参数
* @return 操作结果
*/
int8_t MOTOR_DM_Relax(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_MIT_Output_t output = {0};
return MOTOR_DM_MITCtrl(param, &output);
}
/**
* @brief 使电机离线（设置在线状态为false）
* @param param 电机参数
* @return 操作结果
*/
int8_t MOTOR_DM_Offine(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
motor->motor.header.online = false;
return DEVICE_OK;
}

105
User/device/motor_dm.h Normal file
View File

@ -0,0 +1,105 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "device/motor.h"
#include "bsp/can.h"
/* Exported constants ------------------------------------------------------- */
#define MOTOR_DM_MAX_MOTORS 32
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
MOTOR_DM_J4310,
} MOTOR_DM_Module_t;
/*每个电机需要的参数*/
typedef struct {
BSP_CAN_t can;
uint16_t master_id; /* 主站ID，用于发送控制命令 */
uint16_t can_id; /* 反馈ID，用于接收电机反馈 */
MOTOR_DM_Module_t module;
bool reverse;
} MOTOR_DM_Param_t;
/*电机实例*/
typedef struct{
MOTOR_DM_Param_t param;
MOTOR_t motor;
} MOTOR_DM_t;
/*CAN管理器，管理一个CAN总线上所有的电机*/
typedef struct {
BSP_CAN_t can;
MOTOR_DM_t *motors[MOTOR_DM_MAX_MOTORS];
uint8_t motor_count;
} MOTOR_DM_CANManager_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 注册一个LK电机
* @param param 电机参数
* @return
*/
int8_t MOTOR_DM_Register(MOTOR_DM_Param_t *param);
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return
*/
int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param);
/**
* @brief 更新所有电机数据
* @return
*/
int8_t MOTOR_DM_UpdateAll(void);
int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output);
int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel);
int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel);
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return
*/
MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param);
int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param);
/**
* @brief 使电机松弛（设置输出为0）
* @param param
* @return
*/
int8_t MOTOR_DM_Relax(MOTOR_DM_Param_t *param);
/**
* @brief 使电机离线（设置在线状态为false）
* @param param
* @return
*/
int8_t MOTOR_DM_Offine(MOTOR_DM_Param_t *param);
/**
* @brief 重新设置角度零点（将当前位置设为电机零位）
* @param param 电机参数指针
* @return DEVICE_OK 成功，DEVICE_ERR 失败
*/
int8_t MOTOR_DM_SetZero(MOTOR_DM_Param_t *param);
#ifdef __cplusplus
}
#endif

440
User/device/motor_lz.c Normal file
View File

@ -0,0 +1,440 @@
/*
灵足电机驱动
灵足电机通信协议：
- CAN 2.0通信接口，波特率1Mbps
- 采用扩展帧格式(29位ID)
- ID格式：Bit28~24(通信类型) + Bit23~8(数据区2) + Bit7~0(目标地址)
*/
/* Includes ----------------------------------------------------------------- */
#include "motor_lz.h"
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include "bsp/can.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/user_math.h"
/* Private define ----------------------------------------------------------- */
// 灵足电机协议参数
#define LZ_ANGLE_RANGE_RAD (12.57f) /* 角度范围 ±12.57 rad */
#define LZ_VELOCITY_RANGE_RAD_S (20.0f) /* 角速度范围 ±20 rad/s */
#define LZ_TORQUE_RANGE_NM (60.0f) /* 力矩范围 ±60 Nm */
#define LZ_KP_MAX (5000.0f) /* Kp最大值 */
#define LZ_KD_MAX (100.0f) /* Kd最大值 */
#define LZ_RAW_VALUE_MAX (65535) /* 16位原始值最大值 */
#define LZ_TEMP_SCALE (10.0f) /* 温度缩放因子 */
#define LZ_MAX_RECOVER_DIFF_RAD (0.28f)
#define MOTOR_TX_BUF_SIZE (8)
#define MOTOR_RX_BUF_SIZE (8)
/* Private macro ------------------------------------------------------------ */
MOTOR_LZ_MotionParam_t lz_relax_param = {
.target_angle = 0.0f,
.target_velocity = 0.0f,
.kp = 0.0f,
.kd = 0.0f,
.torque = 0.0f,
};
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static MOTOR_LZ_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
/* Private function prototypes ---------------------------------------------- */
static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can);
static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can);
static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg);
static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id);
static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value);
static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value);
static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc);
static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
/* Private functions -------------------------------------------------------- */
/**
* @brief 获取CAN管理器
*/
static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return NULL;
return can_managers[can];
}
/**
* @brief 创建CAN管理器
*/
static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
if (can_managers[can] != NULL) return DEVICE_OK;
can_managers[can] = (MOTOR_LZ_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LZ_CANManager_t));
if (can_managers[can] == NULL) return DEVICE_ERR;
memset(can_managers[can], 0, sizeof(MOTOR_LZ_CANManager_t));
can_managers[can]->can = can;
return DEVICE_OK;
}
/**
* @brief 构建扩展ID
*/
static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id) {
uint32_t ext_id = 0;
ext_id |= ((uint32_t)cmd_type & 0x1F) << 24; // Bit28~24: 通信类型
ext_id |= ((uint32_t)data2 & 0xFFFF) << 8; // Bit23~8: 数据区2
ext_id |= ((uint32_t)target_id & 0xFF); // Bit7~0: 目标地址
return ext_id;
}
/**
* @brief 浮点值转换为原始值（对称范围：-max_value ~ +max_value）
*/
static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value) {
// 限制范围
if (value > max_value) value = max_value;
if (value < -max_value) value = -max_value;
// 转换为0~65535范围，对应-max_value~max_value
return (uint16_t)((value + max_value) / (2.0f * max_value) * (float)LZ_RAW_VALUE_MAX);
}
/**
* @brief 浮点值转换为原始值（单向范围：0 ~ +max_value）
*/
static uint16_t MOTOR_LZ_FloatToRawPositive(float value, float max_value) {
// 限制范围
if (value > max_value) value = max_value;
if (value < 0.0f) value = 0.0f;
// 转换为0~65535范围，对应0~max_value
return (uint16_t)(value / max_value * (float)LZ_RAW_VALUE_MAX);
}
/**
* @brief 原始值转换为浮点值
*/
static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value) {
// 将0~65535范围转换为-max_value~max_value
return ((float)raw_value / (float)LZ_RAW_VALUE_MAX) * (2.0f * max_value) - max_value;
}
/**
* @brief 发送扩展帧
*/
static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc) {
BSP_CAN_ExtDataFrame_t tx_frame;
tx_frame.id = ext_id;
tx_frame.dlc = dlc;
if (data != NULL) {
memcpy(tx_frame.data, data, dlc);
} else {
memset(tx_frame.data, 0, dlc);
}
return BSP_CAN_TransmitExtDataFrame(can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 灵足电机ID解析器
* @param original_id 原始CAN ID（29位扩展帧）
* @param frame_type 帧类型
* @return 解析后的ID（用于队列匹配）
*
* 灵足电机扩展ID格式：
* Bit28~24: 通信类型 (0x1=运控控制, 0x2=反馈数据, 0x3=使能, 0x4=停止, 0x6=设零位)
* Bit23~8: 数据区2 (根据通信类型而定)
* Bit7~0: 目标地址 (目标电机CAN ID)
*/
static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
// 只处理扩展数据帧
if (frame_type != BSP_CAN_FRAME_EXT_DATA) {
return original_id; // 非扩展帧直接返回原始ID
}
// 解析扩展ID各个字段
uint8_t cmd_type = (original_id >> 24) & 0x1F; // Bit28~24: 通信类型
uint16_t data2 = (original_id >> 8) & 0xFFFF; // Bit23~8: 数据区2
uint8_t host_id = (uint8_t)(original_id & 0xFF); // Bit7~0: 主机CAN ID
// 对于反馈数据帧，我们使用特殊的解析规则
if (cmd_type == MOTOR_LZ_CMD_FEEDBACK) {
// 反馈数据的data2字段包含：
// Bit8~15: 当前电机CAN ID
// Bit16~21: 故障信息
// Bit22~23: 模式状态
uint8_t motor_can_id = data2 & 0xFF; // bit8~15: 当前电机CAN ID
// 返回格式化的ID，便于匹配
// 格式：0x02HHMMTT (02=反馈命令, HH=主机ID, MM=电机ID, TT=主机ID)
return (0x02000000) | (host_id << 16) | (motor_can_id << 8) | host_id;
}
// 对于其他命令类型，直接返回原始ID
return original_id;
}
/**
* @brief 解码灵足电机反馈数据
*/
static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg) {
if (motor == NULL || msg == NULL) return;
uint8_t cmd_type = (msg->original_id >> 24) & 0x1F;
if (cmd_type != MOTOR_LZ_CMD_FEEDBACK) return;
uint16_t id_data2 = (msg->original_id >> 8) & 0xFFFF;
uint8_t motor_can_id = id_data2 & 0xFF;
uint8_t fault_info = (id_data2 >> 8) & 0x3F;
uint8_t mode_state = (id_data2 >> 14) & 0x03;
motor->lz_feedback.motor_can_id = motor_can_id;
motor->lz_feedback.fault.under_voltage = (fault_info & 0x01) != 0;
motor->lz_feedback.fault.driver_fault = (fault_info & 0x02) != 0;
motor->lz_feedback.fault.over_temp = (fault_info & 0x04) != 0;
motor->lz_feedback.fault.encoder_fault = (fault_info & 0x08) != 0;
motor->lz_feedback.fault.stall_overload = (fault_info & 0x10) != 0;
motor->lz_feedback.fault.uncalibrated = (fault_info & 0x20) != 0;
motor->lz_feedback.state = (MOTOR_LZ_State_t)mode_state;
// 反馈解码并自动反向
uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]);
float angle = MOTOR_LZ_RawToFloat(raw_angle, LZ_ANGLE_RANGE_RAD);
uint16_t raw_velocity = (uint16_t)((msg->data[2] << 8) | msg->data[3]);
float velocity = MOTOR_LZ_RawToFloat(raw_velocity, LZ_VELOCITY_RANGE_RAD_S);
uint16_t raw_torque = (uint16_t)((msg->data[4] << 8) | msg->data[5]);
float torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM);
// while (angle <0){
// angle += M_2PI;
// }
// while (angle > M_2PI){
// angle -= M_2PI;
// }
// 自动反向
if (motor->param.reverse) {
angle = - angle;
velocity = -velocity;
torque = -torque;
}
motor->lz_feedback.current_angle = angle;
motor->lz_feedback.current_velocity = velocity;
motor->lz_feedback.current_torque = torque;
uint16_t raw_temp = (uint16_t)((msg->data[6] << 8) | msg->data[7]);
motor->lz_feedback.temperature = (float)raw_temp / LZ_TEMP_SCALE;
motor->motor.feedback.rotor_abs_angle = angle;
motor->motor.feedback.rotor_speed = velocity;
motor->motor.feedback.torque_current = torque;
motor->motor.feedback.temp = (int8_t)motor->lz_feedback.temperature;
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
}
/* Exported functions ------------------------------------------------------- */
/**
* @brief 初始化灵足电机驱动系统
* @return 设备状态码
*/
int8_t MOTOR_LZ_Init(void) {
// 注册灵足电机专用的ID解析器
return BSP_CAN_RegisterIdParser(MOTOR_LZ_IdParser) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
if (MOTOR_LZ_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR;
// 检查是否已注册
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.motor_id == param->motor_id) {
return DEVICE_ERR; // 已注册
}
}
// 检查数量
if (manager->motor_count >= MOTOR_LZ_MAX_MOTORS) return DEVICE_ERR;
// 创建新电机实例
MOTOR_LZ_t *new_motor = (MOTOR_LZ_t*)BSP_Malloc(sizeof(MOTOR_LZ_t));
if (new_motor == NULL) return DEVICE_ERR;
memcpy(&new_motor->param, param, sizeof(MOTOR_LZ_Param_t));
memset(&new_motor->motor, 0, sizeof(MOTOR_t));
memset(&new_motor->lz_feedback, 0, sizeof(MOTOR_LZ_Feedback_t));
memset(&new_motor->motion_param, 0, sizeof(MOTOR_LZ_MotionParam_t));
new_motor->motor.reverse = param->reverse;
// 注册CAN接收ID - 使用解析后的反馈数据ID
// 构建反馈数据的原始扩展ID
// 反馈数据：data2包含电机ID(bit8~15)，target_id是主机ID
uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id);
// 通过ID解析器得到解析后的ID
uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA);
if (BSP_CAN_RegisterId(param->can, parsed_feedback_id, 3) != BSP_OK) {
BSP_Free(new_motor);
return DEVICE_ERR;
}
manager->motors[manager->motor_count] = new_motor;
manager->motor_count++;
return DEVICE_OK;
}
int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LZ_t *motor = manager->motors[i];
if (motor && motor->param.motor_id == param->motor_id) {
// 获取反馈数据 - 使用解析后的ID
uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id);
uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA);
BSP_CAN_Message_t msg;
while (BSP_CAN_GetMessage(param->can, parsed_feedback_id, &msg, 0) == BSP_OK) {
MOTOR_LZ_Decode(motor, &msg);
}
return DEVICE_OK;
}
}
return DEVICE_ERR_NO_DEV;
}
int8_t MOTOR_LZ_UpdateAll(void) {
int8_t ret = DEVICE_OK;
for (int can = 0; can < BSP_CAN_NUM; can++) {
MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager((BSP_CAN_t)can);
if (manager == NULL) continue;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LZ_t *motor = manager->motors[i];
if (motor) {
if (MOTOR_LZ_Update(&motor->param) != DEVICE_OK) {
ret = DEVICE_ERR;
}
}
}
}
return ret;
}
int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param) {
if (param == NULL || motion_param == NULL) return DEVICE_ERR_NULL;
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
if (motor == NULL) return DEVICE_ERR_NO_DEV;
// 自动反向控制
MOTOR_LZ_MotionParam_t send_param = *motion_param;
if (param->reverse) {
send_param.target_angle = -send_param.target_angle;
send_param.target_velocity = -send_param.target_velocity;
send_param.torque = -send_param.torque;
}
memcpy(&motor->motion_param, motion_param, sizeof(MOTOR_LZ_MotionParam_t));
uint16_t raw_torque = MOTOR_LZ_FloatToRaw(send_param.torque, LZ_TORQUE_RANGE_NM);
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_MOTION, raw_torque, param->motor_id);
uint8_t data[8];
uint16_t raw_angle = MOTOR_LZ_FloatToRaw(send_param.target_angle, LZ_ANGLE_RANGE_RAD);
data[0] = (raw_angle >> 8) & 0xFF;
data[1] = raw_angle & 0xFF;
uint16_t raw_velocity = MOTOR_LZ_FloatToRaw(send_param.target_velocity, LZ_VELOCITY_RANGE_RAD_S);
data[2] = (raw_velocity >> 8) & 0xFF;
data[3] = raw_velocity & 0xFF;
uint16_t raw_kp = MOTOR_LZ_FloatToRawPositive(send_param.kp, LZ_KP_MAX);
data[4] = (raw_kp >> 8) & 0xFF;
data[5] = raw_kp & 0xFF;
uint16_t raw_kd = MOTOR_LZ_FloatToRawPositive(send_param.kd, LZ_KD_MAX);
data[6] = (raw_kd >> 8) & 0xFF;
data[7] = raw_kd & 0xFF;
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
}
int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
// 构建扩展ID - 使能命令
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_ENABLE, param->host_id, param->motor_id);
// 数据区清零
uint8_t data[8] = {0};
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
}
int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault) {
if (param == NULL) return DEVICE_ERR_NULL;
// 构建扩展ID - 停止命令
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_DISABLE, param->host_id, param->motor_id);
// 数据区
uint8_t data[8] = {0};
if (clear_fault) {
data[0] = 1; // Byte[0]=1时清故障
}
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
}
int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
// 构建扩展ID - 设置零位命令
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_SET_ZERO, param->host_id, param->motor_id);
// 数据区 - Byte[0]=1
uint8_t data[8] = {1, 0, 0, 0, 0, 0, 0, 0};
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
}
MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param) {
if (param == NULL) return NULL;
MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
if (manager == NULL) return NULL;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LZ_t *motor = manager->motors[i];
if (motor && motor->param.motor_id == param->motor_id) {
return motor;
}
}
return NULL;
}
int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param) {
return MOTOR_LZ_MotionControl(param, &lz_relax_param);
}
int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param) {
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
if (motor) {
motor->motor.header.online = false;
return DEVICE_OK;
}
return DEVICE_ERR_NO_DEV;
}
static MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param) {
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
if (motor && motor->motor.header.online) {
return &motor->lz_feedback;
}
return NULL;
}

212
User/device/motor_lz.h Normal file
View File

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

321
User/device/motor_rm.c Normal file
View File

@ -0,0 +1,321 @@
/*
RM电机驱动
*/
/* Includes ----------------------------------------------------------------- */
#include "motor_rm.h"
#include <stdbool.h>
#include <string.h>
#include "bsp/can.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */
#define GM6020_FB_ID_BASE (0x205)
#define GM6020_CTRL_ID_BASE (0x1ff)
#define GM6020_CTRL_ID_EXTAND (0x2ff)
#define M3508_M2006_FB_ID_BASE (0x201)
#define M3508_M2006_CTRL_ID_BASE (0x200)
#define M3508_M2006_CTRL_ID_EXTAND (0x1ff)
#define M3508_M2006_ID_SETTING_ID (0x700)
#define GM6020_MAX_ABS_LSB (30000)
#define M3508_MAX_ABS_LSB (16384)
#define M2006_MAX_ABS_LSB (10000)
#define MOTOR_TX_BUF_SIZE (8)
#define MOTOR_RX_BUF_SIZE (8)
#define MOTOR_ENC_RES (8192) /* 电机编码器分辨率 */
#define MOTOR_CUR_RES (16384) /* 电机转矩电流分辨率 */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */
static MOTOR_RM_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
/* Private function -------------------------------------------------------- */
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module) {
switch (module) {
case MOTOR_M2006:
case MOTOR_M3508:
if (can_id >= M3508_M2006_FB_ID_BASE && can_id <= M3508_M2006_FB_ID_BASE + 7) {
return can_id - M3508_M2006_FB_ID_BASE;
}
break;
case MOTOR_GM6020:
if (can_id >= GM6020_FB_ID_BASE && can_id <= GM6020_FB_ID_BASE + 6) {
return can_id - GM6020_FB_ID_BASE + 4;
}
break;
default:
break;
}
return DEVICE_ERR;
}
static float MOTOR_RM_GetRatio(MOTOR_RM_Module_t module) {
switch (module) {
case MOTOR_M2006: return 36.0f;
case MOTOR_M3508: return 3591.0f / 187.0f;
case MOTOR_GM6020: return 1.0f;
default: return 1.0f;
}
}
static int16_t MOTOR_RM_GetLSB(MOTOR_RM_Module_t module) {
switch (module) {
case MOTOR_M2006: return M2006_MAX_ABS_LSB;
case MOTOR_M3508: return M3508_MAX_ABS_LSB;
case MOTOR_GM6020: return GM6020_MAX_ABS_LSB;
default: return DEVICE_ERR;
}
}
static MOTOR_RM_CANManager_t* MOTOR_RM_GetCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return NULL;
return can_managers[can];
}
static int8_t MOTOR_RM_CreateCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
if (can_managers[can] != NULL) return DEVICE_OK;
can_managers[can] = (MOTOR_RM_CANManager_t*)BSP_Malloc(sizeof(MOTOR_RM_CANManager_t));
if (can_managers[can] == NULL) return DEVICE_ERR;
memset(can_managers[can], 0, sizeof(MOTOR_RM_CANManager_t));
can_managers[can]->can = can;
return DEVICE_OK;
}
static void Motor_RM_Decode(MOTOR_RM_t *motor, BSP_CAN_Message_t *msg) {
uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]);
int16_t raw_speed = (int16_t)((msg->data[2] << 8) | msg->data[3]);
int16_t raw_current = (int16_t)((msg->data[4] << 8) | msg->data[5]);
int16_t lsb = MOTOR_RM_GetLSB(motor->param.module);
float ratio = MOTOR_RM_GetRatio(motor->param.module);
float rotor_angle = raw_angle / (float)MOTOR_ENC_RES * M_2PI;
float rotor_speed = raw_speed;
float torque_current = raw_current * lsb / (float)MOTOR_CUR_RES;
if (motor->param.gear) {
// 多圈累加
int32_t delta = (int32_t)raw_angle - (int32_t)motor->last_raw_angle;
if (delta > (MOTOR_ENC_RES / 2)) {
motor->gearbox_round_count--;
} else if (delta < -(MOTOR_ENC_RES / 2)) {
motor->gearbox_round_count++;
}
motor->last_raw_angle = raw_angle;
float single_turn = rotor_angle;
motor->gearbox_total_angle = (motor->gearbox_round_count * M_2PI + single_turn) / ratio;
// 输出轴多圈绝对值
motor->feedback.rotor_abs_angle = motor->gearbox_total_angle;
motor->feedback.rotor_speed = rotor_speed / ratio;
motor->feedback.torque_current = torque_current * ratio;
} else {
// 非gear模式，直接用转子单圈
motor->gearbox_round_count = 0;
motor->last_raw_angle = raw_angle;
motor->gearbox_total_angle = 0.0f;
motor->feedback.rotor_abs_angle = rotor_angle;
motor->feedback.rotor_speed = rotor_speed;
motor->feedback.torque_current = torque_current;
}
while (motor->feedback.rotor_abs_angle < 0) {
motor->feedback.rotor_abs_angle += M_2PI;
}
while (motor->feedback.rotor_abs_angle >= M_2PI) {
motor->feedback.rotor_abs_angle -= M_2PI;
}
if (motor->motor.reverse) {
motor->feedback.rotor_abs_angle = M_2PI - motor->feedback.rotor_abs_angle;
motor->feedback.rotor_speed = -motor->feedback.rotor_speed;
motor->feedback.torque_current = -motor->feedback.torque_current;
}
motor->feedback.temp = msg->data[6];
}
/* Exported functions ------------------------------------------------------- */
int8_t MOTOR_RM_Register(MOTOR_RM_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
if (MOTOR_RM_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR;
// 检查是否已注册
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
return DEVICE_ERR_INITED;
}
}
// 检查数量
if (manager->motor_count >= MOTOR_RM_MAX_MOTORS) return DEVICE_ERR;
// 创建新电机实例
MOTOR_RM_t *new_motor = (MOTOR_RM_t*)BSP_Malloc(sizeof(MOTOR_RM_t));
if (new_motor == NULL) return DEVICE_ERR;
memcpy(&new_motor->param, param, sizeof(MOTOR_RM_Param_t));
memset(&new_motor->motor, 0, sizeof(MOTOR_t));
new_motor->motor.reverse = param->reverse;
// 注册CAN接收ID
if (BSP_CAN_RegisterId(param->can, param->id, 3) != BSP_OK) {
BSP_Free(new_motor);
return DEVICE_ERR;
}
manager->motors[manager->motor_count] = new_motor;
manager->motor_count++;
return DEVICE_OK;
}
int8_t MOTOR_RM_Update(MOTOR_RM_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_RM_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
BSP_CAN_Message_t rx_msg;
if (BSP_CAN_GetMessage(param->can, param->id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
uint64_t now_time = BSP_TIME_Get();
if (now_time - motor->motor.header.last_online_time > 1000) {
motor->motor.header.online = false;
return DEVICE_ERR_NO_DEV;
}
return DEVICE_ERR;
}
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
Motor_RM_Decode(motor, &rx_msg);
motor->motor.feedback = motor->feedback;
return DEVICE_OK;
}
}
return DEVICE_ERR_NO_DEV;
}
int8_t MOTOR_RM_UpdateAll(void) {
int8_t ret = DEVICE_OK;
for (int can = 0; can < BSP_CAN_NUM; can++) {
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager((BSP_CAN_t)can);
if (manager == NULL) continue;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_RM_t *motor = manager->motors[i];
if (motor != NULL) {
if (MOTOR_RM_Update(&motor->param) != DEVICE_OK) {
ret = DEVICE_ERR;
}
}
}
}
return ret;
}
int8_t MOTOR_RM_SetOutput(MOTOR_RM_Param_t *param, float value) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
if (value > 1.0f) value = 1.0f;
if (value < -1.0f) value = -1.0f;
if (param->reverse){
value = -value;
}
MOTOR_RM_t *motor = MOTOR_RM_GetMotor(param);
if (motor == NULL) return DEVICE_ERR_NO_DEV;
int8_t logical_index = MOTOR_RM_GetLogicalIndex(param->id, param->module);
if (logical_index < 0) return DEVICE_ERR;
MOTOR_RM_MsgOutput_t *output_msg = &manager->output_msg;
int16_t output_value = (int16_t)(value * (float)MOTOR_RM_GetLSB(param->module));
output_msg->output[logical_index] = output_value;
return DEVICE_OK;
}
int8_t MOTOR_RM_Ctrl(MOTOR_RM_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
MOTOR_RM_MsgOutput_t *output_msg = &manager->output_msg;
BSP_CAN_StdDataFrame_t tx_frame;
uint16_t id = param->id;
switch (id) {
case M3508_M2006_FB_ID_BASE:
case M3508_M2006_FB_ID_BASE+1:
case M3508_M2006_FB_ID_BASE+2:
case M3508_M2006_FB_ID_BASE+3:
tx_frame.id = M3508_M2006_CTRL_ID_BASE;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
for (int i = 0; i < 4; i++) {
tx_frame.data[i*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF);
tx_frame.data[i*2+1] = (uint8_t)(output_msg->output[i] & 0xFF);
}
break;
case M3508_M2006_FB_ID_BASE+4:
case M3508_M2006_FB_ID_BASE+5:
case M3508_M2006_FB_ID_BASE+6:
case M3508_M2006_FB_ID_BASE+7:
tx_frame.id = M3508_M2006_CTRL_ID_EXTAND;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
for (int i = 4; i < 8; i++) {
tx_frame.data[(i-4)*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF);
tx_frame.data[(i-4)*2+1] = (uint8_t)(output_msg->output[i] & 0xFF);
}
break;
case GM6020_FB_ID_BASE+4:
case GM6020_FB_ID_BASE+5:
case GM6020_FB_ID_BASE+6:
tx_frame.id = GM6020_CTRL_ID_EXTAND;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
for (int i = 8; i < 11; i++) {
tx_frame.data[(i-8)*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF);
tx_frame.data[(i-8)*2+1] = (uint8_t)(output_msg->output[i] & 0xFF);
}
tx_frame.data[6] = 0;
tx_frame.data[7] = 0;
break;
default:
return DEVICE_ERR;
}
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
MOTOR_RM_t* MOTOR_RM_GetMotor(MOTOR_RM_Param_t *param) {
if (param == NULL) return NULL;
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
if (manager == NULL) return NULL;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_RM_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
return motor;
}
}
return NULL;
}
int8_t MOTOR_RM_Relax(MOTOR_RM_Param_t *param) {
return MOTOR_RM_SetOutput(param, 0.0f);
}
int8_t MOTOR_RM_Offine(MOTOR_RM_Param_t *param) {
MOTOR_RM_t *motor = MOTOR_RM_GetMotor(param);
if (motor) {
motor->motor.header.online = false;
return DEVICE_OK;
}
return DEVICE_ERR_NO_DEV;
}

132
User/device/motor_rm.h Normal file
View File

@ -0,0 +1,132 @@
#pragma once
#include "motor.h"
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "device/motor.h"
#include "bsp/can.h"
/* Exported constants ------------------------------------------------------- */
#define MOTOR_RM_MAX_MOTORS 11
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
MOTOR_M2006,
MOTOR_M3508,
MOTOR_GM6020,
} MOTOR_RM_Module_t;
/*一个can最多控制11个电机*/
typedef union {
int16_t output[MOTOR_RM_MAX_MOTORS];
struct {
int16_t m3508_m2006_id201;
int16_t m3508_m2006_id202;
int16_t m3508_m2006_id203;
int16_t m3508_m2006_id204;
int16_t m3508_m2006_gm6020_id205;
int16_t m3508_m2006_gm6020_id206;
int16_t m3508_m2006_gm6020_id207;
int16_t m3508_m2006_gm6020_id208;
int16_t gm6020_id209;
int16_t gm6020_id20A;
int16_t gm6020_id20B;
} named;
} MOTOR_RM_MsgOutput_t;
/*每个电机需要的参数*/
typedef struct {
BSP_CAN_t can;
uint16_t id;
MOTOR_RM_Module_t module;
bool reverse;
bool gear;
} MOTOR_RM_Param_t;
typedef MOTOR_Feedback_t MOTOR_RM_Feedback_t;
typedef struct {
MOTOR_RM_Param_t param;
MOTOR_RM_Feedback_t feedback;
MOTOR_t motor;
// 多圈相关变量，仅gear模式下有效
uint16_t last_raw_angle;
int32_t gearbox_round_count;
float gearbox_total_angle;
} MOTOR_RM_t;
/*CAN管理器，管理一个CAN总线上所有的电机*/
typedef struct {
BSP_CAN_t can;
MOTOR_RM_MsgOutput_t output_msg;
MOTOR_RM_t *motors[MOTOR_RM_MAX_MOTORS];
uint8_t motor_count;
} MOTOR_RM_CANManager_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 注册一个RM电机
* @param param 电机参数
* @return
*/
int8_t MOTOR_RM_Register(MOTOR_RM_Param_t *param);
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return
*/
int8_t MOTOR_RM_Update(MOTOR_RM_Param_t *param);
/**
* @brief 设置一个电机的输出
* @param param 电机参数
* @param value 输出值，范围[-1.0, 1.0]
* @return
*/
int8_t MOTOR_RM_SetOutput(MOTOR_RM_Param_t *param, float value);
/**
* @brief 发送控制命令到电机，注意一个CAN可以控制多个电机，所以只需要发送一次即可
* @param param 电机参数
* @return
*/
int8_t MOTOR_RM_Ctrl(MOTOR_RM_Param_t *param);
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return
*/
MOTOR_RM_t* MOTOR_RM_GetMotor(MOTOR_RM_Param_t *param);
/**
* @brief 使电机松弛（设置输出为0）
* @param param
* @return
*/
int8_t MOTOR_RM_Relax(MOTOR_RM_Param_t *param);
/**
* @brief 使电机离线（设置在线状态为false）
* @param param
* @return
*/
int8_t MOTOR_RM_Offine(MOTOR_RM_Param_t *param);
/**
* @brief
* @param
* @return
*/
int8_t MOTOR_RM_UpdateAll(void);
#ifdef __cplusplus
}
#endif

158
User/device/supercap.c Normal file
View File

@ -0,0 +1,158 @@
#include "device/supercap.h"
/* 全局变量 */
CAN_SuperCapRXDataTypeDef CAN_SuperCapRXData = {0};
uint8_t PowerOffset =0;
uint32_t LastCapTick = 0; //上一次收到超电信号的时间戳
uint32_t NowCapTick = 0; //现在收到超电信号的时间戳
uint32_t chassis_energy_in_gamming =0;
/* 静态变量 - 用于CAN接收管理 */
static bool supercap_inited = false;
static osMessageQueueId_t supercap_rx_queue = NULL;
/**
* @brief 获取超级电容在线状态，1在线，0离线
*/
uint8_t get_supercap_online_state(void){
NowCapTick = HAL_GetTick(); //更新时间戳
uint32_t DeltaCapTick = 0;
DeltaCapTick = NowCapTick - LastCapTick; //计算时间差
// if(get_game_progress() == 4){
// //比赛开始的时候，开始统计消耗的能量
chassis_energy_in_gamming += CAN_SuperCapRXData.BatPower * DeltaCapTick *0.001f;
// 因为STM32的系统定时器是1ms的周期，所以*0.001，单位化为秒（S），能量单位才是焦耳（J）
// }
if(DeltaCapTick > 1000){
//如果时间差大于1s，说明超电信号丢失，返回超电离线的标志
return 0;
}else {
//如果时间差小于1s，说明超电信号正常，返回超电在线的标志
return 1;
}
}
/**
* @brief 获取根据超级电容功率统计的底盘消耗能量，单位：焦耳（J）
*/
uint32_t get_chassis_energy_from_supercap(void){
return chassis_energy_in_gamming;
}
/******************超级电容数据从CAN传到结构体******************/
int8_t SuperCap_Init(void)
{
if (supercap_inited) {
return DEVICE_OK; // 已经初始化过
}
if (BSP_CAN_RegisterId( SUPERCAP_CAN , SUPERCAP_RX_ID, 3) != BSP_OK) {
return DEVICE_ERR;
}
supercap_inited = true;
return DEVICE_OK;
}
int8_t SuperCap_Update(void)
{
if (!supercap_inited) {
return DEVICE_ERR;
}
BSP_CAN_Message_t rx_msg;
// 从CAN队列获取数据
if (BSP_CAN_GetMessage( SUPERCAP_CAN , SUPERCAP_RX_ID, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
// 处理接收到的数据
transfer_SuperCap_measure(rx_msg.data);
return DEVICE_OK;
}
return DEVICE_ERR; // 没有新数据
}
void transfer_SuperCap_measure(uint8_t *data)
{
LastCapTick = HAL_GetTick();
CAN_SuperCapRXData.SuperCapReady = (SuperCap_Status_t)data[0];
CAN_SuperCapRXData.SuperCapState = (SuperCap_Status_t)data[1];
CAN_SuperCapRXData.SuperCapEnergy = data[2];
CAN_SuperCapRXData.ChassisPower = data[3] << 1; //左移一位是为了扩大范围，超电发出来的的时候右移了一位
CAN_SuperCapRXData.BatVoltage = data[4];
CAN_SuperCapRXData.BatPower = data[5];
}
/**
* @brief 获取超级电容的运行状态，具体查看枚举SuperCapStateEnum
* @retval none
*/
SuperCapStateEnum get_supercap_state(void){
return (SuperCapStateEnum)CAN_SuperCapRXData.SuperCapState;
}
/**
* @brief 获取超级电容读到的电池电压，单位伏（V）
* @retval none
*/
float get_battery_voltage_from_supercap(void){
return (float)CAN_SuperCapRXData.BatVoltage * 0.1f;
}
/**
* @brief 获取超级电容可用能量，范围：0-100%
* @retval none
*/
uint8_t get_supercap_energy(void){
return CAN_SuperCapRXData.SuperCapEnergy;
}
/**
* @brief 设置超级电容功率补偿
* @retval none
*/
void set_supercap_power_offset(uint8_t offset){
PowerOffset = offset;
}
/**
* @brief 发送超级电容数据
* @param[in] Enable: 超级电容使能
* @param[in] Charge: 超级电容充电，在PLUS版本中无效
* @param[in] PowerLimit: 裁判系统功率限制
* @param[in] Chargepower: 超级电容充电功率，在PLUS版本中无效
* @retval none
*/
int8_t CAN_TX_SuperCapData(CAN_SuperCapTXDataTypeDef * TX_Temp)
{
if (TX_Temp == NULL) return DEVICE_ERR_NULL;
BSP_FDCAN_StdDataFrame_t tx_frame;
tx_frame.id = SUPERCAP_TX_ID;
tx_frame.dlc = 0x08;
tx_frame.data[0] = TX_Temp-> Enable;
tx_frame.data[1] = TX_Temp-> Charge;//PLUS disabled
tx_frame.data[2] = TX_Temp-> Powerlimit - PowerOffset;
tx_frame.data[3] = TX_Temp-> ChargePower;//PLUS disabled
tx_frame.data[4] = 0;
tx_frame.data[5] = 0;
tx_frame.data[6] = 0;
tx_frame.data[7] = 0;
return BSP_CAN_TransmitStdDataFrame( SUPERCAP_CAN , &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}

102
User/device/supercap.h Normal file
View File

@ -0,0 +1,102 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "bsp/can.h"
#include "device/device.h"
//#include "referee.h"
#define SUPERCAP_CAN BSP_CAN_1
//#define SUPERCAP_CAN BSP_CAN_2
#define SUPERCAP_TX_ID 0x001 //C板发给超级电容的ID
#define SUPERCAP_RX_ID 0x100 //超级电容发给C板的ID
//超级电容的状态标志位，超级电容运行或者保护的具体状态反馈
typedef enum
{
DISCHARGE =0 , //放电状态
CHARGE =1, //充电状态
WAIT =2, //待机状态
SOFTSTART_PROTECTION =3,//处于软起动状态
OVER_LOAD_PROTECTION = 4, //超电过载保护状态
BAT_OVER_VOLTAGE_PROTECTION =5, //过压保护状态
BAT_UNDER_VOLTAGE_PROTECTION =6, //电池欠压保护，电池要没电了，换电池
CAP_UNDER_VOLTAGE_PROTECTION =7, //超级电容欠压保护，超级电容用完电了，要充一会才能用
OVER_TEMPERATURE_PROTECTION =8, //过温保护，太热了
BOOM = 9, //超电爆炸了
}SuperCapStateEnum;
//超级电容准备状态，用于判断超级电容是否可以使用
typedef enum
{
SUPERCAP_STATUS_OFFLINE =0 ,
SUPERCAP_STATUS_RUNNING =1,
}SuperCap_Status_t;
// 发送给超级电容的数据
typedef struct {
FunctionalState Enable ; //超级电容使能。1使能，0失能
SuperCapStateEnum Charge ; //V1.1超级电容充电。1充电，0放电，在PLUS版本中，此标志位无效，超电的充放电是自动的
uint8_t Powerlimit; //裁判系统功率限制
uint8_t ChargePower; //V1.1超级电容充电功率，在PLUS版本中，此参数，超电的充电功率随着底盘功率变化
}CAN_SuperCapTXDataTypeDef;
// 从超级电容接收到的数据
typedef struct {
uint8_t SuperCapEnergy;//超级电容可用能量：0-100%
uint16_t ChassisPower; //底盘功率，0-512，由于传输的时候为了扩大量程右移了一位，所以接收的时候需要左移还原（丢精度）。
SuperCap_Status_t SuperCapReady;//超级电容【可用标志】：1为可用，0为不可用
SuperCapStateEnum SuperCapState;//超级电容【状态标志】：各个状态对应的状态码查看E_SuperCapState枚举。
uint8_t BatVoltage; //通过超级电容监控电池电压*10，
uint8_t BatPower;
}CAN_SuperCapRXDataTypeDef;
extern CAN_SuperCapRXDataTypeDef CAN_SuperCapRXData;
void set_supercap_power_offset(uint8_t offset);
// 以下函数是超电控制所需要调用的函数
void transfer_SuperCap_measure( uint8_t *data);
int8_t CAN_TX_SuperCapData(CAN_SuperCapTXDataTypeDef * TX_Temp);
/**
* @brief 获取超级电容在线状态，1在线，0离线
*/
uint8_t get_supercap_online_state(void);
/**
* @brief 获取超级电容的运行状态，具体查看枚举SuperCapStateEnum
*/
SuperCapStateEnum get_supercap_state(void);
/**
* @brief 获取超级电容读到的电池电压，单位伏（V）
*/
float get_battery_voltage_from_supercap(void);
/**
* @brief 获取超级电容可用能量，范围：0-100%
*/
uint8_t get_supercap_energy(void);
/**
* @brief 获取根据超级电容功率统计的底盘消耗能量，单位：焦耳（J）
*/
uint32_t get_chassis_energy_from_supercap(void);
int8_t SuperCap_Init(void);
int8_t SuperCap_Update(void);
#ifdef __cplusplus
}
#endif /*SUPERCAP_H*/

507
User/module/arm.cpp Normal file
View File

@ -0,0 +1,507 @@
// /* Includes ----------------------------------------------------------------- */
// #include <math.h>
// #include <stdint.h>
// #include <string.h>
// #include "arm.h"
// #include "bsp/mm.h"
// #include "bsp/time.h"
// #include "component/user_math.h"
// #include "device/motor_dm.h"
// #include "device/motor_lz.h"
// /* Private typedef ---------------------------------------------------------- */
// /* Private define ----------------------------------------------------------- */
// /* Private macro ------------------------------------------------------------ */
// /* Private variables -------------------------------------------------------- */
// /* Private function -------------------------------------------------------- */
// // ============================================================================
// // Joint（关节）级别操作 - 面向对象接口
// // ============================================================================
// /**
// * @brief 初始化单个关节
// */
// int8_t Joint_Init(Joint_t *joint, uint8_t id, Joint_MotorType_t motor_type,
// const Arm6dof_DHParams_t *dh_params, const KPID_Params_t *pid_params, float freq) {
// if (joint == NULL || dh_params == NULL || pid_params == NULL) {
// return -1;
// }
// joint->id = id;
// joint->motor_type = motor_type;
// memcpy(&joint->dh_params, dh_params, sizeof(Arm6dof_DHParams_t));
// joint->q_offset = 0.0f;
// PID_Init(&joint->pid, KPID_MODE_CALC_D, freq, pid_params);
// joint->state.current_angle = 0.0f;
// joint->state.current_velocity = 0.0f;
// joint->state.current_torque = 0.0f;
// joint->state.target_angle = 0.0f;
// joint->state.target_velocity = 0.0f;
// joint->state.online = false;
// return 0;
// }
// /**
// * @brief 更新关节状态
// */
// int8_t Joint_Update(Joint_t *joint) {
// if (joint == NULL) return -1;
// if (joint->motor_type == JOINT_MOTOR_TYPE_LZ) {
// joint->state.current_angle = joint->motor.lz_motor.motor.feedback.rotor_abs_angle - joint->q_offset;
// joint->state.current_velocity = joint->motor.lz_motor.motor.feedback.rotor_speed;
// joint->state.current_torque = joint->motor.lz_motor.motor.feedback.torque_current;
// joint->state.online = joint->motor.lz_motor.motor.header.online;
// } else {
// joint->state.current_angle = joint->motor.dm_motor.motor.feedback.rotor_abs_angle - joint->q_offset;
// joint->state.current_velocity = joint->motor.dm_motor.motor.feedback.rotor_speed;
// joint->state.current_torque = joint->motor.dm_motor.motor.feedback.torque_current;
// joint->state.online = joint->motor.dm_motor.motor.header.online;
// }
// return 0;
// }
// /**
// * @brief 关节位置控制
// */
// int8_t Joint_PositionControl(Joint_t *joint, float target_angle, float dt) {
// if (joint == NULL) return -1;
// if (target_angle < joint->dh_params.qmin || target_angle > joint->dh_params.qmax) {
// return -1;
// }
// joint->state.target_angle = target_angle;
// float pid_output = PID_Calc(&joint->pid, target_angle, joint->state.current_angle, 0, dt);
// if (joint->motor_type == JOINT_MOTOR_TYPE_LZ) {
// joint->output.lz_output.target_angle = target_angle + joint->q_offset;
// joint->output.lz_output.target_velocity = 0.0f;
// joint->output.lz_output.kp = 10.0f;
// joint->output.lz_output.kd = 0.5f;
// joint->output.lz_output.torque = pid_output;
// MOTOR_LZ_MotionControl(&joint->motor_params.lz_params, &joint->output.lz_output);
// } else {
// joint->output.dm_output.angle = target_angle + joint->q_offset;
// joint->output.dm_output.velocity = 0.0f;
// joint->output.dm_output.kp = 50.0f;
// joint->output.dm_output.kd = 3.0f;
// joint->output.dm_output.torque = pid_output;
// MOTOR_DM_MITCtrl(&joint->motor_params.dm_params, &joint->output.dm_output);
// }
// return 0;
// }
// /**
// * @brief 关节松弛
// */
// int8_t Joint_Relax(Joint_t *joint) {
// if (joint == NULL) return -1;
// if (joint->motor_type == JOINT_MOTOR_TYPE_LZ) {
// MOTOR_LZ_Relax(&joint->motor_params.lz_params);
// } else {
// MOTOR_DM_Relax(&joint->motor_params.dm_params);
// }
// return 0;
// }
// /**
// * @brief 检查关节是否到达目标
// */
// bool Joint_IsReached(const Joint_t *joint, float tolerance) {
// if (joint == NULL) return false;
// return fabsf(joint->state.target_angle - joint->state.current_angle) < tolerance;
// }
// // ============================================================================
// // Arm（机械臂）级别操作
// // ============================================================================
// /* Exported functions ------------------------------------------------------- */
// int8_t Arm_Init(Arm_t *arm, Arm_Params_t *arm_param, float freq) {
// if (arm == NULL||arm_param == NULL) {
// return -1;
// }
// arm->param = arm_param;
// BSP_CAN_Init();
// MOTOR_LZ_Init();
// // 初始化控制参数
// arm->control.mode = ARM_MODE_IDLE;
// arm->control.state = ARM_STATE_STOPPED;
// arm->control.position_tolerance = 0.02f; // 关节角容差：0.02rad ≈ 1.15度
// arm->control.velocity_tolerance = 0.01f; // 速度容差：0.01rad/s
// arm->control.enable = false;
// return 0;
// }
// int8_t Arm_Updata(Arm_t *arm) {
// if (arm == NULL) {
// return -1;
// }
// MOTOR_LZ_Update(&arm->param->jointMotor1_params);
// MOTOR_LZ_Update(&arm->param->jointMotor2_params);
// MOTOR_LZ_Update(&arm->param->jointMotor3_params);
// MOTOR_DM_Update(&arm->param->jointMotor4_params);
// MOTOR_DM_Update(&arm->param->jointMotor5_params);
// MOTOR_DM_Update(&arm->param->jointMotor6_params);
// // 同步电机实例数据到actuator
// MOTOR_LZ_t *motor1 = MOTOR_LZ_GetMotor(&arm->param->jointMotor1_params);
// if (motor1 != NULL) {
// memcpy(&arm->actuator.jointMotor1, motor1, sizeof(MOTOR_LZ_t));
// }
// MOTOR_LZ_t *motor2 = MOTOR_LZ_GetMotor(&arm->param->jointMotor2_params);
// if (motor2 != NULL) {
// memcpy(&arm->actuator.jointMotor2, motor2, sizeof(MOTOR_LZ_t));
// }
// MOTOR_LZ_t *motor3 = MOTOR_LZ_GetMotor(&arm->param->jointMotor3_params);
// if (motor3 != NULL) {
// memcpy(&arm->actuator.jointMotor3, motor3, sizeof(MOTOR_LZ_t));
// }
// MOTOR_DM_t *motor4 = MOTOR_DM_GetMotor(&arm->param->jointMotor4_params);
// if (motor4 != NULL) {
// memcpy(&arm->actuator.jointMotor4, motor4, sizeof(MOTOR_DM_t));
// }
// MOTOR_DM_t *motor5 = MOTOR_DM_GetMotor(&arm->param->jointMotor5_params);
// if (motor5 != NULL) {
// memcpy(&arm->actuator.jointMotor5, motor5, sizeof(MOTOR_DM_t));
// }
// MOTOR_DM_t *motor6 = MOTOR_DM_GetMotor(&arm->param->jointMotor6_params);
// if (motor6 != NULL) {
// memcpy(&arm->actuator.jointMotor6, motor6, sizeof(MOTOR_DM_t));
// }
// // 从电机反馈更新当前关节角度
// arm->stateVariable.currentJointAngles.q[0] =
// arm->actuator.jointMotor1.motor.feedback.rotor_abs_angle - arm->param->q_offset[0];
// arm->stateVariable.currentJointAngles.q[1] =
// arm->actuator.jointMotor2.motor.feedback.rotor_abs_angle - arm->param->q_offset[1];
// arm->stateVariable.currentJointAngles.q[2] =
// arm->actuator.jointMotor3.motor.feedback.rotor_abs_angle - arm->param->q_offset[2];
// arm->stateVariable.currentJointAngles.q[3] =
// arm->actuator.jointMotor4.motor.feedback.rotor_abs_angle - arm->param->q_offset[3];
// arm->stateVariable.currentJointAngles.q[4] =
// arm->actuator.jointMotor5.motor.feedback.rotor_abs_angle - arm->param->q_offset[4];
// arm->stateVariable.currentJointAngles.q[5] =
// arm->actuator.jointMotor6.motor.feedback.rotor_abs_angle - arm->param->q_offset[5];
// // 正运动学：计算当前末端位姿
// Arm6dof_ForwardKinematics(&arm->stateVariable.currentJointAngles,
// &arm->stateVariable.currentEndPose);
// return 0;
// }
// int8_t Arm_Ctrl(Arm_t *arm) {
// if (arm == NULL) {
// return -1;
// }
// arm->timer.now = BSP_TIME_Get_us() / 1000000.0f;
// arm->timer.dt = (BSP_TIME_Get_us() - arm->timer.last_wakeup) / 1000000.0f;
// arm->timer.last_wakeup = BSP_TIME_Get_us();
// // 如果未使能，松弛所有电机
// if (!arm->control.enable) {
// MOTOR_LZ_Relax(&arm->param->jointMotor1_params);
// MOTOR_LZ_Relax(&arm->param->jointMotor2_params);
// MOTOR_LZ_Relax(&arm->param->jointMotor3_params);
// MOTOR_DM_Relax(&arm->param->jointMotor4_params);
// MOTOR_DM_Relax(&arm->param->jointMotor5_params);
// MOTOR_DM_Relax(&arm->param->jointMotor6_params);
// arm->control.state = ARM_STATE_STOPPED;
// return 0;
// }
// // 根据控制模式执行不同的控制策略
// switch (arm->control.mode) {
// case ARM_MODE_IDLE:
// // 空闲模式：电机松弛
// MOTOR_LZ_Relax(&arm->param->jointMotor1_params);
// MOTOR_LZ_Relax(&arm->param->jointMotor2_params);
// MOTOR_LZ_Relax(&arm->param->jointMotor3_params);
// MOTOR_DM_Relax(&arm->param->jointMotor4_params);
// MOTOR_DM_Relax(&arm->param->jointMotor5_params);
// MOTOR_DM_Relax(&arm->param->jointMotor6_params);
// arm->control.state = ARM_STATE_STOPPED;
// break;
// case ARM_MODE_JOINT_POSITION:
// case ARM_MODE_CARTESIAN_POSITION:
// // 关节位置控制（关节空间和笛卡尔空间最终都转为关节角控制）
// {
// // 检查是否到达目标
// bool reached = true;
// float max_error = 0.0f;
// for (int i = 0; i < 6; i++) {
// float error = arm->stateVariable.targetJointAngles.q[i] -
// arm->stateVariable.currentJointAngles.q[i];
// if (fabsf(error) > arm->control.position_tolerance) {
// reached = false;
// }
// if (fabsf(error) > max_error) {
// max_error = fabsf(error);
// }
// }
// if (reached) {
// arm->control.state = ARM_STATE_REACHED;
// } else {
// arm->control.state = ARM_STATE_MOVING;
// }
// // PID位置控制（关节1-3使用LZ电机）
// for (int i = 0; i < 3; i++) {
// float pid_output = PID_Calc(&arm->controller.joint_pid[i],
// arm->stateVariable.targetJointAngles.q[i],
// arm->stateVariable.currentJointAngles.q[i],
// 0,
// arm->timer.dt
// );
// MOTOR_LZ_MotionParam_t* output = NULL;
// if (i == 0) output = &arm->output.jointMotor1_output;
// else if (i == 1) output = &arm->output.jointMotor2_output;
// else output = &arm->output.jointMotor3_output;
// output->target_angle = arm->stateVariable.targetJointAngles.q[i] + arm->param->q_offset[i];
// output->target_velocity = 0.0f; // 位置模式速度由PID控制
// output->kp = 10.0f; // 可调参数
// output->kd = 0.5f;
// output->torque = pid_output;
// }
// // MIT模式控制（关节4-6使用DM电机）
// for (int i = 3; i < 6; i++) {
// float pid_output = PID_Calc(&arm->controller.joint_pid[i],
// arm->stateVariable.targetJointAngles.q[i],
// arm->stateVariable.currentJointAngles.q[i],
// 0,
// arm->timer.dt
// );
// MOTOR_MIT_Output_t* output = NULL;
// if (i == 3) output = &arm->output.jointMotor4_output;
// else if (i == 4) output = &arm->output.jointMotor5_output;
// else output = &arm->output.jointMotor6_output;
// output->angle = arm->stateVariable.targetJointAngles.q[i] + arm->param->q_offset[i];
// output->velocity = 0.0f;
// output->kp = 50.0f; // 位置刚度（提供阻抗和稳定性）
// output->kd = 3.0f; // 速度阻尼（抑制振荡）
// output->torque = pid_output; // PID输出作为前馈力矩（主要驱动力）
// }
// // 发送控制命令
// MOTOR_LZ_MotionControl(&arm->param->jointMotor1_params, &arm->output.jointMotor1_output);
// MOTOR_LZ_MotionControl(&arm->param->jointMotor2_params, &arm->output.jointMotor2_output);
// MOTOR_LZ_MotionControl(&arm->param->jointMotor3_params, &arm->output.jointMotor3_output);
// MOTOR_DM_MITCtrl(&arm->param->jointMotor4_params, &arm->output.jointMotor4_output);
// MOTOR_DM_MITCtrl(&arm->param->jointMotor5_params, &arm->output.jointMotor5_output);
// MOTOR_DM_MITCtrl(&arm->param->jointMotor6_params, &arm->output.jointMotor6_output);
// }
// break;
// case ARM_MODE_TRAJECTORY:
// // 轨迹跟踪模式（待实现）
// arm->control.state = ARM_STATE_ERROR;
// break;
// case ARM_MODE_TEACH:
// // 示教模式（待实现）
// arm->control.state = ARM_STATE_ERROR;
// break;
// default:
// arm->control.state = ARM_STATE_ERROR;
// break;
// }
// return 0;
// }
// /**
// * @brief 逆运动学求解：根据目标末端位姿计算关节角度
// * @note 此函数计算量大，不要在控制循环中频繁调用
// */
// int8_t Arm_SolveIK(Arm_t *arm, const Arm6dof_Pose_t* target_pose, Arm6dof_JointAngles_t* result_angles) {
// if (arm == NULL || target_pose == NULL || result_angles == NULL) {
// return -1;
// }
// // 验证目标位姿的有效性
// if (isnan(target_pose->x) || isnan(target_pose->y) || isnan(target_pose->z) ||
// isnan(target_pose->roll) || isnan(target_pose->pitch) || isnan(target_pose->yaw) ||
// isinf(target_pose->x) || isinf(target_pose->y) || isinf(target_pose->z) ||
// isinf(target_pose->roll) || isinf(target_pose->pitch) || isinf(target_pose->yaw)) {
// return -1; // 无效的目标位姿
// }
// // 使用当前关节角度作为初始猜测值（如果已初始化）
// Arm6dof_JointAngles_t initial_guess;
// // 检查currentJointAngles是否已初始化（不全为0）
// bool has_init = false;
// for (int i = 0; i < 6; i++) {
// if (arm->stateVariable.currentJointAngles.q[i] != 0.0f) {
// has_init = true;
// break;
// }
// }
// if (has_init) {
// // 使用当前角度作为初始猜测
// memcpy(&initial_guess, &arm->stateVariable.currentJointAngles, sizeof(Arm6dof_JointAngles_t));
// } else {
// // 使用零位作为初始猜测（机械臂竖直向上）
// memset(&initial_guess, 0, sizeof(Arm6dof_JointAngles_t));
// }
// // 调用逆运动学求解，容限1mm，最大迭代10次（减少计算量）
// int ret = Arm6dof_InverseKinematics(target_pose, &initial_guess, result_angles, 1.0f, 10);
// if (ret == 0) {
// // 求解成功，更新到inverseKineJointAngles
// memcpy(&arm->stateVariable.inverseKineJointAngles, result_angles, sizeof(Arm6dof_JointAngles_t));
// }
// return ret;
// }
// // ============================================================================
// // 控制API函数实现
// // ============================================================================
// /**
// * @brief 设置控制模式
// */
// int8_t Arm_SetMode(Arm_t *arm, Arm_ControlMode_t mode) {
// if (arm == NULL) {
// return -1;
// }
// arm->control.mode = mode;
// arm->control.state = ARM_STATE_STOPPED;
// return 0;
// }
// /**
// * @brief 使能/失能机械臂
// */
// int8_t Arm_Enable(Arm_t *arm, bool enable) {
// if (arm == NULL) {
// return -1;
// }
// arm->control.enable = enable;
// if (!enable) {
// // 失能时切换到空闲模式
// arm->control.mode = ARM_MODE_IDLE;
// arm->control.state = ARM_STATE_STOPPED;
// }
// return 0;
// }
// /**
// * @brief 关节空间位置控制
// */
// int8_t Arm_MoveJoint(Arm_t *arm, const Arm6dof_JointAngles_t* target_angles) {
// if (arm == NULL || target_angles == NULL) {
// return -1;
// }
// // 检查关节角度是否在限制范围内
// for (int i = 0; i < 6; i++) {
// float qmin = arm->param->arm6dof_params.DH_params[i].qmin;
// float qmax = arm->param->arm6dof_params.DH_params[i].qmax;
// if (target_angles->q[i] < qmin || target_angles->q[i] > qmax) {
// return -1; // 超出关节限位
// }
// }
// // 更新目标角度
// memcpy(&arm->stateVariable.targetJointAngles, target_angles, sizeof(Arm6dof_JointAngles_t));
// // 切换到关节位置控制模式
// arm->control.mode = ARM_MODE_JOINT_POSITION;
// arm->control.state = ARM_STATE_MOVING;
// return 0;
// }
// /**
// * @brief 笛卡尔空间位置控制
// */
// int8_t Arm_MoveCartesian(Arm_t *arm, const Arm6dof_Pose_t* target_pose) {
// if (arm == NULL || target_pose == NULL) {
// return -1;
// }
// // 求解逆运动学
// Arm6dof_JointAngles_t ik_solution;
// if (Arm_SolveIK(arm, target_pose, &ik_solution) != 0) {
// return -1; // 逆运动学求解失败
// }
// // 更新目标
// memcpy(&arm->stateVariable.targetEndPose, target_pose, sizeof(Arm6dof_Pose_t));
// memcpy(&arm->stateVariable.targetJointAngles, &ik_solution, sizeof(Arm6dof_JointAngles_t));
// // 切换到笛卡尔位置控制模式
// arm->control.mode = ARM_MODE_CARTESIAN_POSITION;
// arm->control.state = ARM_STATE_MOVING;
// return 0;
// }
// /**
// * @brief 急停
// */
// int8_t Arm_Stop(Arm_t *arm) {
// if (arm == NULL) {
// return -1;
// }
// // 将目标设置为当前位置
// memcpy(&arm->stateVariable.targetJointAngles,
// &arm->stateVariable.currentJointAngles,
// sizeof(Arm6dof_JointAngles_t));
// arm->control.state = ARM_STATE_STOPPED;
// return 0;
// }
// /**
// * @brief 获取当前运动状态
// */
// Arm_MotionState_t Arm_GetState(Arm_t *arm) {
// if (arm == NULL) {
// return ARM_STATE_ERROR;
// }
// return arm->control.state;
// }

282
User/module/arm.h Normal file
View File

@ -0,0 +1,282 @@
// /*
// * far♂蛇模块
// */
// #pragma once
// #include <stdbool.h>
// #include <stddef.h>
// #include <stdint.h>
// #ifdef __cplusplus
// extern "C" {
// #endif
// #include "main.h"
// #include "component/pid.h"
// #include "component/arm_kinematics/arm6dof.h"
// #include "device/motor_dm.h"
// #include "device/motor_lz.h"
// /* Exported constants ------------------------------------------------------- */
// #define ARM_JOINT_NUM 6 // 关节数量
// /* Exported macro ----------------------------------------------------------- */
// /* Exported types ----------------------------------------------------------- */
// // 电机类型
// typedef enum {
// JOINT_MOTOR_TYPE_LZ = 0, // 凌控电机
// JOINT_MOTOR_TYPE_DM, // 达妙电机
// } Joint_MotorType_t;
// // 控制模式
// typedef enum {
// ARM_MODE_IDLE = 0, // 空闲模式（电机松弛）
// ARM_MODE_JOINT_POSITION, // 关节空间位置控制
// ARM_MODE_CARTESIAN_POSITION, // 笛卡尔空间位置控制
// ARM_MODE_TRAJECTORY, // 轨迹跟踪模式
// ARM_MODE_TEACH, // 示教模式（力控）
// } Arm_ControlMode_t;
// // 运动状态
// typedef enum {
// ARM_STATE_STOPPED = 0, // 停止
// ARM_STATE_MOVING, // 运动中
// ARM_STATE_REACHED, // 到达目标
// ARM_STATE_ERROR, // 错误
// } Arm_MotionState_t;
// struct {
// union {
// MOTOR_LZ_Param_t lzMotor;
// MOTOR_DM_Param_t dmMotor;
// };
// KPID_Params_t pid;
// } *Joint_MotorParams_t;
// typedef struct {
// // 关节标识
// uint8_t id; // 关节编号（0-5）
// Joint_MotorType_t motor_type; // 电机类型
// // 运动学参数（DH参数）
// Arm6dof_DHParams_t dh_params; // DH参数（包含限位）
// float q_offset; // 零位偏移
// // 参数
// *params;
// // 电机实例
// union {
// MOTOR_LZ_t lz_motor;
// MOTOR_DM_t dm_motor;
// } motor;
// // 控制器
// KPID_t pid; // PID控制器
// // 状态变量
// struct {
// float current_angle; // 当前角度（去除零位偏移后）
// float current_velocity; // 当前速度
// float current_torque; // 当前力矩
// float target_angle; // 目标角度
// float target_velocity; // 目标速度
// bool online; // 在线状态
// } state;
// // 控制输出（根据motor_type选择使用）
// union {
// MOTOR_LZ_MotionParam_t lz_output;
// MOTOR_MIT_Output_t dm_output;
// } output;
// } Joint_t;
// // ============================================================================
// // Arm（机械臂）- 由多个关节组成
// // ============================================================================
// typedef struct {
// // 关节数组（面向对象：机械臂由6个关节对象组成）
// Joint_t joints[ARM_JOINT_NUM];
// // 全局运动学参数
// struct {
// float x, y, z; // 工具中心偏移 (mm)
// float roll, pitch, yaw; // 工具姿态偏移 (rad)
// } tool_offset;
// // 时间管理
// struct {
// float now; // 当前时间（秒）
// uint64_t last_wakeup; // 上次唤醒时间（微秒）
// float dt; // 控制周期（秒）
// } timer;
// // 末端状态（笛卡尔空间）
// struct {
// Arm6dof_Pose_t current; // 当前末端位姿（正运动学结果）
// Arm6dof_Pose_t target; // 目标末端位姿
// } end_effector;
// // 控制状态
// struct {
// Arm_ControlMode_t mode; // 控制模式
// Arm_MotionState_t state; // 运动状态
// float position_tolerance; // 位置到达容差（rad）
// float velocity_tolerance; // 速度到达容差（rad/s）
// bool enable; // 使能标志
// } control;
// } Arm_t;
// // 兼容旧接口的参数结构体
// typedef struct {
// Arm6dof_Params_t arm6dof_params;
// KPID_Params_t joint_pid_params[6];
// MOTOR_LZ_Param_t jointMotor1_params;
// MOTOR_LZ_Param_t jointMotor2_params;
// MOTOR_LZ_Param_t jointMotor3_params;
// MOTOR_DM_Param_t jointMotor4_params;
// MOTOR_DM_Param_t jointMotor5_params;
// MOTOR_DM_Param_t jointMotor6_params;
// float q_offset[6];
// } Arm_Params_t;
// /* Exported functions prototypes -------------------------------------------- */
// /**
// * @brief 初始化机械臂（兼容旧接口）
// */
// int8_t Arm_Init(Arm_t *arm, Arm_Params_t *arm_param, float freq);
// /**
// * @brief 更新机械臂状态
// */
// int8_t Arm_Updata(Arm_t *arm);
// /**
// * @brief 机械臂控制
// */
// int8_t Arm_Ctrl(Arm_t *arm);
// // ============================================================================
// // 关节级别操作（面向对象接口）
// // ============================================================================
// /**
// * @brief 初始化单个关节
// * @param joint 关节指针
// * @param id 关节编号（0-5）
// * @param motor_type 电机类型
// * @param dh_params DH参数
// * @param pid_params PID参数
// * @param freq 控制频率
// * @return 0: 成功, -1: 失败
// */
// int8_t Joint_Init(Joint_t *joint, uint8_t id, Joint_MotorType_t motor_type,
// const Arm6dof_DHParams_t *dh_params, const KPID_Params_t *pid_params, float freq);
// /**
// * @brief 更新关节状态（读取电机反馈）
// * @param joint 关节指针
// * @return 0: 成功, -1: 失败
// */
// int8_t Joint_Update(Joint_t *joint);
// /**
// * @brief 关节位置控制
// * @param joint 关节指针
// * @param target_angle 目标角度（rad）
// * @param dt 控制周期（s）
// * @return 0: 成功, -1: 失败
// */
// int8_t Joint_PositionControl(Joint_t *joint, float target_angle, float dt);
// /**
// * @brief 关节松弛
// * @param joint 关节指针
// * @return 0: 成功, -1: 失败
// */
// int8_t Joint_Relax(Joint_t *joint);
// /**
// * @brief 检查关节是否到达目标
// * @param joint 关节指针
// * @param tolerance 位置容差（rad）
// * @return true: 已到达, false: 未到达
// */
// bool Joint_IsReached(const Joint_t *joint, float tolerance);
// // ============================================================================
// // 机械臂级别操作（继续保持现有接口）
// // ============================================================================
// /**
// * @brief 设置控制模式
// * @param arm 机械臂实例
// * @param mode 控制模式
// * @return 0: 成功, -1: 失败
// */
// int8_t Arm_SetMode(Arm_t *arm, Arm_ControlMode_t mode);
// /**
// * @brief 使能/失能机械臂
// * @param arm 机械臂实例
// * @param enable true: 使能, false: 失能
// * @return 0: 成功, -1: 失败
// */
// int8_t Arm_Enable(Arm_t *arm, bool enable);
// /**
// * @brief 关节空间位置控制：设置目标关节角度
// * @param arm 机械臂实例
// * @param target_angles 目标关节角度（rad）
// * @return 0: 成功, -1: 失败
// */
// int8_t Arm_MoveJoint(Arm_t *arm, const Arm6dof_JointAngles_t* target_angles);
// /**
// * @brief 笛卡尔空间位置控制：设置目标末端位姿
// * @param arm 机械臂实例
// * @param target_pose 目标末端位姿
// * @return 0: 成功（含逆解成功）, -1: 失败（含逆解失败）
// */
// int8_t Arm_MoveCartesian(Arm_t *arm, const Arm6dof_Pose_t* target_pose);
// /**
// * @brief 急停：立即停止所有运动
// * @param arm 机械臂实例
// * @return 0: 成功, -1: 失败
// */
// int8_t Arm_Stop(Arm_t *arm);
// /**
// * @brief 获取当前运动状态
// * @param arm 机械臂实例
// * @return 运动状态
// */
// Arm_MotionState_t Arm_GetState(Arm_t *arm);
// /**
// * @brief ;
// }output;
// */
// int8_t Arm_Init(Arm_t *arm, Arm_Params_t *arm_param, float freq);
// int8_t Arm_Updata(Arm_t *arm);
// int8_t Arm_Ctrl(Arm_t *arm);
// /**
// * @brief 逆运动学求解：根据目标末端位姿计算关节角度
// * @param arm 机械臂实例
// * @param target_pose 目标末端位姿
// * @param result_angles 输出的关节角度解
// * @return 0: 成功, -1: 失败
// * @note 使用当前关节角度作为初始猜测值，迭代50次，误差容限1.0mm
// */
// int8_t Arm_SolveIK(Arm_t *arm, const Arm6dof_Pose_t* target_pose, Arm6dof_JointAngles_t* result_angles);
// #ifdef __cplusplus
// }
// #endif

706
User/module/arm_oop.hpp Normal file
View File

@ -0,0 +1,706 @@
/**
* @file arm_oop.hpp
* @brief 机械臂类 - C++面向对象实现
*/
#pragma once
#include <cstring>
#include <cmath>
#include <stdint.h>
#include <array>
#include "joint.hpp"
#include "bsp/time.h"
#include "component/arm_kinematics/arm6dof.h"
#include "component/toolbox/robotics.h"
#define qLimitTolerance 0.5// 关节角度误差容限（rad），用于IK解的有效性验证
namespace arm {
// 控制模式
enum class ControlMode {
IDLE = 0,
JOINT_POSITION,
CARTESIAN_POSITION,
TRAJECTORY,
TEACH, // 示教模式（重力补偿 + 零刚度）
GRAVITY_COMP, // 重力补偿模式（带位置保持）
};
// 运动状态
enum class MotionState {
STOPPED = 0,
MOVING,
REACHED,
ERROR,
};
// ============================================================================
// RoboticArm 类 - 机械臂
// ============================================================================
class RoboticArm {
private:
static constexpr size_t JOINT_NUM = 6;
std::array<Joint*, JOINT_NUM> joints_; // 关节数组（多态指针）
// 末端状态
struct {
Arm6dof_Pose_t current; // 当前末端位姿（FK计算）
Arm6dof_Pose_t target; // 目标末端位姿
} end_effector_;
// 控制状态
struct {
ControlMode mode;
MotionState state;
float position_tolerance;
float velocity_tolerance;
bool enable;
bool gravity_comp_enable; // 重力补偿使能
} control_;
// 逆运动学
struct {
Arm6dof_JointAngles_t angles; // IK解算得到的关节角度
bool valid; // IK解是否有效
} inverseKinematics_;
// 重力补偿
struct {
float torques[JOINT_NUM]; // 各关节重力补偿力矩
float scale; // 补偿比例系数（用于微调）
} gravity_comp_;
// 笛卡尔空间轨迹规划
// 原理：将大步运动拆分为每帧一小步的线性插值，IK初始猜测始终是上一帧解，
// 从而保证牛顿迭代永远在正确分支附近收敛，根本消除大跨度跳变问题
struct {
Arm6dof_Pose_t start; // 轨迹起点（启动时的当前末端位姿）
Arm6dof_Pose_t goal; // 轨迹终点（目标末端位姿）
float t; // 插值进度 [0.0, 1.0]，0=起点，1=终点
bool active; // 是否正在执行轨迹
float max_lin_vel; // 末端线速度限制 (m/s)
float max_ang_vel; // 末端角速度限制 (rad/s)
Arm6dof_JointAngles_t angles; // 轨迹模式下当前插值点的IK解算结果
bool valid; // IK解是否有效
} traj_;
// 时间管理
struct {
float now;
uint64_t last_wakeup;
float dt;
} timer_;
public:
// 构造函数
RoboticArm() {
joints_.fill(nullptr);
control_.mode = ControlMode::IDLE;
control_.state = MotionState::STOPPED;
control_.position_tolerance = 0.02f; // 0.02 rad
control_.velocity_tolerance = 0.01f;
control_.enable = false;
control_.gravity_comp_enable = false;
for (size_t i = 0; i < JOINT_NUM; ++i) {
gravity_comp_.torques[i] = 0.0f;
}
gravity_comp_.scale = 1.0f;
inverseKinematics_.valid = false;
memset(&traj_.start, 0, sizeof(traj_.start));
memset(&traj_.goal, 0, sizeof(traj_.goal));
traj_.t = 0.0f;
traj_.active = false;
traj_.max_lin_vel = 0.15f; // 150 mm/s，可通过 SetLinVelLimit() 调整
traj_.max_ang_vel = 1.0f; // ~57°/s，可通过 SetAngVelLimit() 调整
timer_.now = 0.0f;
timer_.last_wakeup = 0;
timer_.dt = 0.001f;
}
// 禁止拷贝
RoboticArm(const RoboticArm&) = delete;
RoboticArm& operator=(const RoboticArm&) = delete;
// 添加关节（依赖注入）
void AddJoint(size_t index, Joint* joint) {
if (index < JOINT_NUM) {
joints_[index] = joint;
}
}
// 访问关节
Joint* GetJoint(size_t index) {
return (index < JOINT_NUM) ? joints_[index] : nullptr;
}
const Joint* GetJoint(size_t index) const {
return (index < JOINT_NUM) ? joints_[index] : nullptr;
}
// 初始化所有关节
int8_t Init() {
for (Joint* joint : joints_) { // 遍历关节数组
if (joint) {
joint->Register();
joint->Enable();
}
}
memset(&end_effector_.target, 0, sizeof(end_effector_.target));
return 0;
}
// 更新所有关节状态
int8_t Update() {
// 更新每个关节
for (Joint* joint : joints_) { // 遍历关节数组
if (joint) {
joint->Update();
}
}
// 读取当前关节角度（局部变量，正运动学和重力补偿共用一次读取）
Arm6dof_JointAngles_t q;
for (size_t i = 0; i < JOINT_NUM; ++i) {
q.q[i] = joints_[i] ? joints_[i]->GetCurrentAngle() : 0.0f;
}
// 正运动学：计算当前末端位姿
Arm6dof_ForwardKinematics(&q, &end_effector_.current);
// 计算重力补偿力矩
if (control_.gravity_comp_enable) {
CalcGravityTorques(q);
}
return 0;
}
/**
* @brief 计算当前姿态下各关节的重力补偿力矩
* @param q 当前关节角度（由Update()统一读取后传入，避免重复读取）
* @details 基于牛顿-欧拉逆动力学，设速度和加速度为零，
* 求解的力矩即为平衡重力所需的各关节力矩
*/
int8_t CalcGravityTorques(const Arm6dof_JointAngles_t& q) {
float raw_torques[JOINT_NUM];
int ret = Arm6dof_GravityCompensation(&q, raw_torques);
if (ret != 0) return ret;
for (size_t i = 0; i < JOINT_NUM; ++i) {
gravity_comp_.torques[i] = raw_torques[i] * gravity_comp_.scale;
}
return 0;
}
/**
* @brief 轨迹规划核心：每帧插值一小步并对插值点求IK
*
* 每帧执行流程：
* 1. 按速度限制计算本帧 t 步进量 Δt = v_max * dt / 总距离
* 2. P(t) = start + t*(goal-start) 线性插值得到本帧中间目标
* 3. IK(P(t), q_init=当前关节角) → 初始猜测与结果相差极小，必然收敛
* 4. 验证 + 写入 inverseKinematics_.angles
*/
void StepTrajectory() {
if (!traj_.active) return;
constexpr float TWO_PI = 2.0f * (float)M_PI;
// 计算总距离（用于将速度限制转换为 t 步进量）
float dx = traj_.goal.x - traj_.start.x;
float dy = traj_.goal.y - traj_.start.y;
float dz = traj_.goal.z - traj_.start.z;
float pos_dist = sqrtf(dx*dx + dy*dy + dz*dz);
float dr = traj_.goal.roll - traj_.start.roll;
float dp = traj_.goal.pitch - traj_.start.pitch;
float dyw = traj_.goal.yaw - traj_.start.yaw;
// 姿态差 wrap 到 (-π, π]：确保插值走最短路径，避免跨越 r2rpy 值域边界时绕大弯
dr -= roundf(dr / TWO_PI) * TWO_PI;
dp -= roundf(dp / TWO_PI) * TWO_PI;
dyw -= roundf(dyw / TWO_PI) * TWO_PI;
float ang_dist = sqrtf(dr*dr + dp*dp + dyw*dyw);
// Δt = v_max * dt / 总距离
// 物理意义：以限定速度匀速运动，谁先到达限制谁决定步进
// 例：v=0.15m/s，dt=2ms，dist=0.3m → Δt=0.001，需1000帧=2s走完
float dt_pos = (pos_dist > 1e-6f) ? (traj_.max_lin_vel * timer_.dt / pos_dist) : 1.0f;
float dt_ang = (ang_dist > 1e-6f) ? (traj_.max_ang_vel * timer_.dt / ang_dist) : 1.0f;
float dt_step = (dt_pos < dt_ang) ? dt_pos : dt_ang; // 取小值（慢者为约束）
traj_.t += dt_step;
if (traj_.t >= 1.0f) {
traj_.t = 1.0f;
traj_.active = false; // 到达终点
}
// 线性插值：P(t) = start + t*(goal-start)
Arm6dof_Pose_t interp;
interp.x = traj_.start.x + traj_.t * dx;
interp.y = traj_.start.y + traj_.t * dy;
interp.z = traj_.start.z + traj_.t * dz;
interp.roll = traj_.start.roll + traj_.t * dr;
interp.pitch = traj_.start.pitch + traj_.t * dp;
interp.yaw = traj_.start.yaw + traj_.t * dyw;
// IK初始猜测 = 当前关节角（与上一帧解相差极小，牛顿法必然收敛到正确分支）
Arm6dof_JointAngles_t q_init;
for (size_t i = 0; i < JOINT_NUM; ++i) {
q_init.q[i] = joints_[i] ? joints_[i]->GetCurrentAngle() : 0.0f;
}
if (Arm6dof_InverseKinematics(&interp, &q_init, &traj_.angles, 0.001f, 50) != 0) {
traj_.valid = false;
traj_.active = false;
control_.state = MotionState::ERROR;
return;
}
// 角度连续性修正：将IK解调整到与当前关节角最近的等效角
// 仅对范围跨度 > 2π 的关节（如 J1: [-15.7, 15.7]）有意义：
// IK 求解器对多圈关节可能返回与当前角差 2π 整数倍的等效解，导致跳变。
// 通过将差值 wrap 到 (-π, π] 选取最近分支解决此问题。
// 对范围 ≤ 2π 的关节（如 J4/J6: [0, 2π]）：
// 每个角度值在物理上唯一，不存在 2π 等效分支，直接信任 IK 结果，跳过修正。
for (size_t i = 0; i < JOINT_NUM; ++i) {
if (!joints_[i]) continue;
const auto& dh = joints_[i]->GetDHParams();
// 关节范围跨度不足 2π——无等效分支，无需修正
if ((dh.qmax - dh.qmin) <= TWO_PI + 0.1f) continue;
float q_ik = traj_.angles.q[i];
float q_cur = q_init.q[i];
float diff = q_ik - q_cur;
// roundf(diff / 2π) * 2π = 最近的 2π 整数倍，减去后差值落在 (-π, π]
diff -= roundf(diff / TWO_PI) * TWO_PI;
traj_.angles.q[i] = q_cur + diff;
}
// 限位检查
for (size_t i = 0; i < JOINT_NUM; ++i) {
if (joints_[i]) {
const auto& dh = joints_[i]->GetDHParams();
if (traj_.angles.q[i] < (dh.qmin-qLimitTolerance) ||
traj_.angles.q[i] > (dh.qmax+qLimitTolerance)) {
traj_.valid = false;
traj_.active = false;
control_.state = MotionState::ERROR;
return;
}
}
}
// // 突变检查：轨迹模式下每帧步长极小，若仍触发突变说明严重异常
// // 阈值比直接IK时更严：max_lin_vel*dt对应的关节角变化通常远小于0.3rad
// static constexpr float TRAJ_MAX_JOINT_DELTA = 0.3f; // rad ≈ 17°/帧
// for (size_t i = 0; i < JOINT_NUM; ++i) {
// if (joints_[i]) {
// float delta = fabsf(traj_.angles.q[i] -
// joints_[i]->GetCurrentAngle());
// if (delta > TRAJ_MAX_JOINT_DELTA) {
// traj_.valid = false;
// traj_.active = false;
// control_.state = MotionState::ERROR;
// return;
// }
// }
// }
traj_.valid = true;
end_effector_.target = interp;
}
int8_t GetPose(Arm6dof_Pose_t* pose) const {
if (pose == nullptr) {
return -1;
}
std::memcpy(pose, &end_effector_.current, sizeof(Arm6dof_Pose_t));
return 0;
}
int8_t SetTargetPose(const Arm6dof_Pose_t pose) {
end_effector_.target = pose;
return 0;
}
int8_t GetTargetPose(Arm6dof_Pose_t* pose) const {
if (pose == nullptr) {
return -1;
}
std::memcpy(pose, &end_effector_.target, sizeof(Arm6dof_Pose_t));
return 0;
}
// 控制循环
int8_t Control() {
// 更新时间
uint64_t now_us = BSP_TIME_Get_us();
timer_.dt = (now_us - timer_.last_wakeup) / 1000000.0f;
timer_.last_wakeup = now_us;
timer_.now = now_us / 1000000.0f;
// 未使能则松弛
if (!control_.enable) {
for (Joint* joint : joints_) { // 遍历关节数组
if (joint) joint->Relax();
}
control_.state = MotionState::STOPPED;
return 0;
}
// 根据控制模式执行
switch (control_.mode) {
case ControlMode::IDLE:
for (Joint* joint : joints_) { // 遍历关节数组
if (joint) joint->Relax();
}
control_.state = MotionState::STOPPED;
break;
case ControlMode::JOINT_POSITION:
case ControlMode::CARTESIAN_POSITION:
case ControlMode::GRAVITY_COMP: {
// 检查错误状态（不可达或超限）
if (control_.state == MotionState::ERROR) {
// 进入错误状态，松弛所有关节（安全停止）
for (Joint* joint : joints_) {
if (joint) joint->Relax();
}
control_.enable = false; // 自动禁用
return -1;
}
// 将重力补偿力矩写入各关节前馈
if (control_.gravity_comp_enable) {
for (size_t i = 0; i < JOINT_NUM; ++i) {
if (joints_[i]) {
joints_[i]->SetFeedforwardTorque(gravity_comp_.torques[i]);
}
}
}
// GRAVITY_COMP 模式：保持当前位置 + 重力补偿
if (control_.mode == ControlMode::GRAVITY_COMP) {
for (size_t i = 0; i < JOINT_NUM; ++i) {
if (joints_[i]) {
joints_[i]->SetTargetAngle(joints_[i]->GetCurrentAngle());
}
}
}
// CARTESIAN_POSITION 模式：每帧推进轨迹一小步，再应用IK解到关节
if (control_.mode == ControlMode::CARTESIAN_POSITION) {
// 轨迹规划核心调用：插值 + IK，结果写入 traj_
StepTrajectory();
if (traj_.valid) {
for (size_t i = 0; i < JOINT_NUM; ++i) {
if (joints_[i]) {
joints_[i]->SetTargetAngle(traj_.angles.q[i]);
}
}
} else {
// IK解无效：松弛所有关节并进入错误状态
for (Joint* joint : joints_) {
if (joint) joint->Relax();
}
control_.state = MotionState::ERROR;
return -1;
}
}
// 位置控制
bool all_reached = true;
for (Joint* joint : joints_) { // 遍历关节数组
if (joint) {
joint->PositionControl(joint->GetTargetAngle(), timer_.dt);
if (!joint->IsReached(control_.position_tolerance)) {
all_reached = false;
}
}
}
control_.state = all_reached ? MotionState::REACHED : MotionState::MOVING;
break;
}
case ControlMode::TEACH: {
// 示教模式：仅施加重力补偿力矩，不施加位置刚度
// 操作者可以自由拖动机械臂
if (control_.gravity_comp_enable) {
for (size_t i = 0; i < JOINT_NUM; ++i) {
if (joints_[i]) {
joints_[i]->TorqueControl(gravity_comp_.torques[i]);
}
}
} else {
// 未使能重力补偿时，示教模式下松弛
for (Joint* joint : joints_) {
if (joint) joint->Relax();
}
}
control_.state = MotionState::MOVING;
break;
}
default:
control_.state = MotionState::ERROR;
break;
}
return 0;
}
// ========================================================================
// 高层API
// ========================================================================
void Enable(bool enable) {
control_.enable = enable;
if (!enable) {
control_.mode = ControlMode::IDLE;
}
}
void SetMode(ControlMode mode) {
control_.mode = mode;
}
MotionState GetState() const {
return control_.state;
}
/**
* @brief 使能/禁用重力补偿
* @param enable true=使能, false=禁用
*/
void EnableGravityCompensation(bool enable) {
control_.gravity_comp_enable = enable;
if (!enable) {
// 清除补偿力矩
for (size_t i = 0; i < JOINT_NUM; ++i) {
gravity_comp_.torques[i] = 0.0f;
if (joints_[i]) joints_[i]->SetFeedforwardTorque(0.0f);
}
}
}
/**
* @brief 设置重力补偿比例系数（用于微调）
* @param scale 补偿系数，1.0=完全补偿，<1.0=欠补偿，>1.0=过补偿
*/
void SetGravityCompScale(float scale) {
gravity_comp_.scale = scale;
}
/**
* @brief 获取指定关节的重力补偿力矩
*/
float GetGravityTorque(size_t index) const {
return (index < JOINT_NUM) ? gravity_comp_.torques[index] : 0.0f;
}
bool IsGravityCompEnabled() const {
return control_.gravity_comp_enable;
}
// 关节空间控制（直接设置目标关节角度）
int8_t MoveJoint(const float target_angles[JOINT_NUM]) {
// 1. 检查限位
for (size_t i = 0; i < JOINT_NUM; ++i) {
if (joints_[i]) {
const auto& dh = joints_[i]->GetDHParams();
if (target_angles[i] < dh.qmin || target_angles[i] > dh.qmax) {
return -1; // 超限
}
}
}
// 2. 设置目标角度到各关节
for (size_t i = 0; i < JOINT_NUM; ++i) {
if (joints_[i]) {
joints_[i]->SetTargetAngle(target_angles[i]);
}
}
control_.state = MotionState::MOVING;
return 0;
}
/**
* @brief 设置笛卡尔空间目标位姿，自动启动轨迹规划
* 目标发生变化时，从当前末端位姿重新规划轨迹。
* 实际运动由 Control() 每帧调用 StepTrajectory() 推进。
*
* @param goal 目标末端位姿
*/
int8_t MoveCartesian(const Arm6dof_Pose_t& goal) {
// 检测目标是否发生有效变化（避免每帧重置轨迹）
constexpr float POS_THRESH = 0.001f; // 1mm
constexpr float ANG_THRESH = 0.01f; // ~0.57°
bool goal_changed =
fabsf(goal.x - traj_.goal.x) > POS_THRESH ||
fabsf(goal.y - traj_.goal.y) > POS_THRESH ||
fabsf(goal.z - traj_.goal.z) > POS_THRESH ||
fabsf(goal.roll - traj_.goal.roll) > ANG_THRESH ||
fabsf(goal.pitch - traj_.goal.pitch) > ANG_THRESH ||
fabsf(goal.yaw - traj_.goal.yaw) > ANG_THRESH;
if (goal_changed) {
// 从当前末端位姿出发重新规划轨迹
traj_.start = end_effector_.current;
traj_.goal = goal;
traj_.t = 0.0f;
traj_.active = true;
traj_.valid = true; // 允许Control()进入，等待第一帧StepTrajectory
control_.state = MotionState::MOVING;
}
return 0;
}
// 急停
void Stop() {
control_.state = MotionState::STOPPED;
}
/**
* @brief 工具坐标系目标位姿控制
*
* 与 MoveCartesian 的区别：
* - MoveCartesian ：输入世界系绝对位姿
* - MoveCartesianTool：输入工具坐标系下的目标位姿，内部变换到世界系
*
* 变换原理：
* 位置： p_world = R_cur * [x_t, y_t, z_t]^T
* 姿态： R_world = R_cur * R_target
*
* @param target_tool 工具坐标系下的目标位姿（绝对值）
* .x/.y/.z ：工具系下的目标位置 (m)
* .roll/.pitch/.yaw ：工具系下的目标姿态 (rad)
*/
void MoveCartesianTool(const Arm6dof_Pose_t& target_tool) {
const Arm6dof_Pose_t& cur = end_effector_.current;
// ── R_cur = Rz(yaw)*Ry(pitch)*Rx(roll) ──
float rpy_cur_arr[3] = {cur.yaw, cur.pitch, cur.roll};
Matrixf<3,3> R = robotics::rpy2r(Matrixf<3,1>(rpy_cur_arr));
// ── 位置：p_world = R_cur * p_tool（绝对目标，工具系坐标→世界系） ──
Arm6dof_Pose_t goal;
float p_arr[3] = {target_tool.x, target_tool.y, target_tool.z};
Matrixf<3,1> p_world = R * Matrixf<3,1>(p_arr);
goal.x = p_world[0][0];
goal.y = p_world[1][0];
goal.z = p_world[2][0];
// ── 姿态：R_world = R_cur * R_target（工具系绝对姿态→世界系） ──
float rpy_target_arr[3] = {target_tool.yaw, target_tool.pitch, target_tool.roll};
Matrixf<3,3> Rn = R * robotics::rpy2r(Matrixf<3,1>(rpy_target_arr));
// ── R_new → RPY，写入目标位姿 ──
Matrixf<3,1> rpy_new = robotics::r2rpy(Rn); // [yaw; pitch; roll]
goal.yaw = rpy_new[0][0];
goal.pitch = rpy_new[1][0];
goal.roll = rpy_new[2][0];
MoveCartesian(goal);
}
/**
* @brief 航向参考系目标位姿控制
*
* 位置（航向系）：世界系绕Z轴旋转当前yaw角度得到，X/Y始终水平，Z始终世界竖直
* 姿态（航向系）：roll/pitch/yaw 为末端在航向系下的绝对目标姿态，直观地对应末端各轴旋转
*
* 变换原理：
* 位置 X/Y：p_world = Rz(yaw) * [x, y, 0]^T
* 位置 Z ：p_world.z = z (世界竖直直接赋值)
* 姿态 ：R_world = Rz(yaw) * R_target （航向系下绝对姿态→世界系）
*
* @param target_heading
* .x/.y ：航向系（水平）下的目标位置 (m)
* .z ：世界竖直方向目标高度 (m)
* .roll/.pitch/.yaw ：航向系下的末端绝对姿态 (rad)
* 直观含义：yaw转动就是末端 yaw，pitch/roll 同理
*/
void MoveCartesianHeading(const Arm6dof_Pose_t& target_heading) {
const Arm6dof_Pose_t& cur = end_effector_.current;
// ── 位置：航向系→世界系，Rz(yaw) 旋转水平分量，Z 直接为世界Z ──
float cy = cosf(cur.yaw), sy = sinf(cur.yaw);
Arm6dof_Pose_t goal;
goal.x = cy * target_heading.x - sy * target_heading.y; // 世界X
goal.y = sy * target_heading.x + cy * target_heading.y; // 世界Y
goal.z = target_heading.z; // 世界Z 直接赋值
// ── 姿态：航向系下绝对姿态→世界系：R_world = Rz(yaw) * R_target ──
float rpy_yaw_arr[3] = {cur.yaw, 0.0f, 0.0f};
float rpy_target_arr[3] = {target_heading.yaw, target_heading.pitch, target_heading.roll};
Matrixf<3,3> Rn = robotics::rpy2r(Matrixf<3,1>(rpy_yaw_arr))
* robotics::rpy2r(Matrixf<3,1>(rpy_target_arr));
// ── R_new → RPY，写入目标位姿 ──
Matrixf<3,1> rpy_new = robotics::r2rpy(Rn); // [yaw; pitch; roll]
goal.yaw = rpy_new[0][0];
goal.pitch = rpy_new[1][0];
goal.roll = rpy_new[2][0];
MoveCartesian(goal);
}
// 获取末端位姿（FK结果，单位 m/rad）
const Arm6dof_Pose_t& GetEndPose() const {
return end_effector_.current;
}
// 获取最近一次 IK 解算结果
const Arm6dof_JointAngles_t& GetIkAngles() const {
return inverseKinematics_.angles;
}
// 获取最近一次 IK 是否有效
bool IsIkValid() const {
return inverseKinematics_.valid;
}
/** 设置末端线速度限制 (m/s)，默认 0.15 m/s */
void SetLinVelLimit(float vel) { traj_.max_lin_vel = vel; }
/** 设置末端角速度限制 (rad/s)，默认 1.0 rad/s */
void SetAngVelLimit(float vel) { traj_.max_ang_vel = vel; }
/** 获取当前轨迹进度 [0.0, 1.0]，1.0 表示已到达目标 */
float GetTrajProgress() const { return traj_.t; }
/** 轨迹是否正在执行（false 表示已到达目标或尚未启动） */
bool IsTrajActive() const { return traj_.active; }
/** 清除错误状态并同步目标位姿到当前位姿（错误恢复用） */
void ResetError() {
control_.state = MotionState::REACHED;
control_.enable = true;
inverseKinematics_.valid = true;
traj_.valid = true;
traj_.active = false;
Arm6dof_JointAngles_t q;
for (size_t i = 0; i < JOINT_NUM; ++i) {
q.q[i] = joints_[i] ? joints_[i]->GetCurrentAngle() : 0.0f;
}
for (size_t i = 0; i < JOINT_NUM; ++i) {
if (joints_[i]) {
joints_[i]->SetTargetAngle(joints_[i]->GetCurrentAngle());
}
}
inverseKinematics_.angles = q;
// 将轨迹目标同步为当前位姿，防止下次启动时从旧目标出发
traj_.goal = end_effector_.current;
end_effector_.target = end_effector_.current;
}
};
} // namespace arm

314
User/module/chassis.c Normal file
View File

@ -0,0 +1,314 @@
/*
µ×ÅÌÄ£×é
*/
#include "cmsis_os2.h"
#include <stdlib.h>
#include "bsp/mm.h"
#include "bsp/can.h"
#include "component/ahrs.h"
#include "device/motor_rm.h"
#include "device/motor.h"
#include "module/chassis.h"
#include "component/PowerControl.h"
#include "config.h"
#include "math.h"
/**
* @brief µ×ÅÌСÍÓÂÝģʽÏà¹Ø²ÎÊý
*/
#define CHASSIS_ROTOR_WZ_MIN 0.6f //СÍÓÂÝ×îСËÙ¶È
#define CHASSIS_ROTOR_WZ_MAX 0.8f //СÍÓÂÝ×î´óËÙ¶È
#define M_7OVER72PI (M_2PI * 7.0f / 72.0f) //½Ç¶ÈÆ«ÒÆÁ¿£¨ÓÃÔÚ¸úËæÔÆ̨35¡ã£©
#define CHASSIS_ROTOR_OMEGA 0.001f //½ÇËٶȱ仯ƵÂÊ
/**
* @brief ÉèÖõ×ÅÌģʽ
* @param c µ×Å̽ṹÌåÖ¸Õë
* @param mode Ä¿±ê¿ØÖÆģʽ
* @param now µ±Ç°Ê±¼ä´Á(ms)
* @return CHASSIS_OK:³É¹¦ CHASSIS_ERR_NULL:¿Õ
*/
static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode, uint32_t now) {
if (!c)
return CHASSIS_ERR_NULL;
if (mode == c->mode)
return CHASSIS_OK;
//Ëæ»úÖÖ×Ó£¬Ð¡ÍÓÂÝģʽËæ»úÉèÖÃÐýת·½Ïò
if (mode == CHASSIS_MODE_ROTOR && c->mode != CHASSIS_MODE_ROTOR) {
srand(now);
c->wz_multi = (rand() % 2) ? -1 : 1;
}
//ÖØÖÃPIDºÍÂ˲¨
for (uint8_t i = 0; i < c->num_wheel; i++) {
PID_Reset(&c->pid.motor[i]);
LowPassFilter2p_Reset(&c->filter.in[i], 0.0f);
LowPassFilter2p_Reset(&c->filter.out[i], 0.0f);
}
c->mode = mode;
return CHASSIS_OK;
}
/**
* @brief СÍÓÂÝģʽ¯Ì¬½ÇËÙÈ
* @param min ×îСËÙÈ
* @param max ×î´óËÙÈ
* @param now µ±Ç°Ê±¼ä´Á(ms)
* @return ½ÇËÙÈÖµ
*/
static float Chassis_CalcWz(const float min, const float max, uint32_t now) {
float wz_vary = fabsf(0.2f * sinf(CHASSIS_ROTOR_OMEGA * (float)now)) + min;
return (wz_vary > max) ? max : wz_vary;
}
/**
* @brief µ×ÅÌģʽ³õʼ»¯
* @param c µ×Å̽ṹÌåÖ¸Õë
* @param param µ×Å̲ÎÊý½á¹¹ÌåÖ¸Õë
* @param mech_zero »úеÁãµãÅ·À­½Ç
* @param target_freq ¿ØÖÆƵÂÊ(Hz)
* @return CHASSIS_OK:³É¹¦ CHASSIS_ERR_NULL:¿Õ CHASSIS_ERR_TYPE:²»Ö§³ÖµÄģʽ
*/
int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
float target_freq) {
if (!c) return CHASSIS_ERR_NULL;
//³õʼ»¯CANͨÐÅ
BSP_CAN_Init();
c->param = param;
c->mode = CHASSIS_MODE_RELAX;
//¸ù¾Ýµ×Å̲»Í¬ÉèÖÃģʽÂÖ×ÓÓë»ìºÏÆ÷
Mixer_Mode_t mixer_mode;
switch (param->type) {
case CHASSIS_TYPE_MECANUM://ÂóÂÖ
c->num_wheel = 4;
mixer_mode = MIXER_MECANUM;
break;
case CHASSIS_TYPE_PARLFIX4:
c->num_wheel = 4;
mixer_mode = MIXER_PARLFIX4;
break;
case CHASSIS_TYPE_PARLFIX2:
c->num_wheel = 2;
mixer_mode = MIXER_PARLFIX2;
break;
case CHASSIS_TYPE_OMNI_CROSS:
c->num_wheel = 4;
mixer_mode = MIXER_OMNICROSS;
break;
case CHASSIS_TYPE_OMNI_PLUS: //È«ÏòÂÖ£¨Àϲ½±øÀàÐÍ£©
c->num_wheel = 4;
mixer_mode = MIXER_OMNIPLUS;
break;
case CHASSIS_TYPE_SINGLE:
c->num_wheel = 1;
mixer_mode = MIXER_SINGLE;
break;
default:
return CHASSIS_ERR_TYPE;
}
//³õʼ»¯Ê±¼ä´Á
c->last_wakeup = 0;
c->dt = 0.0f;
//³õʼ»¯PIDºÍÂ˲¨
for (uint8_t i = 0; i < c->num_wheel; i++) {
PID_Init(&c->pid.motor[i], KPID_MODE_NO_D, target_freq, &param->pid.motor_pid_param);
LowPassFilter2p_Init(&c->filter.in[i], target_freq, param->low_pass_cutoff_freq.in);
LowPassFilter2p_Init(&c->filter.out[i], target_freq, param->low_pass_cutoff_freq.out);
//ÇåÁãµç»ú·´À¡
c->feedback.motor[i].rotor_speed = 0;
c->feedback.motor[i].torque_current = 0;
c->feedback.motor[i].rotor_abs_angle = 0;
c->feedback.motor[i].temp = 0;
}
//³õʼ»¯PIDºÍ»ìºÏÆ÷
PID_Init(&c->pid.follow, KPID_MODE_CALC_D, target_freq, &param->pid.follow_pid_param);
Mixer_Init(&c->mixer, mixer_mode);
//ÇåÁãÔ˶¯ÏòÁ¿ºÍÊä³ö
c->move_vec.vx = c->move_vec.vy = c->move_vec.wz = 0.0f;
for (uint8_t i = 0; i < c->num_wheel; i++) {
c->out.motor[i] = 0.0f;
}
//×¢²á´ó½®µç»ú
for (int i = 0; i < c->num_wheel; i++) {
MOTOR_RM_Register(&(c->param->motor_param[i]));
}
return CHASSIS_OK;
}
/**
* @brief ¸üеç»ú·´À¡(IMU+µç»ú״̬)
* @param c µ×Å̽ṹÌåÖ¸Õë
* @param feedback µ×ÅÌ·´À¡Ö¸Õë½á¹¹Ìå
* @return CHASSIS_OK:³É¹¦ CHASSIS_ERR_NULL:¿Õ
*/
int8_t Chassis_UpdateFeedback(Chassis_t *c) {
//¸üÐÂËùÓеç»ú·´À¡
for (uint8_t i = 0; i < c->num_wheel; i++) {
MOTOR_RM_Update(&(c->param->motor_param[i]));
MOTOR_RM_t *rm_motor = MOTOR_RM_GetMotor(&(c->param->motor_param[i]));
c->motors[i] = rm_motor;
MOTOR_RM_t *rm = c->motors[i];
if (rm_motor != NULL) {
c->feedback.motor[i] = rm_motor->feedback;
}else
{
return CHASSIS_ERR_NULL;
}
}
return CHASSIS_OK;
}
/**
* @brief µ×Å̵ç»ú¿ØÖÆ
* @param c µ×Å̽ṹÌåÖ¸Õë
* @param c_cmd ¿ØÖÆÃüÁî
* @param now µ±Ç°Ê±¼ä´Á(ms)
* @return CHASSIS_OK:³É¹¦ CHASSIS_ERR_NULL:¿Õ
*/
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd, uint32_t now) {
if (!c || !c_cmd) return CHASSIS_ERR_NULL;
//¼ÆËã¿ØÖÆÖÜÆÚ
c->dt = (float)(now - c->last_wakeup) / 1000.0f;
c->last_wakeup = now;
if (!isfinite(c->dt) || c->dt <= 0.0f) {
c->dt = 0.001f;
}
if (c->dt < 0.0005f) c->dt = 0.0005f;
if (c->dt > 0.050f) c->dt = 0.050f;
//ÉèÖÃģʽ
Chassis_SetMode(c, c_cmd->mode, now);
//²»Í¬Ä£Ê½Ï¶ÔÓ¦½âË㣨Ô˶¯ÏòÁ¿£©
switch (c->mode) {
case CHASSIS_MODE_BREAK:
c->move_vec.vx = c->move_vec.vy = 0.0f;
break;
case CHASSIS_MODE_INDEPENDENT:
c->move_vec.vx = c_cmd->ctrl_vec.vx;
c->move_vec.vy = c_cmd->ctrl_vec.vy;
break;
default: { //Ò£¿ØÆ÷×ø±ê->»úÌå×ø±êϵ
float beta = c->feedback.encoder_gimbalYawMotor - c->mech_zero;
float cosb = cosf(beta);
float sinb = sinf(beta);
c->move_vec.vx = cosb * c_cmd->ctrl_vec.vx - sinb * c_cmd->ctrl_vec.vy;
c->move_vec.vy = sinb * c_cmd->ctrl_vec.vx + cosb * c_cmd->ctrl_vec.vy;
break;
}
}
//¸ù¾Ýģʽ¼ÆËãµ×Å̽ÇËÙ¶È
switch (c->mode) {
case CHASSIS_MODE_RELAX:
case CHASSIS_MODE_BREAK:
case CHASSIS_MODE_INDEPENDENT:
c->move_vec.wz = 0.0f;
break;
case CHASSIS_MODE_OPEN:
c->move_vec.wz = c_cmd->ctrl_vec.wz;
break;
//ÔÆ̨¸úËæ
case CHASSIS_MODE_FOLLOW_GIMBAL:
c->move_vec.wz = PID_Calc(&c->pid.follow, c->mech_zero, c->feedback.encoder_gimbalYawMotor, 0.0f, c->dt);
break;
//ÔÆ̨¸úË棨ƫÒÆ£©
case CHASSIS_MODE_FOLLOW_GIMBAL_35:
c->move_vec.wz = PID_Calc(&c->pid.follow,c->mech_zero +M_7OVER72PI, c->feedback.encoder_gimbalYawMotor, 0.0f, c->dt);
break;
//СÍÓÂÝ
case CHASSIS_MODE_ROTOR:
c->move_vec.wz = c->wz_multi * Chassis_CalcWz(CHASSIS_ROTOR_WZ_MIN,CHASSIS_ROTOR_WZ_MAX, now);
break;
}
//Ô˶¯Ñ§Äæ½âË㣬Ô˶¯ÏòÁ¿·Ö½âΪµç»úתËÙ
Mixer_Apply(&c->mixer, &c->move_vec, c->setpoint.motor_rpm, c->num_wheel, 500.0f);
for (uint8_t i = 0; i < c->num_wheel; i++) {
float rf = c->setpoint.motor_rpm[i];///Ä¿±êתËÙ
float fb = LowPassFilter2p_Apply(&c->filter.in[i], (float)c->feedback.motor[i].rotor_speed);
float out_current;
switch (c->mode) {
case CHASSIS_MODE_BREAK:
case CHASSIS_MODE_FOLLOW_GIMBAL:
case CHASSIS_MODE_FOLLOW_GIMBAL_35:
case CHASSIS_MODE_ROTOR:
case CHASSIS_MODE_INDEPENDENT:
out_current = PID_Calc(&c->pid.motor[i], c->setpoint.motor_rpm[i], fb, 0.0f, c->dt);
break;
case CHASSIS_MODE_OPEN:
out_current = c->setpoint.motor_rpm[i] / 7000.0f;
break;
case CHASSIS_MODE_RELAX:
out_current = 0.0f;
break;
}
//µÍͨÂ˲¨ºÍÏÞ·ù
c->out.motor[i] = LowPassFilter2p_Apply(&c->filter.out[i], out_current);
Clip(&c->out.motor[i], -c->param->limit.max_current, c->param->limit.max_current);
}
return CHASSIS_OK;
}
void Chassis_Power_Control(Chassis_t *c,float max_power)
{
float* rpm=(float[4]){c->motors[0]->feedback.rotor_speed,c->motors[1]->feedback.rotor_speed,c->motors[2]->feedback.rotor_speed,c->motors[3]->feedback.rotor_speed};
power_model_t* param = (c->mode == CHASSIS_MODE_ROTOR) ? &cha2 : &cha;
power_calu(param,(float[4]){c->out.motor[0],c->out.motor[1],c->out.motor[2],c->out.motor[3]},rpm);
float scale = power_scale_calu(&param,1,max_power);
power_out_calu(param,scale,(float[4]){c->out.motor[0],c->out.motor[1],c->out.motor[2],c->out.motor[3]},rpm,c->out.motor);
}
/**
* @brief µç»úÊä³ö
* @param c µ×Å̽ṹÌåÖ¸Õë
*/
void Chassis_Output(Chassis_t *c) {
if (!c)
return;
for (uint8_t i = 0; i < c->num_wheel; i++) {
MOTOR_RM_t *rm = c->motors[i];
if (!rm) continue;
MOTOR_RM_SetOutput(&rm->param, c->out.motor[i]);
}
//µ÷ÓÃctrl
// for (uint8_t i = 0; i < c->num_wheel; i++) {
// MOTOR_RM_t *rm = c->motors[0];
// if (rm) {
// MOTOR_RM_Ctrl(&rm->param);
// }
// }
MOTOR_RM_t *rm = c->motors[0];
if (rm) {
MOTOR_RM_Ctrl(&rm->param);
}
}
/**
* @brief ÖØÖõ×ÅÌÊä³ö
* @param c µ×Å̽ṹÌåÖ¸Õë
*/
void Chassis_ResetOutput(Chassis_t *c) {
if (!c) return;
for (uint8_t i = 0; i < c->num_wheel; i++) {
MOTOR_RM_t *m = c->motors[i];
if (m) {
MOTOR_RM_Relax(&(m->param));
}
}
}

239
User/module/chassis.h Normal file
View File

@ -0,0 +1,239 @@
/*
µ×ÅÌÄ£×é
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include "bsp/can.h"
#include "component/filter.h"
#include "component/mixer.h"
#include "component/pid.h"
#include "component/ahrs.h"
#include "device/motor_rm.h"
/* Exported constants ------------------------------------------------------- */
#define CHASSIS_OK (0) /* ÔËÐÐÕý³£ */
#define CHASSIS_ERR (-1) /* ÔËÐÐʱ³öÏÖÁËһЩС´íÎó */
#define CHASSIS_ERR_NULL (-2) /* ÔËÐÐʱ·¢ÏÖNULLÖ¸Õë */
#define CHASSIS_ERR_MODE (-3) /* ÔËÐÐʱ³öÅäÖÃÁË´íÎóµÄChassisMode_t */
#define CHASSIS_ERR_TYPE (-4) /* ÔËÐÐʱ³öÅäÖÃÁË´íÎóµÄChassis_Type_t */
#define MAX_MOTOR_CURRENT 20.0f
/* µ×ÅÌ¿ØÖÆģʽ */
typedef enum {
CHASSIS_MODE_RELAX, /* ·ÅËÉģʽ,µç»ú²»Êä³ö¡£Ò»°ãÇé¿öµ×Å̳õʼ»¯Ö®ºóµÄģʽ */
CHASSIS_MODE_BREAK, /* ɲ³µÄ£Ê½£¬µç»ú±Õ»·¿ØÖƾ²Ö¹¡£ÓÃÓÚ»úÆ÷ÈËֹͣ״̬ */
CHASSIS_MODE_FOLLOW_GIMBAL, /* ͨ¹ý±Õ»·¿ØÖÆʹ³µÍ··½Ïò¸úËæÔÆ̨ */
CHASSIS_MODE_FOLLOW_GIMBAL_35, /* ͨ¹ý±Õ»·¿ØÖÆʹ³µÍ··½Ïò35¡ã¸úËæÔÆ̨ */
CHASSIS_MODE_ROTOR, /* СÍÓÂÝģʽ£¬Í¨¹ý±Õ»·¿ØÖÆʹµ×Å̲»Í£Ðýת */
CHASSIS_MODE_INDEPENDENT, /*¶ÀÁ¢Ä£Ê½¡£µ×ÅÌÔËÐв»ÊÜÔÆ̨ӰÏì */
CHASSIS_MODE_OPEN, /* ¿ª»·Ä£Ê½¡£µ×ÅÌÔËÐв»ÊÜPID¿ØÖÆ£¬Ö±½ÓÊä³öµ½µç»ú */
} Chassis_Mode_t;
/* СÍÓÂÝת¶¯Ä£Ê½ */
typedef enum {
ROTOR_MODE_CW, /* ˳ʱÕëת¶¯ */
ROTOR_MODE_CCW, /* ÄæʱÕëת¶¯ */
ROTOR_MODE_RAND, /* Ëæ»úת¶¯ */
} Chassis_RotorMode_t;
/* µ×ÅÌ¿ØÖÆÃüÁî */
typedef struct {
Chassis_Mode_t mode; /* µ×ÅÌÔËÐÐģʽ */
Chassis_RotorMode_t mode_rotor; /* СÍÓÂÝת¶¯Ä£Ê½ */
MoveVector_t ctrl_vec; /* µ×ÅÌ¿ØÖÆÏòÁ¿ */
} Chassis_CMD_t;
/* ÏÞλ */
typedef struct {
float max;
float min;
} Chassis_Limit_t;
/* µ×ÅÌÀàÐÍ£¨µ×Å̵ÄÎïÀíÉè¼Æ£© */
typedef enum {
CHASSIS_TYPE_MECANUM, /* Âó¿ËÄÉÄ·ÂÖ */
CHASSIS_TYPE_PARLFIX4, /* ƽÐаڷŵÄËĸöÇý¶¯ÂÖ */
CHASSIS_TYPE_PARLFIX2, /* ƽÐаڷŵÄÁ½¸öÇý¶¯ÂÖ */
CHASSIS_TYPE_OMNI_CROSS, /* ²æÐͰڷŵÄËĸöÈ«ÏòÂÖ */
CHASSIS_TYPE_OMNI_PLUS, /* Ê®×ÖÐÍ°ÚÉèµÄËĸöÈ«ÏòÂÖ */
CHASSIS_TYPE_DRONE, /* ÎÞÈË»úµ×ÅÌ */
CHASSIS_TYPE_SINGLE, /* µ¥¸öĦ²ÁÂÖ */
} Chassis_Type_t;
/* µ×Å̲ÎÊý½á¹¹Ìå,ALL³õʼ»¯²ÎÊý */
typedef struct {
MOTOR_RM_Param_t motor_param[4];
struct {
KPID_Params_t motor_pid_param; /* µ×Å̵ç»úPID²ÎÊý */
KPID_Params_t follow_pid_param; /* ¸úËæÔÆ̨PID²ÎÊý */
} pid;
Chassis_Type_t type; /* µ×ÅÌÀàÐÍ£¬µ×Å̵ĻúеÉè¼ÆºÍÂÖ×ÓÑ¡ÐÍ */
/* µÍͨÂ˲¨Æ÷½ØÖÁƵÂÊ*/
struct {
float in; /* ÊäÈë */
float out; /* Êä³ö */
} low_pass_cutoff_freq;
/* µç»ú·´×°£¬Ó¦¸ÃºÍÔÆ̨ÉèÖÃÏàͬ*/
struct {
bool yaw;
} reverse;
struct {
float max_vx, max_vy, max_wz;
float max_current;
} limit;
} Chassis_Params_t;
typedef struct {
AHRS_Gyro_t gyro;
AHRS_Eulr_t eulr;
} Chassis_IMU_t;
typedef struct {
MOTOR_Feedback_t motor[4]; // Ëĸö 3508µç»ú ·´À¡
float encoder_gimbalYawMotor;
} Chassis_Feedback_t;
/* µ×ÅÌÊä³ö½á¹¹Ìå*/
typedef struct {
float motor[4];
} Chassis_Output_t;
/*
* ÔËÐеÄÖ÷½á¹¹Ìå£þ
* °üº¬³õʼ»¯²ÎÊý,Öмä±äÁ¿,Êä³ö±äÁ¿
*/
typedef struct {
uint64_t last_wakeup;
float dt;
Chassis_Params_t *param; /* µ×Å̲ÎÊý,ÓÃChassis_InitÉ趨 */
/* Ä£¿éͨÓà */
Chassis_Mode_t mode; /* µ×ÅÌģʽ */
/* µ×ÅÌÉè¼Æ */
int8_t num_wheel; /* µ×ÅÌÂÖ×ÓÊýÁ¿ */
Mixer_t mixer; /* »ìºÏÆ÷,Òƶ¯ÏòÁ¿->µç»úÄ¿±êÖµ */
MoveVector_t move_vec; /* µ×ÅÌʵ¼ÊµÄÔ˶¯ÏòÁ¿ */
MOTOR_RM_t *motors[4];/*Ö¸Ïòµ×ÅÌÿ¸öµç»ú²ÎÊý*/
float mech_zero;
float wz_multi; /* СÍÓÂÝÐýתģʽ */
/* PID¼ÆËãÄ¿±êÖµ */
struct {
float motor_rpm[4]; /* µç»úתËٵĶ¯Ì¬Êý×é,µ¥Î»:RPM */
} setpoint;
/* ·´À¡¿ØÖÆÓõÄPID */
struct {
KPID_t motor[4]; /* ¿ØÖÆÂÖ×Óµç»úÓõÄPIDµÄ¶¯Ì¬Êý×é */
KPID_t follow; /* ¸úËæÔÆ̨ÓõÄPID */
} pid;
struct {
Chassis_Limit_t vx, vy, wz;
} limit;
/* Â˲¨Æ÷ */
struct {
LowPassFilter2p_t in[4]; /* ·´À¡ÖµÂ˲¨Æ÷ */
LowPassFilter2p_t out[4]; /* Êä³öÖµÂ˲¨Æ÷ */
} filter;
Chassis_Output_t out; /* µç»úÊä³ö */
Chassis_Feedback_t feedback;
//float out_motor[4];
} Chassis_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* \brief µ×Å̳õʼ»¯
*
* \param c °üº¬µ×ÅÌÊý¾ÝµÄ½á¹¹Ìå
* \param param °üº¬µ×Å̲ÎÊýµÄ½á¹¹ÌåÖ¸Õë
* \param target_freq ÈÎÎñÔ¤ÆÚµÄÔËÐÐƵÂÊ
*
* \return ÔËÐнá¹û
*/
int8_t Chassis_Init(Chassis_t *c, const Chassis_Params_t *param,
float target_freq);
/**
* \brief ¸üе×ÅÌ·´À¡ÐÅÏ¢
*
* \param c °üº¬µ×ÅÌÊý¾ÝµÄ½á¹¹Ìå
* \param can CANÉ豸½á¹¹Ìå
*
* \return ÔËÐнá¹û
*/
int8_t Chassis_UpdateFeedback(Chassis_t *c);
/**
* \brief ÔËÐе×ÅÌ¿ØÖÆÂß¼­
*
* \param c °üº¬µ×ÅÌÊý¾ÝµÄ½á¹¹Ìå
* \param c_cmd µ×ÅÌ¿ØÖÆÖ¸Áî
* \param dt_sec Á½´Îµ÷ÓõÄʱ¼ä¼ä¸ô
*
* \return ÔËÐнá¹û
*/
int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd,
uint32_t now);
/**
* \brief ¸´ÖƵ×ÅÌÊä³öÖµ
*
* \param s °üº¬µ×ÅÌÊý¾ÝµÄ½á¹¹Ìå
* \param out CANÉ豸µ×ÅÌÊä³ö½á¹¹Ìå
*/
void Chassis_Output(Chassis_t *c);
/**
* \brief Çå¿ÕChassisÊä³öÊý¾Ý
*
* \param out CANÉ豸µ×ÅÌÊä³ö½á¹¹Ìå
*/
void Chassis_ResetOutput(Chassis_t *c);
void Chassis_Power_Control(Chassis_t *c,float max_power);
/**
* @brief µ×Å̹¦ÂÊÏÞÖÆ
*
* @param c µ×ÅÌÊý¾Ý
* @param cap µçÈÝÊý¾Ý
* @param ref ²ÃÅÐϵͳÊý¾Ý
* @return º¯ÊýÔËÐнá¹û
*/
//»¹Ã»ÓмÓÈ룬waiting¡£¡£¡£¡£¡£¡£int8_t Chassis_PowerLimit(Chassis_t *c, const CAN_Capacitor_t *cap,
// const Referee_ForChassis_t *ref);
/**
* @brief µ¼³öµ×ÅÌÊý¾Ý
*
* @param chassis µ×ÅÌÊý¾Ý½á¹¹Ìå
* @param ui UIÊý¾Ý½á¹¹Ìå
*/
//void Chassis_DumpUI(const Chassis_t *c, Referee_ChassisUI_t *ui);
#ifdef __cplusplus
}
#endif

502
User/module/cmd/cmd.c Normal file
View File

@ -0,0 +1,502 @@
/*
* CMD 模块 V2 - 主控制模块实现
*/
#include "cmd.h"
#include "bsp/time.h"
#include <stdint.h>
#include <string.h>
/* ========================================================================== */
/* 命令构建函数 */
/* ========================================================================== */
/* 从RC输入生成底盘命令 */
#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_CHASSIS
static void CMD_RC_BuildChassisCmd(CMD_t *ctx) {
CMD_RCModeMap_t *map = &ctx->config->rc_mode_map;
/* 根据左拨杆位置选择模式 */
switch (ctx->input.rc.sw[0]) {
case CMD_SW_UP:
ctx->output.chassis.cmd.mode = map->sw_left_up;
break;
case CMD_SW_MID:
ctx->output.chassis.cmd.mode = map->sw_left_mid;
break;
case CMD_SW_DOWN:
ctx->output.chassis.cmd.mode = map->sw_left_down;
break;
default:
ctx->output.chassis.cmd.mode = CHASSIS_MODE_RELAX;
break;
}
/* 摇杆控制移动 */
ctx->output.chassis.cmd.ctrl_vec.vx = ctx->input.rc.joy_right.x;
ctx->output.chassis.cmd.ctrl_vec.vy = ctx->input.rc.joy_right.y;
}
#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_CHASSIS */
/* 从 RC 输入生成云台命令 */
#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_GIMBAL
static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) {
CMD_RCModeMap_t *map = &ctx->config->rc_mode_map;
ctx->output.gimbal.cmd.is_ai = false;
/* 根据拨杆选择云台模式 */
switch (ctx->input.rc.sw[0]) {
case CMD_SW_UP:
ctx->output.gimbal.cmd.mode = map->gimbal_sw_up;
break;
case CMD_SW_MID:
ctx->output.gimbal.cmd.mode = map->gimbal_sw_mid;
break;
case CMD_SW_DOWN:
ctx->output.gimbal.cmd.mode = map->gimbal_sw_down;
break;
default:
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
break;
}
/* 左摇杆控制云台 */
ctx->output.gimbal.cmd.delta_yaw = -ctx->input.rc.joy_left.x * 4.0f;
ctx->output.gimbal.cmd.delta_pit = -ctx->input.rc.joy_left.y * 2.5f;
}
#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_GIMBAL */
/* 从 RC 输入生成射击命令 */
#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_SHOOT
static void CMD_RC_BuildShootCmd(CMD_t *ctx) {
if (ctx->input.online[CMD_SRC_RC]) {
ctx->output.shoot.cmd.mode = SHOOT_MODE_SINGLE;
} else {
ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE;
}
/* 根据右拨杆控制射击 */
switch (ctx->input.rc.sw[1]) {
case CMD_SW_DOWN:
ctx->output.shoot.cmd.ready = true;
ctx->output.shoot.cmd.firecmd = true;
break;
case CMD_SW_MID:
ctx->output.shoot.cmd.ready = true;
ctx->output.shoot.cmd.firecmd = false;
break;
case CMD_SW_UP:
default:
ctx->output.shoot.cmd.ready = false;
ctx->output.shoot.cmd.firecmd = false;
break;
}
}
#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_SHOOT */
/* 从 RC 输入生成履带命令 */
#if CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_TRACK
static void CMD_RC_BuildTrackCmd(CMD_t *ctx) {
CMD_RCModeMap_t *map = &ctx->config->rc_mode_map;
if (!ctx->input.online[CMD_SRC_RC]) {
ctx->output.track.cmd.enable = false;
ctx->output.track.cmd.vel = 0.0f;
ctx->output.track.cmd.omega = 0.0f;
return;
}
switch (ctx->input.rc.sw[0]) {
case CMD_SW_UP:
ctx->output.track.cmd.enable = map->track_sw_up;
break;
case CMD_SW_MID:
ctx->output.track.cmd.enable = map->track_sw_mid;
break;
case CMD_SW_DOWN:
ctx->output.track.cmd.enable = map->track_sw_down;
break;
default:
ctx->output.track.cmd.enable = false;
break;
}
ctx->output.track.cmd.enable = ctx->input.online[CMD_SRC_RC];
ctx->output.track.cmd.vel = ctx->input.rc.joy_right.y * 2.0f;
if(fabsf(ctx->input.rc.joy_right.y * 2.0f) > 1.0f)
ctx->output.track.cmd.vel = ctx->output.track.cmd.vel > 0 ? 1.0f
: -1.0f;
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_TRACK);
}
#endif /* CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_TRACK */
/* 从PC输入生成底盘命令 */
#if CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_CHASSIS
static void CMD_PC_BuildChassisCmd(CMD_t *ctx) {
if (!ctx->input.online[CMD_SRC_PC]) {
ctx->output.chassis.cmd.mode = CHASSIS_MODE_RELAX;
return;
}
ctx->output.chassis.cmd.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
/* WASD控制移动 */
ctx->output.chassis.cmd.ctrl_vec.vx = 0.0f;
ctx->output.chassis.cmd.ctrl_vec.vy = 0.0f;
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_CHASSIS);
}
#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_CHASSIS */
/* 从PC输入生成云台命令 */
#if CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_GIMBAL
static void CMD_PC_BuildGimbalCmd(CMD_t *ctx) {
CMD_Sensitivity_t *sens = &ctx->config->sensitivity;
if (!ctx->input.online[CMD_SRC_PC]) {
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
return;
}
ctx->output.gimbal.cmd.is_ai = false;
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_ABSOLUTE;
/* 鼠标控制云台 */
ctx->output.gimbal.cmd.delta_yaw = (float)-ctx->input.pc.mouse.x * ctx->timer.dt * sens->mouse_sens;
ctx->output.gimbal.cmd.delta_pit = (float)ctx->input.pc.mouse.y * ctx->timer.dt * sens->mouse_sens * 1.5f;
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_GIMBAL);
}
#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_GIMBAL */
/* 从PC输入生成射击命令 */
#if CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_SHOOT
static void CMD_PC_BuildShootCmd(CMD_t *ctx) {
if (!ctx->input.online[CMD_SRC_PC]) {
ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE;
return;
}
ctx->output.shoot.cmd.ready = true;
ctx->output.shoot.cmd.firecmd = ctx->input.pc.mouse.l_click;
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_SHOOT);
}
#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_SHOOT */
/* 从PC输入生成履带命令 */
#if CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_TRACK
static void CMD_PC_BuildTrackCmd(CMD_t *ctx) {
if (!ctx->input.online[CMD_SRC_PC]) {
ctx->output.track.cmd.enable = false;
ctx->output.track.cmd.vel = 0.0f;
ctx->output.track.cmd.omega = 0.0f;
return;
}
ctx->output.track.cmd.enable = true;
/* 可根据需要添加PC对履带的控制，例如键盘按键 */
ctx->output.track.cmd.vel = 0.0f;
ctx->output.track.cmd.omega = 0.0f;
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_TRACK);
}
#endif /* CMD_ENABLE_SRC_PC && CMD_ENABLE_MODULE_TRACK */
/* 从NUC/AI输入生成云台命令 */
#if CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_GIMBAL
static void CMD_NUC_BuildGimbalCmd(CMD_t *ctx) {
if (!ctx->input.online[CMD_SRC_NUC]) {
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
return;
}
/* 使用AI提供的云台控制数据 */
if (ctx->input.nuc.mode!=0) {
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELATIVE;
ctx->output.gimbal.cmd.is_ai = true;
ctx->output.gimbal.cmd.setpoint_yaw = ctx->input.nuc.gimbal.setpoint.yaw;
ctx->output.gimbal.cmd.setpoint_pit = ctx->input.nuc.gimbal.setpoint.pit;
}
}
#endif /* CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_GIMBAL */
/* 从 NUC/AI 输入生成射击命令 */
#if CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_SHOOT
static void CMD_NUC_BuildShootCmd(CMD_t *ctx) {
if (!ctx->input.online[CMD_SRC_NUC]) {
ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE;
return;
}
/* 根据AI模式决定射击行为 */
switch (ctx->input.nuc.mode) {
case 0:
ctx->output.shoot.cmd.ready = false;
ctx->output.shoot.cmd.firecmd = false;
break;
case 1:
ctx->output.shoot.cmd.ready = true;
ctx->output.shoot.cmd.firecmd = false;
break;
case 2:
if (ctx->input.rc.sw[1]==CMD_SW_DOWN) {
ctx->output.shoot.cmd.ready = true;
ctx->output.shoot.cmd.firecmd = true;
}else {
ctx->output.shoot.cmd.ready = true;
ctx->output.shoot.cmd.firecmd = false;
}
break;
}
}
#endif /* CMD_ENABLE_SRC_NUC && CMD_ENABLE_MODULE_SHOOT */
/* 离线安全模式 */
static void CMD_SetOfflineMode(CMD_t *ctx) {
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.cmd.mode = CHASSIS_MODE_RELAX;
#endif
#if CMD_ENABLE_MODULE_GIMBAL
ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX;
#endif
#if CMD_ENABLE_MODULE_SHOOT
ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE;
#endif
#if CMD_ENABLE_MODULE_TRACK
ctx->output.track.cmd.enable = false;
#endif
}
/* ========================================================================== */
/* 公开API实现 */
/* ========================================================================== */
int8_t CMD_Init(CMD_t *ctx, CMD_Config_t *config) {
if (ctx == NULL || config == NULL) {
return CMD_ERR_NULL;
}
memset(ctx, 0, sizeof(CMD_t));
ctx->config = config;
/* 初始化适配器 */
CMD_Adapter_InitAll();
/* 初始化行为处理器 */
CMD_Behavior_Init();
return CMD_OK;
}
int8_t CMD_UpdateInput(CMD_t *ctx) {
if (ctx == NULL) {
return CMD_ERR_NULL;
}
/* 保存上一帧输入 */
memcpy(&ctx->last_input, &ctx->input, sizeof(ctx->input));
/* 更新所有输入源 */
for (int i = 0; i < CMD_SRC_NUM; i++) {
CMD_Adapter_GetInput((CMD_InputSource_t)i, &ctx->input);
}
return CMD_OK;
}
typedef void (*CMD_BuildCommandFunc)(CMD_t *cmd);
typedef struct {
CMD_InputSource_t source;
CMD_BuildCommandFunc chassisFunc;
CMD_BuildCommandFunc gimbalFunc;
CMD_BuildCommandFunc shootFunc;
CMD_BuildCommandFunc trackFunc;
} CMD_SourceHandler_t;
/* Build-function conditional references */
#if CMD_ENABLE_MODULE_CHASSIS && CMD_ENABLE_SRC_RC
#define _FN_RC_CHASSIS CMD_RC_BuildChassisCmd
#else
#define _FN_RC_CHASSIS
#endif
#if CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_SRC_RC
#define _FN_RC_GIMBAL CMD_RC_BuildGimbalCmd
#else
#define _FN_RC_GIMBAL NULL
#endif
#if CMD_ENABLE_MODULE_SHOOT && CMD_ENABLE_SRC_RC
#define _FN_RC_SHOOT CMD_RC_BuildShootCmd
#else
#define _FN_RC_SHOOT NULL
#endif
#if CMD_ENABLE_MODULE_TRACK && CMD_ENABLE_SRC_RC
#define _FN_RC_TRACK CMD_RC_BuildTrackCmd
#else
#define _FN_RC_TRACK NULL
#endif
#if CMD_ENABLE_MODULE_CHASSIS && CMD_ENABLE_SRC_PC
#define _FN_PC_CHASSIS CMD_PC_BuildChassisCmd
#else
#define _FN_PC_CHASSIS NULL
#endif
#if CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_SRC_PC
#define _FN_PC_GIMBAL CMD_PC_BuildGimbalCmd
#else
#define _FN_PC_GIMBAL NULL
#endif
#if CMD_ENABLE_MODULE_SHOOT && CMD_ENABLE_SRC_PC
#define _FN_PC_SHOOT CMD_PC_BuildShootCmd
#else
#define _FN_PC_SHOOT NULL
#endif
#if CMD_ENABLE_MODULE_TRACK && CMD_ENABLE_SRC_PC
#define _FN_PC_TRACK CMD_PC_BuildTrackCmd
#else
#define _FN_PC_TRACK NULL
#endif
#if CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_SRC_NUC
#define _FN_NUC_GIMBAL CMD_NUC_BuildGimbalCmd
#else
#define _FN_NUC_GIMBAL NULL
#endif
#if CMD_ENABLE_MODULE_SHOOT && CMD_ENABLE_SRC_NUC
#define _FN_NUC_SHOOT CMD_NUC_BuildShootCmd
#else
#define _FN_NUC_SHOOT NULL
#endif
CMD_SourceHandler_t sourceHandlers[CMD_SRC_NUM] = {
#if CMD_ENABLE_SRC_RC
[CMD_SRC_RC] = {CMD_SRC_RC, _FN_RC_CHASSIS, _FN_RC_GIMBAL, _FN_RC_SHOOT, _FN_RC_TRACK},
#endif
#if CMD_ENABLE_SRC_PC
[CMD_SRC_PC] = {CMD_SRC_PC, _FN_PC_CHASSIS, _FN_PC_GIMBAL, _FN_PC_SHOOT, _FN_PC_TRACK},
#endif
#if CMD_ENABLE_SRC_NUC
[CMD_SRC_NUC] = {CMD_SRC_NUC, NULL, _FN_NUC_GIMBAL, _FN_NUC_SHOOT, NULL},
#endif
#if CMD_ENABLE_SRC_REF
[CMD_SRC_REF] = {CMD_SRC_REF, NULL, NULL, NULL, NULL},
#endif
};
int8_t CMD_Arbitrate(CMD_t *ctx) {
if (ctx == NULL) {
return CMD_ERR_NULL;
}
/* RC > PC priority arbitration */
CMD_InputSource_t candidates[] = {
#if CMD_ENABLE_SRC_RC
CMD_SRC_RC,
#endif
#if CMD_ENABLE_SRC_PC
CMD_SRC_PC,
#endif
};
const int num_candidates = sizeof(candidates) / sizeof(candidates[0]);
/* keep current source if still online */
if (ctx->active_source < CMD_SRC_NUM &&
#if CMD_ENABLE_SRC_REF
ctx->active_source != CMD_SRC_REF &&
#endif
ctx->input.online[ctx->active_source]) {
goto seize;
}
/* 否则选择第一个可用的控制输入源 */
for (int i = 0; i < num_candidates; i++) {
CMD_InputSource_t src = candidates[i];
if (ctx->input.online[src]) {
ctx->active_source = src;
break;
}else {
ctx->active_source = CMD_SRC_NUM;
continue;
}
}
/* 优先级抢占逻辑 */
seize:
/* reset output sources */
#if CMD_ENABLE_MODULE_CHASSIS
ctx->output.chassis.source = ctx->active_source;
#endif
#if CMD_ENABLE_MODULE_GIMBAL
ctx->output.gimbal.source = ctx->active_source;
#endif
#if CMD_ENABLE_MODULE_SHOOT
ctx->output.shoot.source = ctx->active_source;
#endif
#if CMD_ENABLE_MODULE_TRACK
ctx->output.track.source = ctx->active_source;
#endif
CMD_Behavior_ProcessAll(ctx, &ctx->input, &ctx->last_input, CMD_MODULE_NONE);
#if CMD_ENABLE_SRC_NUC && CMD_ENABLE_SRC_RC && CMD_ENABLE_MODULE_GIMBAL && CMD_ENABLE_MODULE_SHOOT
if (ctx->input.online[CMD_SRC_NUC]) {
if (ctx->active_source==CMD_SRC_RC) {
if (ctx->input.rc.sw[0] == CMD_SW_DOWN) {
ctx->output.gimbal.source = CMD_SRC_NUC;
ctx->output.shoot.source = CMD_SRC_NUC;
}
}
}
#endif
return CMD_OK;
}
int8_t CMD_GenerateCommands(CMD_t *ctx) {
if (ctx == NULL) {
return CMD_ERR_NULL;
}
/* 更新时间 */
uint64_t now_us = BSP_TIME_Get_us();
ctx->timer.now = now_us / 1000000.0f;
ctx->timer.dt = (now_us - ctx->timer.last_us) / 1000000.0f;
ctx->timer.last_us = now_us;
/* 没有有效输入源 */
if (ctx->active_source >= CMD_SRC_NUM) {
CMD_SetOfflineMode(ctx);
return CMD_ERR_NO_INPUT;
}
#if CMD_ENABLE_MODULE_GIMBAL
if (sourceHandlers[ctx->output.gimbal.source].gimbalFunc != NULL) {
sourceHandlers[ctx->output.gimbal.source].gimbalFunc(ctx);
}
#endif
#if CMD_ENABLE_MODULE_CHASSIS
if (sourceHandlers[ctx->output.chassis.source].chassisFunc != NULL) {
sourceHandlers[ctx->output.chassis.source].chassisFunc(ctx);
}
#endif
#if CMD_ENABLE_MODULE_SHOOT
if (sourceHandlers[ctx->output.shoot.source].shootFunc != NULL) {
sourceHandlers[ctx->output.shoot.source].shootFunc(ctx);
}
#endif
#if CMD_ENABLE_MODULE_TRACK
if (sourceHandlers[ctx->output.track.source].trackFunc != NULL) {
sourceHandlers[ctx->output.track.source].trackFunc(ctx);
}
#endif
return CMD_OK;
}
int8_t CMD_Update(CMD_t *ctx) {
int8_t ret;
ret = CMD_UpdateInput(ctx);
if (ret != CMD_OK) return ret;
CMD_Arbitrate(ctx);
ret = CMD_GenerateCommands(ctx);
return ret;
}

Some files were not shown because too many files have changed in this diff Show More