From 843ca3d9315041123a63679301d6a14990811dae Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sat, 30 Aug 2025 11:37:24 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E9=99=80=E8=9E=BA=E4=BB=AA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Core/Src/freertos.c | 12 +- Core/Src/stm32f4xx_it.c | 1026 +++++++++++++------------- MDK-ARM/DevC.uvprojx | 174 ++++- MDK-ARM/DevC/DevC.lnp | 26 + MDK-ARM/DevC/ExtDll.iex | 2 + MDK-ARM/EventRecorderStub.scvd | 9 + User/bsp/bsp.h | 16 + User/bsp/bsp_config.yaml | 132 ++++ User/bsp/can.c | 617 ++++++++++++++++ User/bsp/can.h | 233 ++++++ User/bsp/dwt.c | 122 +++ User/bsp/dwt.h | 37 + User/bsp/gpio.c | 114 +++ User/bsp/gpio.h | 45 ++ User/bsp/mm.c | 14 + User/bsp/mm.h | 20 + User/bsp/pwm.c | 109 +++ User/bsp/pwm.h | 47 ++ User/bsp/spi.c | 165 +++++ User/bsp/spi.h | 58 ++ User/bsp/time.c | 65 ++ User/bsp/time.h | 31 + User/bsp/uart.c | 139 ++++ User/bsp/uart.h | 53 ++ User/component/ahrs.c | 405 ++++++++++ User/component/ahrs.h | 98 +++ User/component/cmd.c | 375 ++++++++++ User/component/cmd.h | 306 ++++++++ User/component/component_config.yaml | 21 + User/component/filter.c | 185 +++++ User/component/filter.h | 104 +++ User/component/limiter.c | 107 +++ User/component/limiter.h | 55 ++ User/component/pid.c | 158 ++++ User/component/pid.h | 95 +++ User/component/user_math.c | 132 ++++ User/component/user_math.h | 161 ++++ User/device/buzzer.c | 44 ++ User/device/buzzer.h | 35 + User/device/device.h | 31 + User/device/device_config.yaml | 19 + User/device/dm_imu.c | 271 +++++++ User/device/dm_imu.h | 90 +++ User/device/dr16.c | 85 +++ User/device/dr16.h | 47 ++ User/device/motor.c | 49 ++ User/device/motor.h | 34 + User/device/motor_lk.c | 310 ++++++++ User/device/motor_lk.h | 119 +++ User/device/motor_lz.c | 425 +++++++++++ User/device/motor_lz.h | 193 +++++ User/module/config.c | 35 + User/module/config.h | 28 + User/task/atti_esti.c | 51 ++ User/task/blink.c | 50 ++ User/task/config.yaml | 21 + User/task/init.c | 44 ++ User/task/rc.c | 44 ++ User/task/user_task.c | 26 + User/task/user_task.h | 96 +++ 60 files changed, 7094 insertions(+), 521 deletions(-) create mode 100644 MDK-ARM/DevC/ExtDll.iex create mode 100644 MDK-ARM/EventRecorderStub.scvd create mode 100644 User/bsp/bsp.h create mode 100644 User/bsp/bsp_config.yaml create mode 100644 User/bsp/can.c create mode 100644 User/bsp/can.h create mode 100644 User/bsp/dwt.c create mode 100644 User/bsp/dwt.h create mode 100644 User/bsp/gpio.c create mode 100644 User/bsp/gpio.h create mode 100644 User/bsp/mm.c create mode 100644 User/bsp/mm.h create mode 100644 User/bsp/pwm.c create mode 100644 User/bsp/pwm.h create mode 100644 User/bsp/spi.c create mode 100644 User/bsp/spi.h create mode 100644 User/bsp/time.c create mode 100644 User/bsp/time.h create mode 100644 User/bsp/uart.c create mode 100644 User/bsp/uart.h create mode 100644 User/component/ahrs.c create mode 100644 User/component/ahrs.h create mode 100644 User/component/cmd.c create mode 100644 User/component/cmd.h create mode 100644 User/component/component_config.yaml create mode 100644 User/component/filter.c create mode 100644 User/component/filter.h create mode 100644 User/component/limiter.c create mode 100644 User/component/limiter.h create mode 100644 User/component/pid.c create mode 100644 User/component/pid.h create mode 100644 User/component/user_math.c create mode 100644 User/component/user_math.h create mode 100644 User/device/buzzer.c create mode 100644 User/device/buzzer.h create mode 100644 User/device/device.h create mode 100644 User/device/device_config.yaml create mode 100644 User/device/dm_imu.c create mode 100644 User/device/dm_imu.h create mode 100644 User/device/dr16.c create mode 100644 User/device/dr16.h create mode 100644 User/device/motor.c create mode 100644 User/device/motor.h create mode 100644 User/device/motor_lk.c create mode 100644 User/device/motor_lk.h create mode 100644 User/device/motor_lz.c create mode 100644 User/device/motor_lz.h create mode 100644 User/module/config.c create mode 100644 User/module/config.h create mode 100644 User/task/atti_esti.c create mode 100644 User/task/blink.c create mode 100644 User/task/config.yaml create mode 100644 User/task/init.c create mode 100644 User/task/rc.c create mode 100644 User/task/user_task.c create mode 100644 User/task/user_task.h diff --git a/Core/Src/freertos.c b/Core/Src/freertos.c index f9d2db6..cce21c4 100644 --- a/Core/Src/freertos.c +++ b/Core/Src/freertos.c @@ -27,6 +27,7 @@ /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ +#include "task/user_task.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -125,7 +126,8 @@ void MX_FREERTOS_Init(void) { /* USER CODE BEGIN RTOS_THREADS */ /* add threads, ... */ - /* USER CODE END RTOS_THREADS */ + osThreadNew(Task_Init, NULL, &attr_init); // 创建初始化任务 +/* USER CODE END RTOS_THREADS */ /* USER CODE BEGIN RTOS_EVENTS */ /* add events, ... */ @@ -145,12 +147,8 @@ void StartDefaultTask(void *argument) /* init code for USB_DEVICE */ MX_USB_DEVICE_Init(); /* USER CODE BEGIN StartDefaultTask */ - /* Infinite loop */ - for(;;) - { - osDelay(1); - } - /* USER CODE END StartDefaultTask */ + osThreadTerminate(osThreadGetId()); +/* USER CODE END StartDefaultTask */ } /* Private application code --------------------------------------------------*/ diff --git a/Core/Src/stm32f4xx_it.c b/Core/Src/stm32f4xx_it.c index 96a9679..dc1d46b 100644 --- a/Core/Src/stm32f4xx_it.c +++ b/Core/Src/stm32f4xx_it.c @@ -1,512 +1,514 @@ -/* USER CODE BEGIN Header */ -/** - ****************************************************************************** - * @file stm32f4xx_it.c - * @brief Interrupt Service Routines. - ****************************************************************************** - * @attention - * - *

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

- * - * This software component is licensed by ST under Ultimate Liberty license - * SLA0044, the "License"; You may not use this file except in compliance with - * the License. You may obtain a copy of the License at: - * www.st.com/SLA0044 - * - ****************************************************************************** - */ -/* USER CODE END Header */ - -/* Includes ------------------------------------------------------------------*/ -#include "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 PCD_HandleTypeDef hpcd_USB_OTG_FS; -extern CAN_HandleTypeDef hcan1; -extern CAN_HandleTypeDef hcan2; -extern DMA_HandleTypeDef hdma_i2c2_tx; -extern DMA_HandleTypeDef hdma_i2c3_rx; -extern DMA_HandleTypeDef hdma_spi1_rx; -extern DMA_HandleTypeDef hdma_spi1_tx; -extern TIM_HandleTypeDef htim1; -extern TIM_HandleTypeDef htim7; -extern DMA_HandleTypeDef hdma_usart1_tx; -extern DMA_HandleTypeDef hdma_usart1_rx; -extern DMA_HandleTypeDef hdma_usart3_rx; -extern DMA_HandleTypeDef hdma_usart6_rx; -extern DMA_HandleTypeDef hdma_usart6_tx; -extern UART_HandleTypeDef huart1; -extern UART_HandleTypeDef huart6; -/* 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 EXTI line0 interrupt. - */ -void EXTI0_IRQHandler(void) -{ - /* USER CODE BEGIN EXTI0_IRQn 0 */ - - /* USER CODE END EXTI0_IRQn 0 */ - HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_0); - /* USER CODE BEGIN EXTI0_IRQn 1 */ - - /* USER CODE END EXTI0_IRQn 1 */ -} - -/** - * @brief This function handles EXTI line3 interrupt. - */ -void EXTI3_IRQHandler(void) -{ - /* USER CODE BEGIN EXTI3_IRQn 0 */ - - /* USER CODE END EXTI3_IRQn 0 */ - HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_3); - /* USER CODE BEGIN EXTI3_IRQn 1 */ - - /* USER CODE END EXTI3_IRQn 1 */ -} - -/** - * @brief This function handles EXTI line4 interrupt. - */ -void EXTI4_IRQHandler(void) -{ - /* USER CODE BEGIN EXTI4_IRQn 0 */ - - /* USER CODE END EXTI4_IRQn 0 */ - HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_4); - /* USER CODE BEGIN EXTI4_IRQn 1 */ - - /* USER CODE END EXTI4_IRQn 1 */ -} - -/** - * @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 DMA1 stream2 global interrupt. - */ -void DMA1_Stream2_IRQHandler(void) -{ - /* USER CODE BEGIN DMA1_Stream2_IRQn 0 */ - - /* USER CODE END DMA1_Stream2_IRQn 0 */ - HAL_DMA_IRQHandler(&hdma_i2c3_rx); - /* USER CODE BEGIN DMA1_Stream2_IRQn 1 */ - - /* USER CODE END DMA1_Stream2_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 EXTI line[9:5] interrupts. - */ -void EXTI9_5_IRQHandler(void) -{ - /* USER CODE BEGIN EXTI9_5_IRQn 0 */ - - /* USER CODE END EXTI9_5_IRQn 0 */ - HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_5); - /* USER CODE BEGIN EXTI9_5_IRQn 1 */ - - /* USER CODE END EXTI9_5_IRQn 1 */ -} - -/** - * @brief This function handles TIM1 break interrupt and TIM9 global interrupt. - */ -void TIM1_BRK_TIM9_IRQHandler(void) -{ - /* USER CODE BEGIN TIM1_BRK_TIM9_IRQn 0 */ - - /* USER CODE END TIM1_BRK_TIM9_IRQn 0 */ - HAL_TIM_IRQHandler(&htim1); - /* USER CODE BEGIN TIM1_BRK_TIM9_IRQn 1 */ - - /* USER CODE END TIM1_BRK_TIM9_IRQn 1 */ -} - -/** - * @brief This function handles USART1 global interrupt. - */ -void USART1_IRQHandler(void) -{ - /* USER CODE BEGIN USART1_IRQn 0 */ - - /* USER CODE END USART1_IRQn 0 */ - HAL_UART_IRQHandler(&huart1); - /* USER CODE BEGIN USART1_IRQn 1 */ - - /* USER CODE END USART1_IRQn 1 */ -} - -/** - * @brief This function handles DMA1 stream7 global interrupt. - */ -void DMA1_Stream7_IRQHandler(void) -{ - /* USER CODE BEGIN DMA1_Stream7_IRQn 0 */ - - /* USER CODE END DMA1_Stream7_IRQn 0 */ - HAL_DMA_IRQHandler(&hdma_i2c2_tx); - /* USER CODE BEGIN DMA1_Stream7_IRQn 1 */ - - /* USER CODE END DMA1_Stream7_IRQn 1 */ -} - -/** - * @brief This function handles TIM7 global interrupt. - */ -void TIM7_IRQHandler(void) -{ - /* USER CODE BEGIN TIM7_IRQn 0 */ - - /* USER CODE END TIM7_IRQn 0 */ - HAL_TIM_IRQHandler(&htim7); - /* USER CODE BEGIN TIM7_IRQn 1 */ - - /* USER CODE END TIM7_IRQn 1 */ -} - -/** - * @brief This function handles DMA2 stream1 global interrupt. - */ -void DMA2_Stream1_IRQHandler(void) -{ - /* USER CODE BEGIN DMA2_Stream1_IRQn 0 */ - - /* USER CODE END DMA2_Stream1_IRQn 0 */ - HAL_DMA_IRQHandler(&hdma_usart6_rx); - /* USER CODE BEGIN DMA2_Stream1_IRQn 1 */ - - /* USER CODE END DMA2_Stream1_IRQn 1 */ -} - -/** - * @brief This function handles DMA2 stream2 global interrupt. - */ -void DMA2_Stream2_IRQHandler(void) -{ - /* USER CODE BEGIN DMA2_Stream2_IRQn 0 */ - - /* USER CODE END DMA2_Stream2_IRQn 0 */ - HAL_DMA_IRQHandler(&hdma_spi1_rx); - /* USER CODE BEGIN DMA2_Stream2_IRQn 1 */ - - /* USER CODE END DMA2_Stream2_IRQn 1 */ -} - -/** - * @brief This function handles DMA2 stream3 global interrupt. - */ -void DMA2_Stream3_IRQHandler(void) -{ - /* USER CODE BEGIN DMA2_Stream3_IRQn 0 */ - - /* USER CODE END DMA2_Stream3_IRQn 0 */ - HAL_DMA_IRQHandler(&hdma_spi1_tx); - /* USER CODE BEGIN DMA2_Stream3_IRQn 1 */ - - /* USER CODE END DMA2_Stream3_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 */ -} - -/** - * @brief This function handles USB On The Go FS global interrupt. - */ -void OTG_FS_IRQHandler(void) -{ - /* USER CODE BEGIN OTG_FS_IRQn 0 */ - - /* USER CODE END OTG_FS_IRQn 0 */ - HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); - /* USER CODE BEGIN OTG_FS_IRQn 1 */ - - /* USER CODE END OTG_FS_IRQn 1 */ -} - -/** - * @brief This function handles DMA2 stream5 global interrupt. - */ -void DMA2_Stream5_IRQHandler(void) -{ - /* USER CODE BEGIN DMA2_Stream5_IRQn 0 */ - - /* USER CODE END DMA2_Stream5_IRQn 0 */ - HAL_DMA_IRQHandler(&hdma_usart1_rx); - /* USER CODE BEGIN DMA2_Stream5_IRQn 1 */ - - /* USER CODE END DMA2_Stream5_IRQn 1 */ -} - -/** - * @brief This function handles DMA2 stream6 global interrupt. - */ -void DMA2_Stream6_IRQHandler(void) -{ - /* USER CODE BEGIN DMA2_Stream6_IRQn 0 */ - - /* USER CODE END DMA2_Stream6_IRQn 0 */ - HAL_DMA_IRQHandler(&hdma_usart6_tx); - /* USER CODE BEGIN DMA2_Stream6_IRQn 1 */ - - /* USER CODE END DMA2_Stream6_IRQn 1 */ -} - -/** - * @brief This function handles DMA2 stream7 global interrupt. - */ -void DMA2_Stream7_IRQHandler(void) -{ - /* USER CODE BEGIN DMA2_Stream7_IRQn 0 */ - - /* USER CODE END DMA2_Stream7_IRQn 0 */ - HAL_DMA_IRQHandler(&hdma_usart1_tx); - /* USER CODE BEGIN DMA2_Stream7_IRQn 1 */ - - /* USER CODE END DMA2_Stream7_IRQn 1 */ -} - -/** - * @brief This function handles USART6 global interrupt. - */ -void USART6_IRQHandler(void) -{ - /* USER CODE BEGIN USART6_IRQn 0 */ - - /* USER CODE END USART6_IRQn 0 */ - HAL_UART_IRQHandler(&huart6); - /* USER CODE BEGIN USART6_IRQn 1 */ - - /* USER CODE END USART6_IRQn 1 */ -} - -/* USER CODE BEGIN 1 */ - -/* USER CODE END 1 */ -/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * @file stm32f4xx_it.c + * @brief Interrupt Service Routines. + ****************************************************************************** + * @attention + * + *

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

