From 92a75811ae5b02395f533dcb7edc78d780bcf533 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Mon, 6 Oct 2025 21:58:56 +0800 Subject: [PATCH 1/4] =?UTF-8?q?=E6=AF=94=E8=BE=83=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/balance_chassis.c | 10 +++++----- User/task/rc.c | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/User/module/balance_chassis.c b/User/module/balance_chassis.c index e38bad6..ae7ff50 100644 --- a/User/module/balance_chassis.c +++ b/User/module/balance_chassis.c @@ -457,13 +457,13 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd) { // } // c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, c->feedback.imu.euler.yaw, c->feedback.imu.gyro.z, c->dt); - c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle; - c->yaw_control.target_yaw = c->param->mech_zero_yaw; - c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt); + // c->yaw_control.current_yaw = c->feedback.yaw.rotor_abs_angle; + // c->yaw_control.target_yaw = c->param->mech_zero_yaw; + // c->yaw_control.yaw_force = PID_Calc(&c->pid.yaw, c->yaw_control.target_yaw, c->feedback.yaw.rotor_abs_angle, 0.0f, c->dt); /* 轮毂力矩输出（参考C++版本的减速比） */ - c->output.wheel[0] = Tw[0] / 4.5f + c->yaw_control.yaw_force; - c->output.wheel[1] = Tw[1] / 4.5f - c->yaw_control.yaw_force; + c->output.wheel[0] = Tw[0] / 4.5f; + c->output.wheel[1] = Tw[1] / 4.5f; /* 腿长控制和VMC逆解算（使用PID控制） */ float virtual_force[2]; float target_L0[2]; diff --git a/User/task/rc.c b/User/task/rc.c index 53430a6..36843e8 100644 --- a/User/task/rc.c +++ b/User/task/rc.c @@ -21,7 +21,7 @@ DR16_t dr16; RC_CAN_t rc_can; Chassis_CMD_t cmd_to_chassis; -#define USE_RC_CAN +#define USE_DR16 /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ From 96b6389a46c8f3fee2d9fbf446e06a0809bb1fdc Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Tue, 7 Oct 2025 02:41:01 +0800 Subject: [PATCH 2/4] =?UTF-8?q?=E5=B0=8F=E6=94=B9?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Core/Src/stm32f4xx_it.c | 1026 +++++++++++++++++++-------------------- User/bsp/can.c | 291 ++++++----- User/bsp/can.h | 46 +- User/device/dm_imu.c | 4 +- User/device/dr16.c | 6 +- User/device/dr16.h | 49 +- User/device/motor_lk.c | 3 - User/device/motor_lz.c | 11 - User/device/motor_lz.h | 6 - User/device/motor_rm.c | 22 +- User/task/rc.c | 6 +- 11 files changed, 742 insertions(+), 728 deletions(-) diff --git a/Core/Src/stm32f4xx_it.c b/Core/Src/stm32f4xx_it.c index 97aabfe..abebd0f 100644 --- a/Core/Src/stm32f4xx_it.c +++ b/Core/Src/stm32f4xx_it.c @@ -1,513 +1,513 @@ -/* 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(USER_KEY_Pin); - /* 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(CMPS_INT_Pin); - /* 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(ACCL_INT_Pin); - /* 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(GYRO_INT_Pin); - /* 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 */ +/* 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(USER_KEY_Pin); + /* 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(CMPS_INT_Pin); + /* 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(ACCL_INT_Pin); + /* 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(GYRO_INT_Pin); + /* 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 */ diff --git a/User/bsp/can.c b/User/bsp/can.c index 193dd2d..e401cdd 100644 --- a/User/bsp/can.c +++ b/User/bsp/can.c @@ -37,16 +37,21 @@ static osMutexId_t queue_mutex = NULL; static void (*CAN_Callback[BSP_CAN_NUM][BSP_CAN_CB_NUM])(void); static bool inited = false; static BSP_CAN_IdParser_t id_parser = NULL; /* ID解析器 */ +static BSP_CAN_TxQueue_t tx_queues[BSP_CAN_NUM]; /* 每个CAN的发送队列 */ /* Private function prototypes ---------------------------------------------- */ static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan); static osMessageQueueId_t BSP_CAN_FindQueue(BSP_CAN_t can, uint32_t can_id); static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size); -static 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 void BSP_CAN_TxCompleteCallback(void); static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header); static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type); +static void BSP_CAN_TxQueueInit(BSP_CAN_t can); +static bool BSP_CAN_TxQueuePush(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg); +static bool BSP_CAN_TxQueuePop(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg); +static bool BSP_CAN_TxQueueIsEmpty(BSP_CAN_t can); /* Private functions -------------------------------------------------------- */ /* USER FUNCTION BEGIN */ @@ -121,29 +126,7 @@ static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queu 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 获取帧类型 */ @@ -163,6 +146,106 @@ static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_ return original_id; } +/** + * @brief 初始化发送队列 + */ +static void BSP_CAN_TxQueueInit(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return; + + tx_queues[can].head = 0; + tx_queues[can].tail = 0; +} + +/** + * @brief 向发送队列添加消息（无锁） + */ +static bool BSP_CAN_TxQueuePush(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg) { + if (can >= BSP_CAN_NUM || msg == NULL) return false; + + BSP_CAN_TxQueue_t *queue = &tx_queues[can]; + uint32_t next_head = (queue->head + 1) % BSP_CAN_TX_QUEUE_SIZE; + + // 队列满 + if (next_head == queue->tail) { + return false; + } + + // 复制消息 + queue->buffer[queue->head] = *msg; + + // 更新头指针（原子操作） + queue->head = next_head; + + return true; +} + + +/** + * @brief 从发送队列取出消息（无锁） + */ +static bool BSP_CAN_TxQueuePop(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg) { + if (can >= BSP_CAN_NUM || msg == NULL) return false; + + BSP_CAN_TxQueue_t *queue = &tx_queues[can]; + + // 队列空 + if (queue->head == queue->tail) { + return false; + } + + // 复制消息 + *msg = queue->buffer[queue->tail]; + + // 更新尾指针（原子操作） + queue->tail = (queue->tail + 1) % BSP_CAN_TX_QUEUE_SIZE; + + return true; +} + +/** + * @brief 检查发送队列是否为空 + */ +static bool BSP_CAN_TxQueueIsEmpty(BSP_CAN_t can) { + if (can >= BSP_CAN_NUM) return true; + + return tx_queues[can].head == tx_queues[can].tail; +} + +/** + * @brief 处理所有CAN实例的发送队列 + */ +static void BSP_CAN_TxCompleteCallback(void) { + // 处理所有CAN实例的发送队列 + for (int i = 0; i < BSP_CAN_NUM; i++) { + BSP_CAN_t can = (BSP_CAN_t)i; + CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can); + if (hcan == NULL) continue; + + BSP_CAN_TxMessage_t msg; + uint32_t mailbox; + + // 尝试发送队列中的消息 + while (!BSP_CAN_TxQueueIsEmpty(can)) { + // 检查是否有空闲邮箱 + if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) == 0) { + break; // 没有空闲邮箱，等待下次中断 + } + + // 从队列中取出消息 + if (!BSP_CAN_TxQueuePop(can, &msg)) { + break; + } + + // 发送消息 + if (HAL_CAN_AddTxMessage(hcan, &msg.header, msg.data, &mailbox) != HAL_OK) { + // 发送失败，消息已经从队列中移除，直接丢弃 + break; + } + } + } +} + + /** * @brief FIFO0接收处理函数 */ @@ -347,7 +430,12 @@ int8_t BSP_CAN_Init(void) { // 清零回调函数数组 memset(CAN_Callback, 0, sizeof(CAN_Callback)); - + + // 初始化发送队列 + for (int i = 0; i < BSP_CAN_NUM; i++) { + BSP_CAN_TxQueueInit((BSP_CAN_t)i); + } + // 初始化ID解析器为默认解析器 id_parser = BSP_CAN_DefaultIdParser; @@ -377,6 +465,9 @@ int8_t BSP_CAN_Init(void) { // 自动注册CAN1接收回调函数 BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback); + BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX0_CPLT_CB, BSP_CAN_TxCompleteCallback); + BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX1_CPLT_CB, BSP_CAN_TxCompleteCallback); + BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_TxCompleteCallback); // 激活CAN1中断 HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING | @@ -390,48 +481,19 @@ int8_t BSP_CAN_Init(void) { // 自动注册CAN2接收回调函数 BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, BSP_CAN_RxFifo1Callback); + BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX0_CPLT_CB, BSP_CAN_TxCompleteCallback); + BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX1_CPLT_CB, BSP_CAN_TxCompleteCallback); + BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_TxCompleteCallback); // 激活CAN2中断 - HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING); + HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING | + CAN_IT_TX_MAILBOX_EMPTY); // 激活发送邮箱空中断 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) { @@ -487,44 +549,58 @@ int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format, return BSP_ERR_NULL; } - CAN_TxHeaderTypeDef header = {0}; - uint32_t mailbox; + // 准备发送消息 + BSP_CAN_TxMessage_t tx_msg = {0}; switch (format) { case BSP_CAN_FORMAT_STD_DATA: - header.StdId = id; - header.IDE = CAN_ID_STD; - header.RTR = CAN_RTR_DATA; + tx_msg.header.StdId = id; + tx_msg.header.IDE = CAN_ID_STD; + tx_msg.header.RTR = CAN_RTR_DATA; break; case BSP_CAN_FORMAT_EXT_DATA: - header.ExtId = id; - header.IDE = CAN_ID_EXT; - header.RTR = CAN_RTR_DATA; + tx_msg.header.ExtId = id; + tx_msg.header.IDE = CAN_ID_EXT; + tx_msg.header.RTR = CAN_RTR_DATA; break; case BSP_CAN_FORMAT_STD_REMOTE: - header.StdId = id; - header.IDE = CAN_ID_STD; - header.RTR = CAN_RTR_REMOTE; + tx_msg.header.StdId = id; + tx_msg.header.IDE = CAN_ID_STD; + tx_msg.header.RTR = CAN_RTR_REMOTE; break; case BSP_CAN_FORMAT_EXT_REMOTE: - header.ExtId = id; - header.IDE = CAN_ID_EXT; - header.RTR = CAN_RTR_REMOTE; + tx_msg.header.ExtId = id; + tx_msg.header.IDE = CAN_ID_EXT; + tx_msg.header.RTR = CAN_RTR_REMOTE; break; default: return BSP_ERR; } - header.DLC = dlc; - header.TransmitGlobalTime = DISABLE; + tx_msg.header.DLC = dlc; + tx_msg.header.TransmitGlobalTime = DISABLE; - HAL_StatusTypeDef result = HAL_CAN_AddTxMessage(hcan, &header, data, &mailbox); - - if (result != HAL_OK) { - return BSP_ERR; + // 复制数据 + if (data != NULL && dlc > 0) { + memcpy(tx_msg.data, data, dlc); } - return BSP_OK; + // 尝试直接发送到邮箱 + uint32_t mailbox; + if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) > 0) { + HAL_StatusTypeDef result = HAL_CAN_AddTxMessage(hcan, &tx_msg.header, tx_msg.data, &mailbox); + if (result == HAL_OK) { + return BSP_OK; // 发送成功 + } + } + + // 邮箱满，尝试放入队列 + if (BSP_CAN_TxQueuePush(can, &tx_msg)) { + return BSP_OK; // 成功放入队列 + } + + // 队列也满，丢弃数据 + return BSP_ERR; // 数据丢弃 } int8_t BSP_CAN_TransmitStdDataFrame(BSP_CAN_t can, BSP_CAN_StdDataFrame_t *frame) { @@ -556,12 +632,6 @@ int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size) { 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) { @@ -628,15 +698,6 @@ int8_t BSP_CAN_RegisterIdParser(BSP_CAN_IdParser_t 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); @@ -644,42 +705,4 @@ uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type) { return BSP_CAN_DefaultIdParser(original_id, frame_type); } -int8_t BSP_CAN_WaitTxMailboxEmpty(BSP_CAN_t can, uint32_t timeout) { - if (!inited) { - return BSP_ERR_INITED; - } - if (can >= BSP_CAN_NUM) { - return BSP_ERR; - } - - CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can); - if (hcan == NULL) { - return BSP_ERR_NULL; - } - - uint32_t start_time = HAL_GetTick(); - - // 如果超时时间为0，立即检查并返回 - if (timeout == 0) { - uint32_t free_level = HAL_CAN_GetTxMailboxesFreeLevel(hcan); - return (free_level > 0) ? BSP_OK : BSP_ERR_TIMEOUT; - } - - // 等待至少有一个邮箱空闲 - while (true) { - uint32_t free_level = HAL_CAN_GetTxMailboxesFreeLevel(hcan); - if (free_level > 0) { - return BSP_OK; - } - - // 检查超时 - if (timeout != BSP_CAN_TIMEOUT_FOREVER) { - uint32_t elapsed = HAL_GetTick() - start_time; - if (elapsed >= timeout) { - return BSP_ERR_TIMEOUT; - } - } - - osDelay(1); - } -} + diff --git a/User/bsp/can.h b/User/bsp/can.h index 00b60fe..e6b5f71 100644 --- a/User/bsp/can.h +++ b/User/bsp/can.h @@ -21,6 +21,7 @@ extern "C" { #define BSP_CAN_DEFAULT_QUEUE_SIZE 10 #define BSP_CAN_TIMEOUT_IMMEDIATE 0 #define BSP_CAN_TIMEOUT_FOREVER osWaitForever +#define BSP_CAN_TX_QUEUE_SIZE 32 /* 发送队列大小 */ /* USER DEFINE BEGIN */ @@ -102,6 +103,19 @@ typedef struct { /* ID解析回调函数类型 */ typedef uint32_t (*BSP_CAN_IdParser_t)(uint32_t original_id, BSP_CAN_FrameType_t frame_type); +/* CAN发送消息结构体 */ +typedef struct { + CAN_TxHeaderTypeDef header; /* 发送头 */ + uint8_t data[BSP_CAN_MAX_DLC]; /* 数据 */ +} BSP_CAN_TxMessage_t; + +/* 无锁环形队列结构体 */ +typedef struct { + BSP_CAN_TxMessage_t buffer[BSP_CAN_TX_QUEUE_SIZE]; /* 缓冲区 */ + volatile uint32_t head; /* 队列头 */ + volatile uint32_t tail; /* 队列尾 */ +} BSP_CAN_TxQueue_t; + /* USER STRUCT BEGIN */ /* USER STRUCT END */ @@ -114,12 +128,6 @@ typedef uint32_t (*BSP_CAN_IdParser_t)(uint32_t original_id, BSP_CAN_FrameType_t */ int8_t BSP_CAN_Init(void); -/** - * @brief 反初始化 CAN 模块 - * @return BSP_OK 成功，其他值失败 - */ -int8_t BSP_CAN_DeInit(void); - /** * @brief 获取 CAN 句柄 * @param can CAN 枚举 @@ -173,13 +181,20 @@ int8_t BSP_CAN_TransmitExtDataFrame(BSP_CAN_t can, BSP_CAN_ExtDataFrame_t *frame */ int8_t BSP_CAN_TransmitRemoteFrame(BSP_CAN_t can, BSP_CAN_RemoteFrame_t *frame); + /** - * @brief 等待CAN发送邮箱空闲 + * @brief 获取发送队列中待发送消息数量 + * @param can CAN 枚举 + * @return 队列中消息数量，-1表示错误 + */ +int32_t BSP_CAN_GetTxQueueCount(BSP_CAN_t can); + +/** + * @brief 清空发送队列 * @param can CAN 枚举 - * @param timeout 超时时间（毫秒），0为立即返回，osWaitForever为永久等待 * @return BSP_OK 成功，其他值失败 */ -int8_t BSP_CAN_WaitTxMailboxEmpty(BSP_CAN_t can, uint32_t timeout); +int8_t BSP_CAN_FlushTxQueue(BSP_CAN_t can); /** * @brief 注册 CAN ID 接收队列 @@ -190,13 +205,7 @@ int8_t BSP_CAN_WaitTxMailboxEmpty(BSP_CAN_t can, uint32_t timeout); */ 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 消息 @@ -231,11 +240,6 @@ int8_t BSP_CAN_FlushQueue(BSP_CAN_t can, uint32_t can_id); */ int8_t BSP_CAN_RegisterIdParser(BSP_CAN_IdParser_t parser); -/** - * @brief 注销ID解析器 - * @return BSP_OK 成功，其他值失败 - */ -int8_t BSP_CAN_UnregisterIdParser(void); /** * @brief 解析CAN ID diff --git a/User/device/dm_imu.c b/User/device/dm_imu.c index e05c1de..e5380d9 100644 --- a/User/device/dm_imu.c +++ b/User/device/dm_imu.c @@ -166,7 +166,6 @@ int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) { .dlc = 4, }; memcpy(frame.data, tx_data, 4); - BSP_CAN_WaitTxMailboxEmpty(imu->param.can, 1); // 等待发送邮箱空闲 int8_t result = BSP_CAN_TransmitStdDataFrame(imu->param.can, &frame); return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR; } @@ -253,8 +252,9 @@ int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu){ count++; if (count >= 4) { count = 0; // 重置计数器 + return DEVICE_OK; } - return DEVICE_OK; + return DEVICE_ERR; } /** diff --git a/User/device/dr16.c b/User/device/dr16.c index 929e4b5..120997b 100644 --- a/User/device/dr16.c +++ b/User/device/dr16.c @@ -141,13 +141,13 @@ int8_t DR16_ParseData(DR16_t *dr16){ uint16_t key_value = dr16->raw_data.key; // 解析键盘位映射（W-B键，位0-15） - for (int i = CMD_KEY_W; i <= CMD_KEY_B; i++) { + for (int i = DR16_KEY_W; i <= DR16_KEY_B; i++) { dr16->data.keyboard.key[i] = (key_value & (1 << i)) != 0; } // 解析鼠标点击 - dr16->data.keyboard.key[CMD_L_CLICK] = dr16->data.mouse.l_click; - dr16->data.keyboard.key[CMD_R_CLICK] = dr16->data.mouse.r_click; + dr16->data.keyboard.key[DR16_L_CLICK] = dr16->data.mouse.l_click; + dr16->data.keyboard.key[DR16_R_CLICK] = dr16->data.mouse.r_click; // 解析第五通道 dr16->data.ch_res = 2.0f * ((float)dr16->raw_data.res - DR16_CH_VALUE_MID) / full_range; diff --git a/User/device/dr16.h b/User/device/dr16.h index 73ebbf2..03fa526 100644 --- a/User/device/dr16.h +++ b/User/device/dr16.h @@ -38,33 +38,33 @@ typedef struct __packed { } DR16_RawData_t; typedef enum { - CMD_SW_ERR = 0, - CMD_SW_UP = 1, - CMD_SW_MID = 3, - CMD_SW_DOWN = 2, + DR16_SW_ERR = 0, + DR16_SW_UP = 1, + DR16_SW_MID = 3, + DR16_SW_DOWN = 2, } DR16_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, + DR16_KEY_W = 0, + DR16_KEY_S, + DR16_KEY_A, + DR16_KEY_D, + DR16_KEY_SHIFT, + DR16_KEY_CTRL, + DR16_KEY_Q, + DR16_KEY_E, + DR16_KEY_R, + DR16_KEY_F, + DR16_KEY_G, + DR16_KEY_Z, + DR16_KEY_X, + DR16_KEY_C, + DR16_KEY_V, + DR16_KEY_B, + DR16_L_CLICK, + DR16_R_CLICK, + DR16_KEY_NUM, } DR16_Key_t; typedef struct { @@ -87,14 +87,13 @@ typedef struct { } mouse; /* 鼠标值 */ union { - bool key[CMD_KEY_NUM]; /* 键盘按键值 */ + bool key[DR16_KEY_NUM]; /* 键盘按键值 */ uint16_t value; /* 键盘按键值的位映射 */ } keyboard; uint16_t res; /* 保留，未启用 */ } DR16_Data_t; - typedef struct { DEVICE_Header_t header; DR16_RawData_t raw_data; diff --git a/User/device/motor_lk.c b/User/device/motor_lk.c index c7911d6..c26878e 100644 --- a/User/device/motor_lk.c +++ b/User/device/motor_lk.c @@ -253,7 +253,6 @@ int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) { tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF); tx_frame.data[6] = 0x00; tx_frame.data[7] = 0x00; - BSP_CAN_WaitTxMailboxEmpty(param->can, 1); // 等待发送邮箱空闲 return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; } @@ -279,7 +278,6 @@ int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param) { tx_frame.data[5] = 0x00; tx_frame.data[6] = 0x00; tx_frame.data[7] = 0x00; - BSP_CAN_WaitTxMailboxEmpty(param->can, 1); // 等待发送邮箱空闲 return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; } @@ -299,7 +297,6 @@ int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param) { tx_frame.data[5] = 0x00; tx_frame.data[6] = 0x00; tx_frame.data[7] = 0x00; - BSP_CAN_WaitTxMailboxEmpty(param->can, 1); // 等待发送邮箱空闲 return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; } diff --git a/User/device/motor_lz.c b/User/device/motor_lz.c index 59ba153..7ed7344 100644 --- a/User/device/motor_lz.c +++ b/User/device/motor_lz.c @@ -134,7 +134,6 @@ static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *dat } else { memset(tx_frame.data, 0, dlc); } - BSP_CAN_WaitTxMailboxEmpty(can, 1); // 等待发送邮箱空闲 return BSP_CAN_TransmitExtDataFrame(can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; } @@ -244,14 +243,6 @@ int8_t MOTOR_LZ_Init(void) { 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; @@ -369,7 +360,6 @@ int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *m uint16_t raw_kd = MOTOR_LZ_FloatToRawPositive(send_param.kd, LZ_KD_MAX); data[6] = (raw_kd >> 8) & 0xFF; data[7] = raw_kd & 0xFF; - BSP_CAN_WaitTxMailboxEmpty(param->can, 1); // 等待发送邮箱空闲 return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8); } @@ -378,7 +368,6 @@ int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param) { if (param == NULL) return DEVICE_ERR_NULL; // 构建扩展ID - 使能命令 - // 格式: 0x300FF7X, 其中X是motor_id的低4位 uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_ENABLE, param->host_id, param->motor_id); // 数据区清零 diff --git a/User/device/motor_lz.h b/User/device/motor_lz.h index 76a72ae..d2a8002 100644 --- a/User/device/motor_lz.h +++ b/User/device/motor_lz.h @@ -111,12 +111,6 @@ typedef struct { */ int8_t MOTOR_LZ_Init(void); -/** - * @brief 反初始化灵足电机驱动系统 - * @return 设备状态码 - */ -int8_t MOTOR_LZ_DeInit(void); - /** * @brief 注册一个灵足电机 * @param param 电机参数 diff --git a/User/device/motor_rm.c b/User/device/motor_rm.c index b852c48..9b5ea44 100644 --- a/User/device/motor_rm.c +++ b/User/device/motor_rm.c @@ -138,17 +138,23 @@ static void Motor_RM_Decode(MOTOR_RM_t *motor, BSP_CAN_Message_t *msg) { motor->feedback.rotor_abs_angle = rotor_angle; motor->feedback.rotor_speed = rotor_speed; motor->feedback.torque_current = torque_current; + } + while (motor->feedback.rotor_abs_angle < 0) { + motor->feedback.rotor_abs_angle += M_2PI; + } + while (motor->feedback.rotor_abs_angle >= M_2PI) { + motor->feedback.rotor_abs_angle -= M_2PI; + } + if (motor->motor.reverse) { + motor->feedback.rotor_abs_angle = M_2PI - motor->feedback.rotor_abs_angle; + motor->feedback.rotor_speed = -motor->feedback.rotor_speed; + motor->feedback.torque_current = -motor->feedback.torque_current; } motor->feedback.temp = msg->data[6]; - motor->motor.feedback.rotor_abs_angle = motor->feedback.rotor_abs_angle; - motor->motor.feedback.rotor_speed = motor->feedback.rotor_speed; - motor->motor.feedback.torque_current = motor->feedback.torque_current; - motor->motor.feedback.temp = motor->feedback.temp; } /* Exported functions ------------------------------------------------------- */ - int8_t MOTOR_RM_Register(MOTOR_RM_Param_t *param) { if (param == NULL) return DEVICE_ERR_NULL; if (MOTOR_RM_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR; @@ -197,7 +203,7 @@ int8_t MOTOR_RM_Update(MOTOR_RM_Param_t *param) { motor->motor.header.online = true; motor->motor.header.last_online_time = BSP_TIME_Get(); Motor_RM_Decode(motor, &rx_msg); - + motor->motor.feedback = motor->feedback; return DEVICE_OK; } } @@ -227,6 +233,9 @@ int8_t MOTOR_RM_SetOutput(MOTOR_RM_Param_t *param, float value) { if (manager == NULL) return DEVICE_ERR_NO_DEV; if (value > 1.0f) value = 1.0f; if (value < -1.0f) value = -1.0f; + if (param->reverse){ + value = -value; + } MOTOR_RM_t *motor = MOTOR_RM_GetMotor(param); if (motor == NULL) return DEVICE_ERR_NO_DEV; int8_t logical_index = MOTOR_RM_GetLogicalIndex(param->id, param->module); @@ -282,7 +291,6 @@ int8_t MOTOR_RM_Ctrl(MOTOR_RM_Param_t *param) { default: return DEVICE_ERR; } - BSP_CAN_WaitTxMailboxEmpty(param->can, 1); // 等待发送邮箱空闲 return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR; } diff --git a/User/task/rc.c b/User/task/rc.c index 36843e8..04223fe 100644 --- a/User/task/rc.c +++ b/User/task/rc.c @@ -50,13 +50,13 @@ void Task_rc(void *argument) { if (DR16_WaitDmaCplt(20)) { DR16_ParseData(&dr16); switch (dr16.data.sw_l) { - case CMD_SW_UP: // 上位 + case DR16_SW_UP: // 上位 cmd_to_chassis.mode = CHASSIS_MODE_RELAX; break; - case CMD_SW_MID: // 中位 + case DR16_SW_MID: // 中位 cmd_to_chassis.mode = CHASSIS_MODE_RECOVER; break; - case CMD_SW_DOWN: // 下位 + case DR16_SW_DOWN: // 下位 cmd_to_chassis.mode = CHASSIS_MODE_WHELL_BALANCE; break; default: From 83354623d1f56647937463979dd7393a11977193 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Tue, 7 Oct 2025 02:53:20 +0800 Subject: [PATCH 3/4] =?UTF-8?q?=E6=B7=BB=E5=8A=A0088?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/device/bmi088.c | 381 +++++++++++++++++++++++++++++++++ User/device/bmi088.h | 81 +++++++ User/device/device.h | 4 + User/device/device_config.yaml | 8 + User/task/config.yaml | 7 + User/task/ctrl_gimbal.c | 5 +- User/task/imu.c | 44 ++++ User/task/init.c | 1 + User/task/user_task.c | 5 + User/task/user_task.h | 8 + 10 files changed, 541 insertions(+), 3 deletions(-) create mode 100644 User/device/bmi088.c create mode 100644 User/device/bmi088.h create mode 100644 User/task/imu.c diff --git a/User/device/bmi088.c b/User/device/bmi088.c new file mode 100644 index 0000000..0f4c154 --- /dev/null +++ b/User/device/bmi088.c @@ -0,0 +1,381 @@ +/* + BMI088 陀螺仪+加速度计传感器。 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "bmi088.h" + +#include +#include +#include +#include + +#include "bsp/time.h" +#include "bsp/gpio.h" +#include "bsp/spi.h" +#include "component/user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +#define BMI088_REG_ACCL_CHIP_ID (0x00) +#define BMI088_REG_ACCL_ERR (0x02) +#define BMI088_REG_ACCL_STATUS (0x03) +#define BMI088_REG_ACCL_X_LSB (0x12) +#define BMI088_REG_ACCL_X_MSB (0x13) +#define BMI088_REG_ACCL_Y_LSB (0x14) +#define BMI088_REG_ACCL_Y_MSB (0x15) +#define BMI088_REG_ACCL_Z_LSB (0x16) +#define BMI088_REG_ACCL_Z_MSB (0x17) +#define BMI088_REG_ACCL_SENSORTIME_0 (0x18) +#define BMI088_REG_ACCL_SENSORTIME_1 (0x19) +#define BMI088_REG_ACCL_SENSORTIME_2 (0x1A) +#define BMI088_REG_ACCL_INT_STAT_1 (0x1D) +#define BMI088_REG_ACCL_TEMP_MSB (0x22) +#define BMI088_REG_ACCL_TEMP_LSB (0x23) +#define BMI088_REG_ACCL_CONF (0x40) +#define BMI088_REG_ACCL_RANGE (0x41) +#define BMI088_REG_ACCL_INT1_IO_CONF (0x53) +#define BMI088_REG_ACCL_INT2_IO_CONF (0x54) +#define BMI088_REG_ACCL_INT1_INT2_MAP_DATA (0x58) +#define BMI088_REG_ACCL_SELF_TEST (0x6D) +#define BMI088_REG_ACCL_PWR_CONF (0x7C) +#define BMI088_REG_ACCL_PWR_CTRL (0x7D) +#define BMI088_REG_ACCL_SOFTRESET (0x7E) + +#define BMI088_REG_GYRO_CHIP_ID (0x00) +#define BMI088_REG_GYRO_X_LSB (0x02) +#define BMI088_REG_GYRO_X_MSB (0x03) +#define BMI088_REG_GYRO_Y_LSB (0x04) +#define BMI088_REG_GYRO_Y_MSB (0x05) +#define BMI088_REG_GYRO_Z_LSB (0x06) +#define BMI088_REG_GYRO_Z_MSB (0x07) +#define BMI088_REG_GYRO_INT_STAT_1 (0x0A) +#define BMI088_REG_GYRO_RANGE (0x0F) +#define BMI088_REG_GYRO_BANDWIDTH (0x10) +#define BMI088_REG_GYRO_LPM1 (0x11) +#define BMI088_REG_GYRO_SOFTRESET (0x14) +#define BMI088_REG_GYRO_INT_CTRL (0x15) +#define BMI088_REG_GYRO_INT3_INT4_IO_CONF (0x16) +#define BMI088_REG_GYRO_INT3_INT4_IO_MAP (0x18) +#define BMI088_REG_GYRO_SELF_TEST (0x3C) + +#define BMI088_CHIP_ID_ACCL (0x1E) +#define BMI088_CHIP_ID_GYRO (0x0F) + +#define BMI088_LEN_RX_BUFF (19) +/* Private macro ------------------------------------------------------------ */ +#define BMI088_ACCL_NSS_SET() \ + BSP_GPIO_WritePin(BSP_GPIO_ACCL_CS, GPIO_PIN_SET) +#define BMI088_ACCL_NSS_RESET() \ + BSP_GPIO_WritePin(BSP_GPIO_ACCL_CS, GPIO_PIN_RESET) + +#define BMI088_GYRO_NSS_SET() \ + BSP_GPIO_WritePin(BSP_GPIO_GYRO_CS, GPIO_PIN_SET) +#define BMI088_GYRO_NSS_RESET() \ + BSP_GPIO_WritePin(BSP_GPIO_GYRO_CS, GPIO_PIN_RESET) + +/* Private typedef ---------------------------------------------------------- */ +typedef enum { + BMI_ACCL, + BMI_GYRO, +} BMI_Device_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static uint8_t buffer[2]; +static uint8_t bmi088_rxbuf[BMI088_LEN_RX_BUFF]; + +static osThreadId_t thread_alert; +static bool inited = false; + +/* Private function -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +static void BMI_WriteSingle(BMI_Device_t dv, uint8_t reg, uint8_t data) { + buffer[0] = (reg & 0x7f); + buffer[1] = data; + + BSP_TIME_Delay(1); + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_RESET(); + break; + + case BMI_GYRO: + BMI088_GYRO_NSS_RESET(); + break; + } + + BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 2u, false); + + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_SET(); + break; + + case BMI_GYRO: + BMI088_GYRO_NSS_SET(); + break; + } +} + +static uint8_t BMI_ReadSingle(BMI_Device_t dv, uint8_t reg) { + BSP_TIME_Delay(1); + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_RESET(); + break; + + case BMI_GYRO: + BMI088_GYRO_NSS_RESET(); + break; + } + buffer[0] = (uint8_t)(reg | 0x80); + BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 1u, false); + BSP_SPI_Receive(BSP_SPI_BMI088, buffer, 2u, false); + + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_SET(); + return buffer[1]; + + case BMI_GYRO: + BMI088_GYRO_NSS_SET(); + return buffer[0]; + } +} + +static void BMI_Read(BMI_Device_t dv, uint8_t reg, uint8_t *data, uint8_t len) { + if (data == NULL) return; + + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_RESET(); + break; + + case BMI_GYRO: + BMI088_GYRO_NSS_RESET(); + break; + } + buffer[0] = (uint8_t)(reg | 0x80); + BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 1u, false); + BSP_SPI_Receive(BSP_SPI_BMI088, data, len, true); +} + +static void BMI088_RxCpltCallback(void) { + if (BSP_GPIO_ReadPin(BSP_GPIO_ACCL_CS) == GPIO_PIN_RESET) { + BMI088_ACCL_NSS_SET(); + osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_RAW_REDY); + } + if (BSP_GPIO_ReadPin(BSP_GPIO_GYRO_CS) == GPIO_PIN_RESET) { + BMI088_GYRO_NSS_SET(); + osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_RAW_REDY); + } +} + +static void BMI088_AcclIntCallback(void) { + osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_NEW_DATA); +} + +static void BMI088_GyroIntCallback(void) { + osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_NEW_DATA); +} + +/* Exported functions ------------------------------------------------------- */ +int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali) { + if (bmi088 == NULL) return DEVICE_ERR_NULL; + if (cali == NULL) return DEVICE_ERR_NULL; + if (inited) return DEVICE_ERR_INITED; + if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL; + + bmi088->cali = cali; + + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_SOFTRESET, 0xB6); + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_SOFTRESET, 0xB6); + BSP_TIME_Delay(30); + + /* Switch accl to SPI mode. */ + BMI_ReadSingle(BMI_ACCL, BMI088_CHIP_ID_ACCL); + + if (BMI_ReadSingle(BMI_ACCL, BMI088_REG_ACCL_CHIP_ID) != BMI088_CHIP_ID_ACCL) + return DEVICE_ERR_NO_DEV; + + if (BMI_ReadSingle(BMI_GYRO, BMI088_REG_GYRO_CHIP_ID) != BMI088_CHIP_ID_GYRO) + return DEVICE_ERR_NO_DEV; + + BSP_GPIO_DisableIRQ(BSP_GPIO_ACCL_INT); + BSP_GPIO_DisableIRQ(BSP_GPIO_GYRO_INT); + + BSP_SPI_RegisterCallback(BSP_SPI_BMI088, BSP_SPI_RX_CPLT_CB, + BMI088_RxCpltCallback); + BSP_GPIO_RegisterCallback(BSP_GPIO_ACCL_INT, BMI088_AcclIntCallback); + BSP_GPIO_RegisterCallback(BSP_GPIO_GYRO_INT, BMI088_GyroIntCallback); + + /* Accl init. */ + /* Filter setting: Normal. */ + /* ODR: 0xAB: 800Hz. 0xAA: 400Hz. 0xA9: 200Hz. 0xA8: 100Hz. 0xA6: 25Hz. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_CONF, 0xAA); + + /* 0x00: +-3G. 0x01: +-6G. 0x02: +-12G. 0x03: +-24G. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_RANGE, 0x01); + + /* INT1 as output. Push-pull. Active low. Output. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_IO_CONF, 0x08); + + /* Map data ready interrupt to INT1. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_INT2_MAP_DATA, 0x04); + + /* Turn on accl. Now we can read data. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_PWR_CTRL, 0x04); + BSP_TIME_Delay(50); + + /* Gyro init. */ + /* 0x00: +-2000. 0x01: +-1000. 0x02: +-500. 0x03: +-250. 0x04: +-125. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_RANGE, 0x01); + + /* Filter bw: 47Hz. */ + /* ODR: 0x02: 1000Hz. 0x03: 400Hz. 0x06: 200Hz. 0x07: 100Hz. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_BANDWIDTH, 0x03); + + /* INT3 and INT4 as output. Push-pull. Active low. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_CONF, 0x00); + + /* Map data ready interrupt to INT3. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_MAP, 0x01); + + /* Enable new data interrupt. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT_CTRL, 0x80); + + BSP_TIME_Delay(10); + + inited = true; + + BSP_GPIO_EnableIRQ(BSP_GPIO_ACCL_INT); + BSP_GPIO_EnableIRQ(BSP_GPIO_GYRO_INT); + return DEVICE_OK; +} + +bool BMI088_GyroStable(AHRS_Gyro_t *gyro) { + return ((gyro->x < 0.03f) && (gyro->y < 0.03f) && (gyro->z < 0.03f)); +} + +uint32_t BMI088_WaitNew() { + return osThreadFlagsWait( + SIGNAL_BMI088_ACCL_NEW_DATA | SIGNAL_BMI088_GYRO_NEW_DATA, osFlagsWaitAll, + osWaitForever); +} + +int8_t BMI088_AcclStartDmaRecv() { + BMI_Read(BMI_ACCL, BMI088_REG_ACCL_X_LSB, bmi088_rxbuf, BMI088_LEN_RX_BUFF); + return DEVICE_OK; +} + +uint32_t BMI088_AcclWaitDmaCplt() { + return osThreadFlagsWait(SIGNAL_BMI088_ACCL_RAW_REDY, osFlagsWaitAll, + osWaitForever); +} + +int8_t BMI088_GyroStartDmaRecv() { + BMI_Read(BMI_GYRO, BMI088_REG_GYRO_X_LSB, bmi088_rxbuf + 7, 6u); + return DEVICE_OK; +} + +uint32_t BMI088_GyroWaitDmaCplt() { + return osThreadFlagsWait(SIGNAL_BMI088_GYRO_RAW_REDY, osFlagsWaitAll, + osWaitForever); +} + +int8_t BMI088_ParseAccl(BMI088_t *bmi088) { + if (bmi088 == NULL) return DEVICE_ERR_NULL; + +#if 1 + int16_t raw_x, raw_y, raw_z; + memcpy(&raw_x, bmi088_rxbuf + 1, sizeof(raw_x)); + memcpy(&raw_y, bmi088_rxbuf + 3, sizeof(raw_y)); + memcpy(&raw_z, bmi088_rxbuf + 5, sizeof(raw_z)); + + bmi088->accl.x = (float)raw_x; + bmi088->accl.y = (float)raw_y; + bmi088->accl.z = (float)raw_z; + +#else + const int16_t *praw_x = (int16_t *)(bmi088_rxbuf + 1); + const int16_t *praw_y = (int16_t *)(bmi088_rxbuf + 3); + const int16_t *praw_z = (int16_t *)(bmi088_rxbuf + 5); + + bmi088->accl.x = (float)*praw_x; + bmi088->accl.y = (float)*praw_y; + bmi088->accl.z = (float)*praw_z; + +#endif + + /* 3G: 10920. 6G: 5460. 12G: 2730. 24G: 1365. */ + bmi088->accl.x /= 5460.0f; + bmi088->accl.y /= 5460.0f; + bmi088->accl.z /= 5460.0f; + + int16_t raw_temp = + (uint16_t)((bmi088_rxbuf[17] << 3) | (bmi088_rxbuf[18] >> 5)); + + if (raw_temp > 1023) raw_temp -= 2048; + + bmi088->temp = (float)raw_temp * 0.125f + 23.0f; + + return DEVICE_OK; +} + +int8_t BMI088_ParseGyro(BMI088_t *bmi088) { + if (bmi088 == NULL) return DEVICE_ERR_NULL; + +#if 1 + /* Gyroscope imu_raw -> degrees/sec -> radians/sec */ + int16_t raw_x, raw_y, raw_z; + memcpy(&raw_x, bmi088_rxbuf + 7, sizeof(raw_x)); + memcpy(&raw_y, bmi088_rxbuf + 9, sizeof(raw_y)); + memcpy(&raw_z, bmi088_rxbuf + 11, sizeof(raw_z)); + + bmi088->gyro.x = (float)raw_x; + bmi088->gyro.y = (float)raw_y; + bmi088->gyro.z = (float)raw_z; + +#else + /* Gyroscope imu_raw -> degrees/sec -> radians/sec */ + const int16_t *raw_x = (int16_t *)(bmi088_rxbuf + 7); + const int16_t *raw_y = (int16_t *)(bmi088_rxbuf + 9); + const int16_t *raw_z = (int16_t *)(bmi088_rxbuf + 11); + + bmi088->gyro.x = (float)*raw_x; + bmi088->gyro.y = (float)*raw_y; + bmi088->gyro.z = (float)*raw_z; +#endif + + /* FS125: 262.144. FS250: 131.072. FS500: 65.536. FS1000: 32.768. + * FS2000: 16.384.*/ + bmi088->gyro.x /= 32.768f; + bmi088->gyro.y /= 32.768f; + bmi088->gyro.z /= 32.768f; + + bmi088->gyro.x *= M_DEG2RAD_MULT; + bmi088->gyro.y *= M_DEG2RAD_MULT; + bmi088->gyro.z *= M_DEG2RAD_MULT; + + bmi088->gyro.x -= bmi088->cali->gyro_offset.x; + bmi088->gyro.y -= bmi088->cali->gyro_offset.y; + bmi088->gyro.z -= bmi088->cali->gyro_offset.z; + + return DEVICE_ERR_NULL; +} + +float BMI088_GetUpdateFreq(BMI088_t *bmi088) { + (void)bmi088; + return 400.0f; +} diff --git a/User/device/bmi088.h b/User/device/bmi088.h new file mode 100644 index 0000000..eb44e0c --- /dev/null +++ b/User/device/bmi088.h @@ -0,0 +1,81 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include + +#include "component/ahrs.h" +#include "device/device.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef struct { + struct { + float x; + float y; + float z; + } gyro_offset; /* 陀螺仪偏置 */ +} BMI088_Cali_t; /* BMI088校准数据 */ + +typedef struct { + DEVICE_Header_t header; + AHRS_Accl_t accl; + AHRS_Gyro_t gyro; + + float temp; /* 温度 */ + + const BMI088_Cali_t *cali; +} BMI088_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Exported functions prototypes -------------------------------------------- */ +int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali); +int8_t BMI088_Restart(void); + +bool BMI088_GyroStable(AHRS_Gyro_t *gyro); + +/* Sensor use right-handed coordinate system. */ +/* + x < R(logo) + y + UP is z + All implementation should follow this rule. + */ +uint32_t BMI088_WaitNew(); + +/* + BMI088的Accl和Gyro共用同一个DMA通道，所以一次只能读一个传感器。 + 即BMI088_AcclStartDmaRecv() 和 BMI088_AcclWaitDmaCplt() 中间不能 + 出现 BMI088_GyroStartDmaRecv()。 +*/ +int8_t BMI088_AcclStartDmaRecv(); +uint32_t BMI088_AcclWaitDmaCplt(); +int8_t BMI088_GyroStartDmaRecv(); +uint32_t BMI088_GyroWaitDmaCplt(); +int8_t BMI088_ParseAccl(BMI088_t *bmi088); +int8_t BMI088_ParseGyro(BMI088_t *bmi088); +float BMI088_GetUpdateFreq(BMI088_t *bmi088); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/device/device.h b/User/device/device.h index 9230af5..18ffc73 100644 --- a/User/device/device.h +++ b/User/device/device.h @@ -23,6 +23,10 @@ extern "C" { /* AUTO GENERATED SIGNALS BEGIN */ #define SIGNAL_DR16_RAW_REDY (1u << 0) +#define SIGNAL_BMI088_ACCL_RAW_REDY (1u << 1) +#define SIGNAL_BMI088_GYRO_RAW_REDY (1u << 2) +#define SIGNAL_BMI088_ACCL_NEW_DATA (1u << 3) +#define SIGNAL_BMI088_GYRO_NEW_DATA (1u << 4) /* AUTO GENERATED SIGNALS END */ /* USER SIGNALS BEGIN */ diff --git a/User/device/device_config.yaml b/User/device/device_config.yaml index 70844c0..7aaee45 100644 --- a/User/device/device_config.yaml +++ b/User/device/device_config.yaml @@ -1,3 +1,11 @@ +bmi088: + bsp_config: + BSP_GPIO_ACCL_CS: BSP_GPIO_ACCL_CS + BSP_GPIO_ACCL_INT: BSP_GPIO_ACCL_INT + BSP_GPIO_GYRO_CS: BSP_GPIO_GYRO_CS + BSP_GPIO_GYRO_INT: BSP_GPIO_GYRO_INT + BSP_SPI_BMI088: BSP_SPI_BMI088 + enabled: true buzzer: bsp_config: BSP_PWM_BUZZER: BSP_PWM_TIM8_CH1 diff --git a/User/task/config.yaml b/User/task/config.yaml index 5d60486..9a45577 100644 --- a/User/task/config.yaml +++ b/User/task/config.yaml @@ -33,3 +33,10 @@ function: Task_ctrl_gimbal name: ctrl_gimbal stack: 256 +- delay: 0 + description: '' + freq_control: true + frequency: 500.0 + function: Task_imu + name: imu + stack: 256 diff --git a/User/task/ctrl_gimbal.c b/User/task/ctrl_gimbal.c index 8054f87..f533078 100644 --- a/User/task/ctrl_gimbal.c +++ b/User/task/ctrl_gimbal.c @@ -4,7 +4,6 @@ */ /* Includes ----------------------------------------------------------------- */ -#include "device/motor_rm.h" #include "task/user_task.h" /* USER INCLUDE BEGIN */ #include "bsp/can.h" @@ -75,9 +74,9 @@ void Task_ctrl_gimbal(void *argument) { uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ - BSP_CAN_Init(); - // MOTOR_DM_Register(&dm_4310_param); + /* USER CODE INIT END */ + while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ diff --git a/User/task/imu.c b/User/task/imu.c new file mode 100644 index 0000000..25ac9de --- /dev/null +++ b/User/task/imu.c @@ -0,0 +1,44 @@ +/* + imu Task + +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +void Task_imu(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + + + /* 计算任务运行到指定频率需要等待的tick数 */ + const uint32_t delay_tick = osKernelGetTickFreq() / IMU_FREQ; + + osDelay(IMU_INIT_DELAY); /* 延时一段时间再开启任务 */ + + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ + /* USER CODE INIT BEGIN */ + + /* USER CODE INIT END */ + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* USER CODE BEGIN */ + + /* USER CODE END */ + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ + } + +} \ No newline at end of file diff --git a/User/task/init.c b/User/task/init.c index f41b86a..c641fd8 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -37,6 +37,7 @@ void Task_Init(void *argument) { task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti); task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis); task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal); + task_runtime.thread.imu = osThreadNew(Task_imu, NULL, &attr_imu); // 创建消息队列 /* USER MESSAGE BEGIN */ diff --git a/User/task/user_task.c b/User/task/user_task.c index 49fe884..4e530ba 100644 --- a/User/task/user_task.c +++ b/User/task/user_task.c @@ -33,4 +33,9 @@ const osThreadAttr_t attr_ctrl_gimbal = { .name = "ctrl_gimbal", .priority = osPriorityNormal, .stack_size = 256 * 4, +}; +const osThreadAttr_t attr_imu = { + .name = "imu", + .priority = osPriorityNormal, + .stack_size = 256 * 4, }; \ No newline at end of file diff --git a/User/task/user_task.h b/User/task/user_task.h index c02317c..8701a74 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -18,6 +18,7 @@ extern "C" { #define ATTI_ESTI_FREQ (1000.0) #define CTRL_CHASSIS_FREQ (500.0) #define CTRL_GIMBAL_FREQ (500.0) +#define IMU_FREQ (500.0) /* 任务初始化延时ms */ #define TASK_INIT_DELAY (100u) @@ -26,6 +27,7 @@ extern "C" { #define ATTI_ESTI_INIT_DELAY (0) #define CTRL_CHASSIS_INIT_DELAY (500) #define CTRL_GIMBAL_INIT_DELAY (0) +#define IMU_INIT_DELAY (0) /* Exported defines --------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */ @@ -40,6 +42,7 @@ typedef struct { osThreadId_t atti_esti; osThreadId_t ctrl_chassis; osThreadId_t ctrl_gimbal; + osThreadId_t imu; } thread; /* USER MESSAGE BEGIN */ @@ -70,6 +73,7 @@ typedef struct { UBaseType_t atti_esti; UBaseType_t ctrl_chassis; UBaseType_t ctrl_gimbal; + UBaseType_t imu; } stack_water_mark; /* 各任务运行频率 */ @@ -79,6 +83,7 @@ typedef struct { float atti_esti; float ctrl_chassis; float ctrl_gimbal; + float imu; } freq; /* 任务最近运行时间 */ @@ -88,6 +93,7 @@ typedef struct { float atti_esti; float ctrl_chassis; float ctrl_gimbal; + float imu; } last_up_time; } Task_Runtime_t; @@ -102,6 +108,7 @@ extern const osThreadAttr_t attr_rc; extern const osThreadAttr_t attr_atti_esti; extern const osThreadAttr_t attr_ctrl_chassis; extern const osThreadAttr_t attr_ctrl_gimbal; +extern const osThreadAttr_t attr_imu; /* 任务函数声明 */ void Task_Init(void *argument); @@ -110,6 +117,7 @@ void Task_rc(void *argument); void Task_atti_esti(void *argument); void Task_ctrl_chassis(void *argument); void Task_ctrl_gimbal(void *argument); +void Task_imu(void *argument); #ifdef __cplusplus } From 489415aa3437c22aab8cad3864ec37c275c9f2e5 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Tue, 7 Oct 2025 03:11:45 +0800 Subject: [PATCH 4/4] =?UTF-8?q?=E7=BA=A0=E6=AD=A3=E9=99=80=E8=9E=BA?= =?UTF-8?q?=E4=BB=AA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 2 + User/task/imu.c | 136 ++++++++++++++++++++++++++++++++++++++++++++++-- 2 files changed, 135 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 750eaea..e025a25 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -67,6 +67,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE User/component/vmc.c # User/device sources + User/device/bmi088.c User/device/buzzer.c User/device/dm_imu.c User/device/dr16.c @@ -86,6 +87,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE User/task/blink.c User/task/ctrl_chassis.c User/task/ctrl_gimbal.c + User/task/imu.c User/task/init.c User/task/rc.c User/task/user_task.c diff --git a/User/task/imu.c b/User/task/imu.c index 25ac9de..24c2ed0 100644 --- a/User/task/imu.c +++ b/User/task/imu.c @@ -6,7 +6,13 @@ /* Includes ----------------------------------------------------------------- */ #include "task/user_task.h" /* USER INCLUDE BEGIN */ - +#include "bsp/mm.h" +#include "bsp/pwm.h" +#include "bsp/gpio.h" +#include "component/ahrs.h" +#include "component/pid.h" +#include "device/bmi088.h" +#include "task/user_task.h" /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -14,7 +20,54 @@ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ +BMI088_t bmi088; +AHRS_t gimbal_ahrs; +AHRS_Magn_t magn; +AHRS_Eulr_t eulr_to_send; + +KPID_t imu_temp_ctrl_pid; + +BMI088_Cali_t cali_bmi088= { + .gyro_offset = {0.00379243772f,0.00133061118f,-0.00154866849f}, +}; + AHRS_Accl_t accl_modified; + AHRS_Gyro_t gyro_modified; + AHRS_Eulr_t eulr_modified; + +static const KPID_Params_t imu_temp_ctrl_pid_param = { + .k = 0.2f, + .p = 1.0f, + .i = 0.01f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, +}; + +/* 校准相关变量 */ +typedef enum { + CALIB_IDLE, // 空闲状态 + CALIB_RUNNING, // 正在校准 + CALIB_DONE // 校准完成 +} CalibState_t; + +static CalibState_t calib_state = CALIB_IDLE; +static uint16_t calib_count = 0; +static struct { + float x_sum; + float y_sum; + float z_sum; +} gyro_sum = {0.00341676874f, 0.000505680335f, -0.00134522165f}; +static void start_gyro_calibration(void) { + if (calib_state == CALIB_IDLE) { + calib_state = CALIB_RUNNING; + calib_count = 0; + gyro_sum.x_sum = 0.0f; + gyro_sum.y_sum = 0.0f; + gyro_sum.z_sum = 0.0f; + } +} +#define CALIB_SAMPLES 5000 // 校准采样数量 /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -30,15 +83,92 @@ void Task_imu(void *argument) { uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ - + BMI088_Init(&bmi088,&cali_bmi088); + AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088)); + PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D, + 1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param); + BSP_PWM_Start(BSP_PWM_IMU_HEAT_PWM); + + /* 注册按钮回调函数并启用中断 */ + BSP_GPIO_RegisterCallback(BSP_GPIO_USER_KEY, start_gyro_calibration); + BSP_GPIO_EnableIRQ(BSP_GPIO_USER_KEY); /* USER CODE INIT END */ while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ + BMI088_WaitNew(); + BMI088_AcclStartDmaRecv(); + BMI088_AcclWaitDmaCplt(); + + BMI088_GyroStartDmaRecv(); + BMI088_GyroWaitDmaCplt(); + + /* 锁住RTOS内核防止数据解析过程中断，造成错误 */ + osKernelLock(); + /* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */ + BMI088_ParseAccl(&bmi088); + BMI088_ParseGyro(&bmi088); + // IST8310_Parse(&ist8310); + + /* 陀螺仪校准处理 */ + if (calib_state == CALIB_RUNNING) { + /* 累加陀螺仪数据用于计算零偏 */ + gyro_sum.x_sum += bmi088.gyro.x; + gyro_sum.y_sum += bmi088.gyro.y; + gyro_sum.z_sum += bmi088.gyro.z; + calib_count++; + + /* 达到采样数量后计算平均值并更新零偏 */ + if (calib_count >= CALIB_SAMPLES) { + /* 计算平均值作为零偏 */ + cali_bmi088.gyro_offset.x = gyro_sum.x_sum / CALIB_SAMPLES; + cali_bmi088.gyro_offset.y = gyro_sum.y_sum / CALIB_SAMPLES; + cali_bmi088.gyro_offset.z = gyro_sum.z_sum / CALIB_SAMPLES; + + /* 校准完成，重置为空闲状态以便下次校准 */ + calib_state = CALIB_IDLE; + + /* 重新初始化BMI088以应用新的校准数据 */ + BMI088_Init(&bmi088, &cali_bmi088); + AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088)); + } + } + + /* 只有在非校准状态下才进行正常的姿态解析 */ + if (calib_state != CALIB_RUNNING) { + /* 根据设备接收到的数据进行姿态解析 */ + AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn); + + /* 根据解析出来的四元数计算欧拉角 */ + AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs); + } + + osKernelUnlock(); + + /* 创建修改后的数据副本用于发送到消息队列 */ + + /* 加速度数据：x和y互换，y取负值，全部乘10 */ + accl_modified.x = bmi088.accl.y * 10.0f; + accl_modified.y = -bmi088.accl.x * 10.0f; + accl_modified.z = bmi088.accl.z * 10.0f; + + + /* 陀螺仪数据：x和y互换，y取负值 */ + gyro_modified.x = bmi088.gyro.y; + gyro_modified.y = -bmi088.gyro.x; + gyro_modified.z = bmi088.gyro.z; + + /* 欧拉角数据：roll和pitch互换 */ + eulr_modified.yaw = eulr_to_send.yaw; + eulr_modified.pit = -eulr_to_send.pit; + eulr_modified.rol = eulr_to_send.rol; + + BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 50.0f, bmi088.temp, 0.0f, 0.0f)); /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } -} \ No newline at end of file +} +