+ * + * This software component is licensed by ST under Ultimate Liberty license + * SLA0044, the "License"; You may not use this file except in compliance with + * the License. You may obtain a copy of the License at: + * www.st.com/SLA0044 + * + ****************************************************************************** + */ +/* USER CODE END Header */ + +/* Includes ------------------------------------------------------------------*/ +#include "main.h" +#include "stm32f4xx_it.h" +#include "FreeRTOS.h" +#include "task.h" +/* Private includes ----------------------------------------------------------*/ +/* USER CODE BEGIN Includes */ +#include "bsp/uart.h" +/* 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 PCD_HandleTypeDef hpcd_USB_OTG_FS; +extern CAN_HandleTypeDef hcan1; +extern CAN_HandleTypeDef hcan2; +extern DMA_HandleTypeDef hdma_i2c2_tx; +extern DMA_HandleTypeDef hdma_i2c3_rx; +extern DMA_HandleTypeDef hdma_spi1_rx; +extern DMA_HandleTypeDef hdma_spi1_tx; +extern TIM_HandleTypeDef htim1; +extern TIM_HandleTypeDef htim7; +extern DMA_HandleTypeDef hdma_usart1_tx; +extern DMA_HandleTypeDef hdma_usart1_rx; +extern DMA_HandleTypeDef hdma_usart3_rx; +extern DMA_HandleTypeDef hdma_usart6_rx; +extern DMA_HandleTypeDef hdma_usart6_tx; +extern UART_HandleTypeDef huart1; +extern UART_HandleTypeDef huart6; +/* 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 EXTI line0 interrupt. + */ +void EXTI0_IRQHandler(void) +{ + /* USER CODE BEGIN EXTI0_IRQn 0 */ + + /* USER CODE END EXTI0_IRQn 0 */ + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_0); + /* USER CODE BEGIN EXTI0_IRQn 1 */ + + /* USER CODE END EXTI0_IRQn 1 */ +} + +/** + * @brief This function handles EXTI line3 interrupt. + */ +void EXTI3_IRQHandler(void) +{ + /* USER CODE BEGIN EXTI3_IRQn 0 */ + + /* USER CODE END EXTI3_IRQn 0 */ + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_3); + /* USER CODE BEGIN EXTI3_IRQn 1 */ + + /* USER CODE END EXTI3_IRQn 1 */ +} + +/** + * @brief This function handles EXTI line4 interrupt. + */ +void EXTI4_IRQHandler(void) +{ + /* USER CODE BEGIN EXTI4_IRQn 0 */ + + /* USER CODE END EXTI4_IRQn 0 */ + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_4); + /* USER CODE BEGIN EXTI4_IRQn 1 */ + + /* USER CODE END EXTI4_IRQn 1 */ +} + +/** + * @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 DMA1 stream2 global interrupt. + */ +void DMA1_Stream2_IRQHandler(void) +{ + /* USER CODE BEGIN DMA1_Stream2_IRQn 0 */ + + /* USER CODE END DMA1_Stream2_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_i2c3_rx); + /* USER CODE BEGIN DMA1_Stream2_IRQn 1 */ + + /* USER CODE END DMA1_Stream2_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 EXTI line[9:5] interrupts. + */ +void EXTI9_5_IRQHandler(void) +{ + /* USER CODE BEGIN EXTI9_5_IRQn 0 */ + + /* USER CODE END EXTI9_5_IRQn 0 */ + HAL_GPIO_EXTI_IRQHandler(GPIO_PIN_5); + /* USER CODE BEGIN EXTI9_5_IRQn 1 */ + + /* USER CODE END EXTI9_5_IRQn 1 */ +} + +/** + * @brief This function handles TIM1 break interrupt and TIM9 global interrupt. + */ +void TIM1_BRK_TIM9_IRQHandler(void) +{ + /* USER CODE BEGIN TIM1_BRK_TIM9_IRQn 0 */ + + /* USER CODE END TIM1_BRK_TIM9_IRQn 0 */ + HAL_TIM_IRQHandler(&htim1); + /* USER CODE BEGIN TIM1_BRK_TIM9_IRQn 1 */ + + /* USER CODE END TIM1_BRK_TIM9_IRQn 1 */ +} + +/** + * @brief This function handles USART1 global interrupt. + */ +void USART1_IRQHandler(void) +{ + /* USER CODE BEGIN USART1_IRQn 0 */ + + /* USER CODE END USART1_IRQn 0 */ + HAL_UART_IRQHandler(&huart1); + /* USER CODE BEGIN USART1_IRQn 1 */ + + /* USER CODE END USART1_IRQn 1 */ +} + +/** + * @brief This function handles DMA1 stream7 global interrupt. + */ +void DMA1_Stream7_IRQHandler(void) +{ + /* USER CODE BEGIN DMA1_Stream7_IRQn 0 */ + + /* USER CODE END DMA1_Stream7_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_i2c2_tx); + /* USER CODE BEGIN DMA1_Stream7_IRQn 1 */ + + /* USER CODE END DMA1_Stream7_IRQn 1 */ +} + +/** + * @brief This function handles TIM7 global interrupt. + */ +void TIM7_IRQHandler(void) +{ + /* USER CODE BEGIN TIM7_IRQn 0 */ + + /* USER CODE END TIM7_IRQn 0 */ + HAL_TIM_IRQHandler(&htim7); + /* USER CODE BEGIN TIM7_IRQn 1 */ + + /* USER CODE END TIM7_IRQn 1 */ +} + +/** + * @brief This function handles DMA2 stream1 global interrupt. + */ +void DMA2_Stream1_IRQHandler(void) +{ + /* USER CODE BEGIN DMA2_Stream1_IRQn 0 */ + + /* USER CODE END DMA2_Stream1_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_usart6_rx); + /* USER CODE BEGIN DMA2_Stream1_IRQn 1 */ + + /* USER CODE END DMA2_Stream1_IRQn 1 */ +} + +/** + * @brief This function handles DMA2 stream2 global interrupt. + */ +void DMA2_Stream2_IRQHandler(void) +{ + /* USER CODE BEGIN DMA2_Stream2_IRQn 0 */ + + /* USER CODE END DMA2_Stream2_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_spi1_rx); + /* USER CODE BEGIN DMA2_Stream2_IRQn 1 */ + + /* USER CODE END DMA2_Stream2_IRQn 1 */ +} + +/** + * @brief This function handles DMA2 stream3 global interrupt. + */ +void DMA2_Stream3_IRQHandler(void) +{ + /* USER CODE BEGIN DMA2_Stream3_IRQn 0 */ + + /* USER CODE END DMA2_Stream3_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_spi1_tx); + /* USER CODE BEGIN DMA2_Stream3_IRQn 1 */ + + /* USER CODE END DMA2_Stream3_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 */ +} + +/** + * @brief This function handles USB On The Go FS global interrupt. + */ +void OTG_FS_IRQHandler(void) +{ + /* USER CODE BEGIN OTG_FS_IRQn 0 */ + + /* USER CODE END OTG_FS_IRQn 0 */ + HAL_PCD_IRQHandler(&hpcd_USB_OTG_FS); + /* USER CODE BEGIN OTG_FS_IRQn 1 */ + + /* USER CODE END OTG_FS_IRQn 1 */ +} + +/** + * @brief This function handles DMA2 stream5 global interrupt. + */ +void DMA2_Stream5_IRQHandler(void) +{ + /* USER CODE BEGIN DMA2_Stream5_IRQn 0 */ + + /* USER CODE END DMA2_Stream5_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_usart1_rx); + /* USER CODE BEGIN DMA2_Stream5_IRQn 1 */ + + /* USER CODE END DMA2_Stream5_IRQn 1 */ +} + +/** + * @brief This function handles DMA2 stream6 global interrupt. + */ +void DMA2_Stream6_IRQHandler(void) +{ + /* USER CODE BEGIN DMA2_Stream6_IRQn 0 */ + + /* USER CODE END DMA2_Stream6_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_usart6_tx); + /* USER CODE BEGIN DMA2_Stream6_IRQn 1 */ + + /* USER CODE END DMA2_Stream6_IRQn 1 */ +} + +/** + * @brief This function handles DMA2 stream7 global interrupt. + */ +void DMA2_Stream7_IRQHandler(void) +{ + /* USER CODE BEGIN DMA2_Stream7_IRQn 0 */ + + /* USER CODE END DMA2_Stream7_IRQn 0 */ + HAL_DMA_IRQHandler(&hdma_usart1_tx); + /* USER CODE BEGIN DMA2_Stream7_IRQn 1 */ + + /* USER CODE END DMA2_Stream7_IRQn 1 */ +} + +/** + * @brief This function handles USART6 global interrupt. + */ +void USART6_IRQHandler(void) +{ + /* USER CODE BEGIN USART6_IRQn 0 */ + + /* USER CODE END USART6_IRQn 0 */ + HAL_UART_IRQHandler(&huart6); + /* USER CODE BEGIN USART6_IRQn 1 */ + BSP_UART_IRQHandler(&huart6); + + /* USER CODE END USART6_IRQn 1 */ +} + +/* USER CODE BEGIN 1 */ + +/* USER CODE END 1 */ +/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ diff --git a/MDK-ARM/DevC.uvprojx b/MDK-ARM/DevC.uvprojx index b13a510..8dc5743 100644 --- a/MDK-ARM/DevC.uvprojx +++ b/MDK-ARM/DevC.uvprojx @@ -11,6 +11,7 @@ 0x4 ARM-ADS 6160000::V6.16::ARMCLANG + 6160000::V6.16::ARMCLANG 1 @@ -81,7 +82,7 @@ 0 - 1 + 0 0 @@ -339,7 +340,7 @@ USE_HAL_DRIVER,STM32F407xx - ../Core/Inc;../USB_DEVICE/App;../USB_DEVICE/Target;../Drivers/STM32F4xx_HAL_Driver/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Middlewares/ST/STM32_USB_Device_Library/Core/Inc;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include + ../Core/Inc;../USB_DEVICE/App;../USB_DEVICE/Target;../Drivers/STM32F4xx_HAL_Driver/Inc;../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../Middlewares/ST/STM32_USB_Device_Library/Core/Inc;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc;../Drivers/CMSIS/Device/ST/STM32F4xx/Include;../Drivers/CMSIS/Include;..\User @@ -730,6 +731,166 @@ + + User/bsp + + + can.c + 1 + ..\User\bsp\can.c + + + dwt.c + 1 + ..\User\bsp\dwt.c + + + gpio.c + 1 + ..\User\bsp\gpio.c + + + mm.c + 1 + ..\User\bsp\mm.c + + + pwm.c + 1 + ..\User\bsp\pwm.c + + + spi.c + 1 + ..\User\bsp\spi.c + + + time.c + 1 + ..\User\bsp\time.c + + + uart.c + 1 + ..\User\bsp\uart.c + + + + + User/component + + + ahrs.c + 1 + ..\User\component\ahrs.c + + + cmd.c + 1 + ..\User\component\cmd.c + + + filter.c + 1 + ..\User\component\filter.c + + + limiter.c + 1 + ..\User\component\limiter.c + + + pid.c + 1 + ..\User\component\pid.c + + + user_math.c + 1 + ..\User\component\user_math.c + + + + + User/device + + + buzzer.c + 1 + ..\User\device\buzzer.c + + + dm_imu.c + 1 + ..\User\device\dm_imu.c + + + dr16.c + 1 + ..\User\device\dr16.c + + + motor.c + 1 + ..\User\device\motor.c + + + motor_lk.c + 1 + ..\User\device\motor_lk.c + + + motor_lz.c + 1 + ..\User\device\motor_lz.c + + + + + User/module + + + config.c + 1 + ..\User\module\config.c + + + + + User/task + + + blink.c + 1 + ..\User\task\blink.c + + + init.c + 1 + ..\User\task\init.c + + + rc.c + 1 + ..\User\task\rc.c + + + user_task.c + 1 + ..\User\task\user_task.c + + + user_task.h + 5 + ..\User\task\user_task.h + + + atti_esti.c + 1 + ..\User\task\atti_esti.c + + + ::CMSIS @@ -750,4 +911,13 @@ + + + + DevC + 1 + + + + diff --git a/MDK-ARM/DevC/DevC.lnp b/MDK-ARM/DevC/DevC.lnp index c59ad88..60b8d66 100644 --- a/MDK-ARM/DevC/DevC.lnp +++ b/MDK-ARM/DevC/DevC.lnp @@ -61,6 +61,32 @@ "devc\usbd_ctlreq.o" "devc\usbd_ioreq.o" "devc\usbd_cdc.o" +"devc\can_1.o" +"devc\dwt.o" +"devc\gpio_1.o" +"devc\mm.o" +"devc\pwm.o" +"devc\spi_1.o" +"devc\time.o" +"devc\uart.o" +"devc\ahrs.o" +"devc\cmd.o" +"devc\filter.o" +"devc\limiter.o" +"devc\pid.o" +"devc\user_math.o" +"devc\buzzer.o" +"devc\dm_imu.o" +"devc\dr16.o" +"devc\motor.o" +"devc\motor_lk.o" +"devc\motor_lz.o" +"devc\config.o" +"devc\blink.o" +"devc\init.o" +"devc\rc.o" +"devc\user_task.o" +"devc\atti_esti.o" --strict --scatter "DevC\DevC.sct" --summary_stderr --info summarysizes --map --load_addr_map_info --xref --callgraph --symbols --info sizes --info totals --info unused --info veneers diff --git a/MDK-ARM/DevC/ExtDll.iex b/MDK-ARM/DevC/ExtDll.iex new file mode 100644 index 0000000..b661f48 --- /dev/null +++ b/MDK-ARM/DevC/ExtDll.iex @@ -0,0 +1,2 @@ +[EXTDLL] +Count=0 diff --git a/MDK-ARM/EventRecorderStub.scvd b/MDK-ARM/EventRecorderStub.scvd new file mode 100644 index 0000000..0fb3ee5 --- /dev/null +++ b/MDK-ARM/EventRecorderStub.scvd @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/User/bsp/bsp.h b/User/bsp/bsp.h new file mode 100644 index 0000000..6a3171b --- /dev/null +++ b/User/bsp/bsp.h @@ -0,0 +1,16 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#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) + +#ifdef __cplusplus +} +#endif diff --git a/User/bsp/bsp_config.yaml b/User/bsp/bsp_config.yaml new file mode 100644 index 0000000..b14c354 --- /dev/null +++ b/User/bsp/bsp_config.yaml @@ -0,0 +1,132 @@ +can: + devices: + - instance: CAN1 + name: '1' + - instance: CAN2 + name: '2' + enabled: true +dwt: + enabled: true +gpio: + configs: + - custom_name: USER_KEY + has_exti: true + ioc_label: USER_KEY + pin: PA0-WKUP + type: EXTI + - custom_name: ACCL_CS + has_exti: false + ioc_label: ACCL_CS + pin: PA4 + type: OUTPUT + - custom_name: GYRO_CS + has_exti: false + ioc_label: GYRO_CS + pin: PB0 + type: OUTPUT + - custom_name: SPI2_CS + has_exti: false + ioc_label: SPI2_CS + pin: PB12 + type: OUTPUT + - custom_name: HW0 + has_exti: false + ioc_label: HW0 + pin: PC0 + type: INPUT + - custom_name: HW1 + has_exti: false + ioc_label: HW1 + pin: PC1 + type: INPUT + - custom_name: HW2 + has_exti: false + ioc_label: HW2 + pin: PC2 + type: INPUT + - custom_name: ACCL_INT + has_exti: true + ioc_label: ACCL_INT + pin: PC4 + type: EXTI + - custom_name: GYRO_INT + has_exti: true + ioc_label: GYRO_INT + pin: PC5 + type: EXTI + - custom_name: CMPS_INT + has_exti: true + ioc_label: CMPS_INT + pin: PG3 + type: EXTI + - custom_name: CMPS_RST + has_exti: false + ioc_label: CMPS_RST + pin: PG6 + type: OUTPUT + enabled: true +mm: + enabled: true +pwm: + configs: + - channel: TIM_CHANNEL_1 + custom_name: TIM8_CH1 + label: TIM8_CH1 + timer: TIM8 + - channel: TIM_CHANNEL_3 + custom_name: LASER + label: LASER + timer: TIM3 + - channel: TIM_CHANNEL_3 + custom_name: BUZZER + label: BUZZER + timer: TIM4 + - channel: TIM_CHANNEL_2 + custom_name: TIM1_CH2 + label: TIM1_CH2 + timer: TIM1 + - channel: TIM_CHANNEL_3 + custom_name: TIM1_CH3 + label: TIM1_CH3 + timer: TIM1 + - channel: TIM_CHANNEL_4 + custom_name: TIM1_CH4 + label: TIM1_CH4 + timer: TIM1 + - channel: TIM_CHANNEL_1 + custom_name: TIM1_CH1 + label: TIM1_CH1 + timer: TIM1 + - channel: TIM_CHANNEL_1 + custom_name: IMU_HEAT_PWM + label: IMU_HEAT_PWM + timer: TIM10 + - channel: TIM_CHANNEL_1 + custom_name: LED_B + label: LED_B + timer: TIM5 + - channel: TIM_CHANNEL_2 + custom_name: LED_G + label: LED_G + timer: TIM5 + - channel: TIM_CHANNEL_3 + custom_name: LED_R + label: LED_R + timer: TIM5 + - channel: TIM_CHANNEL_2 + custom_name: TIM8_CH2 + label: TIM8_CH2 + timer: TIM8 + enabled: true +spi: + devices: + - instance: SPI1 + name: BMI088 + enabled: true +time: + enabled: true +uart: + devices: + - instance: USART3 + name: DR16 + enabled: true diff --git a/User/bsp/can.c b/User/bsp/can.c new file mode 100644 index 0000000..f68144b --- /dev/null +++ b/User/bsp/can.c @@ -0,0 +1,617 @@ +/* Includes ----------------------------------------------------------------- */ +#include "bsp/can.h" +#include "bsp/bsp.h" +#include +#include +#include + +/* Private define ----------------------------------------------------------- */ +#define CAN_QUEUE_MUTEX_TIMEOUT 100 /* 队列互斥锁超时时间(ms) */ + +/* 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; + +/* 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解析器 */ + +/* 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 int8_t BSP_CAN_DeleteIdQueue(BSP_CAN_t can, uint32_t can_id); +static void BSP_CAN_RxFifo0Callback(void); +static void BSP_CAN_RxFifo1Callback(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); + +/* Private functions -------------------------------------------------------- */ + +/** + * @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 删除指定CAN ID的消息队列 + * @note 内部函数，已包含互斥锁保护 + */ +static int8_t BSP_CAN_DeleteIdQueue(BSP_CAN_t can, uint32_t can_id) { + if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) != osOK) { + return BSP_ERR_TIMEOUT; + } + BSP_CAN_QueueNode_t **current = &queue_list; + while (*current != NULL) { + if ((*current)->can == can && (*current)->can_id == can_id) { + BSP_CAN_QueueNode_t *to_delete = *current; + *current = (*current)->next; + osMessageQueueDelete(to_delete->queue); + BSP_Free(to_delete); + osMutexRelease(queue_mutex); + return BSP_OK; + } + current = &(*current)->next; + } + osMutexRelease(queue_mutex); + return BSP_ERR; // 未找到 +} +/** + * @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 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)); + + // 初始化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); + + HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING); + + // 初始化 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); + HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING); + + + inited = true; + return BSP_OK; +} + +int8_t BSP_CAN_DeInit(void) { + if (!inited) { + return BSP_ERR; + } + + // 删除所有队列 + if (osMutexAcquire(queue_mutex, CAN_QUEUE_MUTEX_TIMEOUT) == osOK) { + BSP_CAN_QueueNode_t *current = queue_list; + while (current != NULL) { + BSP_CAN_QueueNode_t *next = current->next; + osMessageQueueDelete(current->queue); + BSP_Free(current); + current = next; + } + queue_list = NULL; + osMutexRelease(queue_mutex); + } + + // 删除互斥锁 + if (queue_mutex != NULL) { + osMutexDelete(queue_mutex); + queue_mutex = NULL; + } + + // 清零回调函数数组 + memset(CAN_Callback, 0, sizeof(CAN_Callback)); + + // 重置ID解析器 + id_parser = NULL; + + inited = false; + 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; + } + + CAN_TxHeaderTypeDef header = {0}; + uint32_t mailbox; + + switch (format) { + case BSP_CAN_FORMAT_STD_DATA: + header.StdId = id; + header.IDE = CAN_ID_STD; + header.RTR = CAN_RTR_DATA; + break; + case BSP_CAN_FORMAT_EXT_DATA: + header.ExtId = id; + header.IDE = CAN_ID_EXT; + header.RTR = CAN_RTR_DATA; + break; + case BSP_CAN_FORMAT_STD_REMOTE: + header.StdId = id; + header.IDE = CAN_ID_STD; + header.RTR = CAN_RTR_REMOTE; + break; + case BSP_CAN_FORMAT_EXT_REMOTE: + header.ExtId = id; + header.IDE = CAN_ID_EXT; + header.RTR = CAN_RTR_REMOTE; + break; + default: + return BSP_ERR; + } + + header.DLC = dlc; + header.TransmitGlobalTime = DISABLE; + + HAL_StatusTypeDef result = HAL_CAN_AddTxMessage(hcan, &header, data, &mailbox); + return (result == HAL_OK) ? BSP_OK : 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_UnregisterIdQueue(BSP_CAN_t can, uint32_t can_id) { + if (!inited) { + return BSP_ERR_INITED; + } + return BSP_CAN_DeleteIdQueue(can, can_id); +} + +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; +} + +int8_t BSP_CAN_UnregisterIdParser(void) { + if (!inited) { + return BSP_ERR_INITED; + } + + id_parser = BSP_CAN_DefaultIdParser; + 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); +} + +/* USER CAN FUNCTIONS BEGIN */ +/* USER CAN FUNCTIONS END */ \ No newline at end of file diff --git a/User/bsp/can.h b/User/bsp/can.h new file mode 100644 index 0000000..d4f14d6 --- /dev/null +++ b/User/bsp/can.h @@ -0,0 +1,233 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include "bsp/bsp.h" +#include "bsp/mm.h" +#include +#include +#include + +/* 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 + +/* 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); + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 初始化 CAN 模块 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_Init(void); + +/** + * @brief 反初始化 CAN 模块 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_DeInit(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 注册 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 ID 接收队列 + * @param can CAN 枚举 + * @param can_id 解析后的CAN ID + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_UnregisterIdQueue(BSP_CAN_t can, uint32_t can_id); + +/** + * @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 注销ID解析器 + * @return BSP_OK 成功，其他值失败 + */ +int8_t BSP_CAN_UnregisterIdParser(void); + +/** + * @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 CAN FUNCTIONS BEGIN */ +/* USER CAN FUNCTIONS END */ + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/User/bsp/dwt.c b/User/bsp/dwt.c new file mode 100644 index 0000000..15e3e51 --- /dev/null +++ b/User/bsp/dwt.c @@ -0,0 +1,122 @@ +/** + ****************************************************************************** + * @file dwt.c + * @author Wang Hongxi + * @version V1.1.0 + * @date 2022/3/8 + * @brief + ****************************************************************************** + * @attention + * + ****************************************************************************** + */ +#include "bsp/dwt.h" + +DWT_Time_t SysTime; +static uint32_t CPU_FREQ_Hz, CPU_FREQ_Hz_ms, CPU_FREQ_Hz_us; +static uint32_t CYCCNT_RountCount; +static uint32_t CYCCNT_LAST; +uint64_t CYCCNT64; +static void DWT_CNT_Update(void); + +void DWT_Init(uint32_t CPU_Freq_mHz) +{ + /* 使能DWT外设 */ + CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk; + + /* DWT CYCCNT寄存器计数清0 */ + DWT->CYCCNT = (uint32_t)0u; + + /* 使能Cortex-M DWT CYCCNT寄存器 */ + DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk; + + CPU_FREQ_Hz = CPU_Freq_mHz * 1000000; + CPU_FREQ_Hz_ms = CPU_FREQ_Hz / 1000; + CPU_FREQ_Hz_us = CPU_FREQ_Hz / 1000000; + CYCCNT_RountCount = 0; +} + +float DWT_GetDeltaT(uint32_t *cnt_last) +{ + volatile uint32_t cnt_now = DWT->CYCCNT; + float dt = ((uint32_t)(cnt_now - *cnt_last)) / ((float)(CPU_FREQ_Hz)); + *cnt_last = cnt_now; + + DWT_CNT_Update(); + + return dt; +} + +double DWT_GetDeltaT64(uint32_t *cnt_last) +{ + volatile uint32_t cnt_now = DWT->CYCCNT; + double dt = ((uint32_t)(cnt_now - *cnt_last)) / ((double)(CPU_FREQ_Hz)); + *cnt_last = cnt_now; + + DWT_CNT_Update(); + + return dt; +} + +void DWT_SysTimeUpdate(void) +{ + volatile uint32_t cnt_now = DWT->CYCCNT; + static uint64_t CNT_TEMP1, CNT_TEMP2, CNT_TEMP3; + + DWT_CNT_Update(); + + CYCCNT64 = (uint64_t)CYCCNT_RountCount * (uint64_t)UINT32_MAX + (uint64_t)cnt_now; + CNT_TEMP1 = CYCCNT64 / CPU_FREQ_Hz; + CNT_TEMP2 = CYCCNT64 - CNT_TEMP1 * CPU_FREQ_Hz; + SysTime.s = CNT_TEMP1; + SysTime.ms = CNT_TEMP2 / CPU_FREQ_Hz_ms; + CNT_TEMP3 = CNT_TEMP2 - SysTime.ms * CPU_FREQ_Hz_ms; + SysTime.us = CNT_TEMP3 / CPU_FREQ_Hz_us; +} + +float DWT_GetTimeline_s(void) +{ + DWT_SysTimeUpdate(); + + float DWT_Timelinef32 = SysTime.s + SysTime.ms * 0.001f + SysTime.us * 0.000001f; + + return DWT_Timelinef32; +} + +float DWT_GetTimeline_ms(void) +{ + DWT_SysTimeUpdate(); + + float DWT_Timelinef32 = SysTime.s * 1000 + SysTime.ms + SysTime.us * 0.001f; + + return DWT_Timelinef32; +} + +uint64_t DWT_GetTimeline_us(void) +{ + DWT_SysTimeUpdate(); + + uint64_t DWT_Timelinef32 = SysTime.s * 1000000 + SysTime.ms * 1000 + SysTime.us; + + return DWT_Timelinef32; +} + +static void DWT_CNT_Update(void) +{ + volatile uint32_t cnt_now = DWT->CYCCNT; + + if (cnt_now < CYCCNT_LAST) + CYCCNT_RountCount++; + + CYCCNT_LAST = cnt_now; +} + +void DWT_Delay(float Delay) +{ + uint32_t tickstart = DWT->CYCCNT; + float wait = Delay; + + while ((DWT->CYCCNT - tickstart) < wait * (float)CPU_FREQ_Hz) + { + } +} diff --git a/User/bsp/dwt.h b/User/bsp/dwt.h new file mode 100644 index 0000000..817e922 --- /dev/null +++ b/User/bsp/dwt.h @@ -0,0 +1,37 @@ +/** + ****************************************************************************** + * @file dwt.h + * @author Wang Hongxi + * @version V1.1.0 + * @date 2022/3/8 + * @brief + ****************************************************************************** + * @attention + * + ****************************************************************************** + */ +#ifndef _DWT_H +#define _DWT_H + +#include "main.h" +#include "stdint.h" + +typedef struct +{ + uint32_t s; + uint16_t ms; + uint16_t us; +} DWT_Time_t; + +void DWT_Init(uint32_t CPU_Freq_mHz); +float DWT_GetDeltaT(uint32_t *cnt_last); +double DWT_GetDeltaT64(uint32_t *cnt_last); +float DWT_GetTimeline_s(void); +float DWT_GetTimeline_ms(void); +uint64_t DWT_GetTimeline_us(void); +void DWT_Delay(float Delay); +void DWT_SysTimeUpdate(void); + +extern DWT_Time_t SysTime; + +#endif /* DWT_H_ */ diff --git a/User/bsp/gpio.c b/User/bsp/gpio.c new file mode 100644 index 0000000..9e83fd7 --- /dev/null +++ b/User/bsp/gpio.c @@ -0,0 +1,114 @@ +/* Includes ----------------------------------------------------------------- */ +#include "bsp/gpio.h" + +#include +#include + +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +typedef struct { + uint16_t pin; + GPIO_TypeDef *gpio; +} BSP_GPIO_MAP_t; + +/* Private variables -------------------------------------------------------- */ +static const BSP_GPIO_MAP_t GPIO_Map[BSP_GPIO_NUM] = { + {USER_KEY_Pin, USER_KEY_GPIO_Port}, + {ACCL_CS_Pin, ACCL_CS_GPIO_Port}, + {GYRO_CS_Pin, GYRO_CS_GPIO_Port}, + {SPI2_CS_Pin, SPI2_CS_GPIO_Port}, + {HW0_Pin, HW0_GPIO_Port}, + {HW1_Pin, HW1_GPIO_Port}, + {HW2_Pin, HW2_GPIO_Port}, + {ACCL_INT_Pin, ACCL_INT_GPIO_Port}, + {GYRO_INT_Pin, GYRO_INT_GPIO_Port}, + {CMPS_INT_Pin, CMPS_INT_GPIO_Port}, + {CMPS_RST_Pin, CMPS_RST_GPIO_Port}, +}; + +static void (*GPIO_Callback[16])(void); + +/* Private function -------------------------------------------------------- */ +void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { + for (uint8_t i = 0; i < 16; i++) { + if (GPIO_Pin & (1 << i)) { + if (GPIO_Callback[i]) { + GPIO_Callback[i](); + } + } + } +} + +/* Exported functions ------------------------------------------------------- */ +int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, void (*callback)(void)) { + if (callback == NULL) return BSP_ERR_NULL; + if (gpio >= BSP_GPIO_NUM) return BSP_ERR; + + // 从GPIO映射中获取对应的pin值 + uint16_t pin = GPIO_Map[gpio].pin; + + for (uint8_t i = 0; i < 16; i++) { + if (pin & (1 << i)) { + GPIO_Callback[i] = callback; + break; + } + } + return BSP_OK; +} + +int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) { + switch (gpio) { + case BSP_GPIO_USER_KEY: + HAL_NVIC_EnableIRQ(USER_KEY_EXTI_IRQn); + break; + case BSP_GPIO_ACCL_INT: + HAL_NVIC_EnableIRQ(ACCL_INT_EXTI_IRQn); + break; + case BSP_GPIO_GYRO_INT: + HAL_NVIC_EnableIRQ(GYRO_INT_EXTI_IRQn); + break; + case BSP_GPIO_CMPS_INT: + HAL_NVIC_EnableIRQ(CMPS_INT_EXTI_IRQn); + break; + default: + return BSP_ERR; + } + return BSP_OK; +} + +int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) { + switch (gpio) { + case BSP_GPIO_USER_KEY: + HAL_NVIC_DisableIRQ(USER_KEY_EXTI_IRQn); + break; + case BSP_GPIO_ACCL_INT: + HAL_NVIC_DisableIRQ(ACCL_INT_EXTI_IRQn); + break; + case BSP_GPIO_GYRO_INT: + HAL_NVIC_DisableIRQ(GYRO_INT_EXTI_IRQn); + break; + case BSP_GPIO_CMPS_INT: + HAL_NVIC_DisableIRQ(CMPS_INT_EXTI_IRQn); + break; + default: + return BSP_ERR; + } + return BSP_OK; +} +int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value){ + if (gpio >= BSP_GPIO_NUM) return BSP_ERR; + HAL_GPIO_WritePin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin, value); + return BSP_OK; +} + +int8_t BSP_GPIO_TogglePin(BSP_GPIO_t gpio){ + if (gpio >= BSP_GPIO_NUM) return BSP_ERR; + HAL_GPIO_TogglePin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin); + return BSP_OK; +} + +bool BSP_GPIO_ReadPin(BSP_GPIO_t gpio){ + if (gpio >= BSP_GPIO_NUM) return false; + return HAL_GPIO_ReadPin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin) == GPIO_PIN_SET; +} \ No newline at end of file diff --git a/User/bsp/gpio.h b/User/bsp/gpio.h new file mode 100644 index 0000000..ebe7746 --- /dev/null +++ b/User/bsp/gpio.h @@ -0,0 +1,45 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include + +#include "bsp/bsp.h" + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + BSP_GPIO_USER_KEY, + BSP_GPIO_ACCL_CS, + BSP_GPIO_GYRO_CS, + BSP_GPIO_SPI2_CS, + BSP_GPIO_HW0, + BSP_GPIO_HW1, + BSP_GPIO_HW2, + BSP_GPIO_ACCL_INT, + BSP_GPIO_GYRO_INT, + BSP_GPIO_CMPS_INT, + BSP_GPIO_CMPS_RST, + BSP_GPIO_NUM, + BSP_GPIO_ERR, +} BSP_GPIO_t; + +/* Exported functions prototypes -------------------------------------------- */ +int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, void (*callback)(void)); + +int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio); +int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio); + +int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value); +int8_t BSP_GPIO_TogglePin(BSP_GPIO_t gpio); + +bool BSP_GPIO_ReadPin(BSP_GPIO_t gpio); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/User/bsp/mm.c b/User/bsp/mm.c new file mode 100644 index 0000000..b78e3c4 --- /dev/null +++ b/User/bsp/mm.c @@ -0,0 +1,14 @@ +/* Includes ----------------------------------------------------------------- */ +#include "bsp\mm.h" + +#include "FreeRTOS.h" + +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ +/* Private function -------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +inline void *BSP_Malloc(size_t size) { return pvPortMalloc(size); } + +inline void BSP_Free(void *pv) { vPortFree(pv); } diff --git a/User/bsp/mm.h b/User/bsp/mm.h new file mode 100644 index 0000000..57bdcac --- /dev/null +++ b/User/bsp/mm.h @@ -0,0 +1,20 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +/* Exported functions prototypes -------------------------------------------- */ +void *BSP_Malloc(size_t size); +void BSP_Free(void *pv); + +#ifdef __cplusplus +} +#endif diff --git a/User/bsp/pwm.c b/User/bsp/pwm.c new file mode 100644 index 0000000..94f1e04 --- /dev/null +++ b/User/bsp/pwm.c @@ -0,0 +1,109 @@ +/* Includes ----------------------------------------------------------------- */ +#include "tim.h" +#include "bsp/pwm.h" +#include "bsp.h" + +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +typedef struct { + TIM_HandleTypeDef *tim; + uint16_t channel; +} BSP_PWM_Config_t; + +/* Private variables -------------------------------------------------------- */ +static const BSP_PWM_Config_t PWM_Map[BSP_PWM_NUM] = { + {&htim8, TIM_CHANNEL_1}, + {&htim3, TIM_CHANNEL_3}, + {&htim4, TIM_CHANNEL_3}, + {&htim1, TIM_CHANNEL_2}, + {&htim1, TIM_CHANNEL_3}, + {&htim1, TIM_CHANNEL_4}, + {&htim1, TIM_CHANNEL_1}, + {&htim10, TIM_CHANNEL_1}, + {&htim5, TIM_CHANNEL_1}, + {&htim5, TIM_CHANNEL_2}, + {&htim5, TIM_CHANNEL_3}, + {&htim8, TIM_CHANNEL_2}, +}; + +/* Private function -------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ + +int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Start(PWM_Map[ch].tim, PWM_Map[ch].channel); + return BSP_OK; +} + +int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + if (duty_cycle > 1.0f) { + duty_cycle = 1.0f; + } + if (duty_cycle < 0.0f) { + duty_cycle = 0.0f; + } + + // 获取ARR值（周期值） + uint32_t arr = __HAL_TIM_GET_AUTORELOAD(PWM_Map[ch].tim); + + // 计算比较值：CCR = duty_cycle * (ARR + 1) + uint32_t ccr = (uint32_t)(duty_cycle * (arr + 1)); + + __HAL_TIM_SET_COMPARE(PWM_Map[ch].tim, PWM_Map[ch].channel, ccr); + return BSP_OK; +} + +int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + uint32_t timer_clock = HAL_RCC_GetPCLK1Freq(); // Get the timer clock frequency + uint32_t prescaler = PWM_Map[ch].tim->Init.Prescaler; + uint32_t period = (timer_clock / (prescaler + 1)) / freq - 1; + + if (period > UINT16_MAX) { + return BSP_ERR; // Frequency too low + } + __HAL_TIM_SET_AUTORELOAD(PWM_Map[ch].tim, period); + + return BSP_OK; +} + +int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Stop(PWM_Map[ch].tim, PWM_Map[ch].channel); + return BSP_OK; +} + +uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + return PWM_Map[ch].tim->Init.AutoReloadPreload; +} + +TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch) { + return PWM_Map[ch].tim; +} + + +uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + return PWM_Map[ch].channel; +} + +int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Start_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel, pData, Length); + return BSP_OK; +} + +int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Stop_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel); + return BSP_OK; +} \ No newline at end of file diff --git a/User/bsp/pwm.h b/User/bsp/pwm.h new file mode 100644 index 0000000..7cb0954 --- /dev/null +++ b/User/bsp/pwm.h @@ -0,0 +1,47 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include "tim.h" +#include "bsp.h" + + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +/* PWM通道 */ +typedef enum { + BSP_PWM_TIM8_CH1, + BSP_PWM_LASER, + BSP_PWM_BUZZER, + BSP_PWM_TIM1_CH2, + BSP_PWM_TIM1_CH3, + BSP_PWM_TIM1_CH4, + BSP_PWM_TIM1_CH1, + BSP_PWM_IMU_HEAT_PWM, + BSP_PWM_LED_B, + BSP_PWM_LED_G, + BSP_PWM_LED_R, + BSP_PWM_TIM8_CH2, + BSP_PWM_NUM, + BSP_PWM_ERR, +} BSP_PWM_Channel_t; + +/* Exported functions prototypes -------------------------------------------- */ +int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch); +int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle); +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); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/User/bsp/spi.c b/User/bsp/spi.c new file mode 100644 index 0000000..b4701de --- /dev/null +++ b/User/bsp/spi.c @@ -0,0 +1,165 @@ +/* Includes ----------------------------------------------------------------- */ +#include +#include "bsp/spi.h" + +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ +static void (*SPI_Callback[BSP_SPI_NUM][BSP_SPI_CB_NUM])(void); + +/* Private function -------------------------------------------------------- */ +static BSP_SPI_t SPI_Get(SPI_HandleTypeDef *hspi) { + if (hspi->Instance == SPI1) + return BSP_SPI_BMI088; + else + return BSP_SPI_ERR; +} + +void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[bsp_spi][BSP_SPI_TX_CPLT_CB]) { + SPI_Callback[bsp_spi][BSP_SPI_TX_CPLT_CB](); + } + } +} + +void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_CPLT_CB](); + } +} + +void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_CPLT_CB](); + } +} + +void HAL_SPI_TxHalfCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_HALF_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_HALF_CPLT_CB](); + } +} + +void HAL_SPI_RxHalfCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_HALF_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_HALF_CPLT_CB](); + } +} + +void HAL_SPI_TxRxHalfCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_HALF_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_HALF_CPLT_CB](); + } +} + +void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_ERROR_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_ERROR_CB](); + } +} + +void HAL_SPI_AbortCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_ABORT_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_ABORT_CPLT_CB](); + } +} + +/* Exported functions ------------------------------------------------------- */ +SPI_HandleTypeDef *BSP_SPI_GetHandle(BSP_SPI_t spi) { + switch (spi) { + case BSP_SPI_BMI088: + return &hspi1; + default: + return NULL; + } +} + +int8_t BSP_SPI_RegisterCallback(BSP_SPI_t spi, BSP_SPI_Callback_t type, + void (*callback)(void)) { + if (callback == NULL) return BSP_ERR_NULL; + SPI_Callback[spi][type] = callback; + return BSP_OK; +} + +int8_t BSP_SPI_Transmit(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi); + if (hspi == NULL) return BSP_ERR; + + if (dma) { + return HAL_SPI_Transmit_DMA(hspi, data, size)!= HAL_OK;; + } else { + return HAL_SPI_Transmit(hspi, data, size, 20)!= HAL_OK;; + } +} + +int8_t BSP_SPI_Receive(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi); + if (hspi == NULL) return BSP_ERR; + + if (dma) { + return HAL_SPI_Receive_DMA(hspi, data, size)!= HAL_OK;; + } else { + return HAL_SPI_Receive(hspi, data, size, 20)!= HAL_OK;; + } +} + +int8_t BSP_SPI_TransmitReceive(BSP_SPI_t spi, uint8_t *txData, uint8_t *rxData, + uint16_t size, bool dma) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi); + if (hspi == NULL) return BSP_ERR; + + if (dma) { + return HAL_SPI_TransmitReceive_DMA(hspi, txData, rxData, size)!= HAL_OK;; + } else { + return HAL_SPI_TransmitReceive(hspi, txData, rxData, size, 20)!= HAL_OK;; + } +} + +uint8_t BSP_SPI_MemReadByte(BSP_SPI_t spi, uint8_t reg) { + if (spi >= BSP_SPI_NUM) return 0xFF; + uint8_t tmp[2] = {reg | 0x80, 0x00}; + BSP_SPI_TransmitReceive(spi, tmp, tmp, 2u, true); + return tmp[1]; +} + +int8_t BSP_SPI_MemWriteByte(BSP_SPI_t spi, uint8_t reg, uint8_t data) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + uint8_t tmp[2] = {reg & 0x7f, data}; + return BSP_SPI_Transmit(spi, tmp, 2u, true); +} + +int8_t BSP_SPI_MemRead(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + if (data == NULL || size == 0) return BSP_ERR_NULL; + reg = reg | 0x80; + BSP_SPI_Transmit(spi, ®, 1u, true); + return BSP_SPI_Receive(spi, data, size, true); +} + +int8_t BSP_SPI_MemWrite(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + if (data == NULL || size == 0) return BSP_ERR_NULL; + reg = reg & 0x7f; + BSP_SPI_Transmit(spi, ®, 1u, true); + return BSP_SPI_Transmit(spi, data, size, true); +} diff --git a/User/bsp/spi.h b/User/bsp/spi.h new file mode 100644 index 0000000..29c95f9 --- /dev/null +++ b/User/bsp/spi.h @@ -0,0 +1,58 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include +#include + +#include "bsp/bsp.h" + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + +/* 要添加使用SPI的新设备，需要先在此添加对应的枚举值 */ + +/* SPI实体枚举，与设备对应 */ +typedef enum { + BSP_SPI_BMI088, + BSP_SPI_NUM, + BSP_SPI_ERR, +} BSP_SPI_t; + +/* SPI支持的中断回调函数类型，具体参考HAL中定义 */ +typedef enum { + BSP_SPI_TX_CPLT_CB, + BSP_SPI_RX_CPLT_CB, + BSP_SPI_TX_RX_CPLT_CB, + BSP_SPI_TX_HALF_CPLT_CB, + BSP_SPI_RX_HALF_CPLT_CB, + BSP_SPI_TX_RX_HALF_CPLT_CB, + BSP_SPI_ERROR_CB, + BSP_SPI_ABORT_CPLT_CB, + BSP_SPI_CB_NUM, +} BSP_SPI_Callback_t; + +/* Exported functions prototypes -------------------------------------------- */ +SPI_HandleTypeDef *BSP_SPI_GetHandle(BSP_SPI_t spi); +int8_t BSP_SPI_RegisterCallback(BSP_SPI_t spi, BSP_SPI_Callback_t type, + void (*callback)(void)); + + +int8_t BSP_SPI_Transmit(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma); +int8_t BSP_SPI_Receive(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma); +int8_t BSP_SPI_TransmitReceive(BSP_SPI_t spi, uint8_t *txData, uint8_t *rxData, + uint16_t size, bool dma); + +uint8_t BSP_SPI_MemReadByte(BSP_SPI_t spi, uint8_t reg); +int8_t BSP_SPI_MemWriteByte(BSP_SPI_t spi, uint8_t reg, uint8_t data); +int8_t BSP_SPI_MemRead(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size); +int8_t BSP_SPI_MemWrite(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size); + +#ifdef __cplusplus +} +#endif diff --git a/User/bsp/time.c b/User/bsp/time.c new file mode 100644 index 0000000..5817cea --- /dev/null +++ b/User/bsp/time.c @@ -0,0 +1,65 @@ +/* Includes ----------------------------------------------------------------- */ +#include "bsp/time.h" +#include "bsp.h" + +#include +#include "FreeRTOS.h" +#include "main.h" +#include "task.h" +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* 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"))); \ No newline at end of file diff --git a/User/bsp/time.h b/User/bsp/time.h new file mode 100644 index 0000000..5273f0f --- /dev/null +++ b/User/bsp/time.h @@ -0,0 +1,31 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include + +#include "bsp/bsp.h" + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* 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); + +#ifdef __cplusplus +} +#endif diff --git a/User/bsp/uart.c b/User/bsp/uart.c new file mode 100644 index 0000000..9e56842 --- /dev/null +++ b/User/bsp/uart.c @@ -0,0 +1,139 @@ +/* Includes ----------------------------------------------------------------- */ +#include + +#include "bsp/uart.h" + +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* 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 == USART3) + return BSP_UART_DR16; + 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_DR16: + 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); + } +} \ No newline at end of file diff --git a/User/bsp/uart.h b/User/bsp/uart.h new file mode 100644 index 0000000..61082fd --- /dev/null +++ b/User/bsp/uart.h @@ -0,0 +1,53 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include +#include + +#include "bsp/bsp.h" + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + +/* 要添加使用UART的新设备，需要先在此添加对应的枚举值 */ + +/* UART实体枚举，与设备对应 */ +typedef enum { + BSP_UART_DR16, + 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); +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); + +#ifdef __cplusplus +} +#endif diff --git a/User/component/ahrs.c b/User/component/ahrs.c new file mode 100644 index 0000000..5886297 --- /dev/null +++ b/User/component/ahrs.c @@ -0,0 +1,405 @@ +/* + 开源的AHRS算法。 + MadgwickAHRS +*/ + +#include "ahrs.h" + +#include + +#include "user_math.h" + +#define BETA_IMU (0.033f) +#define BETA_AHRS (0.041f) + +/* 2 * proportional gain (Kp) */ +static float beta = BETA_IMU; + +/** + * @brief 不使用磁力计计算姿态 + * + * @param ahrs 姿态解算主结构体 + * @param accl 加速度计数据 + * @param gyro 陀螺仪数据 + * @return int8_t 0对应没有错误 + */ +static int8_t AHRS_UpdateIMU(AHRS_t *ahrs, const AHRS_Accl_t *accl, + const AHRS_Gyro_t *gyro) { + if (ahrs == NULL) return -1; + if (accl == NULL) return -1; + if (gyro == NULL) return -1; + + beta = BETA_IMU; + + float ax = accl->x; + float ay = accl->y; + float az = accl->z; + + float gx = gyro->x; + float gy = gyro->y; + float gz = gyro->z; + + float recip_norm; + float s0, s1, s2, s3; + float q_dot1, q_dot2, q_dot3, q_dot4; + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, + q3q3; + + /* Rate of change of quaternion from gyroscope */ + q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy - + ahrs->quat.q3 * gz); + q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz - + ahrs->quat.q3 * gy); + q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz + + ahrs->quat.q3 * gx); + q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy - + ahrs->quat.q2 * gx); + + /* Compute feedback only if accelerometer measurement valid (avoids NaN in + * accelerometer normalisation) */ + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + /* Normalise accelerometer measurement */ + recip_norm = InvSqrt(ax * ax + ay * ay + az * az); + ax *= recip_norm; + ay *= recip_norm; + az *= recip_norm; + + /* Auxiliary variables to avoid repeated arithmetic */ + _2q0 = 2.0f * ahrs->quat.q0; + _2q1 = 2.0f * ahrs->quat.q1; + _2q2 = 2.0f * ahrs->quat.q2; + _2q3 = 2.0f * ahrs->quat.q3; + _4q0 = 4.0f * ahrs->quat.q0; + _4q1 = 4.0f * ahrs->quat.q1; + _4q2 = 4.0f * ahrs->quat.q2; + _8q1 = 8.0f * ahrs->quat.q1; + _8q2 = 8.0f * ahrs->quat.q2; + q0q0 = ahrs->quat.q0 * ahrs->quat.q0; + q1q1 = ahrs->quat.q1 * ahrs->quat.q1; + q2q2 = ahrs->quat.q2 * ahrs->quat.q2; + q3q3 = ahrs->quat.q3 * ahrs->quat.q3; + + /* Gradient decent algorithm corrective step */ + s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; + s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * ahrs->quat.q1 - + _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; + s2 = 4.0f * q0q0 * ahrs->quat.q2 + _2q0 * ax + _4q2 * q3q3 - + _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; + s3 = 4.0f * q1q1 * ahrs->quat.q3 - _2q1 * ax + + 4.0f * q2q2 * ahrs->quat.q3 - _2q2 * ay; + + /* normalise step magnitude */ + recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); + + s0 *= recip_norm; + s1 *= recip_norm; + s2 *= recip_norm; + s3 *= recip_norm; + + /* Apply feedback step */ + q_dot1 -= beta * s0; + q_dot2 -= beta * s1; + q_dot3 -= beta * s2; + q_dot4 -= beta * s3; + } + + /* Integrate rate of change of quaternion to yield quaternion */ + ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq; + ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq; + ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq; + ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq; + + /* Normalise quaternion */ + recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 + + ahrs->quat.q1 * ahrs->quat.q1 + + ahrs->quat.q2 * ahrs->quat.q2 + + ahrs->quat.q3 * ahrs->quat.q3); + ahrs->quat.q0 *= recip_norm; + ahrs->quat.q1 *= recip_norm; + ahrs->quat.q2 *= recip_norm; + ahrs->quat.q3 *= recip_norm; + + return 0; +} + +/** + * @brief 初始化姿态解算 + * + * @param ahrs 姿态解算主结构体 + * @param magn 磁力计数据 + * @param sample_freq 采样频率 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq) { + if (ahrs == NULL) return -1; + + ahrs->inv_sample_freq = 1.0f / sample_freq; + + ahrs->quat.q0 = 1.0f; + ahrs->quat.q1 = 0.0f; + ahrs->quat.q2 = 0.0f; + ahrs->quat.q3 = 0.0f; + + if (magn) { + float yaw = -atan2(magn->y, magn->x); + + if ((magn->x == 0.0f) && (magn->y == 0.0f) && (magn->z == 0.0f)) { + ahrs->quat.q0 = 0.800884545f; + ahrs->quat.q1 = 0.00862364192f; + ahrs->quat.q2 = -0.00283267116f; + ahrs->quat.q3 = 0.598749936f; + + } else if ((yaw < (M_PI / 2.0f)) || (yaw > 0.0f)) { + ahrs->quat.q0 = 0.997458339f; + ahrs->quat.q1 = 0.000336312107f; + ahrs->quat.q2 = -0.0057230792f; + ahrs->quat.q3 = 0.0740156546; + + } else if ((yaw < M_PI) || (yaw > (M_PI / 2.0f))) { + ahrs->quat.q0 = 0.800884545f; + ahrs->quat.q1 = 0.00862364192f; + ahrs->quat.q2 = -0.00283267116f; + ahrs->quat.q3 = 0.598749936f; + + } else if ((yaw < 90.0f) || (yaw > M_PI)) { + ahrs->quat.q0 = 0.800884545f; + ahrs->quat.q1 = 0.00862364192f; + ahrs->quat.q2 = -0.00283267116f; + ahrs->quat.q3 = 0.598749936f; + + } else if ((yaw < 90.0f) || (yaw > 0.0f)) { + ahrs->quat.q0 = 0.800884545f; + ahrs->quat.q1 = 0.00862364192f; + ahrs->quat.q2 = -0.00283267116f; + ahrs->quat.q3 = 0.598749936f; + } + } + return 0; +} + +/** + * @brief 姿态运算更新一次 + * @note 输入数据必须是NED(North East Down) 参考坐标系 + * + * @param ahrs 姿态解算主结构体 + * @param accl 加速度计数据 + * @param gyro 陀螺仪数据 + * @param magn 磁力计数据 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl, + const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn) { + if (ahrs == NULL) return -1; + if (accl == NULL) return -1; + if (gyro == NULL) return -1; + + beta = BETA_AHRS; + + float recip_norm; + float s0, s1, s2, s3; + float q_dot1, q_dot2, q_dot3, q_dot4; + float hx, hy; + float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, + _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, + q2q2, q2q3, q3q3; + + if (magn == NULL) return AHRS_UpdateIMU(ahrs, accl, gyro); + + float mx = magn->x; + float my = magn->y; + float mz = magn->z; + + /* Use IMU algorithm if magnetometer measurement invalid (avoids NaN in */ + /* magnetometer normalisation) */ + if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { + return AHRS_UpdateIMU(ahrs, accl, gyro); + } + + float ax = accl->x; + float ay = accl->y; + float az = accl->z; + + float gx = gyro->x; + float gy = gyro->y; + float gz = gyro->z; + + /* Rate of change of quaternion from gyroscope */ + q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy - + ahrs->quat.q3 * gz); + q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz - + ahrs->quat.q3 * gy); + q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz + + ahrs->quat.q3 * gx); + q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy - + ahrs->quat.q2 * gx); + + /* Compute feedback only if accelerometer measurement valid (avoids NaN in + * accelerometer normalisation) */ + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + /* Normalise accelerometer measurement */ + recip_norm = InvSqrt(ax * ax + ay * ay + az * az); + ax *= recip_norm; + ay *= recip_norm; + az *= recip_norm; + + /* Normalise magnetometer measurement */ + recip_norm = InvSqrt(mx * mx + my * my + mz * mz); + mx *= recip_norm; + my *= recip_norm; + mz *= recip_norm; + + /* Auxiliary variables to avoid repeated arithmetic */ + _2q0mx = 2.0f * ahrs->quat.q0 * mx; + _2q0my = 2.0f * ahrs->quat.q0 * my; + _2q0mz = 2.0f * ahrs->quat.q0 * mz; + _2q1mx = 2.0f * ahrs->quat.q1 * mx; + _2q0 = 2.0f * ahrs->quat.q0; + _2q1 = 2.0f * ahrs->quat.q1; + _2q2 = 2.0f * ahrs->quat.q2; + _2q3 = 2.0f * ahrs->quat.q3; + _2q0q2 = 2.0f * ahrs->quat.q0 * ahrs->quat.q2; + _2q2q3 = 2.0f * ahrs->quat.q2 * ahrs->quat.q3; + q0q0 = ahrs->quat.q0 * ahrs->quat.q0; + q0q1 = ahrs->quat.q0 * ahrs->quat.q1; + q0q2 = ahrs->quat.q0 * ahrs->quat.q2; + q0q3 = ahrs->quat.q0 * ahrs->quat.q3; + q1q1 = ahrs->quat.q1 * ahrs->quat.q1; + q1q2 = ahrs->quat.q1 * ahrs->quat.q2; + q1q3 = ahrs->quat.q1 * ahrs->quat.q3; + q2q2 = ahrs->quat.q2 * ahrs->quat.q2; + q2q3 = ahrs->quat.q2 * ahrs->quat.q3; + q3q3 = ahrs->quat.q3 * ahrs->quat.q3; + + /* Reference direction of Earth's magnetic field */ + hx = mx * q0q0 - _2q0my * ahrs->quat.q3 + + _2q0mz * ahrs->quat.q2 + mx * q1q1 + + _2q1 * my * ahrs->quat.q2 + _2q1 * mz * ahrs->quat.q3 - + mx * q2q2 - mx * q3q3; + hy = _2q0mx * ahrs->quat.q3 + my * q0q0 - + _2q0mz * ahrs->quat.q1 + _2q1mx * ahrs->quat.q2 - + my * q1q1 + my * q2q2 + _2q2 * mz * ahrs->quat.q3 - my * q3q3; + // _2bx = sqrtf(hx * hx + hy * hy); + // 改为invsqrt + _2bx = 1.f / InvSqrt(hx * hx + hy * hy); + _2bz = -_2q0mx * ahrs->quat.q2 + _2q0my * ahrs->quat.q1 + + mz * q0q0 + _2q1mx * ahrs->quat.q3 - mz * q1q1 + + _2q2 * my * ahrs->quat.q3 - mz * q2q2 + mz * q3q3; + _4bx = 2.0f * _2bx; + _4bz = 2.0f * _2bz; + + /* Gradient decent algorithm corrective step */ + s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - + _2bz * ahrs->quat.q2 * + (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (-_2bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) * + (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + _2bx * ahrs->quat.q2 * + (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - + 4.0f * ahrs->quat.q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + + _2bz * ahrs->quat.q3 * + (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (_2bx * ahrs->quat.q2 + _2bz * ahrs->quat.q0) * + (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + (_2bx * ahrs->quat.q3 - _4bz * ahrs->quat.q1) * + (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - + 4.0f * ahrs->quat.q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + + (-_4bx * ahrs->quat.q2 - _2bz * ahrs->quat.q0) * + (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (_2bx * ahrs->quat.q1 + _2bz * ahrs->quat.q3) * + (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + (_2bx * ahrs->quat.q0 - _4bz * ahrs->quat.q2) * + (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + + (-_4bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) * + (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (-_2bx * ahrs->quat.q0 + _2bz * ahrs->quat.q2) * + (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + _2bx * ahrs->quat.q1 * + (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + /* normalise step magnitude */ + recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); + s0 *= recip_norm; + s1 *= recip_norm; + s2 *= recip_norm; + s3 *= recip_norm; + + /* Apply feedback step */ + q_dot1 -= beta * s0; + q_dot2 -= beta * s1; + q_dot3 -= beta * s2; + q_dot4 -= beta * s3; + } + + /* Integrate rate of change of quaternion to yield quaternion */ + ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq; + ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq; + ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq; + ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq; + + /* Normalise quaternion */ + recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 + + ahrs->quat.q1 * ahrs->quat.q1 + + ahrs->quat.q2 * ahrs->quat.q2 + + ahrs->quat.q3 * ahrs->quat.q3); + ahrs->quat.q0 *= recip_norm; + ahrs->quat.q1 *= recip_norm; + ahrs->quat.q2 *= recip_norm; + ahrs->quat.q3 *= recip_norm; + + return 0; +} + +/** + * @brief 通过姿态解算主结构体中的四元数计算欧拉角 + * + * @param eulr 欧拉角 + * @param ahrs 姿态解算主结构体 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs) { + if (eulr == NULL) return -1; + if (ahrs == NULL) return -1; + + const float sinr_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q1 + + ahrs->quat.q2 * ahrs->quat.q3); + const float cosr_cosp = + 1.0f - 2.0f * (ahrs->quat.q1 * ahrs->quat.q1 + + ahrs->quat.q2 * ahrs->quat.q2); + eulr->pit = atan2f(sinr_cosp, cosr_cosp); + + const float sinp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q2 - + ahrs->quat.q3 * ahrs->quat.q1); + + if (fabsf(sinp) >= 1.0f) + eulr->rol = copysignf(M_PI / 2.0f, sinp); + else + eulr->rol = asinf(sinp); + + const float siny_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q3 + + ahrs->quat.q1 * ahrs->quat.q2); + const float cosy_cosp = + 1.0f - 2.0f * (ahrs->quat.q2 * ahrs->quat.q2 + + ahrs->quat.q3 * ahrs->quat.q3); + eulr->yaw = atan2f(siny_cosp, cosy_cosp); + +#if 0 + eulr->yaw *= M_RAD2DEG_MULT; + eulr->rol *= M_RAD2DEG_MULT; + eulr->pit *= M_RAD2DEG_MULT; +#endif + + return 0; +} + +/** + * \brief 将对应数据置零 + * + * \param eulr 被操作的数据 + */ +void AHRS_ResetEulr(AHRS_Eulr_t *eulr) { memset(eulr, 0, sizeof(*eulr)); } diff --git a/User/component/ahrs.h b/User/component/ahrs.h new file mode 100644 index 0000000..add8b8b --- /dev/null +++ b/User/component/ahrs.h @@ -0,0 +1,98 @@ +/* + 开源的AHRS算法。 + MadgwickAHRS +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "user_math.h" + +/* 欧拉角（Euler angle） */ +typedef struct { + float yaw; /* 偏航角（Yaw angle） */ + float pit; /* 俯仰角（Pitch angle） */ + float rol; /* 翻滚角（Roll angle） */ +} AHRS_Eulr_t; + +/* 加速度计 Accelerometer */ +typedef struct { + float x; + float y; + float z; +} AHRS_Accl_t; + +/* 陀螺仪 Gyroscope */ +typedef struct { + float x; + float y; + float z; +} AHRS_Gyro_t; + +/* 磁力计 Magnetometer */ +typedef struct { + float x; + float y; + float z; +} AHRS_Magn_t; + +/* 四元数 */ +typedef struct { + float q0; + float q1; + float q2; + float q3; +} AHRS_Quaternion_t; + +/* 姿态解算算法主结构体 */ +typedef struct { + /* 四元数 */ + AHRS_Quaternion_t quat; + + float inv_sample_freq; /* 采样频率的的倒数 */ +} AHRS_t; + +/** + * @brief 初始化姿态解算 + * + * @param ahrs 姿态解算主结构体 + * @param magn 磁力计数据 + * @param sample_freq 采样频率 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq); + +/** + * @brief 姿态运算更新一次 + * + * @param ahrs 姿态解算主结构体 + * @param accl 加速度计数据 + * @param gyro 陀螺仪数据 + * @param magn 磁力计数据 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl, + const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn); + +/** + * @brief 通过姿态解算主结构体中的四元数计算欧拉角 + * + * @param eulr 欧拉角 + * @param ahrs 姿态解算主结构体 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs); + +/** + * \brief 将对应数据置零 + * + * \param eulr 被操作的数据 + */ +void AHRS_ResetEulr(AHRS_Eulr_t *eulr); + +#ifdef __cplusplus +} +#endif diff --git a/User/component/cmd.c b/User/component/cmd.c new file mode 100644 index 0000000..205869c --- /dev/null +++ b/User/component/cmd.c @@ -0,0 +1,375 @@ +/* + 控制命令 +*/ + +#include "cmd.h" + +#include + +/** + * @brief 行为转换为对应按键 + * + * @param cmd 主结构体 + * @param behavior 行为 + * @return uint16_t 行为对应的按键 + */ +static inline CMD_KeyValue_t CMD_BehaviorToKey(CMD_t *cmd, + CMD_Behavior_t behavior) { + return cmd->param->map.key_map[behavior].key; +} + +static inline CMD_ActiveType_t CMD_BehaviorToActive(CMD_t *cmd, + CMD_Behavior_t behavior) { + return cmd->param->map.key_map[behavior].active; +} + +/** + * @brief 检查按键是否按下 + * + * @param rc 遥控器数据 + * @param key 按键名称 + * @param stateful 是否为状态切换按键 + * @return true 按下 + * @return false 未按下 + */ +static bool CMD_KeyPressedRc(const CMD_RC_t *rc, CMD_KeyValue_t key) { + /* 按下按键为鼠标左、右键 */ + if (key == CMD_L_CLICK) { + return rc->mouse.l_click; + } + if (key == CMD_R_CLICK) { + return rc->mouse.r_click; + } + return rc->key & (1u << key); +} + +static bool CMD_BehaviorOccurredRc(const CMD_RC_t *rc, CMD_t *cmd, + CMD_Behavior_t behavior) { + CMD_KeyValue_t key = CMD_BehaviorToKey(cmd, behavior); + CMD_ActiveType_t active = CMD_BehaviorToActive(cmd, behavior); + + bool now_key_pressed, last_key_pressed; + + /* 按下按键为鼠标左、右键 */ + if (key == CMD_L_CLICK) { + now_key_pressed = rc->mouse.l_click; + last_key_pressed = cmd->mouse_last.l_click; + } else if (key == CMD_R_CLICK) { + now_key_pressed = rc->mouse.r_click; + last_key_pressed = cmd->mouse_last.r_click; + } else { + now_key_pressed = rc->key & (1u << key); + last_key_pressed = cmd->key_last & (1u << key); + } + + switch (active) { + case CMD_ACTIVE_PRESSING: + return now_key_pressed && !last_key_pressed; + case CMD_ACTIVE_RASING: + return !now_key_pressed && last_key_pressed; + case CMD_ACTIVE_PRESSED: + return now_key_pressed; + } +} + +/** + * @brief 解析pc行为逻辑 + * + * @param rc 遥控器数据 + * @param cmd 主结构体 + * @param dt_sec 两次解析的间隔 + */ +static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) { + cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE; + + /* 云台设置为鼠标控制欧拉角的变化，底盘的控制向量设置为零 */ + cmd->gimbal.delta_eulr.yaw = + (float)rc->mouse.x * dt_sec * cmd->param->sens_mouse; + cmd->gimbal.delta_eulr.pit = + (float)(-rc->mouse.y) * dt_sec * cmd->param->sens_mouse; + cmd->chassis.ctrl_vec.vx = cmd->chassis.ctrl_vec.vy = 0.0f; + cmd->shoot.reverse_trig = false; + + /* 按键行为映射相关逻辑 */ + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FORE)) { + cmd->chassis.ctrl_vec.vy += cmd->param->move.move_sense; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BACK)) { + cmd->chassis.ctrl_vec.vy -= cmd->param->move.move_sense; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_LEFT)) { + cmd->chassis.ctrl_vec.vx -= cmd->param->move.move_sense; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_RIGHT)) { + cmd->chassis.ctrl_vec.vx += cmd->param->move.move_sense; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ACCELERATE)) { + cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_fast_sense; + cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_fast_sense; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_DECELEBRATE)) { + cmd->chassis.ctrl_vec.vx *= cmd->param->move.move_slow_sense; + cmd->chassis.ctrl_vec.vy *= cmd->param->move.move_slow_sense; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE)) { + /* 切换至开火模式，设置相应的射击频率和弹丸初速度 */ + cmd->shoot.mode = SHOOT_MODE_LOADED; + cmd->shoot.fire = true; + } else { + /* 切换至准备模式，停止射击 */ + cmd->shoot.mode = SHOOT_MODE_LOADED; + cmd->shoot.fire = false; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FIRE_MODE)) { + /* 每按一次依次切换开火下一个模式 */ + cmd->shoot.fire_mode++; + cmd->shoot.fire_mode %= FIRE_MODE_NUM; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_ROTOR)) { + /* 切换到小陀螺模式 */ + cmd->chassis.mode = CHASSIS_MODE_ROTOR; + cmd->chassis.mode_rotor = ROTOR_MODE_RAND; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_OPENCOVER)) { + /* 每按一次开、关弹舱盖 */ + cmd->shoot.cover_open = !cmd->shoot.cover_open; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_BUFF)) { + if (cmd->ai_status == AI_STATUS_HITSWITCH) { + /* 停止ai的打符模式，停用host控制 */ + CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_STOP); + cmd->host_overwrite = false; + cmd->ai_status = AI_STATUS_STOP; + } else if (cmd->ai_status == AI_STATUS_AUTOAIM) { + /* 自瞄模式中切换失败提醒 */ + } else { + /* ai切换至打符模式，启用host控制 */ + CMD_RefereeAdd(&(cmd->referee), CMD_UI_HIT_SWITCH_START); + cmd->ai_status = AI_STATUS_HITSWITCH; + cmd->host_overwrite = true; + } + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_AUTOAIM)) { + if (cmd->ai_status == AI_STATUS_AUTOAIM) { + /* 停止ai的自瞄模式，停用host控制 */ + cmd->host_overwrite = false; + cmd->ai_status = AI_STATUS_STOP; + CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP); + } else { + /* ai切换至自瞄模式，启用host控制 */ + cmd->ai_status = AI_STATUS_AUTOAIM; + cmd->host_overwrite = true; + CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START); + } + } else { + cmd->host_overwrite = false; + // TODO: 修复逻辑 + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_REVTRIG)) { + /* 按下拨弹反转 */ + cmd->shoot.reverse_trig = true; + } + if (CMD_BehaviorOccurredRc(rc, cmd, CMD_BEHAVIOR_FOLLOWGIMBAL35)) { + cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL_35; + } + /* 保存当前按下的键位状态 */ + cmd->key_last = rc->key; + memcpy(&(cmd->mouse_last), &(rc->mouse), sizeof(cmd->mouse_last)); +} + +/** + * @brief 解析rc行为逻辑 + * + * @param rc 遥控器数据 + * @param cmd 主结构体 + * @param dt_sec 两次解析的间隔 + */ +static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) { + switch (rc->sw_l) { + /* 左拨杆相应行为选择和解析 */ + case CMD_SW_UP: + cmd->chassis.mode = CHASSIS_MODE_BREAK; + break; + + case CMD_SW_MID: + cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL; + break; + + case CMD_SW_DOWN: + cmd->chassis.mode = CHASSIS_MODE_ROTOR; + cmd->chassis.mode_rotor = ROTOR_MODE_CW; + break; + + case CMD_SW_ERR: + cmd->chassis.mode = CHASSIS_MODE_RELAX; + break; + } + switch (rc->sw_r) { + /* 右拨杆相应行为选择和解析*/ + case CMD_SW_UP: + cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE; + cmd->shoot.mode = SHOOT_MODE_SAFE; + break; + + case CMD_SW_MID: + cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE; + cmd->shoot.fire = false; + cmd->shoot.mode = SHOOT_MODE_LOADED; + break; + + case CMD_SW_DOWN: + cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE; + cmd->shoot.mode = SHOOT_MODE_LOADED; + cmd->shoot.fire_mode = FIRE_MODE_SINGLE; + cmd->shoot.fire = true; + break; + /* + case CMD_SW_UP: + cmd->gimbal.mode = GIMBAL_MODE_RELAX; + cmd->shoot.mode = SHOOT_MODE_SAFE; + break; + + case CMD_SW_MID: + cmd->gimbal.mode = GIMBAL_MODE_RELAX; + cmd->shoot.fire = false; + cmd->shoot.mode = SHOOT_MODE_LOADED; + break; + + case CMD_SW_DOWN: + cmd->gimbal.mode = GIMBAL_MODE_RELAX; + cmd->shoot.mode = SHOOT_MODE_LOADED; + cmd->shoot.fire_mode = FIRE_MODE_SINGLE; + cmd->shoot.fire = true; + break; + */ + case CMD_SW_ERR: + cmd->gimbal.mode = GIMBAL_MODE_RELAX; + cmd->shoot.mode = SHOOT_MODE_RELAX; + } + /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */ + cmd->chassis.ctrl_vec.vx = rc->ch_l_x; + cmd->chassis.ctrl_vec.vy = rc->ch_l_y; + cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc; + cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc; +} + +/** + * @brief rc失控时机器人恢复放松模式 + * + * @param cmd 主结构体 + */ +static void CMD_RcLostLogic(CMD_t *cmd) { + /* 机器人底盘、云台、射击运行模式恢复至放松模式 */ + cmd->chassis.mode = CHASSIS_MODE_RELAX; + cmd->gimbal.mode = GIMBAL_MODE_RELAX; + cmd->shoot.mode = SHOOT_MODE_RELAX; +} + +/** + * @brief 初始化命令解析 + * + * @param cmd 主结构体 + * @param param 参数 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param) { + /* 指针检测 */ + if (cmd == NULL) return -1; + if (param == NULL) return -1; + + /* 设置机器人的命令参数，初始化控制方式为rc控制 */ + cmd->pc_ctrl = false; + cmd->param = param; + + return 0; +} + +/** + * @brief 检查是否启用上位机控制指令覆盖 + * + * @param cmd 主结构体 + * @return true 启用 + * @return false 不启用 + */ +inline bool CMD_CheckHostOverwrite(CMD_t *cmd) { return cmd->host_overwrite; } + +/** + * @brief 解析命令 + * + * @param rc 遥控器数据 + * @param cmd 命令 + * @param dt_sec 两次解析的间隔 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec) { + /* 指针检测 */ + if (rc == NULL) return -1; + if (cmd == NULL) return -1; + + /* 在pc控制和rc控制间切换 */ + if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) && + CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_Q)) + cmd->pc_ctrl = true; + + if (CMD_KeyPressedRc(rc, CMD_KEY_SHIFT) && + CMD_KeyPressedRc(rc, CMD_KEY_CTRL) && CMD_KeyPressedRc(rc, CMD_KEY_E)) + cmd->pc_ctrl = false; + /*c当rc丢控时，恢复机器人至默认状态 */ + if ((rc->sw_l == CMD_SW_ERR) || (rc->sw_r == CMD_SW_ERR)) { + CMD_RcLostLogic(cmd); + } else { + if (cmd->pc_ctrl) { + CMD_PcLogic(rc, cmd, dt_sec); + } else { + CMD_RcLogic(rc, cmd, dt_sec); + } + } + return 0; +} + +/** + * @brief 解析上位机命令 + * + * @param host host数据 + * @param cmd 命令 + * @param dt_sec 两次解析的间隔 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) { + (void)dt_sec; /* 未使用dt_sec，消除警告 */ + /* 指针检测 */ + if (host == NULL) return -1; + if (cmd == NULL) return -1; + + /* 云台欧拉角设置为host相应的变化的欧拉角 */ + cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw; + cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit; + + /* host射击命令，设置不同的射击频率和弹丸初速度 */ + if (host->fire) { + cmd->shoot.mode = SHOOT_MODE_LOADED; + cmd->shoot.fire = true; + } else { + cmd->shoot.mode = SHOOT_MODE_SAFE; + } + return 0; +} + +/** + * @brief 添加向Referee发送的命令 + * + * @param ref 命令队列 + * @param cmd 要添加的命令 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd) { + /* 指针检测 */ + if (ref == NULL) return -1; + /* 越界检测 */ + if (ref->counter >= CMD_REFEREE_MAX_NUM || ref->counter < 0) return -1; + + /* 添加机器人当前行为状态到画图的命令队列中 */ + ref->cmd[ref->counter] = cmd; + ref->counter++; + return 0; +} diff --git a/User/component/cmd.h b/User/component/cmd.h new file mode 100644 index 0000000..0b352dd --- /dev/null +++ b/User/component/cmd.h @@ -0,0 +1,306 @@ +/* + 控制命令 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +#include "ahrs.h" + +#define CMD_REFEREE_MAX_NUM (3) /* 发送命令限定的最大数量 */ + +/* 机器人型号 */ +typedef enum { + ROBOT_MODEL_INFANTRY = 0, /* 步兵机器人 */ + ROBOT_MODEL_HERO, /* 英雄机器人 */ + ROBOT_MODEL_ENGINEER, /* 工程机器人 */ + ROBOT_MODEL_DRONE, /* 空中机器人 */ + ROBOT_MODEL_SENTRY, /* 哨兵机器人 */ + ROBOT_MODEL_NUM, /* 型号数量 */ +} CMD_RobotModel_t; + +/* 底盘运行模式 */ +typedef enum { + CHASSIS_MODE_RELAX, /* 放松模式，电机不输出。一般情况底盘初始化之后的模式 */ + CHASSIS_MODE_BREAK, /* 刹车模式，电机闭环控制保持静止。用于机器人停止状态 */ + CHASSIS_MODE_FOLLOW_GIMBAL, /* 通过闭环控制使车头方向跟随云台 */ + CHASSIS_MODE_FOLLOW_GIMBAL_35, /* 通过闭环控制使车头方向35度跟随云台 */ + CHASSIS_MODE_ROTOR, /* 小陀螺模式，通过闭环控制使底盘不停旋转 */ + CHASSIS_MODE_INDENPENDENT, /* 独立模式。底盘运行不受云台影响 */ + CHASSIS_MODE_OPEN, /* 开环模式。底盘运行不受PID控制，直接输出到电机 */ +} CMD_ChassisMode_t; + +/* 云台运行模式 */ +typedef enum { + GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */ + GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */ + GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */ +} CMD_GimbalMode_t; + +/* 射击运行模式 */ +typedef enum { + SHOOT_MODE_RELAX, /* 放松模式，电机不输出 */ + SHOOT_MODE_SAFE, /* 保险模式，电机闭环控制保持静止 */ + SHOOT_MODE_LOADED, /* 上膛模式，摩擦轮开启。随时准备开火 */ +} CMD_ShootMode_t; + +typedef enum { + FIRE_MODE_SINGLE, /* 单发开火模式 */ + FIRE_MODE_BURST, /* N连发开火模式 */ + FIRE_MODE_CONT, /* 持续开火模式 */ + FIRE_MODE_NUM, +} CMD_FireMode_t; + +/* 小陀螺转动模式 */ +typedef enum { + ROTOR_MODE_CW, /* 顺时针转动 */ + ROTOR_MODE_CCW, /* 逆时针转动 */ + ROTOR_MODE_RAND, /* 随机转动 */ +} CMD_RotorMode_t; + +/* 底盘控制命令 */ +typedef struct { + CMD_ChassisMode_t mode; /* 底盘运行模式 */ + CMD_RotorMode_t mode_rotor; /* 小陀螺转动模式 */ + MoveVector_t ctrl_vec; /* 底盘控制向量 */ +} CMD_ChassisCmd_t; + +/* 云台控制命令 */ +typedef struct { + CMD_GimbalMode_t mode; /* 云台运行模式 */ + AHRS_Eulr_t delta_eulr; /* 欧拉角变化角度 */ +} CMD_GimbalCmd_t; + +/* 射击控制命令 */ +typedef struct { + CMD_ShootMode_t mode; /* 射击运行模式 */ + CMD_FireMode_t fire_mode; /* 开火模式 */ + bool fire; /*开火*/ + bool cover_open; /* 弹舱盖开关 */ + bool reverse_trig; /* 拨弹电机状态 */ +} CMD_ShootCmd_t; + +/* 拨杆位置 */ +typedef enum { + CMD_SW_ERR = 0, + CMD_SW_UP = 1, + CMD_SW_MID = 3, + CMD_SW_DOWN = 2, +} CMD_SwitchPos_t; + +/* 键盘按键值 */ +typedef enum { + CMD_KEY_W = 0, + CMD_KEY_S, + CMD_KEY_A, + CMD_KEY_D, + CMD_KEY_SHIFT, + CMD_KEY_CTRL, + CMD_KEY_Q, + CMD_KEY_E, + CMD_KEY_R, + CMD_KEY_F, + CMD_KEY_G, + CMD_KEY_Z, + CMD_KEY_X, + CMD_KEY_C, + CMD_KEY_V, + CMD_KEY_B, + CMD_L_CLICK, + CMD_R_CLICK, + CMD_KEY_NUM, +} CMD_KeyValue_t; + +/* 行为值序列 */ +typedef enum { + CMD_BEHAVIOR_FORE = 0, /* 向前 */ + CMD_BEHAVIOR_BACK, /* 向后 */ + CMD_BEHAVIOR_LEFT, /* 向左 */ + CMD_BEHAVIOR_RIGHT, /* 向右 */ + CMD_BEHAVIOR_ACCELERATE, /* 加速 */ + CMD_BEHAVIOR_DECELEBRATE, /* 减速 */ + CMD_BEHAVIOR_FIRE, /* 开火 */ + CMD_BEHAVIOR_FIRE_MODE, /* 切换开火模式 */ + CMD_BEHAVIOR_BUFF, /* 打符模式 */ + CMD_BEHAVIOR_AUTOAIM, /* 自瞄模式 */ + CMD_BEHAVIOR_OPENCOVER, /* 弹舱盖开关 */ + CMD_BEHAVIOR_ROTOR, /* 小陀螺模式 */ + CMD_BEHAVIOR_REVTRIG, /* 反转拨弹 */ + CMD_BEHAVIOR_FOLLOWGIMBAL35, /* 跟随云台呈35度 */ + CMD_BEHAVIOR_NUM, +} CMD_Behavior_t; + +typedef enum { + CMD_ACTIVE_PRESSING, /* 按下时触发 */ + CMD_ACTIVE_RASING, /* 抬起时触发 */ + CMD_ACTIVE_PRESSED, /* 按住时触发 */ +} CMD_ActiveType_t; + +typedef struct { + CMD_ActiveType_t active; + CMD_KeyValue_t key; +} CMD_KeyMapItem_t; + +/* 行为映射的对应按键数组 */ +typedef struct { + CMD_KeyMapItem_t key_map[CMD_BEHAVIOR_NUM]; +} CMD_KeyMap_Params_t; + +/* 位移灵敏度参数 */ +typedef struct { + float move_sense; /* 移动灵敏度 */ + float move_fast_sense; /* 加速灵敏度 */ + float move_slow_sense; /* 减速灵敏度 */ +} CMD_Move_Params_t; + +typedef struct { + uint16_t width; + uint16_t height; +} CMD_Screen_t; + +/* 命令参数 */ +typedef struct { + float sens_mouse; /* 鼠标灵敏度 */ + float sens_rc; /* 遥控器摇杆灵敏度 */ + CMD_KeyMap_Params_t map; /* 按键映射行为命令 */ + CMD_Move_Params_t move; /* 位移灵敏度参数 */ + CMD_Screen_t screen; /* 屏幕分辨率参数 */ +} CMD_Params_t; + +/* AI行为状态 */ +typedef enum { + AI_STATUS_STOP, /* 停止状态 */ + AI_STATUS_AUTOAIM, /* 自瞄状态 */ + AI_STATUS_HITSWITCH, /* 打符状态 */ + AI_STATUS_AUTOMATIC /* 自动状态 */ +} CMD_AI_Status_t; + +/* UI所用行为状态 */ +typedef enum { + CMD_UI_NOTHING, /* 当前无状态 */ + CMD_UI_AUTO_AIM_START, /* 自瞄状态开启 */ + CMD_UI_AUTO_AIM_STOP, /* 自瞄状态关闭 */ + CMD_UI_HIT_SWITCH_START, /* 打符状态开启 */ + CMD_UI_HIT_SWITCH_STOP /* 打符状态关闭 */ +} CMD_UI_t; + +/*裁判系统发送的命令*/ +typedef struct { + CMD_UI_t cmd[CMD_REFEREE_MAX_NUM]; /* 命令数组 */ + uint8_t counter; /* 命令计数 */ +} CMD_RefereeCmd_t; + +typedef struct { + bool pc_ctrl; /* 是否使用键鼠控制 */ + bool host_overwrite; /* 是否Host控制 */ + uint16_t key_last; /* 上次按键键值 */ + + struct { + int16_t x; + int16_t y; + int16_t z; + bool l_click; /* 左键 */ + bool r_click; /* 右键 */ + } mouse_last; /* 鼠标值 */ + + CMD_AI_Status_t ai_status; /* AI状态 */ + + const CMD_Params_t *param; /* 命令参数 */ + + CMD_ChassisCmd_t chassis; /* 底盘控制命令 */ + CMD_GimbalCmd_t gimbal; /* 云台控制命令 */ + CMD_ShootCmd_t shoot; /* 射击控制命令 */ + CMD_RefereeCmd_t referee; /* 裁判系统发送命令 */ +} CMD_t; + +typedef struct { + float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */ + float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */ + float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */ + float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */ + + float ch_res; /* 第五通道值 */ + + CMD_SwitchPos_t sw_r; /* 右侧拨杆位置 */ + CMD_SwitchPos_t sw_l; /* 左侧拨杆位置 */ + + struct { + int16_t x; + int16_t y; + int16_t z; + bool l_click; /* 左键 */ + bool r_click; /* 右键 */ + } mouse; /* 鼠标值 */ + + uint16_t key; /* 按键值 */ + + uint16_t res; /* 保留，未启用 */ +} CMD_RC_t; + +typedef struct { + AHRS_Eulr_t gimbal_delta; /* 欧拉角的变化量 */ + + struct { + float vx; /* x轴移动速度 */ + float vy; /* y轴移动速度 */ + float wz; /* z轴转动速度 */ + } chassis_move_vec; /* 底盘移动向量 */ + + bool fire; /* 开火状态 */ +} CMD_Host_t; + +/** + * @brief 解析行为命令 + * + * @param rc 遥控器数据 + * @param cmd 主结构体 + */ +int8_t CMD_Init(CMD_t *cmd, const CMD_Params_t *param); + +/** + * @brief 检查是否启用上位机控制指令覆盖 + * + * @param cmd 主结构体 + * @return true 启用 + * @return false 不启用 + */ +bool CMD_CheckHostOverwrite(CMD_t *cmd); + +/** + * @brief 解析命令 + * + * @param rc 遥控器数据 + * @param cmd 命令 + * @param dt_sec 两次解析的间隔 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_ParseRc(CMD_RC_t *rc, CMD_t *cmd, float dt_sec); + +/** + * @brief 解析上位机命令 + * + * @param host host数据 + * @param cmd 命令 + * @param dt_sec 两次解析的间隔 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec); + +/** + * @brief 添加向Referee发送的命令 + * + * @param ref 命令队列 + * @param cmd 要添加的命令 + * @return int8_t 0对应没有错误 + */ +int8_t CMD_RefereeAdd(CMD_RefereeCmd_t *ref, CMD_UI_t cmd); + +#ifdef __cplusplus +} +#endif diff --git a/User/component/component_config.yaml b/User/component/component_config.yaml new file mode 100644 index 0000000..9f062aa --- /dev/null +++ b/User/component/component_config.yaml @@ -0,0 +1,21 @@ +ahrs: + dependencies: + - component/filter + enabled: true +cmd: + dependencies: [] + enabled: true +filter: + dependencies: + - component/ahrs + enabled: true +limiter: + dependencies: [] + enabled: true +pid: + dependencies: + - component/filter + enabled: true +user_math: + dependencies: [] + enabled: true diff --git a/User/component/filter.c b/User/component/filter.c new file mode 100644 index 0000000..5375b8e --- /dev/null +++ b/User/component/filter.c @@ -0,0 +1,185 @@ +/* + 各类滤波器。 +*/ + +#include "filter.h" + +#include "user_math.h" + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param cutoff_freq 截止频率 + */ +void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq, + float cutoff_freq) { + if (f == NULL) return; + + f->cutoff_freq = cutoff_freq; + + f->delay_element_1 = 0.0f; + f->delay_element_2 = 0.0f; + + if (f->cutoff_freq <= 0.0f) { + /* no filtering */ + f->b0 = 1.0f; + f->b1 = 0.0f; + f->b2 = 0.0f; + + f->a1 = 0.0f; + f->a2 = 0.0f; + + return; + } + const float fr = sample_freq / f->cutoff_freq; + const float ohm = tanf(M_PI / fr); + const float c = 1.0f + 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm; + + f->b0 = ohm * ohm / c; + f->b1 = 2.0f * f->b0; + f->b2 = f->b0; + + f->a1 = 2.0f * (ohm * ohm - 1.0f) / c; + f->a2 = (1.0f - 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm) / c; +} + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample) { + if (f == NULL) return 0.0f; + + /* do the filtering */ + float delay_element_0 = + sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2; + + if (isinf(delay_element_0)) { + /* don't allow bad values to propagate via the filter */ + delay_element_0 = sample; + } + + const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 + + f->delay_element_2 * f->b2; + + f->delay_element_2 = f->delay_element_1; + f->delay_element_1 = delay_element_0; + + /* return the value. Should be no need to check limits */ + return output; +} + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample) { + if (f == NULL) return 0.0f; + + const float dval = sample / (f->b0 + f->b1 + f->b2); + + if (isfinite(dval)) { + f->delay_element_1 = dval; + f->delay_element_2 = dval; + + } else { + f->delay_element_1 = sample; + f->delay_element_2 = sample; + } + + return LowPassFilter2p_Apply(f, sample); +} + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param notch_freq 中心频率 + * @param bandwidth 带宽 + */ +void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq, + float bandwidth) { + if (f == NULL) return; + + f->notch_freq = notch_freq; + f->bandwidth = bandwidth; + + f->delay_element_1 = 0.0f; + f->delay_element_2 = 0.0f; + + if (notch_freq <= 0.0f) { + /* no filtering */ + f->b0 = 1.0f; + f->b1 = 0.0f; + f->b2 = 0.0f; + + f->a1 = 0.0f; + f->a2 = 0.0f; + + return; + } + + const float alpha = tanf(M_PI * bandwidth / sample_freq); + const float beta = -cosf(M_2PI * notch_freq / sample_freq); + const float a0_inv = 1.0f / (alpha + 1.0f); + + f->b0 = a0_inv; + f->b1 = 2.0f * beta * a0_inv; + f->b2 = a0_inv; + + f->a1 = f->b1; + f->a2 = (1.0f - alpha) * a0_inv; +} + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +inline float NotchFilter_Apply(NotchFilter_t *f, float sample) { + if (f == NULL) return 0.0f; + + /* Direct Form II implementation */ + const float delay_element_0 = + sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2; + const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 + + f->delay_element_2 * f->b2; + + f->delay_element_2 = f->delay_element_1; + f->delay_element_1 = delay_element_0; + + return output; +} + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float NotchFilter_Reset(NotchFilter_t *f, float sample) { + if (f == NULL) return 0.0f; + + float dval = sample; + + if (fabsf(f->b0 + f->b1 + f->b2) > FLT_EPSILON) { + dval = dval / (f->b0 + f->b1 + f->b2); + } + + f->delay_element_1 = dval; + f->delay_element_2 = dval; + + return NotchFilter_Apply(f, sample); +} diff --git a/User/component/filter.h b/User/component/filter.h new file mode 100644 index 0000000..c075946 --- /dev/null +++ b/User/component/filter.h @@ -0,0 +1,104 @@ +/* + 各类滤波器。 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "user_math.h" + +/* 二阶低通滤波器 */ +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; + +/** + * @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); + +#ifdef __cplusplus +} +#endif diff --git a/User/component/limiter.c b/User/component/limiter.c new file mode 100644 index 0000000..71e4bf1 --- /dev/null +++ b/User/component/limiter.c @@ -0,0 +1,107 @@ +/* + 限制器 +*/ + +#include "limiter.h" + +#include +#include + +#define POWER_BUFF_THRESHOLD 20 +#define CHASSIS_POWER_CHECK_FREQ 10 +#define CHASSIS_POWER_FACTOR_PASS 0.9f +#define CHASSIS_POWER_FACTOR_NO_PASS 1.5f + +#define CHASSIS_MOTOR_CIRCUMFERENCE 0.12f + +/** + * @brief 限制底盘功率不超过power_limit + * + * @param power_limit 最大功率 + * @param motor_out 电机输出值 + * @param speed 电机转速 + * @param len 电机数量 + * @return int8_t 0对应没有错误 + */ +int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out, + float *speed, uint32_t len) { + /* power_limit小于0时不进行限制 */ + if (motor_out == NULL || speed == NULL || power_limit < 0) return -1; + + float sum_motor_out = 0.0f; + for (uint32_t i = 0; i < len; i++) { + /* 总功率计算 P=F(由转矩电流表示)*V(由转速表示) */ + sum_motor_out += + fabsf(motor_out[i]) * fabsf(speed[i]) * CHASSIS_MOTOR_CIRCUMFERENCE; + } + + /* 保持每个电机输出值缩小时比例不变 */ + if (sum_motor_out > power_limit) { + for (uint32_t i = 0; i < len; i++) { + motor_out[i] *= power_limit / sum_motor_out; + } + } + + return 0; +} + +/** + * @brief 电容输入功率计算 + * + * @param power_in 底盘当前功率 + * @param power_limit 裁判系统功率限制值 + * @param power_buffer 缓冲能量 + * @return float 裁判系统输出最大值 + */ +float PowerLimit_CapInput(float power_in, float power_limit, + float power_buffer) { + float target_power = 0.0f; + + /* 计算下一个检测周期的剩余缓冲能量 */ + float heat_buff = power_buffer - (float)(power_in - power_limit) / + (float)CHASSIS_POWER_CHECK_FREQ; + if (heat_buff < POWER_BUFF_THRESHOLD) { /* 功率限制 */ + target_power = power_limit * CHASSIS_POWER_FACTOR_PASS; + } else { + target_power = power_limit * CHASSIS_POWER_FACTOR_NO_PASS; + } + + return target_power; +} + +/** + * @brief 使用缓冲能量计算底盘最大功率 + * + * @param power_limit 裁判系统功率限制值 + * @param power_buffer 缓冲能量 + * @return float 底盘输出最大值 + */ +float PowerLimit_TargetPower(float power_limit, float power_buffer) { + float target_power = 0.0f; + + /* 根据剩余缓冲能量计算输出功率 */ + target_power = power_limit * (power_buffer - 10.0f) / 20.0f; + if (target_power < 0.0f) target_power = 0.0f; + + return target_power; +} + +/** + * @brief 射击频率控制 + * + * @param heat 当前热量 + * @param heat_limit 热量上限 + * @param cooling_rate 冷却速率 + * @param heat_increase 冷却增加 + * @param shoot_freq 经过热量限制后的射击频率 + * @return float 射击频率 + */ +float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate, + float heat_increase, bool is_big) { + float heat_percent = heat / heat_limit; + float stable_freq = cooling_rate / heat_increase; + if (is_big) + return stable_freq; + else + return (heat_percent > 0.7f) ? stable_freq : 3.0f * stable_freq; +} diff --git a/User/component/limiter.h b/User/component/limiter.h new file mode 100644 index 0000000..7c24cb5 --- /dev/null +++ b/User/component/limiter.h @@ -0,0 +1,55 @@ +/* + 限制器 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +/** + * @brief 限制底盘功率不超过power_limit + * + * @param power_limit 最大功率 + * @param motor_out 电机输出值 + * @param speed 电机转速 + * @param len 电机数量 + * @return int8_t 0对应没有错误 + */ +int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out, + float *speed, uint32_t len); +/** + * @brief 电容输入功率计算 + * + * @param power_in 底盘当前功率 + * @param power_limit 裁判系统功率限制值 + * @param power_buffer 缓冲能量 + * @return float 裁判系统输出最大值 + */ +float PowerLimit_CapInput(float power_in, float power_limit, + float power_buffer); +/** + * @brief 使用缓冲能量计算底盘最大功率 + * + * @param power_limit 裁判系统功率限制值 + * @param power_buffer 缓冲能量 + * @return float 底盘输出最大值 + */ +float PowerLimit_TargetPower(float power_limit, float power_buffer); + +/** + * @brief 射击频率控制 + * + * @param heat 当前热量 + * @param heat_limit 热量上限 + * @param cooling_rate 冷却速率 + * @param heat_increase 冷却增加 + * @param shoot_freq 经过热量限制后的射击频率 + * @return float 射击频率 + */ +float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate, + float heat_increase, bool is_big); diff --git a/User/component/pid.c b/User/component/pid.c new file mode 100644 index 0000000..0a3c7d4 --- /dev/null +++ b/User/component/pid.c @@ -0,0 +1,158 @@ +/* + Modified from + https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.cpp + + 参考资料： + https://github.com/PX4/Firmware/issues/12362 + https://dev.px4.io/master/en/flight_stack/controller_diagrams.html + https://docs.px4.io/master/en/config_mc/pid_tuning_guide_multicopter.html#standard_form + https://www.controleng.com/articles/not-all-pid-controllers-are-the-same/ + https://en.wikipedia.org/wiki/PID_controller + http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/ +*/ + +#include "pid.h" + +#define SIGMA 0.000001f + +/** + * @brief 初始化PID + * + * @param pid PID结构体 + * @param mode PID模式 + * @param sample_freq 采样频率 + * @param param PID参数 + * @return int8_t 0对应没有错误 + */ +int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq, + const KPID_Params_t *param) { + if (pid == NULL) return -1; + + if (!isfinite(param->p)) return -1; + if (!isfinite(param->i)) return -1; + if (!isfinite(param->d)) return -1; + if (!isfinite(param->i_limit)) return -1; + if (!isfinite(param->out_limit)) return -1; + pid->param = param; + + float dt_min = 1.0f / sample_freq; + if (isfinite(dt_min)) + pid->dt_min = dt_min; + else + return -1; + + LowPassFilter2p_Init(&(pid->dfilter), sample_freq, pid->param->d_cutoff_freq); + + pid->mode = mode; + PID_Reset(pid); + return 0; +} + +/** + * @brief PID计算 + * + * @param pid PID结构体 + * @param sp 设定值 + * @param fb 反馈值 + * @param fb_dot 反馈值微分 + * @param dt 间隔时间 + * @return float 计算的输出 + */ +float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt) { + if (!isfinite(sp) || !isfinite(fb) || !isfinite(fb_dot) || !isfinite(dt)) { + return pid->last.out; + } + + /* 计算误差值 */ + const float err = CircleError(sp, fb, pid->param->range); + + /* 计算P项 */ + const float k_err = err * pid->param->k; + + /* 计算D项 */ + const float k_fb = pid->param->k * fb; + const float filtered_k_fb = LowPassFilter2p_Apply(&(pid->dfilter), k_fb); + + float d; + switch (pid->mode) { + case KPID_MODE_CALC_D: + /* 通过fb计算D，避免了由于sp变化导致err突变的问题 */ + /* 当sp不变时，err的微分等于负的fb的微分 */ + d = (filtered_k_fb - pid->last.k_fb) / fmaxf(dt, pid->dt_min); + break; + + case KPID_MODE_SET_D: + d = fb_dot; + break; + + case KPID_MODE_NO_D: + d = 0.0f; + break; + } + pid->last.err = err; + pid->last.k_fb = filtered_k_fb; + + if (!isfinite(d)) d = 0.0f; + + /* 计算PD输出 */ + float output = (k_err * pid->param->p) - (d * pid->param->d); + + /* 计算I项 */ + const float i = pid->i + (k_err * dt); + const float i_out = i * pid->param->i; + + if (pid->param->i > SIGMA) { + /* 检查是否饱和 */ + if (isfinite(i)) { + if ((fabsf(output + i_out) <= pid->param->out_limit) && + (fabsf(i) <= pid->param->i_limit)) { + /* 未饱和，使用新积分 */ + pid->i = i; + } + } + } + + /* 计算PID输出 */ + output += i_out; + + /* 限制输出 */ + if (isfinite(output)) { + if (pid->param->out_limit > SIGMA) { + output = AbsClip(output, pid->param->out_limit); + } + pid->last.out = output; + } + return pid->last.out; +} + +/** + * @brief 重置微分项 + * + * @param pid PID结构体 + * @return int8_t 0对应没有错误 + */ +int8_t PID_ResetIntegral(KPID_t *pid) { + if (pid == NULL) return -1; + + pid->i = 0.0f; + + return 0; +} + +/** + * @brief 重置PID + * + * @param pid PID结构体 + * @return int8_t 0对应没有错误 + */ +int8_t PID_Reset(KPID_t *pid) { + if (pid == NULL) return -1; + + pid->i = 0.0f; + pid->last.err = 0.0f; + pid->last.k_fb = 0.0f; + pid->last.out = 0.0f; + LowPassFilter2p_Reset(&(pid->dfilter), 0.0f); + + return 0; +} diff --git a/User/component/pid.h b/User/component/pid.h new file mode 100644 index 0000000..a93acaa --- /dev/null +++ b/User/component/pid.h @@ -0,0 +1,95 @@ +/* + Modified from + https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.h +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#include "filter.h" +#include "user_math.h" + +/* 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); + +#ifdef __cplusplus +} +#endif diff --git a/User/component/user_math.c b/User/component/user_math.c new file mode 100644 index 0000000..f3f3964 --- /dev/null +++ b/User/component/user_math.c @@ -0,0 +1,132 @@ +/* + 自定义的数学运算。 +*/ + +#include "user_math.h" + +#include + +inline float InvSqrt(float x) { +//#if 0 + /* Fast inverse square-root */ + /* See: http://en.wikipedia.org/wiki/Fast_inverse_square_root */ + float halfx = 0.5f * x; + float y = x; + long i = *(long*)&y; + i = 0x5f3759df - (i>>1); + y = *(float*)&i; + y = y * (1.5f - (halfx * y * y)); + y = y * (1.5f - (halfx * y * y)); + return y; +//#else +// return 1.0f / sqrtf(x); +//#endif +} + +inline float AbsClip(float in, float limit) { + return (in < -limit) ? -limit : ((in > limit) ? limit : in); +} + +float fAbs(float in){ + return (in > 0) ? in : -in; +} + +inline void Clip(float *origin, float min, float max) { + if (*origin > max) *origin = max; + if (*origin < min) *origin = min; +} + +inline float Sign(float in) { return (in > 0) ? 1.0f : 0.0f; } + +/** + * \brief 将运动向量置零 + * + * \param mv 被操作的值 + */ +inline void ResetMoveVector(MoveVector_t *mv) { memset(mv, 0, sizeof(*mv)); } + +/** + * \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值 + * 例如编码器：相差1.5PI其实等于相差-0.5PI + * + * \param sp 被操作的值 + * \param fb 变化量 + * \param range 被操作的值变化范围，正数时起效 + * + * \return 函数运行结果 + */ +inline float CircleError(float sp, float fb, float range) { + float error = sp - fb; + if (range > 0.0f) { + float half_range = range / 2.0f; + + if (error > half_range) + error -= range; + else if (error < -half_range) + error += range; + } + return error; +} + +/** + * \brief 循环加法，用于没有负数值，并在一定范围内变化的值 + * 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI + * + * \param origin 被操作的值 + * \param delta 变化量 + * \param range 被操作的值变化范围，正数时起效 + */ +inline void CircleAdd(float *origin, float delta, float range) { + float out = *origin + delta; + if (range > 0.0f) { + if (out >= range) + out -= range; + else if (out < 0.0f) + out += range; + } + *origin = out; +} + +/** + * @brief 循环值取反 + * + * @param origin 被操作的值 + */ +inline void CircleReverse(float *origin) { *origin = -(*origin) + M_2PI; } + +/** + * @brief 根据目标弹丸速度计算摩擦轮转速 + * + * @param bullet_speed 弹丸速度 + * @param fric_radius 摩擦轮半径 + * @param is17mm 是否为17mm + * @return 摩擦轮转速 + */ +inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) { + if (bullet_speed == 0.0f) return 0.f; + if (is17mm) { + if (bullet_speed == 15.0f) return 4670.f; + if (bullet_speed == 18.0f) return 5200.f; + if (bullet_speed == 30.0f) return 7350.f; + } else { + if (bullet_speed == 10.0f) return 4450.f; + if (bullet_speed == 16.0f) return 5800.f; + } + + /* 不为裁判系统设定值时,计算转速 */ + return 60.0f * (float)bullet_speed / (M_2PI * fric_radius); +} + +// /** +// * @brief 断言失败处理 +// * +// * @param file 文件名 +// * @param line 行号 +// */ +// void VerifyFailed(const char *file, uint32_t line) { +// UNUSED(file); +// UNUSED(line); +// while (1) { +// __NOP(); +// } +// } diff --git a/User/component/user_math.h b/User/component/user_math.h new file mode 100644 index 0000000..fea820f --- /dev/null +++ b/User/component/user_math.h @@ -0,0 +1,161 @@ +/* + 自定义的数学运算。 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include +#include +#include + +#define M_DEG2RAD_MULT (0.01745329251f) +#define M_RAD2DEG_MULT (57.2957795131f) + +#ifndef M_PI +#define M_PI 3.14159265358979323846f +#endif + +#ifndef M_2PI +#define M_2PI 6.28318530717958647692f +#endif + +#ifndef __packed + #define __packed __attribute__((__packed__)) +#endif /* __packed */ + +#define max(a, b) \ + ({ \ + __typeof__(a) _a = (a); \ + __typeof__(b) _b = (b); \ + _a > _b ? _a : _b; \ + }) + +#define min(a, b) \ + ({ \ + __typeof__(a) _a = (a); \ + __typeof__(b) _b = (b); \ + _a < _b ? _a : _b; \ + }) + +/* 移动向量 */ +typedef struct { + float vx; /* 前后平移 */ + float vy; /* 左右平移 */ + float wz; /* 转动 */ +} MoveVector_t; + +float InvSqrt(float x); + +float AbsClip(float in, float limit); + +float fAbs(float in); + +void Clip(float *origin, float min, float max); + +float Sign(float in); + +/** + * \brief 将运动向量置零 + * + * \param mv 被操作的值 + */ +void ResetMoveVector(MoveVector_t *mv); + +/** + * \brief 计算循环值的误差，用于没有负数值，并在一定范围内变化的值 + * 例如编码器：相差1.5PI其实等于相差-0.5PI + * + * \param sp 被操作的值 + * \param fb 变化量 + * \param range 被操作的值变化范围，正数时起效 + * + * \return 函数运行结果 + */ +float CircleError(float sp, float fb, float range); + +/** + * \brief 循环加法，用于没有负数值，并在一定范围内变化的值 + * 例如编码器，在0-2PI内变化，1.5PI + 1.5PI = 1PI + * + * \param origin 被操作的值 + * \param delta 变化量 + * \param range 被操作的值变化范围，正数时起效 + */ +void CircleAdd(float *origin, float delta, float range); + +/** + * @brief 循环值取反 + * + * @param origin 被操作的值 + */ +void CircleReverse(float *origin); + +/** + * @brief 根据目标弹丸速度计算摩擦轮转速 + * + * @param bullet_speed 弹丸速度 + * @param fric_radius 摩擦轮半径 + * @param is17mm 是否为17mm + * @return 摩擦轮转速 + */ +float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm); + +#ifdef __cplusplus +} +#endif + +#ifdef DEBUG + +/** + * @brief 如果表达式的值为假则运行处理函数 + * + */ +#define ASSERT(expr) \ + do { \ + if (!(expr)) { \ + VerifyFailed(__FILE__, __LINE__); \ + } \ + } while (0) +#else + +/** + * @brief 未定DEBUG，表达式不会运行，断言被忽略 + * + */ +#define ASSERT(expr) ((void)(0)) +#endif + +#ifdef DEBUG + +/** + * @brief 如果表达式的值为假则运行处理函数 + * + */ +#define VERIFY(expr) \ + do { \ + if (!(expr)) { \ + VerifyFailed(__FILE__, __LINE__); \ + } \ + } while (0) +#else + +/** + * @brief 表达式会运行，忽略表达式结果 + * + */ +#define VERIFY(expr) ((void)(expr)) +#endif + +// /** +// * @brief 断言失败处理 +// * +// * @param file 文件名 +// * @param line 行号 +// */ +// void VerifyFailed(const char *file, uint32_t line); diff --git a/User/device/buzzer.c b/User/device/buzzer.c new file mode 100644 index 0000000..e244825 --- /dev/null +++ b/User/device/buzzer.c @@ -0,0 +1,44 @@ +#include "device/buzzer.h" + + +int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel) { + if (buzzer == NULL) return DEVICE_ERR; + + buzzer->channel = channel; + buzzer->header.online = true; + + BUZZER_Stop(buzzer); + + return DEVICE_OK ; +} + +int8_t BUZZER_Start(BUZZER_t *buzzer) { + if (buzzer == NULL || !buzzer->header.online) + return DEVICE_ERR; + + return (BSP_PWM_Start(buzzer->channel) == BSP_OK) ? + DEVICE_OK : DEVICE_ERR; +} + +int8_t BUZZER_Stop(BUZZER_t *buzzer) { + if (buzzer == NULL || !buzzer->header.online) + return DEVICE_ERR; + + return (BSP_PWM_Stop(buzzer->channel) == BSP_OK) ? + DEVICE_OK : DEVICE_ERR; +} + +int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle) { + if (buzzer == NULL || !buzzer->header.online) + return DEVICE_ERR; + + int result = DEVICE_OK ; + + if (BSP_PWM_SetFreq(buzzer->channel, freq) != BSP_OK) + result = DEVICE_ERR; + + if (BSP_PWM_SetComp(buzzer->channel, duty_cycle) != BSP_OK) + result = DEVICE_ERR; + + return result; +} diff --git a/User/device/buzzer.h b/User/device/buzzer.h new file mode 100644 index 0000000..3939e23 --- /dev/null +++ b/User/device/buzzer.h @@ -0,0 +1,35 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device.h" +#include "bsp/pwm.h" +#include + +/* Exported constants ------------------------------------------------------- */ + +/* Exported types ----------------------------------------------------------- */ +typedef struct { + DEVICE_Header_t header; + BSP_PWM_Channel_t channel; +} BUZZER_t; + +/* Exported functions prototypes -------------------------------------------- */ + +int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel); + + +int8_t BUZZER_Start(BUZZER_t *buzzer); + + +int8_t BUZZER_Stop(BUZZER_t *buzzer); + + +int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle); + +#ifdef __cplusplus +} +#endif diff --git a/User/device/device.h b/User/device/device.h new file mode 100644 index 0000000..6475d69 --- /dev/null +++ b/User/device/device.h @@ -0,0 +1,31 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +#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) +/* AUTO GENERATED SIGNALS END */ + +/* USER SIGNALS BEGIN */ + +/* USER SIGNALS END */ +/*设备层通用Header*/ +typedef struct { + bool online; + uint64_t last_online_time; +} DEVICE_Header_t; + +#ifdef __cplusplus +} +#endif diff --git a/User/device/device_config.yaml b/User/device/device_config.yaml new file mode 100644 index 0000000..689b116 --- /dev/null +++ b/User/device/device_config.yaml @@ -0,0 +1,19 @@ +buzzer: + bsp_config: {} + enabled: true +dm_imu: + bsp_config: {} + enabled: true +dr16: + bsp_config: + BSP_UART_DR16: BSP_UART_DR16 + enabled: true +motor: + bsp_config: {} + enabled: true +motor_lk: + bsp_config: {} + enabled: true +motor_lz: + bsp_config: {} + enabled: true diff --git a/User/device/dm_imu.c b/User/device/dm_imu.c new file mode 100644 index 0000000..8d699c8 --- /dev/null +++ b/User/device/dm_imu.c @@ -0,0 +1,271 @@ +/* + DM_IMU数据获取 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "dm_imu.h" + +#include "bsp/can.h" +#include "bsp/time.h" +#include "component/user_math.h" +#include + +/* Private define ----------------------------------------------------------- */ +#define DM_IMU_OFFLINE_TIMEOUT 1000 // 设备离线判定时间1000ms + +#define ACCEL_CAN_MAX (58.8f) +#define ACCEL_CAN_MIN (-58.8f) +#define GYRO_CAN_MAX (34.88f) +#define GYRO_CAN_MIN (-34.88f) +#define PITCH_CAN_MAX (90.0f) +#define PITCH_CAN_MIN (-90.0f) +#define ROLL_CAN_MAX (180.0f) +#define ROLL_CAN_MIN (-180.0f) +#define YAW_CAN_MAX (180.0f) +#define YAW_CAN_MIN (-180.0f) +#define TEMP_MIN (0.0f) +#define TEMP_MAX (60.0f) +#define Quaternion_MIN (-1.0f) +#define Quaternion_MAX (1.0f) + + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ +/* Private function --------------------------------------------------------- */ + +static uint8_t count = 0; // 计数器，用于判断设备是否离线 +/** + * @brief: 无符号整数转换为浮点数函数 + */ +static float uint_to_float(int x_int, float x_min, float x_max, int bits) +{ + float span = x_max - x_min; + float offset = x_min; + return ((float)x_int)*span/((float)((1<data.temp = (float)temp; + imu->data.accl.x = uint_to_float(acc_x_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); + imu->data.accl.y = uint_to_float(acc_y_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); + imu->data.accl.z = uint_to_float(acc_z_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); + return DEVICE_OK; +} +/** + * @brief 解析陀螺仪数据 + */ +static int8_t DM_IMU_ParseGyroData(DM_IMU_t *imu, uint8_t *data, uint8_t len) { + if (imu == NULL || data == NULL || len < 8) { + return DEVICE_ERR; + } + uint16_t gyro_x_raw = (data[3] << 8) | data[2]; + uint16_t gyro_y_raw = (data[5] << 8) | data[4]; + uint16_t gyro_z_raw = (data[7] << 8) | data[6]; + imu->data.gyro.x = uint_to_float(gyro_x_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); + imu->data.gyro.y = uint_to_float(gyro_y_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); + imu->data.gyro.z = uint_to_float(gyro_z_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); + return DEVICE_OK; +} +/** + * @brief 解析欧拉角数据 + */ +static int8_t DM_IMU_ParseEulerData(DM_IMU_t *imu, uint8_t *data, uint8_t len) { + if (imu == NULL || data == NULL || len < 8) { + return DEVICE_ERR; + } + uint16_t pit_raw = (data[3] << 8) | data[2]; + uint16_t yaw_raw = (data[5] << 8) | data[4]; + uint16_t rol_raw = (data[7] << 8) | data[6]; + imu->data.euler.pit = uint_to_float(pit_raw, PITCH_CAN_MIN, PITCH_CAN_MAX, 16) * M_DEG2RAD_MULT; + imu->data.euler.yaw = uint_to_float(yaw_raw, YAW_CAN_MIN, YAW_CAN_MAX, 16) * M_DEG2RAD_MULT; + imu->data.euler.rol = uint_to_float(rol_raw, ROLL_CAN_MIN, ROLL_CAN_MAX, 16) * M_DEG2RAD_MULT; + return DEVICE_OK; +} + +/** + * @brief 解析四元数数据 + */ +static int8_t DM_IMU_ParseQuaternionData(DM_IMU_t *imu, uint8_t *data, uint8_t len) { + if (imu == NULL || data == NULL || len < 8) { + return DEVICE_ERR; + } + int w = (data[1] << 6) | ((data[2] & 0xF8) >> 2); + int x = ((data[2] & 0x03) << 12) | (data[3] << 4) | ((data[4] & 0xF0) >> 4); + int y = ((data[4] & 0x0F) << 10) | (data[5] << 2) | ((data[6] & 0xC0) >> 6); + int z = ((data[6] & 0x3F) << 8) | data[7]; + imu->data.quat.q0 = uint_to_float(w, Quaternion_MIN, Quaternion_MAX, 14); + imu->data.quat.q1 = uint_to_float(x, Quaternion_MIN, Quaternion_MAX, 14); + imu->data.quat.q2 = uint_to_float(y, Quaternion_MIN, Quaternion_MAX, 14); + imu->data.quat.q3 = uint_to_float(z, Quaternion_MIN, Quaternion_MAX, 14); + return DEVICE_OK; +} + + +/* Exported functions ------------------------------------------------------- */ + +/** + * @brief 初始化DM IMU设备 + */ +int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param) { + if (imu == NULL || param == NULL) { + return DEVICE_ERR_NULL; + } + + // 初始化设备头部 + imu->header.online = false; + imu->header.last_online_time = 0; + + // 配置参数 + imu->param.can = param->can; + imu->param.can_id = param->can_id; + imu->param.device_id = param->device_id; + imu->param.master_id = param->master_id; + + // 清零数据 + memset(&imu->data, 0, sizeof(DM_IMU_Data_t)); + + // 注册CAN接收队列，用于接收回复报文 + int8_t result = BSP_CAN_RegisterId(imu->param.can, imu->param.master_id, 10); + if (result != BSP_OK) { + return DEVICE_ERR; + } + + return DEVICE_OK; +} + +/** + * @brief 请求IMU数据 + */ +int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) { + if (imu == NULL) { + return DEVICE_ERR_NULL; + } + + // 构造发送数据：id_L, id_H(DM_IMU_ID), RID, 0xcc + uint8_t tx_data[4] = { + imu->param.device_id & 0xFF, // id_L + (imu->param.device_id >> 8) & 0xFF, // id_H + (uint8_t)rid, // RID + 0xCC // 固定值 + }; + + // 发送标准数据帧 + BSP_CAN_StdDataFrame_t frame = { + .id = imu->param.can_id, + .dlc = 4, + }; + memcpy(frame.data, tx_data, 4); + + int8_t result = BSP_CAN_TransmitStdDataFrame(imu->param.can, &frame); + return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 更新IMU数据（从CAN中获取所有数据并解析） + */ +int8_t DM_IMU_Update(DM_IMU_t *imu) { + if (imu == NULL) { + return DEVICE_ERR_NULL; + } + + BSP_CAN_Message_t msg; + int8_t result; + bool data_received = false; + + // 持续接收所有可用消息 + while ((result = BSP_CAN_GetMessage(imu->param.can, imu->param.master_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE)) == BSP_OK) { + // 验证回复数据格式（至少检查数据长度） + if (msg.dlc < 3) { + continue; // 跳过无效消息 + } + + // 根据数据位的第0位确定反馈报文类型 + uint8_t rid = msg.data[0] & 0x0F; // 取第0位的低4位作为RID + + // 根据RID类型解析数据 + int8_t parse_result = DEVICE_ERR; + switch (rid) { + case 0x01: // RID_ACCL + parse_result = DM_IMU_ParseAccelData(imu, msg.data, msg.dlc); + break; + case 0x02: // RID_GYRO + parse_result = DM_IMU_ParseGyroData(imu, msg.data, msg.dlc); + break; + case 0x03: // RID_EULER + parse_result = DM_IMU_ParseEulerData(imu, msg.data, msg.dlc); + break; + case 0x04: // RID_QUATERNION + parse_result = DM_IMU_ParseQuaternionData(imu, msg.data, msg.dlc); + break; + default: + continue; // 跳过未知类型的消息 + } + + // 如果解析成功，标记为收到数据 + if (parse_result == DEVICE_OK) { + data_received = true; + } + } + + // 如果收到任何有效数据，更新设备状态 + if (data_received) { + imu->header.online = true; + imu->header.last_online_time = BSP_TIME_Get_ms(); + return DEVICE_OK; + } + + return DEVICE_ERR; +} + +/** + * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz） + */ +int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu){ + if (imu == NULL) { + return DEVICE_ERR_NULL; + } + switch (count) { + case 0: + DM_IMU_Request(imu, RID_ACCL); + break; + case 1: + DM_IMU_Request(imu, RID_GYRO); + break; + case 2: + DM_IMU_Request(imu, RID_EULER); + break; + case 3: + DM_IMU_Request(imu, RID_QUATERNION); + DM_IMU_Update(imu); // 更新所有数据 + break; + } + count++; + if (count >= 4) { + count = 0; // 重置计数器 + } + return DEVICE_OK; +} + +/** + * @brief 检查设备是否在线 + */ +bool DM_IMU_IsOnline(DM_IMU_t *imu) { + if (imu == NULL) { + return false; + } + + uint32_t current_time = BSP_TIME_Get_ms(); + return imu->header.online && + (current_time - imu->header.last_online_time < DM_IMU_OFFLINE_TIMEOUT); +} diff --git a/User/device/dm_imu.h b/User/device/dm_imu.h new file mode 100644 index 0000000..3965980 --- /dev/null +++ b/User/device/dm_imu.h @@ -0,0 +1,90 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "component/ahrs.h" +#include "bsp/can.h" +/* Exported constants ------------------------------------------------------- */ + +#define DM_IMU_CAN_ID_DEFAULT 0x6FF +#define DM_IMU_ID_DEFAULT 0x42 +#define DM_IMU_MST_ID_DEFAULT 0x43 + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + +typedef struct { + BSP_CAN_t can; // CAN总线句柄 + uint16_t can_id; // CAN通信ID + uint8_t device_id; // 设备ID + uint8_t master_id; // 主机ID +} DM_IMU_Param_t; +typedef enum { + RID_ACCL = 0x01, // 加速度计 + RID_GYRO = 0x02, // 陀螺仪 + RID_EULER = 0x03, // 欧拉角 + RID_QUATERNION = 0x04, // 四元数 +} DM_IMU_RID_t; + +typedef struct { + AHRS_Accl_t accl; // 加速度计 + AHRS_Gyro_t gyro; // 陀螺仪 + AHRS_Eulr_t euler; // 欧拉角 + AHRS_Quaternion_t quat; // 四元数 + float temp; // 温度 +} DM_IMU_Data_t; + +typedef struct { + DEVICE_Header_t header; + DM_IMU_Param_t param; // IMU参数配置 + DM_IMU_Data_t data; // IMU数据 +} DM_IMU_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 初始化DM IMU设备 + * @param imu DM IMU设备结构体指针 + * @param param IMU参数配置指针 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param); + +/** + * @brief 请求IMU数据 + * @param imu DM IMU设备结构体指针 + * @param rid 请求的数据类型 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid); + + +/** + * @brief 更新IMU数据（从CAN中获取所有数据并解析） + * @param imu DM IMU设备结构体指针 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t DM_IMU_Update(DM_IMU_t *imu); + +/** + * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz，运行4次才有完整数据） + * @param imu DM IMU设备结构体指针 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu); + +/** + * @brief 检查设备是否在线 + * @param imu DM IMU设备结构体指针 + * @return true 在线，false 离线 + */ +bool DM_IMU_IsOnline(DM_IMU_t *imu); + + +#ifdef __cplusplus +} +#endif diff --git a/User/device/dr16.c b/User/device/dr16.c new file mode 100644 index 0000000..8014653 --- /dev/null +++ b/User/device/dr16.c @@ -0,0 +1,85 @@ +/* + DR16接收机 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "dr16.h" + +#include + +#include "bsp/uart.h" +#include "bsp/time.h" +/* Private define ----------------------------------------------------------- */ +#define DR16_CH_VALUE_MIN (364u) +#define DR16_CH_VALUE_MID (1024u) +#define DR16_CH_VALUE_MAX (1684u) + +/* 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 bool DR16_DataCorrupted(const DR16_t *dr16) { + if (dr16 == NULL) return DEVICE_ERR_NULL; + + if ((dr16->data.ch_r_x < DR16_CH_VALUE_MIN) || + (dr16->data.ch_r_x > DR16_CH_VALUE_MAX)) + return true; + + if ((dr16->data.ch_r_y < DR16_CH_VALUE_MIN) || + (dr16->data.ch_r_y > DR16_CH_VALUE_MAX)) + return true; + + if ((dr16->data.ch_l_x < DR16_CH_VALUE_MIN) || + (dr16->data.ch_l_x > DR16_CH_VALUE_MAX)) + return true; + + if ((dr16->data.ch_l_y < DR16_CH_VALUE_MIN) || + (dr16->data.ch_l_y > DR16_CH_VALUE_MAX)) + return true; + + if (dr16->data.sw_l == 0) return true; + + if (dr16->data.sw_r == 0) return true; + + return false; +} + +/* 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; + + BSP_UART_RegisterCallback(BSP_UART_DR16, BSP_UART_RX_CPLT_CB, + DR16_RxCpltCallback); + + inited = true; + return DEVICE_OK; +} + +int8_t DR16_Restart(void) { + __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_DR16)); + __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_DR16)); + return DEVICE_OK; +} + +int8_t DR16_StartDmaRecv(DR16_t *dr16) { + if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_DR16), + (uint8_t *)&(dr16->data), + sizeof(dr16->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); +} diff --git a/User/device/dr16.h b/User/device/dr16.h new file mode 100644 index 0000000..6972647 --- /dev/null +++ b/User/device/dr16.h @@ -0,0 +1,47 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include + +#include "component/user_math.h" +#include "device/device.h" + +/* 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_Data_t; + +typedef struct { + DEVICE_Header_t header; + 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); + + +#ifdef __cplusplus +} +#endif diff --git a/User/device/motor.c b/User/device/motor.c new file mode 100644 index 0000000..9ea23b1 --- /dev/null +++ b/User/device/motor.c @@ -0,0 +1,49 @@ +/* + DR16接收机 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "motor.h" + +#include + + +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ + +/* Private function -------------------------------------------------------- */ + +/* Exported functions ------------------------------------------------------- */ +float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + if (motor->reverse) { + return -motor->feedback.rotor_abs_angle; + }else{ + return motor->feedback.rotor_abs_angle; + } +} + +float MOTOR_GetRotorSpeed(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + if (motor->reverse) { + return -motor->feedback.rotor_speed; + }else{ + return motor->feedback.rotor_speed; + } +} + +float MOTOR_GetTorqueCurrent(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + if (motor->reverse) { + return -motor->feedback.torque_current; + }else{ + return motor->feedback.torque_current; + } +} + +float MOTOR_GetTemp(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + return motor->feedback.temp; +} diff --git a/User/device/motor.h b/User/device/motor.h new file mode 100644 index 0000000..bfe8b53 --- /dev/null +++ b/User/device/motor.h @@ -0,0 +1,34 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef struct { + float rotor_abs_angle; /* 转子绝对角度 */ + float rotor_speed; /* 实际转子转速 */ + float torque_current; /* 转矩电流 */ + float temp; /* 温度 */ +} MOTOR_Feedback_t; + +typedef struct { + DEVICE_Header_t header; + bool reverse; /* 是否反装 true表示反装 */ + MOTOR_Feedback_t feedback; +} MOTOR_t; + +/* 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); + +#ifdef __cplusplus +} +#endif diff --git a/User/device/motor_lk.c b/User/device/motor_lk.c new file mode 100644 index 0000000..1527f66 --- /dev/null +++ b/User/device/motor_lk.c @@ -0,0 +1,310 @@ +/* + LK电机驱动 +*/ +/* Includes ----------------------------------------------------------------- */ +#include "motor_lk.h" +#include +#include +#include "bsp/can.h" +#include "bsp/mm.h" +#include "bsp/time.h" +#include "component/user_math.h" + +/* Private define ----------------------------------------------------------- */ +#define LK_CTRL_ID_BASE (0x140) +#define LK_FB_ID_BASE (0x240) + +// LK电机命令字节定义 +#define LK_CMD_FEEDBACK (0x9C) // 反馈命令字节 +#define LK_CMD_MOTOR_OFF (0x80) // 电机关闭命令 +#define LK_CMD_MOTOR_ON (0x88) // 电机运行命令 +#define LK_CMD_TORQUE_CTRL (0xA1) // 转矩闭环控制命令 + +// LK电机参数定义 +#define LK_CURR_LSB_MF (33.0f / 4096.0f) // MF电机转矩电流分辨率 A/LSB +#define LK_CURR_LSB_MG (66.0f / 4096.0f) // MG电机转矩电流分辨率 A/LSB +#define LK_POWER_RANGE_MS (1000) // MS电机功率范围 ±1000 +#define LK_TORQUE_RANGE (2048) // 转矩控制值范围 ±2048 +#define LK_TORQUE_CURRENT_MF (16.5f) // MF电机最大转矩电流 A +#define LK_TORQUE_CURRENT_MG (33.0f) // MG电机最大转矩电流 A + +#define MOTOR_TX_BUF_SIZE (8) +#define MOTOR_RX_BUF_SIZE (8) + +// 编码器分辨率定义 +#define LK_ENC_14BIT_MAX (16383) // 14位编码器最大值 +#define LK_ENC_15BIT_MAX (32767) // 15位编码器最大值 +#define LK_ENC_16BIT_MAX (65535) // 16位编码器最大值 + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ +static MOTOR_LK_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL}; + +/* Private functions -------------------------------------------------------- */ +static float MOTOR_LK_GetCurrentLSB(MOTOR_LK_Module_t module) { + switch (module) { + case MOTOR_MF9025: + case MOTOR_MF9035: + return LK_CURR_LSB_MF; + default: + return LK_CURR_LSB_MG; // 默认使用MG的分辨率 + } +} + +static uint16_t MOTOR_LK_GetEncoderMax(MOTOR_LK_Module_t module) { + // 根据电机型号返回编码器最大值，这里假设都使用16位编码器 + // 实际使用时需要根据具体电机型号配置 + return LK_ENC_16BIT_MAX; +} + +static MOTOR_LK_CANManager_t* MOTOR_LK_GetCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return NULL; + return can_managers[can]; +} + +static int8_t MOTOR_LK_CreateCANManager(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return DEVICE_ERR; + if (can_managers[can] != NULL) return DEVICE_OK; + + can_managers[can] = (MOTOR_LK_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LK_CANManager_t)); + if (can_managers[can] == NULL) return DEVICE_ERR; + + memset(can_managers[can], 0, sizeof(MOTOR_LK_CANManager_t)); + can_managers[can]->can = can; + return DEVICE_OK; +} + +static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) { + // 检查命令字节是否为反馈命令 + if (msg->data[0] != LK_CMD_FEEDBACK) { + return; + } + + // 解析温度 (DATA[1]) + motor->motor.feedback.temp = (int8_t)msg->data[1]; + + // 解析转矩电流值或功率值 (DATA[2], DATA[3]) + int16_t raw_current_or_power = (int16_t)((msg->data[3] << 8) | msg->data[2]); + + // 根据电机类型解析电流或功率 + switch (motor->param.module) { + case MOTOR_MF9025: + case MOTOR_MF9035: + // MF/MG电机：转矩电流值 + motor->motor.feedback.torque_current = raw_current_or_power * MOTOR_LK_GetCurrentLSB(motor->param.module); + break; + default: + // MS电机：功率值（范围-1000~1000） + motor->motor.feedback.torque_current = (float)raw_current_or_power; // 将功率存储在torque_current字段中 + break; + } + + // 解析转速 (DATA[4], DATA[5]) - 单位：1dps/LSB + motor->motor.feedback.rotor_speed = (int16_t)((msg->data[5] << 8) | msg->data[4]); + + // 解析编码器值 (DATA[6], DATA[7]) + uint16_t raw_encoder = (uint16_t)((msg->data[7] << 8) | msg->data[6]); + uint16_t encoder_max = MOTOR_LK_GetEncoderMax(motor->param.module); + + // 将编码器值转换为弧度 (0 ~ 2π) + motor->motor.feedback.rotor_abs_angle = (float)raw_encoder / (float)encoder_max * M_2PI; +} + +/* Exported functions ------------------------------------------------------- */ + +int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + if (MOTOR_LK_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR; + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR; + + // 检查是否已注册 + for (int i = 0; i < manager->motor_count; i++) { + if (manager->motors[i] && manager->motors[i]->param.id == param->id) { + return DEVICE_ERR_INITED; + } + } + + // 检查数量 + if (manager->motor_count >= MOTOR_LK_MAX_MOTORS) return DEVICE_ERR; + + // 创建新电机实例 + MOTOR_LK_t *new_motor = (MOTOR_LK_t*)BSP_Malloc(sizeof(MOTOR_LK_t)); + if (new_motor == NULL) return DEVICE_ERR; + + memcpy(&new_motor->param, param, sizeof(MOTOR_LK_Param_t)); + memset(&new_motor->motor, 0, sizeof(MOTOR_t)); + new_motor->motor.reverse = param->reverse; + + // 计算反馈ID（假设为控制ID + 0x40） + uint16_t feedback_id = param->id + 0x40; + + // 注册CAN接收ID + if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) { + BSP_Free(new_motor); + return DEVICE_ERR; + } + + manager->motors[manager->motor_count] = new_motor; + manager->motor_count++; + return DEVICE_OK; +} + +int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LK_t *motor = manager->motors[i]; + if (motor && motor->param.id == param->id) { + // 计算反馈ID + uint16_t feedback_id = param->id + 0x100; + + BSP_CAN_Message_t rx_msg; + if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) { + uint64_t now_time = BSP_TIME_Get(); + if (now_time - motor->motor.header.last_online_time > 1000) { + motor->motor.header.online = false; + return DEVICE_ERR_NO_DEV; + } + return DEVICE_ERR; + } + + motor->motor.header.online = true; + motor->motor.header.last_online_time = BSP_TIME_Get(); + MOTOR_LK_Decode(motor, &rx_msg); + return DEVICE_OK; + } + } + return DEVICE_ERR_NO_DEV; +} + +int8_t MOTOR_LK_UpdateAll(void) { + int8_t ret = DEVICE_OK; + for (int can = 0; can < BSP_CAN_NUM; can++) { + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager((BSP_CAN_t)can); + if (manager == NULL) continue; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LK_t *motor = manager->motors[i]; + if (motor != NULL) { + if (MOTOR_LK_Update(&motor->param) != DEVICE_OK) { + ret = DEVICE_ERR; + } + } + } + } + return ret; +} + +int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) { + if (param == NULL) return DEVICE_ERR_NULL; + + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can); + if (manager == NULL) return DEVICE_ERR_NO_DEV; + + // 限制输出值范围 + if (value > 1.0f) value = 1.0f; + if (value < -1.0f) value = -1.0f; + + MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param); + if (motor == NULL) return DEVICE_ERR_NO_DEV; + + // 转矩闭环控制命令 - 将输出值转换为转矩控制值 + int16_t torque_control = (int16_t)(value * (float)LK_TORQUE_RANGE); + + // 构建CAN帧 + BSP_CAN_StdDataFrame_t tx_frame; + tx_frame.id = param->id; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + + // 设置转矩闭环控制命令数据 + tx_frame.data[0] = LK_CMD_TORQUE_CTRL; // 命令字节 + tx_frame.data[1] = 0x00; // NULL + tx_frame.data[2] = 0x00; // NULL + tx_frame.data[3] = 0x00; // NULL + tx_frame.data[4] = (uint8_t)(torque_control & 0xFF); // 转矩电流控制值低字节 + tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF); // 转矩电流控制值高字节 + tx_frame.data[6] = 0x00; // NULL + tx_frame.data[7] = 0x00; // NULL + + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param) { + // 对于LK电机，每次设置输出时就直接发送控制命令 + // 这个函数可以用于发送其他控制命令，如电机开启/关闭 + return DEVICE_OK; +} + +int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + BSP_CAN_StdDataFrame_t tx_frame; + tx_frame.id = param->id; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + + // 电机运行命令 + tx_frame.data[0] = LK_CMD_MOTOR_ON; // 命令字节 + tx_frame.data[1] = 0x00; + tx_frame.data[2] = 0x00; + tx_frame.data[3] = 0x00; + tx_frame.data[4] = 0x00; + tx_frame.data[5] = 0x00; + tx_frame.data[6] = 0x00; + tx_frame.data[7] = 0x00; + + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param) { + if (param == NULL) return DEVICE_ERR_NULL; + + BSP_CAN_StdDataFrame_t tx_frame; + tx_frame.id = param->id; + tx_frame.dlc = MOTOR_TX_BUF_SIZE; + + // 电机关闭命令 + tx_frame.data[0] = LK_CMD_MOTOR_OFF; // 命令字节 + tx_frame.data[1] = 0x00; + tx_frame.data[2] = 0x00; + tx_frame.data[3] = 0x00; + tx_frame.data[4] = 0x00; + tx_frame.data[5] = 0x00; + tx_frame.data[6] = 0x00; + tx_frame.data[7] = 0x00; + + return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; +} + +MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param) { + if (param == NULL) return NULL; + + MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can); + if (manager == NULL) return NULL; + + for (int i = 0; i < manager->motor_count; i++) { + MOTOR_LK_t *motor = manager->motors[i]; + if (motor && motor->param.id == param->id) { + return motor; + } + } + return NULL; +} + +int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param) { + return MOTOR_LK_SetOutput(param, 0.0f); +} + +int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param) { + MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param); + if (motor) { + motor->motor.header.online = false; + return DEVICE_OK; + } + return DEVICE_ERR_NO_DEV; +} \ No newline at end of file diff --git a/User/device/motor_lk.h b/User/device/motor_lk.h new file mode 100644 index 0000000..ada3f41 --- /dev/null +++ b/User/device/motor_lk.h @@ -0,0 +1,119 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "device/motor.h" +#include "bsp/can.h" + +/* Exported constants ------------------------------------------------------- */ +#define MOTOR_LK_MAX_MOTORS 32 + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + MOTOR_MF9025, + MOTOR_MF9035, +} MOTOR_LK_Module_t; + + +/*每个电机需要的参数*/ +typedef struct { + BSP_CAN_t can; + uint16_t id; + MOTOR_LK_Module_t module; + bool reverse; +} MOTOR_LK_Param_t; + +/*电机实例*/ +typedef struct{ + MOTOR_LK_Param_t param; + MOTOR_t motor; +} MOTOR_LK_t; + +/*CAN管理器，管理一个CAN总线上所有的电机*/ +typedef struct { + BSP_CAN_t can; + MOTOR_LK_t *motors[MOTOR_LK_MAX_MOTORS]; + uint8_t motor_count; +} MOTOR_LK_CANManager_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 注册一个LK电机 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param); + +/** + * @brief 更新指定电机数据 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param); + +/** + * @brief 设置一个电机的输出 + * @param param 电机参数 + * @param value 输出值，范围[-1.0, 1.0] + * @return + */ +int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value); + +/** + * @brief 发送控制命令到电机，注意一个CAN可以控制多个电机，所以只需要发送一次即可 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param); + +/** + * @brief 发送电机开启命令 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param); + +/** + * @brief 发送电机关闭命令 + * @param param 电机参数 + * @return + */ +int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param); + +/** + * @brief 获取指定电机的实例指针 + * @param param 电机参数 + * @return + */ +MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param); + +/** + * @brief 使电机松弛（设置输出为0） + * @param param + * @return + */ +int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param); + +/** + * @brief 使电机离线（设置在线状态为false） + * @param param + * @return + */ +int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param); + +/** + * @brief + * @param + * @return + */ +int8_t MOTOR_LK_UpdateAll(void); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/User/device/motor_lz.c b/User/device/motor_lz.c new file mode 100644 index 0000000..8c7afc7 --- /dev/null +++ b/User/device/motor_lz.c @@ -0,0 +1,425 @@ +/* + 灵足电机驱动 + + 灵足电机通信协议： + - CAN 2.0通信接口，波特率1Mbps + - 采用扩展帧格式(29位ID) + - ID格式：Bit28~24(通信类型) + Bit23~8(数据区2) + Bit7~0(目标地址) +*/ +/* Includes ----------------------------------------------------------------- */ +#include "motor_lz.h" +#include +#include +#include +#include "bsp/can.h" +#include "bsp/mm.h" +#include "bsp/time.h" +#include "component/user_math.h" + +/* Private define ----------------------------------------------------------- */ +// 灵足电机协议参数 +#define LZ_ANGLE_RANGE_RAD (12.57f) /* 角度范围 ±12.57 rad */ +#define LZ_VELOCITY_RANGE_RAD_S (20.0f) /* 角速度范围 ±20 rad/s */ +#define LZ_TORQUE_RANGE_NM (60.0f) /* 力矩范围 ±60 Nm */ +#define LZ_KP_MAX (5000.0f) /* Kp最大值 */ +#define LZ_KD_MAX (100.0f) /* Kd最大值 */ + +#define LZ_RAW_VALUE_MAX (65535) /* 16位原始值最大值 */ +#define LZ_TEMP_SCALE (10.0f) /* 温度缩放因子 */ + +#define MOTOR_TX_BUF_SIZE (8) +#define MOTOR_RX_BUF_SIZE (8) + +/* Private macro ------------------------------------------------------------ */ +/* 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 浮点值转换为原始值 + */ +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 原始值转换为浮点值 + */ +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 target_id = original_id & 0xFF; // Bit7~0: 目标地址 + + // 对于反馈数据帧，我们使用特殊的解析规则 + if (cmd_type == MOTOR_LZ_CMD_FEEDBACK) { + // 反馈数据的data2字段包含： + // Bit8~15: 当前电机CAN ID + // Bit16~21: 故障信息 + // Bit22~23: 模式状态 + uint8_t motor_can_id = data2 & 0xFF; + + // 返回格式化的ID，便于匹配 + // 格式：0x02HHMMTT (02=反馈命令, HH=主机ID, MM=电机ID, TT=目标ID) + return (0x02000000) | ((data2 & 0xFF00) << 8) | (motor_can_id << 8) | target_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; + + // 检查是否为反馈数据帧 (通信类型2) + uint8_t cmd_type = (msg->parsed_id >> 24) & 0x1F; + if (cmd_type != MOTOR_LZ_CMD_FEEDBACK) return; + + // 解析ID中的信息 + uint16_t id_data2 = (msg->parsed_id >> 8) & 0xFFFF; + uint8_t motor_can_id = id_data2 & 0xFF; // Bit8~15: 当前电机CAN ID + uint8_t fault_info = (id_data2 >> 8) & 0x3F; // Bit16~21: 故障信息 + uint8_t mode_state = (id_data2 >> 14) & 0x03; // Bit22~23: 模式状态 + + // 更新电机CAN ID + motor->lz_feedback.motor_can_id = motor_can_id; + + // 解析故障信息 + motor->lz_feedback.fault.under_voltage = (fault_info & 0x01) != 0; // bit16 + motor->lz_feedback.fault.driver_fault = (fault_info & 0x02) != 0; // bit17 + motor->lz_feedback.fault.over_temp = (fault_info & 0x04) != 0; // bit18 + motor->lz_feedback.fault.encoder_fault = (fault_info & 0x08) != 0; // bit19 + motor->lz_feedback.fault.stall_overload = (fault_info & 0x10) != 0; // bit20 + motor->lz_feedback.fault.uncalibrated = (fault_info & 0x20) != 0; // bit21 + + // 解析模式状态 + motor->lz_feedback.state = (MOTOR_LZ_State_t)mode_state; + + // 解析数据区 + // Byte0~1: 当前角度 + uint16_t raw_angle = (uint16_t)((msg->data[1] << 8) | msg->data[0]); + motor->lz_feedback.current_angle = MOTOR_LZ_RawToFloat(raw_angle, LZ_ANGLE_RANGE_RAD); + + // Byte2~3: 当前角速度 + uint16_t raw_velocity = (uint16_t)((msg->data[3] << 8) | msg->data[2]); + motor->lz_feedback.current_velocity = MOTOR_LZ_RawToFloat(raw_velocity, LZ_VELOCITY_RANGE_RAD_S); + + // Byte4~5: 当前力矩 + uint16_t raw_torque = (uint16_t)((msg->data[5] << 8) | msg->data[4]); + motor->lz_feedback.current_torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM); + + // Byte6~7: 当前温度 (温度*10) + uint16_t raw_temp = (uint16_t)((msg->data[7] << 8) | msg->data[6]); + motor->lz_feedback.temperature = (float)raw_temp / LZ_TEMP_SCALE; + + // 更新通用电机反馈信息 + motor->motor.feedback.rotor_abs_angle = motor->lz_feedback.current_angle; + motor->motor.feedback.rotor_speed = motor->lz_feedback.current_velocity * 180.0f / M_PI * 6.0f; // 转换为RPM + motor->motor.feedback.torque_current = motor->lz_feedback.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; +} + +/** + * @brief 反初始化灵足电机驱动系统 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_DeInit(void) { + // 注销ID解析器 + return BSP_CAN_UnregisterIdParser() == 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 + uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->host_id, param->motor_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->host_id, param->motor_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; + + // 更新运控参数 + memcpy(&motor->motion_param, motion_param, sizeof(MOTOR_LZ_MotionParam_t)); + + // 构建扩展ID + uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_MOTION, 0, param->motor_id); + + // 准备数据 + uint8_t data[8]; + + // Byte0~1: 目标角度 + uint16_t raw_angle = MOTOR_LZ_FloatToRaw(motion_param->target_angle, LZ_ANGLE_RANGE_RAD); + data[0] = raw_angle & 0xFF; + data[1] = (raw_angle >> 8) & 0xFF; + + // Byte2~3: 目标角速度 + uint16_t raw_velocity = MOTOR_LZ_FloatToRaw(motion_param->target_velocity, LZ_VELOCITY_RANGE_RAD_S); + data[2] = raw_velocity & 0xFF; + data[3] = (raw_velocity >> 8) & 0xFF; + + // Byte4~5: Kp + uint16_t raw_kp = MOTOR_LZ_FloatToRaw(motion_param->kp, LZ_KP_MAX); + data[4] = raw_kp & 0xFF; + data[5] = (raw_kp >> 8) & 0xFF; + + // Byte6~7: Kd + uint16_t raw_kd = MOTOR_LZ_FloatToRaw(motion_param->kd, LZ_KD_MAX); + data[6] = raw_kd & 0xFF; + data[7] = (raw_kd >> 8) & 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_Disable(param, false); +} + +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; +} \ No newline at end of file diff --git a/User/device/motor_lz.h b/User/device/motor_lz.h new file mode 100644 index 0000000..1b65ced --- /dev/null +++ b/User/device/motor_lz.h @@ -0,0 +1,193 @@ +#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 反初始化灵足电机驱动系统 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_DeInit(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 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param); + +/** + * @brief 电机停止运行 + * @param param 电机参数 + * @param clear_fault 是否清除故障 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault); + +/** + * @brief 设置电机机械零位 + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param); + +/** + * @brief 获取指定电机的实例指针 + * @param param 电机参数 + * @return 电机实例指针，失败返回NULL + */ +MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param); + +/** + * @brief 使电机松弛（发送停止命令） + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param); + +/** + * @brief 使电机离线（设置在线状态为false） + * @param param 电机参数 + * @return 设备状态码 + */ +int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param); + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/User/module/config.c b/User/module/config.c new file mode 100644 index 0000000..8af91c7 --- /dev/null +++ b/User/module/config.c @@ -0,0 +1,35 @@ +/* + * 配置相关 + */ + +/* Includes ----------------------------------------------------------------- */ +#include "module/config.h" +#include "bsp/can.h" + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ + +/* Exported variables ------------------------------------------------------- */ + +// 机器人参数配置 +Config_RobotParam_t robot_config = { + .imu_param = { + .can = BSP_CAN_2, + .can_id = 0x6FF, + .device_id = 0x42, + .master_id = 0x43, + }, +}; + +/* Private function prototypes ---------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ + +/** + * @brief 获取机器人配置参数 + * @return 机器人配置参数指针 + */ +Config_RobotParam_t* Config_GetRobotParam(void) { + return &robot_config; +} \ No newline at end of file diff --git a/User/module/config.h b/User/module/config.h new file mode 100644 index 0000000..6fdb560 --- /dev/null +++ b/User/module/config.h @@ -0,0 +1,28 @@ +/* + * 配置相关 + */ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include "device/dm_imu.h" + +typedef struct { + DM_IMU_Param_t imu_param; +} Config_RobotParam_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 获取机器人配置参数 + * @return 机器人配置参数指针 + */ +Config_RobotParam_t* Config_GetRobotParam(void); + +#ifdef __cplusplus +} +#endif diff --git a/User/task/atti_esti.c b/User/task/atti_esti.c new file mode 100644 index 0000000..6ba181d --- /dev/null +++ b/User/task/atti_esti.c @@ -0,0 +1,51 @@ +/* + atti_esti Task + +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" +/* USER INCLUDE BEGIN */ +#include "bsp/can.h" +#include "device/dm_imu.h" +#include "module/config.h" +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ +DM_IMU_t dm_imu; +/* USER STRUCT END */ + +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +void Task_atti_esti(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + + + /* 计算任务运行到指定频率需要等待的tick数 */ + const uint32_t delay_tick = osKernelGetTickFreq() / ATTI_ESTI_FREQ; + + osDelay(ATTI_ESTI_INIT_DELAY); /* 延时一段时间再开启任务 */ + + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ + /* USER CODE INIT BEGIN */ + BSP_CAN_Init(); + // 初始化DM IMU设备 + DM_IMU_Init(&dm_imu, &Config_GetRobotParam()->imu_param); + /* USER CODE INIT END */ + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* USER CODE BEGIN */ + + if (DM_IMU_AutoUpdateAll(&dm_imu) == DEVICE_OK) { + + } + /* USER CODE END */ + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ + } + +} \ No newline at end of file diff --git a/User/task/blink.c b/User/task/blink.c new file mode 100644 index 0000000..72b351f --- /dev/null +++ b/User/task/blink.c @@ -0,0 +1,50 @@ +/* + blink Task + +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" +/* USER INCLUDE BEGIN */ +#include "bsp/pwm.h" +#include +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +void Task_blink(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + + + /* 计算任务运行到指定频率需要等待的tick数 */ + const uint32_t delay_tick = osKernelGetTickFreq() / BLINK_FREQ; + + osDelay(BLINK_INIT_DELAY); /* 延时一段时间再开启任务 */ + + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ + /* USER CODE INIT BEGIN */ + BSP_PWM_Stop(BSP_PWM_LED_R); + BSP_PWM_Stop(BSP_PWM_LED_B); + BSP_PWM_SetComp(BSP_PWM_LED_G, 0.0f); + BSP_PWM_Start(BSP_PWM_LED_G); + /* USER CODE INIT END */ + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* USER CODE BEGIN */ + // 呼吸灯 - 基于tick的正弦波 + float duty = (sinf(tick * 0.003f) + 1.0f) * 0.5f; // 0到1之间的正弦波，加快频率 + BSP_PWM_SetComp(BSP_PWM_LED_G, duty); + /* USER CODE END */ + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ + } + +} \ No newline at end of file diff --git a/User/task/config.yaml b/User/task/config.yaml new file mode 100644 index 0000000..119b420 --- /dev/null +++ b/User/task/config.yaml @@ -0,0 +1,21 @@ +- delay: 0 + description: '' + freq_control: true + frequency: 100.0 + function: Task_blink + name: blink + stack: 256 +- delay: 0 + description: '' + freq_control: true + frequency: 500.0 + function: Task_rc + name: rc + stack: 256 +- delay: 0 + description: '' + freq_control: true + frequency: 1000.0 + function: Task_atti_esti + name: atti_esti + stack: 256 diff --git a/User/task/init.c b/User/task/init.c new file mode 100644 index 0000000..db28a1b --- /dev/null +++ b/User/task/init.c @@ -0,0 +1,44 @@ +/* + Init Task + 任务初始化，创建各个线程任务和消息队列 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE BEGIN */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ + +/** + * \brief 初始化 + * + * \param argument 未使用 + */ +void Task_Init(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + /* USER CODE INIT BEGIN */ + + /* USER CODE INIT END */ + osKernelLock(); /* 锁定内核，防止任务切换 */ + + /* 创建任务线程 */ + task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink); + task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc); + task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti); + + // 创建消息队列 + /* USER MESSAGE BEGIN */ + task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL); + /* USER MESSAGE END */ + + osKernelUnlock(); // 解锁内核 + osThreadTerminate(osThreadGetId()); // 任务完成后结束自身 +} \ No newline at end of file diff --git a/User/task/rc.c b/User/task/rc.c new file mode 100644 index 0000000..e1897c1 --- /dev/null +++ b/User/task/rc.c @@ -0,0 +1,44 @@ +/* + rc Task + +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +void Task_rc(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + + + /* 计算任务运行到指定频率需要等待的tick数 */ + const uint32_t delay_tick = osKernelGetTickFreq() / RC_FREQ; + + osDelay(RC_INIT_DELAY); /* 延时一段时间再开启任务 */ + + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ + /* USER CODE INIT BEGIN */ + + /* USER CODE INIT END */ + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* USER CODE BEGIN */ + + /* USER CODE END */ + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ + } + +} \ No newline at end of file diff --git a/User/task/user_task.c b/User/task/user_task.c new file mode 100644 index 0000000..efbfb1c --- /dev/null +++ b/User/task/user_task.c @@ -0,0 +1,26 @@ +#include "task/user_task.h" + +Task_Runtime_t task_runtime; + +const osThreadAttr_t attr_init = { + .name = "Task_Init", + .priority = osPriorityRealtime, + .stack_size = 256 * 4, +}; + +/* User_task */ +const osThreadAttr_t attr_blink = { + .name = "blink", + .priority = osPriorityNormal, + .stack_size = 256 * 4, +}; +const osThreadAttr_t attr_rc = { + .name = "rc", + .priority = osPriorityNormal, + .stack_size = 256 * 4, +}; +const osThreadAttr_t attr_atti_esti = { + .name = "atti_esti", + .priority = osPriorityNormal, + .stack_size = 256 * 4, +}; \ No newline at end of file diff --git a/User/task/user_task.h b/User/task/user_task.h new file mode 100644 index 0000000..51283cf --- /dev/null +++ b/User/task/user_task.h @@ -0,0 +1,96 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif +/* Includes ----------------------------------------------------------------- */ +#include +#include "FreeRTOS.h" +#include "task.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ +/* Exported constants ------------------------------------------------------- */ +/* 任务运行频率 */ +#define BLINK_FREQ (100.0) +#define RC_FREQ (500.0) +#define ATTI_ESTI_FREQ (1000.0) + +/* 任务初始化延时ms */ +#define TASK_INIT_DELAY (100u) +#define BLINK_INIT_DELAY (0) +#define RC_INIT_DELAY (0) +#define ATTI_ESTI_INIT_DELAY (0) + +/* Exported defines --------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + +/* 任务运行时结构体 */ +typedef struct { + /* 各任务，也可以叫做线程 */ + struct { + osThreadId_t blink; + osThreadId_t rc; + osThreadId_t atti_esti; + } thread; + + /* USER MESSAGE BEGIN */ + struct { + osMessageQueueId_t user_msg; /* 用户自定义任务消息队列 */ + } msgq; + /* USER MESSAGE END */ + + /* 机器人状态 */ + struct { + float battery; /* 电池电量百分比 */ + float vbat; /* 电池电压 */ + float cpu_temp; /* CPU温度 */ + } status; + + /* USER CONFIG BEGIN */ + + /* USER CONFIG END */ + + /* 各任务的stack使用 */ + struct { + UBaseType_t blink; + UBaseType_t rc; + UBaseType_t atti_esti; + } stack_water_mark; + + /* 各任务运行频率 */ + struct { + float blink; + float rc; + float atti_esti; + } freq; + + /* 任务最近运行时间 */ + struct { + float blink; + float rc; + float atti_esti; + } last_up_time; + +} Task_Runtime_t; + +/* 任务运行时结构体 */ +extern Task_Runtime_t task_runtime; + +/* 初始化任务句柄 */ +extern const osThreadAttr_t attr_init; +extern const osThreadAttr_t attr_blink; +extern const osThreadAttr_t attr_rc; +extern const osThreadAttr_t attr_atti_esti; + +/* 任务函数声明 */ +void Task_Init(void *argument); +void Task_blink(void *argument); +void Task_rc(void *argument); +void Task_atti_esti(void *argument); + +#ifdef __cplusplus +} +#endif \ No newline at end of file