From 4f4e485912625870007f6f2ed61f7b25c970a484 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 5 Oct 2025 03:03:27 +0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0imu?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 9 +- User/bsp/bsp_config.yaml | 114 ++++++++ User/bsp/gpio.c | 130 +++++++++ User/bsp/gpio.h | 57 ++++ User/bsp/pwm.c | 121 +++++++++ User/bsp/pwm.h | 59 +++++ User/bsp/spi.c | 181 +++++++++++++ User/bsp/spi.h | 70 +++++ User/component/component_config.yaml | 8 + User/component/filter.c | 185 +++++++++++++ User/component/filter.h | 120 +++++++++ User/component/pid.c | 158 +++++++++++ User/component/pid.h | 107 ++++++++ User/device/bmi088.c | 381 +++++++++++++++++++++++++++ User/device/bmi088.h | 81 ++++++ User/device/device.h | 5 +- User/device/device_config.yaml | 8 + User/device/dm_imu.c | 271 ------------------- User/device/dm_imu.h | 90 ------- User/module/config.c | 7 - User/module/config.h | 2 - User/task/config.yaml | 6 +- User/task/ctrl_chassis.c | 44 ++++ User/task/ctrl_lz.c | 134 ---------- User/task/imu.c | 201 +++++++------- User/task/init.c | 2 +- User/task/user_task.c | 4 +- User/task/user_task.h | 18 +- 28 files changed, 1959 insertions(+), 614 deletions(-) create mode 100644 User/bsp/gpio.c create mode 100644 User/bsp/gpio.h create mode 100644 User/bsp/pwm.c create mode 100644 User/bsp/pwm.h create mode 100644 User/bsp/spi.c create mode 100644 User/bsp/spi.h create mode 100644 User/component/filter.c create mode 100644 User/component/filter.h create mode 100644 User/component/pid.c create mode 100644 User/component/pid.h create mode 100644 User/device/bmi088.c create mode 100644 User/device/bmi088.h delete mode 100644 User/device/dm_imu.c delete mode 100644 User/device/dm_imu.h create mode 100644 User/task/ctrl_chassis.c delete mode 100644 User/task/ctrl_lz.c diff --git a/CMakeLists.txt b/CMakeLists.txt index c171d6d..4a4a3fe 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,15 +47,20 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE # Add user sources here # User/bsp sources User/bsp/can.c + User/bsp/gpio.c User/bsp/mm.c + User/bsp/pwm.c + User/bsp/spi.c User/bsp/time.c # User/component sources User/component/ahrs.c + User/component/filter.c + User/component/pid.c User/component/user_math.c # User/device sources - User/device/dm_imu.c + User/device/bmi088.c User/device/motor.c User/device/motor_lk.c User/device/motor_lz.c @@ -65,7 +70,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE # User/task sources User/task/blink.c - User/task/ctrl_lz.c + User/task/ctrl_chassis.c User/task/imu.c User/task/init.c User/task/user_task.c diff --git a/User/bsp/bsp_config.yaml b/User/bsp/bsp_config.yaml index 2c32a85..6f0ec46 100644 --- a/User/bsp/bsp_config.yaml +++ b/User/bsp/bsp_config.yaml @@ -5,7 +5,121 @@ can: - instance: CAN2 name: '2' enabled: true +gpio: + configs: + - custom_name: USER_KEY + has_exti: true + ioc_label: USER_KEY + pin: PA0-WKUP + type: EXTI + - custom_name: ACCL_CS + has_exti: false + ioc_label: ACCL_CS + pin: PA4 + type: OUTPUT + - custom_name: GYRO_CS + has_exti: false + ioc_label: GYRO_CS + pin: PB0 + type: OUTPUT + - custom_name: SPI2_CS + has_exti: false + ioc_label: SPI2_CS + pin: PB12 + type: OUTPUT + - custom_name: HW0 + has_exti: false + ioc_label: HW0 + pin: PC0 + type: INPUT + - custom_name: HW1 + has_exti: false + ioc_label: HW1 + pin: PC1 + type: INPUT + - custom_name: HW2 + has_exti: false + ioc_label: HW2 + pin: PC2 + type: INPUT + - custom_name: ACCL_INT + has_exti: true + ioc_label: ACCL_INT + pin: PC4 + type: EXTI + - custom_name: GYRO_INT + has_exti: true + ioc_label: GYRO_INT + pin: PC5 + type: EXTI + - custom_name: CMPS_INT + has_exti: true + ioc_label: CMPS_INT + pin: PG3 + type: EXTI + - custom_name: CMPS_RST + has_exti: false + ioc_label: CMPS_RST + pin: PG6 + type: OUTPUT + enabled: true mm: enabled: true +pwm: + configs: + - channel: TIM_CHANNEL_1 + custom_name: TIM8_CH1 + label: TIM8_CH1 + timer: TIM8 + - channel: TIM_CHANNEL_3 + custom_name: LASER + label: LASER + timer: TIM3 + - channel: TIM_CHANNEL_3 + custom_name: BUZZER + label: BUZZER + timer: TIM4 + - channel: TIM_CHANNEL_2 + custom_name: TIM1_CH2 + label: TIM1_CH2 + timer: TIM1 + - channel: TIM_CHANNEL_3 + custom_name: TIM1_CH3 + label: TIM1_CH3 + timer: TIM1 + - channel: TIM_CHANNEL_4 + custom_name: TIM1_CH4 + label: TIM1_CH4 + timer: TIM1 + - channel: TIM_CHANNEL_1 + custom_name: TIM1_CH1 + label: TIM1_CH1 + timer: TIM1 + - channel: TIM_CHANNEL_1 + custom_name: IMU_HEAT_PWM + label: IMU_HEAT_PWM + timer: TIM10 + - channel: TIM_CHANNEL_1 + custom_name: LED_B + label: LED_B + timer: TIM5 + - channel: TIM_CHANNEL_2 + custom_name: LED_G + label: LED_G + timer: TIM5 + - channel: TIM_CHANNEL_3 + custom_name: LED_R + label: LED_R + timer: TIM5 + - channel: TIM_CHANNEL_2 + custom_name: TIM8_CH2 + label: TIM8_CH2 + timer: TIM8 + enabled: true +spi: + devices: + - instance: SPI1 + name: BMI088 + enabled: true time: enabled: true diff --git a/User/bsp/gpio.c b/User/bsp/gpio.c new file mode 100644 index 0000000..490a5d1 --- /dev/null +++ b/User/bsp/gpio.c @@ -0,0 +1,130 @@ +/* Includes ----------------------------------------------------------------- */ +#include "bsp/gpio.h" + +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +typedef struct { + uint16_t pin; + GPIO_TypeDef *gpio; +} BSP_GPIO_MAP_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static const BSP_GPIO_MAP_t GPIO_Map[BSP_GPIO_NUM] = { + {USER_KEY_Pin, USER_KEY_GPIO_Port}, + {ACCL_CS_Pin, ACCL_CS_GPIO_Port}, + {GYRO_CS_Pin, GYRO_CS_GPIO_Port}, + {SPI2_CS_Pin, SPI2_CS_GPIO_Port}, + {HW0_Pin, HW0_GPIO_Port}, + {HW1_Pin, HW1_GPIO_Port}, + {HW2_Pin, HW2_GPIO_Port}, + {ACCL_INT_Pin, ACCL_INT_GPIO_Port}, + {GYRO_INT_Pin, GYRO_INT_GPIO_Port}, + {CMPS_INT_Pin, CMPS_INT_GPIO_Port}, + {CMPS_RST_Pin, CMPS_RST_GPIO_Port}, +}; + +static void (*GPIO_Callback[16])(void); + +/* Private function -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { + for (uint8_t i = 0; i < 16; i++) { + if (GPIO_Pin & (1 << i)) { + if (GPIO_Callback[i]) { + GPIO_Callback[i](); + } + } + } +} + +/* Exported functions ------------------------------------------------------- */ +int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, void (*callback)(void)) { + if (callback == NULL) return BSP_ERR_NULL; + if (gpio >= BSP_GPIO_NUM) return BSP_ERR; + + // 从GPIO映射中获取对应的pin值 + uint16_t pin = GPIO_Map[gpio].pin; + + for (uint8_t i = 0; i < 16; i++) { + if (pin & (1 << i)) { + GPIO_Callback[i] = callback; + break; + } + } + return BSP_OK; +} + +int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) { + switch (gpio) { + case BSP_GPIO_USER_KEY: + HAL_NVIC_EnableIRQ(USER_KEY_EXTI_IRQn); + break; + case BSP_GPIO_ACCL_INT: + HAL_NVIC_EnableIRQ(ACCL_INT_EXTI_IRQn); + break; + case BSP_GPIO_GYRO_INT: + HAL_NVIC_EnableIRQ(GYRO_INT_EXTI_IRQn); + break; + case BSP_GPIO_CMPS_INT: + HAL_NVIC_EnableIRQ(CMPS_INT_EXTI_IRQn); + break; + default: + return BSP_ERR; + } + return BSP_OK; +} + +int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) { + switch (gpio) { + case BSP_GPIO_USER_KEY: + HAL_NVIC_DisableIRQ(USER_KEY_EXTI_IRQn); + break; + case BSP_GPIO_ACCL_INT: + HAL_NVIC_DisableIRQ(ACCL_INT_EXTI_IRQn); + break; + case BSP_GPIO_GYRO_INT: + HAL_NVIC_DisableIRQ(GYRO_INT_EXTI_IRQn); + break; + case BSP_GPIO_CMPS_INT: + HAL_NVIC_DisableIRQ(CMPS_INT_EXTI_IRQn); + break; + default: + return BSP_ERR; + } + return BSP_OK; +} +int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value){ + if (gpio >= BSP_GPIO_NUM) return BSP_ERR; + HAL_GPIO_WritePin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin, value); + return BSP_OK; +} + +int8_t BSP_GPIO_TogglePin(BSP_GPIO_t gpio){ + if (gpio >= BSP_GPIO_NUM) return BSP_ERR; + HAL_GPIO_TogglePin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin); + return BSP_OK; +} + +bool BSP_GPIO_ReadPin(BSP_GPIO_t gpio){ + if (gpio >= BSP_GPIO_NUM) return false; + return HAL_GPIO_ReadPin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin) == GPIO_PIN_SET; +} \ No newline at end of file diff --git a/User/bsp/gpio.h b/User/bsp/gpio.h new file mode 100644 index 0000000..4d9707b --- /dev/null +++ b/User/bsp/gpio.h @@ -0,0 +1,57 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include + +#include "bsp/bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ +typedef enum { + BSP_GPIO_USER_KEY, + BSP_GPIO_ACCL_CS, + BSP_GPIO_GYRO_CS, + BSP_GPIO_SPI2_CS, + BSP_GPIO_HW0, + BSP_GPIO_HW1, + BSP_GPIO_HW2, + BSP_GPIO_ACCL_INT, + BSP_GPIO_GYRO_INT, + BSP_GPIO_CMPS_INT, + BSP_GPIO_CMPS_RST, + BSP_GPIO_NUM, + BSP_GPIO_ERR, +} BSP_GPIO_t; + +/* Exported functions prototypes -------------------------------------------- */ +int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, void (*callback)(void)); + +int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio); +int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio); + +int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value); +int8_t BSP_GPIO_TogglePin(BSP_GPIO_t gpio); + +bool BSP_GPIO_ReadPin(BSP_GPIO_t gpio); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/User/bsp/pwm.c b/User/bsp/pwm.c new file mode 100644 index 0000000..6b0b96c --- /dev/null +++ b/User/bsp/pwm.c @@ -0,0 +1,121 @@ +/* Includes ----------------------------------------------------------------- */ +#include "tim.h" +#include "bsp/pwm.h" +#include "bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +typedef struct { + TIM_HandleTypeDef *tim; + uint16_t channel; +} BSP_PWM_Config_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static const BSP_PWM_Config_t PWM_Map[BSP_PWM_NUM] = { + {&htim8, TIM_CHANNEL_1}, + {&htim3, TIM_CHANNEL_3}, + {&htim4, TIM_CHANNEL_3}, + {&htim1, TIM_CHANNEL_2}, + {&htim1, TIM_CHANNEL_3}, + {&htim1, TIM_CHANNEL_4}, + {&htim1, TIM_CHANNEL_1}, + {&htim10, TIM_CHANNEL_1}, + {&htim5, TIM_CHANNEL_1}, + {&htim5, TIM_CHANNEL_2}, + {&htim5, TIM_CHANNEL_3}, + {&htim8, TIM_CHANNEL_2}, +}; + +/* Private function -------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ + +int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Start(PWM_Map[ch].tim, PWM_Map[ch].channel); + return BSP_OK; +} + +int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + if (duty_cycle > 1.0f) { + duty_cycle = 1.0f; + } + if (duty_cycle < 0.0f) { + duty_cycle = 0.0f; + } + // 获取ARR值（周期值） + uint32_t arr = __HAL_TIM_GET_AUTORELOAD(PWM_Map[ch].tim); + + // 计算比较值：CCR = duty_cycle * (ARR + 1) + uint32_t ccr = (uint32_t)(duty_cycle * (arr + 1)); + + __HAL_TIM_SET_COMPARE(PWM_Map[ch].tim, PWM_Map[ch].channel, ccr); + + return BSP_OK; +} + +int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + uint32_t timer_clock = HAL_RCC_GetPCLK1Freq(); // Get the timer clock frequency + uint32_t prescaler = PWM_Map[ch].tim->Init.Prescaler; + uint32_t period = (timer_clock / (prescaler + 1)) / freq - 1; + + if (period > UINT16_MAX) { + return BSP_ERR; // Frequency too low + } + __HAL_TIM_SET_AUTORELOAD(PWM_Map[ch].tim, period); + + return BSP_OK; +} + +int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Stop(PWM_Map[ch].tim, PWM_Map[ch].channel); + return BSP_OK; +} + +uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + return PWM_Map[ch].tim->Init.AutoReloadPreload; +} + +TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch) { + return PWM_Map[ch].tim; +} + + +uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + return PWM_Map[ch].channel; +} + +int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Start_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel, pData, Length); + return BSP_OK; +} + +int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Stop_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel); + return BSP_OK; +} \ No newline at end of file diff --git a/User/bsp/pwm.h b/User/bsp/pwm.h new file mode 100644 index 0000000..94cb40c --- /dev/null +++ b/User/bsp/pwm.h @@ -0,0 +1,59 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include "tim.h" +#include "bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ +/* PWM通道 */ +typedef enum { + BSP_PWM_TIM8_CH1, + BSP_PWM_LASER, + BSP_PWM_BUZZER, + BSP_PWM_TIM1_CH2, + BSP_PWM_TIM1_CH3, + BSP_PWM_TIM1_CH4, + BSP_PWM_TIM1_CH1, + BSP_PWM_IMU_HEAT_PWM, + BSP_PWM_LED_B, + BSP_PWM_LED_G, + BSP_PWM_LED_R, + BSP_PWM_TIM8_CH2, + BSP_PWM_NUM, + BSP_PWM_ERR, +} BSP_PWM_Channel_t; + +/* Exported functions prototypes -------------------------------------------- */ +int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch); +int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle); //设置占空比，范围0.0~1.0 +int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq); //设置频率，单位Hz +int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch); +uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch); +uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch); +TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch); +int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length); +int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/User/bsp/spi.c b/User/bsp/spi.c new file mode 100644 index 0000000..d8ac00e --- /dev/null +++ b/User/bsp/spi.c @@ -0,0 +1,181 @@ +/* Includes ----------------------------------------------------------------- */ +#include +#include "bsp/spi.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static void (*SPI_Callback[BSP_SPI_NUM][BSP_SPI_CB_NUM])(void); + +/* Private function -------------------------------------------------------- */ +static BSP_SPI_t SPI_Get(SPI_HandleTypeDef *hspi) { + if (hspi->Instance == SPI1) + return BSP_SPI_BMI088; + else + return BSP_SPI_ERR; +} + +void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[bsp_spi][BSP_SPI_TX_CPLT_CB]) { + SPI_Callback[bsp_spi][BSP_SPI_TX_CPLT_CB](); + } + } +} + +void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_CPLT_CB](); + } +} + +void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_CPLT_CB](); + } +} + +void HAL_SPI_TxHalfCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_HALF_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_HALF_CPLT_CB](); + } +} + +void HAL_SPI_RxHalfCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_HALF_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_HALF_CPLT_CB](); + } +} + +void HAL_SPI_TxRxHalfCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_HALF_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_HALF_CPLT_CB](); + } +} + +void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_ERROR_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_ERROR_CB](); + } +} + +void HAL_SPI_AbortCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_ABORT_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_ABORT_CPLT_CB](); + } +} + +/* Exported functions ------------------------------------------------------- */ +SPI_HandleTypeDef *BSP_SPI_GetHandle(BSP_SPI_t spi) { + switch (spi) { + case BSP_SPI_BMI088: + return &hspi1; + default: + return NULL; + } +} + +int8_t BSP_SPI_RegisterCallback(BSP_SPI_t spi, BSP_SPI_Callback_t type, + void (*callback)(void)) { + if (callback == NULL) return BSP_ERR_NULL; + SPI_Callback[spi][type] = callback; + return BSP_OK; +} + +int8_t BSP_SPI_Transmit(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi); + if (hspi == NULL) return BSP_ERR; + + if (dma) { + return HAL_SPI_Transmit_DMA(hspi, data, size)!= HAL_OK;; + } else { + return HAL_SPI_Transmit(hspi, data, size, 20)!= HAL_OK;; + } +} + +int8_t BSP_SPI_Receive(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi); + if (hspi == NULL) return BSP_ERR; + + if (dma) { + return HAL_SPI_Receive_DMA(hspi, data, size)!= HAL_OK;; + } else { + return HAL_SPI_Receive(hspi, data, size, 20)!= HAL_OK;; + } +} + +int8_t BSP_SPI_TransmitReceive(BSP_SPI_t spi, uint8_t *txData, uint8_t *rxData, + uint16_t size, bool dma) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi); + if (hspi == NULL) return BSP_ERR; + + if (dma) { + return HAL_SPI_TransmitReceive_DMA(hspi, txData, rxData, size)!= HAL_OK;; + } else { + return HAL_SPI_TransmitReceive(hspi, txData, rxData, size, 20)!= HAL_OK;; + } +} + +uint8_t BSP_SPI_MemReadByte(BSP_SPI_t spi, uint8_t reg) { + if (spi >= BSP_SPI_NUM) return 0xFF; + uint8_t tmp[2] = {reg | 0x80, 0x00}; + BSP_SPI_TransmitReceive(spi, tmp, tmp, 2u, true); + return tmp[1]; +} + +int8_t BSP_SPI_MemWriteByte(BSP_SPI_t spi, uint8_t reg, uint8_t data) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + uint8_t tmp[2] = {reg & 0x7f, data}; + return BSP_SPI_Transmit(spi, tmp, 2u, true); +} + +int8_t BSP_SPI_MemRead(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + if (data == NULL || size == 0) return BSP_ERR_NULL; + reg = reg | 0x80; + BSP_SPI_Transmit(spi, ®, 1u, true); + return BSP_SPI_Receive(spi, data, size, true); +} + +int8_t BSP_SPI_MemWrite(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + if (data == NULL || size == 0) return BSP_ERR_NULL; + reg = reg & 0x7f; + BSP_SPI_Transmit(spi, ®, 1u, true); + return BSP_SPI_Transmit(spi, data, size, true); +} + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/User/bsp/spi.h b/User/bsp/spi.h new file mode 100644 index 0000000..2abb0c6 --- /dev/null +++ b/User/bsp/spi.h @@ -0,0 +1,70 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include +#include + +#include "bsp/bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ + +/* 要添加使用SPI的新设备，需要先在此添加对应的枚举值 */ + +/* SPI实体枚举，与设备对应 */ +typedef enum { + BSP_SPI_BMI088, + BSP_SPI_NUM, + BSP_SPI_ERR, +} BSP_SPI_t; + +/* SPI支持的中断回调函数类型，具体参考HAL中定义 */ +typedef enum { + BSP_SPI_TX_CPLT_CB, + BSP_SPI_RX_CPLT_CB, + BSP_SPI_TX_RX_CPLT_CB, + BSP_SPI_TX_HALF_CPLT_CB, + BSP_SPI_RX_HALF_CPLT_CB, + BSP_SPI_TX_RX_HALF_CPLT_CB, + BSP_SPI_ERROR_CB, + BSP_SPI_ABORT_CPLT_CB, + BSP_SPI_CB_NUM, +} BSP_SPI_Callback_t; + +/* Exported functions prototypes -------------------------------------------- */ +SPI_HandleTypeDef *BSP_SPI_GetHandle(BSP_SPI_t spi); +int8_t BSP_SPI_RegisterCallback(BSP_SPI_t spi, BSP_SPI_Callback_t type, + void (*callback)(void)); + + +int8_t BSP_SPI_Transmit(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma); +int8_t BSP_SPI_Receive(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma); +int8_t BSP_SPI_TransmitReceive(BSP_SPI_t spi, uint8_t *txData, uint8_t *rxData, + uint16_t size, bool dma); + +uint8_t BSP_SPI_MemReadByte(BSP_SPI_t spi, uint8_t reg); +int8_t BSP_SPI_MemWriteByte(BSP_SPI_t spi, uint8_t reg, uint8_t data); +int8_t BSP_SPI_MemRead(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size); +int8_t BSP_SPI_MemWrite(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/component/component_config.yaml b/User/component/component_config.yaml index 4caf761..0d07966 100644 --- a/User/component/component_config.yaml +++ b/User/component/component_config.yaml @@ -2,6 +2,14 @@ ahrs: dependencies: - component/user_math.h enabled: true +filter: + dependencies: + - component/ahrs + enabled: true +pid: + dependencies: + - component/filter + enabled: true user_math: dependencies: [] enabled: true diff --git a/User/component/filter.c b/User/component/filter.c new file mode 100644 index 0000000..5375b8e --- /dev/null +++ b/User/component/filter.c @@ -0,0 +1,185 @@ +/* + 各类滤波器。 +*/ + +#include "filter.h" + +#include "user_math.h" + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param cutoff_freq 截止频率 + */ +void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq, + float cutoff_freq) { + if (f == NULL) return; + + f->cutoff_freq = cutoff_freq; + + f->delay_element_1 = 0.0f; + f->delay_element_2 = 0.0f; + + if (f->cutoff_freq <= 0.0f) { + /* no filtering */ + f->b0 = 1.0f; + f->b1 = 0.0f; + f->b2 = 0.0f; + + f->a1 = 0.0f; + f->a2 = 0.0f; + + return; + } + const float fr = sample_freq / f->cutoff_freq; + const float ohm = tanf(M_PI / fr); + const float c = 1.0f + 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm; + + f->b0 = ohm * ohm / c; + f->b1 = 2.0f * f->b0; + f->b2 = f->b0; + + f->a1 = 2.0f * (ohm * ohm - 1.0f) / c; + f->a2 = (1.0f - 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm) / c; +} + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample) { + if (f == NULL) return 0.0f; + + /* do the filtering */ + float delay_element_0 = + sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2; + + if (isinf(delay_element_0)) { + /* don't allow bad values to propagate via the filter */ + delay_element_0 = sample; + } + + const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 + + f->delay_element_2 * f->b2; + + f->delay_element_2 = f->delay_element_1; + f->delay_element_1 = delay_element_0; + + /* return the value. Should be no need to check limits */ + return output; +} + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample) { + if (f == NULL) return 0.0f; + + const float dval = sample / (f->b0 + f->b1 + f->b2); + + if (isfinite(dval)) { + f->delay_element_1 = dval; + f->delay_element_2 = dval; + + } else { + f->delay_element_1 = sample; + f->delay_element_2 = sample; + } + + return LowPassFilter2p_Apply(f, sample); +} + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param notch_freq 中心频率 + * @param bandwidth 带宽 + */ +void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq, + float bandwidth) { + if (f == NULL) return; + + f->notch_freq = notch_freq; + f->bandwidth = bandwidth; + + f->delay_element_1 = 0.0f; + f->delay_element_2 = 0.0f; + + if (notch_freq <= 0.0f) { + /* no filtering */ + f->b0 = 1.0f; + f->b1 = 0.0f; + f->b2 = 0.0f; + + f->a1 = 0.0f; + f->a2 = 0.0f; + + return; + } + + const float alpha = tanf(M_PI * bandwidth / sample_freq); + const float beta = -cosf(M_2PI * notch_freq / sample_freq); + const float a0_inv = 1.0f / (alpha + 1.0f); + + f->b0 = a0_inv; + f->b1 = 2.0f * beta * a0_inv; + f->b2 = a0_inv; + + f->a1 = f->b1; + f->a2 = (1.0f - alpha) * a0_inv; +} + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +inline float NotchFilter_Apply(NotchFilter_t *f, float sample) { + if (f == NULL) return 0.0f; + + /* Direct Form II implementation */ + const float delay_element_0 = + sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2; + const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 + + f->delay_element_2 * f->b2; + + f->delay_element_2 = f->delay_element_1; + f->delay_element_1 = delay_element_0; + + return output; +} + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float NotchFilter_Reset(NotchFilter_t *f, float sample) { + if (f == NULL) return 0.0f; + + float dval = sample; + + if (fabsf(f->b0 + f->b1 + f->b2) > FLT_EPSILON) { + dval = dval / (f->b0 + f->b1 + f->b2); + } + + f->delay_element_1 = dval; + f->delay_element_2 = dval; + + return NotchFilter_Apply(f, sample); +} diff --git a/User/component/filter.h b/User/component/filter.h new file mode 100644 index 0000000..ae2b072 --- /dev/null +++ b/User/component/filter.h @@ -0,0 +1,120 @@ +/* + 各类滤波器。 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* 二阶低通滤波器 */ +typedef struct { + float cutoff_freq; /* 截止频率 */ + + float a1; + float a2; + + float b0; + float b1; + float b2; + + float delay_element_1; + float delay_element_2; + +} LowPassFilter2p_t; + +/* 带阻滤波器 */ +typedef struct { + float notch_freq; /* 阻止频率 */ + float bandwidth; /* 带宽 */ + + float a1; + float a2; + + float b0; + float b1; + float b2; + float delay_element_1; + float delay_element_2; + +} NotchFilter_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param cutoff_freq 截止频率 + */ +void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq, + float cutoff_freq); + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample); + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample); + +/** + * @brief 初始化滤波器 + * + * @param f 滤波器 + * @param sample_freq 采样频率 + * @param notch_freq 中心频率 + * @param bandwidth 带宽 + */ +void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq, + float bandwidth); + +/** + * @brief 施加一次滤波计算 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float NotchFilter_Apply(NotchFilter_t *f, float sample); + +/** + * @brief 重置滤波器 + * + * @param f 滤波器 + * @param sample 采样的值 + * @return float 滤波后的值 + */ +float NotchFilter_Reset(NotchFilter_t *f, float sample); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/component/pid.c b/User/component/pid.c new file mode 100644 index 0000000..0a3c7d4 --- /dev/null +++ b/User/component/pid.c @@ -0,0 +1,158 @@ +/* + Modified from + https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.cpp + + 参考资料： + https://github.com/PX4/Firmware/issues/12362 + https://dev.px4.io/master/en/flight_stack/controller_diagrams.html + https://docs.px4.io/master/en/config_mc/pid_tuning_guide_multicopter.html#standard_form + https://www.controleng.com/articles/not-all-pid-controllers-are-the-same/ + https://en.wikipedia.org/wiki/PID_controller + http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/ +*/ + +#include "pid.h" + +#define SIGMA 0.000001f + +/** + * @brief 初始化PID + * + * @param pid PID结构体 + * @param mode PID模式 + * @param sample_freq 采样频率 + * @param param PID参数 + * @return int8_t 0对应没有错误 + */ +int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq, + const KPID_Params_t *param) { + if (pid == NULL) return -1; + + if (!isfinite(param->p)) return -1; + if (!isfinite(param->i)) return -1; + if (!isfinite(param->d)) return -1; + if (!isfinite(param->i_limit)) return -1; + if (!isfinite(param->out_limit)) return -1; + pid->param = param; + + float dt_min = 1.0f / sample_freq; + if (isfinite(dt_min)) + pid->dt_min = dt_min; + else + return -1; + + LowPassFilter2p_Init(&(pid->dfilter), sample_freq, pid->param->d_cutoff_freq); + + pid->mode = mode; + PID_Reset(pid); + return 0; +} + +/** + * @brief PID计算 + * + * @param pid PID结构体 + * @param sp 设定值 + * @param fb 反馈值 + * @param fb_dot 反馈值微分 + * @param dt 间隔时间 + * @return float 计算的输出 + */ +float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt) { + if (!isfinite(sp) || !isfinite(fb) || !isfinite(fb_dot) || !isfinite(dt)) { + return pid->last.out; + } + + /* 计算误差值 */ + const float err = CircleError(sp, fb, pid->param->range); + + /* 计算P项 */ + const float k_err = err * pid->param->k; + + /* 计算D项 */ + const float k_fb = pid->param->k * fb; + const float filtered_k_fb = LowPassFilter2p_Apply(&(pid->dfilter), k_fb); + + float d; + switch (pid->mode) { + case KPID_MODE_CALC_D: + /* 通过fb计算D，避免了由于sp变化导致err突变的问题 */ + /* 当sp不变时，err的微分等于负的fb的微分 */ + d = (filtered_k_fb - pid->last.k_fb) / fmaxf(dt, pid->dt_min); + break; + + case KPID_MODE_SET_D: + d = fb_dot; + break; + + case KPID_MODE_NO_D: + d = 0.0f; + break; + } + pid->last.err = err; + pid->last.k_fb = filtered_k_fb; + + if (!isfinite(d)) d = 0.0f; + + /* 计算PD输出 */ + float output = (k_err * pid->param->p) - (d * pid->param->d); + + /* 计算I项 */ + const float i = pid->i + (k_err * dt); + const float i_out = i * pid->param->i; + + if (pid->param->i > SIGMA) { + /* 检查是否饱和 */ + if (isfinite(i)) { + if ((fabsf(output + i_out) <= pid->param->out_limit) && + (fabsf(i) <= pid->param->i_limit)) { + /* 未饱和，使用新积分 */ + pid->i = i; + } + } + } + + /* 计算PID输出 */ + output += i_out; + + /* 限制输出 */ + if (isfinite(output)) { + if (pid->param->out_limit > SIGMA) { + output = AbsClip(output, pid->param->out_limit); + } + pid->last.out = output; + } + return pid->last.out; +} + +/** + * @brief 重置微分项 + * + * @param pid PID结构体 + * @return int8_t 0对应没有错误 + */ +int8_t PID_ResetIntegral(KPID_t *pid) { + if (pid == NULL) return -1; + + pid->i = 0.0f; + + return 0; +} + +/** + * @brief 重置PID + * + * @param pid PID结构体 + * @return int8_t 0对应没有错误 + */ +int8_t PID_Reset(KPID_t *pid) { + if (pid == NULL) return -1; + + pid->i = 0.0f; + pid->last.err = 0.0f; + pid->last.k_fb = 0.0f; + pid->last.out = 0.0f; + LowPassFilter2p_Reset(&(pid->dfilter), 0.0f); + + return 0; +} diff --git a/User/component/pid.h b/User/component/pid.h new file mode 100644 index 0000000..4b451eb --- /dev/null +++ b/User/component/pid.h @@ -0,0 +1,107 @@ +/* + Modified from + https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.h +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#include "filter.h" +#include "user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* PID模式 */ +typedef enum { + KPID_MODE_NO_D = 0, /* 不使用微分项，PI控制器 */ + KPID_MODE_CALC_D, /* 根据反馈的值计算离散微分，忽略PID_Calc中的fb_dot */ + KPID_MODE_SET_D /* 直接提供微分值，PID_Calc中的fb_dot将被使用，(Gyros) */ +} KPID_Mode_t; + +/* PID参数 */ +typedef struct { + float k; /* 控制器增益，设置为1用于并行模式 */ + float p; /* 比例项增益，设置为1用于标准形式 */ + float i; /* 积分项增益 */ + float d; /* 微分项增益 */ + float i_limit; /* 积分项上限 */ + float out_limit; /* 输出绝对值限制 */ + float d_cutoff_freq; /* D项低通截止频率 */ + float range; /* 计算循环误差时使用，大于0时启用 */ +} KPID_Params_t; + +/* PID主结构体 */ +typedef struct { + KPID_Mode_t mode; + const KPID_Params_t *param; + + float dt_min; /* 最小PID_Calc调用间隔 */ + float i; /* 积分 */ + + struct { + float err; /* 上次误差 */ + float k_fb; /* 上次反馈值 */ + float out; /* 上次输出 */ + } last; + + LowPassFilter2p_t dfilter; /* D项低通滤波器 */ +} KPID_t; + +/** + * @brief 初始化PID + * + * @param pid PID结构体 + * @param mode PID模式 + * @param sample_freq 采样频率 + * @param param PID参数 + * @return int8_t 0对应没有错误 + */ +int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq, + const KPID_Params_t *param); + +/** + * @brief PID计算 + * + * @param pid PID结构体 + * @param sp 设定值 + * @param fb 反馈值 + * @param fb_dot 反馈值微分 + * @param dt 间隔时间 + * @return float 计算的输出 + */ +float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt); + +/** + * @brief 重置微分项 + * + * @param pid PID结构体 + * @return int8_t 0对应没有错误 + */ +int8_t PID_ResetIntegral(KPID_t *pid); + +/** + * @brief 重置PID + * + * @param pid PID结构体 + * @return int8_t 0对应没有错误 + */ +int8_t PID_Reset(KPID_t *pid); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/device/bmi088.c b/User/device/bmi088.c new file mode 100644 index 0000000..0f4c154 --- /dev/null +++ b/User/device/bmi088.c @@ -0,0 +1,381 @@ +/* + BMI088 陀螺仪+加速度计传感器。 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "bmi088.h" + +#include +#include +#include +#include + +#include "bsp/time.h" +#include "bsp/gpio.h" +#include "bsp/spi.h" +#include "component/user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +#define BMI088_REG_ACCL_CHIP_ID (0x00) +#define BMI088_REG_ACCL_ERR (0x02) +#define BMI088_REG_ACCL_STATUS (0x03) +#define BMI088_REG_ACCL_X_LSB (0x12) +#define BMI088_REG_ACCL_X_MSB (0x13) +#define BMI088_REG_ACCL_Y_LSB (0x14) +#define BMI088_REG_ACCL_Y_MSB (0x15) +#define BMI088_REG_ACCL_Z_LSB (0x16) +#define BMI088_REG_ACCL_Z_MSB (0x17) +#define BMI088_REG_ACCL_SENSORTIME_0 (0x18) +#define BMI088_REG_ACCL_SENSORTIME_1 (0x19) +#define BMI088_REG_ACCL_SENSORTIME_2 (0x1A) +#define BMI088_REG_ACCL_INT_STAT_1 (0x1D) +#define BMI088_REG_ACCL_TEMP_MSB (0x22) +#define BMI088_REG_ACCL_TEMP_LSB (0x23) +#define BMI088_REG_ACCL_CONF (0x40) +#define BMI088_REG_ACCL_RANGE (0x41) +#define BMI088_REG_ACCL_INT1_IO_CONF (0x53) +#define BMI088_REG_ACCL_INT2_IO_CONF (0x54) +#define BMI088_REG_ACCL_INT1_INT2_MAP_DATA (0x58) +#define BMI088_REG_ACCL_SELF_TEST (0x6D) +#define BMI088_REG_ACCL_PWR_CONF (0x7C) +#define BMI088_REG_ACCL_PWR_CTRL (0x7D) +#define BMI088_REG_ACCL_SOFTRESET (0x7E) + +#define BMI088_REG_GYRO_CHIP_ID (0x00) +#define BMI088_REG_GYRO_X_LSB (0x02) +#define BMI088_REG_GYRO_X_MSB (0x03) +#define BMI088_REG_GYRO_Y_LSB (0x04) +#define BMI088_REG_GYRO_Y_MSB (0x05) +#define BMI088_REG_GYRO_Z_LSB (0x06) +#define BMI088_REG_GYRO_Z_MSB (0x07) +#define BMI088_REG_GYRO_INT_STAT_1 (0x0A) +#define BMI088_REG_GYRO_RANGE (0x0F) +#define BMI088_REG_GYRO_BANDWIDTH (0x10) +#define BMI088_REG_GYRO_LPM1 (0x11) +#define BMI088_REG_GYRO_SOFTRESET (0x14) +#define BMI088_REG_GYRO_INT_CTRL (0x15) +#define BMI088_REG_GYRO_INT3_INT4_IO_CONF (0x16) +#define BMI088_REG_GYRO_INT3_INT4_IO_MAP (0x18) +#define BMI088_REG_GYRO_SELF_TEST (0x3C) + +#define BMI088_CHIP_ID_ACCL (0x1E) +#define BMI088_CHIP_ID_GYRO (0x0F) + +#define BMI088_LEN_RX_BUFF (19) +/* Private macro ------------------------------------------------------------ */ +#define BMI088_ACCL_NSS_SET() \ + BSP_GPIO_WritePin(BSP_GPIO_ACCL_CS, GPIO_PIN_SET) +#define BMI088_ACCL_NSS_RESET() \ + BSP_GPIO_WritePin(BSP_GPIO_ACCL_CS, GPIO_PIN_RESET) + +#define BMI088_GYRO_NSS_SET() \ + BSP_GPIO_WritePin(BSP_GPIO_GYRO_CS, GPIO_PIN_SET) +#define BMI088_GYRO_NSS_RESET() \ + BSP_GPIO_WritePin(BSP_GPIO_GYRO_CS, GPIO_PIN_RESET) + +/* Private typedef ---------------------------------------------------------- */ +typedef enum { + BMI_ACCL, + BMI_GYRO, +} BMI_Device_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static uint8_t buffer[2]; +static uint8_t bmi088_rxbuf[BMI088_LEN_RX_BUFF]; + +static osThreadId_t thread_alert; +static bool inited = false; + +/* Private function -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +static void BMI_WriteSingle(BMI_Device_t dv, uint8_t reg, uint8_t data) { + buffer[0] = (reg & 0x7f); + buffer[1] = data; + + BSP_TIME_Delay(1); + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_RESET(); + break; + + case BMI_GYRO: + BMI088_GYRO_NSS_RESET(); + break; + } + + BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 2u, false); + + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_SET(); + break; + + case BMI_GYRO: + BMI088_GYRO_NSS_SET(); + break; + } +} + +static uint8_t BMI_ReadSingle(BMI_Device_t dv, uint8_t reg) { + BSP_TIME_Delay(1); + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_RESET(); + break; + + case BMI_GYRO: + BMI088_GYRO_NSS_RESET(); + break; + } + buffer[0] = (uint8_t)(reg | 0x80); + BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 1u, false); + BSP_SPI_Receive(BSP_SPI_BMI088, buffer, 2u, false); + + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_SET(); + return buffer[1]; + + case BMI_GYRO: + BMI088_GYRO_NSS_SET(); + return buffer[0]; + } +} + +static void BMI_Read(BMI_Device_t dv, uint8_t reg, uint8_t *data, uint8_t len) { + if (data == NULL) return; + + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_RESET(); + break; + + case BMI_GYRO: + BMI088_GYRO_NSS_RESET(); + break; + } + buffer[0] = (uint8_t)(reg | 0x80); + BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 1u, false); + BSP_SPI_Receive(BSP_SPI_BMI088, data, len, true); +} + +static void BMI088_RxCpltCallback(void) { + if (BSP_GPIO_ReadPin(BSP_GPIO_ACCL_CS) == GPIO_PIN_RESET) { + BMI088_ACCL_NSS_SET(); + osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_RAW_REDY); + } + if (BSP_GPIO_ReadPin(BSP_GPIO_GYRO_CS) == GPIO_PIN_RESET) { + BMI088_GYRO_NSS_SET(); + osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_RAW_REDY); + } +} + +static void BMI088_AcclIntCallback(void) { + osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_NEW_DATA); +} + +static void BMI088_GyroIntCallback(void) { + osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_NEW_DATA); +} + +/* Exported functions ------------------------------------------------------- */ +int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali) { + if (bmi088 == NULL) return DEVICE_ERR_NULL; + if (cali == NULL) return DEVICE_ERR_NULL; + if (inited) return DEVICE_ERR_INITED; + if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL; + + bmi088->cali = cali; + + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_SOFTRESET, 0xB6); + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_SOFTRESET, 0xB6); + BSP_TIME_Delay(30); + + /* Switch accl to SPI mode. */ + BMI_ReadSingle(BMI_ACCL, BMI088_CHIP_ID_ACCL); + + if (BMI_ReadSingle(BMI_ACCL, BMI088_REG_ACCL_CHIP_ID) != BMI088_CHIP_ID_ACCL) + return DEVICE_ERR_NO_DEV; + + if (BMI_ReadSingle(BMI_GYRO, BMI088_REG_GYRO_CHIP_ID) != BMI088_CHIP_ID_GYRO) + return DEVICE_ERR_NO_DEV; + + BSP_GPIO_DisableIRQ(BSP_GPIO_ACCL_INT); + BSP_GPIO_DisableIRQ(BSP_GPIO_GYRO_INT); + + BSP_SPI_RegisterCallback(BSP_SPI_BMI088, BSP_SPI_RX_CPLT_CB, + BMI088_RxCpltCallback); + BSP_GPIO_RegisterCallback(BSP_GPIO_ACCL_INT, BMI088_AcclIntCallback); + BSP_GPIO_RegisterCallback(BSP_GPIO_GYRO_INT, BMI088_GyroIntCallback); + + /* Accl init. */ + /* Filter setting: Normal. */ + /* ODR: 0xAB: 800Hz. 0xAA: 400Hz. 0xA9: 200Hz. 0xA8: 100Hz. 0xA6: 25Hz. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_CONF, 0xAA); + + /* 0x00: +-3G. 0x01: +-6G. 0x02: +-12G. 0x03: +-24G. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_RANGE, 0x01); + + /* INT1 as output. Push-pull. Active low. Output. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_IO_CONF, 0x08); + + /* Map data ready interrupt to INT1. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_INT2_MAP_DATA, 0x04); + + /* Turn on accl. Now we can read data. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_PWR_CTRL, 0x04); + BSP_TIME_Delay(50); + + /* Gyro init. */ + /* 0x00: +-2000. 0x01: +-1000. 0x02: +-500. 0x03: +-250. 0x04: +-125. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_RANGE, 0x01); + + /* Filter bw: 47Hz. */ + /* ODR: 0x02: 1000Hz. 0x03: 400Hz. 0x06: 200Hz. 0x07: 100Hz. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_BANDWIDTH, 0x03); + + /* INT3 and INT4 as output. Push-pull. Active low. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_CONF, 0x00); + + /* Map data ready interrupt to INT3. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_MAP, 0x01); + + /* Enable new data interrupt. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT_CTRL, 0x80); + + BSP_TIME_Delay(10); + + inited = true; + + BSP_GPIO_EnableIRQ(BSP_GPIO_ACCL_INT); + BSP_GPIO_EnableIRQ(BSP_GPIO_GYRO_INT); + return DEVICE_OK; +} + +bool BMI088_GyroStable(AHRS_Gyro_t *gyro) { + return ((gyro->x < 0.03f) && (gyro->y < 0.03f) && (gyro->z < 0.03f)); +} + +uint32_t BMI088_WaitNew() { + return osThreadFlagsWait( + SIGNAL_BMI088_ACCL_NEW_DATA | SIGNAL_BMI088_GYRO_NEW_DATA, osFlagsWaitAll, + osWaitForever); +} + +int8_t BMI088_AcclStartDmaRecv() { + BMI_Read(BMI_ACCL, BMI088_REG_ACCL_X_LSB, bmi088_rxbuf, BMI088_LEN_RX_BUFF); + return DEVICE_OK; +} + +uint32_t BMI088_AcclWaitDmaCplt() { + return osThreadFlagsWait(SIGNAL_BMI088_ACCL_RAW_REDY, osFlagsWaitAll, + osWaitForever); +} + +int8_t BMI088_GyroStartDmaRecv() { + BMI_Read(BMI_GYRO, BMI088_REG_GYRO_X_LSB, bmi088_rxbuf + 7, 6u); + return DEVICE_OK; +} + +uint32_t BMI088_GyroWaitDmaCplt() { + return osThreadFlagsWait(SIGNAL_BMI088_GYRO_RAW_REDY, osFlagsWaitAll, + osWaitForever); +} + +int8_t BMI088_ParseAccl(BMI088_t *bmi088) { + if (bmi088 == NULL) return DEVICE_ERR_NULL; + +#if 1 + int16_t raw_x, raw_y, raw_z; + memcpy(&raw_x, bmi088_rxbuf + 1, sizeof(raw_x)); + memcpy(&raw_y, bmi088_rxbuf + 3, sizeof(raw_y)); + memcpy(&raw_z, bmi088_rxbuf + 5, sizeof(raw_z)); + + bmi088->accl.x = (float)raw_x; + bmi088->accl.y = (float)raw_y; + bmi088->accl.z = (float)raw_z; + +#else + const int16_t *praw_x = (int16_t *)(bmi088_rxbuf + 1); + const int16_t *praw_y = (int16_t *)(bmi088_rxbuf + 3); + const int16_t *praw_z = (int16_t *)(bmi088_rxbuf + 5); + + bmi088->accl.x = (float)*praw_x; + bmi088->accl.y = (float)*praw_y; + bmi088->accl.z = (float)*praw_z; + +#endif + + /* 3G: 10920. 6G: 5460. 12G: 2730. 24G: 1365. */ + bmi088->accl.x /= 5460.0f; + bmi088->accl.y /= 5460.0f; + bmi088->accl.z /= 5460.0f; + + int16_t raw_temp = + (uint16_t)((bmi088_rxbuf[17] << 3) | (bmi088_rxbuf[18] >> 5)); + + if (raw_temp > 1023) raw_temp -= 2048; + + bmi088->temp = (float)raw_temp * 0.125f + 23.0f; + + return DEVICE_OK; +} + +int8_t BMI088_ParseGyro(BMI088_t *bmi088) { + if (bmi088 == NULL) return DEVICE_ERR_NULL; + +#if 1 + /* Gyroscope imu_raw -> degrees/sec -> radians/sec */ + int16_t raw_x, raw_y, raw_z; + memcpy(&raw_x, bmi088_rxbuf + 7, sizeof(raw_x)); + memcpy(&raw_y, bmi088_rxbuf + 9, sizeof(raw_y)); + memcpy(&raw_z, bmi088_rxbuf + 11, sizeof(raw_z)); + + bmi088->gyro.x = (float)raw_x; + bmi088->gyro.y = (float)raw_y; + bmi088->gyro.z = (float)raw_z; + +#else + /* Gyroscope imu_raw -> degrees/sec -> radians/sec */ + const int16_t *raw_x = (int16_t *)(bmi088_rxbuf + 7); + const int16_t *raw_y = (int16_t *)(bmi088_rxbuf + 9); + const int16_t *raw_z = (int16_t *)(bmi088_rxbuf + 11); + + bmi088->gyro.x = (float)*raw_x; + bmi088->gyro.y = (float)*raw_y; + bmi088->gyro.z = (float)*raw_z; +#endif + + /* FS125: 262.144. FS250: 131.072. FS500: 65.536. FS1000: 32.768. + * FS2000: 16.384.*/ + bmi088->gyro.x /= 32.768f; + bmi088->gyro.y /= 32.768f; + bmi088->gyro.z /= 32.768f; + + bmi088->gyro.x *= M_DEG2RAD_MULT; + bmi088->gyro.y *= M_DEG2RAD_MULT; + bmi088->gyro.z *= M_DEG2RAD_MULT; + + bmi088->gyro.x -= bmi088->cali->gyro_offset.x; + bmi088->gyro.y -= bmi088->cali->gyro_offset.y; + bmi088->gyro.z -= bmi088->cali->gyro_offset.z; + + return DEVICE_ERR_NULL; +} + +float BMI088_GetUpdateFreq(BMI088_t *bmi088) { + (void)bmi088; + return 400.0f; +} diff --git a/User/device/bmi088.h b/User/device/bmi088.h new file mode 100644 index 0000000..eb44e0c --- /dev/null +++ b/User/device/bmi088.h @@ -0,0 +1,81 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include + +#include "component/ahrs.h" +#include "device/device.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef struct { + struct { + float x; + float y; + float z; + } gyro_offset; /* 陀螺仪偏置 */ +} BMI088_Cali_t; /* BMI088校准数据 */ + +typedef struct { + DEVICE_Header_t header; + AHRS_Accl_t accl; + AHRS_Gyro_t gyro; + + float temp; /* 温度 */ + + const BMI088_Cali_t *cali; +} BMI088_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Exported functions prototypes -------------------------------------------- */ +int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali); +int8_t BMI088_Restart(void); + +bool BMI088_GyroStable(AHRS_Gyro_t *gyro); + +/* Sensor use right-handed coordinate system. */ +/* + x < R(logo) + y + UP is z + All implementation should follow this rule. + */ +uint32_t BMI088_WaitNew(); + +/* + BMI088的Accl和Gyro共用同一个DMA通道，所以一次只能读一个传感器。 + 即BMI088_AcclStartDmaRecv() 和 BMI088_AcclWaitDmaCplt() 中间不能 + 出现 BMI088_GyroStartDmaRecv()。 +*/ +int8_t BMI088_AcclStartDmaRecv(); +uint32_t BMI088_AcclWaitDmaCplt(); +int8_t BMI088_GyroStartDmaRecv(); +uint32_t BMI088_GyroWaitDmaCplt(); +int8_t BMI088_ParseAccl(BMI088_t *bmi088); +int8_t BMI088_ParseGyro(BMI088_t *bmi088); +float BMI088_GetUpdateFreq(BMI088_t *bmi088); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/device/device.h b/User/device/device.h index b8acfe3..7a3f17d 100644 --- a/User/device/device.h +++ b/User/device/device.h @@ -22,7 +22,10 @@ extern "C" { #define DEVICE_ERR_NO_DEV (-4) /* AUTO GENERATED SIGNALS BEGIN */ -/* No signals defined */ +#define SIGNAL_BMI088_ACCL_RAW_REDY (1u << 0) +#define SIGNAL_BMI088_GYRO_RAW_REDY (1u << 1) +#define SIGNAL_BMI088_ACCL_NEW_DATA (1u << 2) +#define SIGNAL_BMI088_GYRO_NEW_DATA (1u << 3) /* AUTO GENERATED SIGNALS END */ /* USER SIGNALS BEGIN */ diff --git a/User/device/device_config.yaml b/User/device/device_config.yaml index 299aa61..095c9e6 100644 --- a/User/device/device_config.yaml +++ b/User/device/device_config.yaml @@ -1,3 +1,11 @@ +bmi088: + bsp_config: + BSP_GPIO_ACCL_CS: BSP_GPIO_ACCL_CS + BSP_GPIO_ACCL_INT: BSP_GPIO_ACCL_INT + BSP_GPIO_GYRO_CS: BSP_GPIO_GYRO_CS + BSP_GPIO_GYRO_INT: BSP_GPIO_GYRO_INT + BSP_SPI_BMI088: BSP_SPI_BMI088 + enabled: true dm_imu: bsp_config: {} enabled: true diff --git a/User/device/dm_imu.c b/User/device/dm_imu.c deleted file mode 100644 index 064abdf..0000000 --- a/User/device/dm_imu.c +++ /dev/null @@ -1,271 +0,0 @@ -/* - DM_IMU数据获取（CAN） -*/ - -/* Includes ----------------------------------------------------------------- */ -#include "dm_imu.h" - -#include "bsp/can.h" -#include "bsp/time.h" -#include "component/user_math.h" -#include - -/* Private define ----------------------------------------------------------- */ -#define DM_IMU_OFFLINE_TIMEOUT 1000 // 设备离线判定时间1000ms - -#define ACCEL_CAN_MAX (58.8f) -#define ACCEL_CAN_MIN (-58.8f) -#define GYRO_CAN_MAX (34.88f) -#define GYRO_CAN_MIN (-34.88f) -#define PITCH_CAN_MAX (90.0f) -#define PITCH_CAN_MIN (-90.0f) -#define ROLL_CAN_MAX (180.0f) -#define ROLL_CAN_MIN (-180.0f) -#define YAW_CAN_MAX (180.0f) -#define YAW_CAN_MIN (-180.0f) -#define TEMP_MIN (0.0f) -#define TEMP_MAX (60.0f) -#define Quaternion_MIN (-1.0f) -#define Quaternion_MAX (1.0f) - - -/* Private macro ------------------------------------------------------------ */ -/* Private typedef ---------------------------------------------------------- */ -/* Private variables -------------------------------------------------------- */ -/* Private function --------------------------------------------------------- */ - -static uint8_t count = 0; // 计数器，用于判断设备是否离线 -/** - * @brief: 无符号整数转换为浮点数函数 - */ -static float uint_to_float(int x_int, float x_min, float x_max, int bits) -{ - float span = x_max - x_min; - float offset = x_min; - return ((float)x_int)*span/((float)((1<data.temp = (float)temp; - imu->data.accl.x = uint_to_float(acc_x_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); - imu->data.accl.y = uint_to_float(acc_y_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); - imu->data.accl.z = uint_to_float(acc_z_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); - return DEVICE_OK; -} -/** - * @brief 解析陀螺仪数据 - */ -static int8_t DM_IMU_ParseGyroData(DM_IMU_t *imu, uint8_t *data, uint8_t len) { - if (imu == NULL || data == NULL || len < 8) { - return DEVICE_ERR; - } - uint16_t gyro_x_raw = (data[3] << 8) | data[2]; - uint16_t gyro_y_raw = (data[5] << 8) | data[4]; - uint16_t gyro_z_raw = (data[7] << 8) | data[6]; - imu->data.gyro.x = uint_to_float(gyro_x_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); - imu->data.gyro.y = uint_to_float(gyro_y_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); - imu->data.gyro.z = uint_to_float(gyro_z_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); - return DEVICE_OK; -} -/** - * @brief 解析欧拉角数据 - */ -static int8_t DM_IMU_ParseEulerData(DM_IMU_t *imu, uint8_t *data, uint8_t len) { - if (imu == NULL || data == NULL || len < 8) { - return DEVICE_ERR; - } - uint16_t pit_raw = (data[3] << 8) | data[2]; - uint16_t yaw_raw = (data[5] << 8) | data[4]; - uint16_t rol_raw = (data[7] << 8) | data[6]; - imu->data.euler.pit = uint_to_float(pit_raw, PITCH_CAN_MIN, PITCH_CAN_MAX, 16) * M_DEG2RAD_MULT; - imu->data.euler.yaw = uint_to_float(yaw_raw, YAW_CAN_MIN, YAW_CAN_MAX, 16) * M_DEG2RAD_MULT; - imu->data.euler.rol = uint_to_float(rol_raw, ROLL_CAN_MIN, ROLL_CAN_MAX, 16) * M_DEG2RAD_MULT; - return DEVICE_OK; -} - -/** - * @brief 解析四元数数据 - */ -static int8_t DM_IMU_ParseQuaternionData(DM_IMU_t *imu, uint8_t *data, uint8_t len) { - if (imu == NULL || data == NULL || len < 8) { - return DEVICE_ERR; - } - int w = (data[1] << 6) | ((data[2] & 0xF8) >> 2); - int x = ((data[2] & 0x03) << 12) | (data[3] << 4) | ((data[4] & 0xF0) >> 4); - int y = ((data[4] & 0x0F) << 10) | (data[5] << 2) | ((data[6] & 0xC0) >> 6); - int z = ((data[6] & 0x3F) << 8) | data[7]; - imu->data.quat.q0 = uint_to_float(w, Quaternion_MIN, Quaternion_MAX, 14); - imu->data.quat.q1 = uint_to_float(x, Quaternion_MIN, Quaternion_MAX, 14); - imu->data.quat.q2 = uint_to_float(y, Quaternion_MIN, Quaternion_MAX, 14); - imu->data.quat.q3 = uint_to_float(z, Quaternion_MIN, Quaternion_MAX, 14); - return DEVICE_OK; -} - - -/* Exported functions ------------------------------------------------------- */ - -/** - * @brief 初始化DM IMU设备 - */ -int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param) { - if (imu == NULL || param == NULL) { - return DEVICE_ERR_NULL; - } - - // 初始化设备头部 - imu->header.online = false; - imu->header.last_online_time = 0; - - // 配置参数 - imu->param.can = param->can; - imu->param.can_id = param->can_id; - imu->param.device_id = param->device_id; - imu->param.master_id = param->master_id; - - // 清零数据 - memset(&imu->data, 0, sizeof(DM_IMU_Data_t)); - - // 注册CAN接收队列，用于接收回复报文 - int8_t result = BSP_CAN_RegisterId(imu->param.can, imu->param.master_id, 10); - if (result != BSP_OK) { - return DEVICE_ERR; - } - - return DEVICE_OK; -} - -/** - * @brief 请求IMU数据 - */ -int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) { - if (imu == NULL) { - return DEVICE_ERR_NULL; - } - - // 构造发送数据：id_L, id_H(DM_IMU_ID), RID, 0xcc - uint8_t tx_data[4] = { - imu->param.device_id & 0xFF, // id_L - (imu->param.device_id >> 8) & 0xFF, // id_H - (uint8_t)rid, // RID - 0xCC // 固定值 - }; - - // 发送标准数据帧 - BSP_CAN_StdDataFrame_t frame = { - .id = imu->param.can_id, - .dlc = 4, - }; - memcpy(frame.data, tx_data, 4); - int8_t result = BSP_CAN_TransmitStdDataFrame(imu->param.can, &frame); - return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR; -} - -/** - * @brief 更新IMU数据（从CAN中获取所有数据并解析） - */ -int8_t DM_IMU_Update(DM_IMU_t *imu) { - if (imu == NULL) { - return DEVICE_ERR_NULL; - } - - BSP_CAN_Message_t msg; - int8_t result; - bool data_received = false; - - // 持续接收所有可用消息 - while ((result = BSP_CAN_GetMessage(imu->param.can, imu->param.master_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE)) == BSP_OK) { - // 验证回复数据格式（至少检查数据长度） - if (msg.dlc < 3) { - continue; // 跳过无效消息 - } - - // 根据数据位的第0位确定反馈报文类型 - uint8_t rid = msg.data[0] & 0x0F; // 取第0位的低4位作为RID - - // 根据RID类型解析数据 - int8_t parse_result = DEVICE_ERR; - switch (rid) { - case 0x01: // RID_ACCL - parse_result = DM_IMU_ParseAccelData(imu, msg.data, msg.dlc); - break; - case 0x02: // RID_GYRO - parse_result = DM_IMU_ParseGyroData(imu, msg.data, msg.dlc); - break; - case 0x03: // RID_EULER - parse_result = DM_IMU_ParseEulerData(imu, msg.data, msg.dlc); - break; - case 0x04: // RID_QUATERNION - parse_result = DM_IMU_ParseQuaternionData(imu, msg.data, msg.dlc); - break; - default: - continue; // 跳过未知类型的消息 - } - - // 如果解析成功，标记为收到数据 - if (parse_result == DEVICE_OK) { - data_received = true; - } - } - - // 如果收到任何有效数据，更新设备状态 - if (data_received) { - imu->header.online = true; - imu->header.last_online_time = BSP_TIME_Get_ms(); - return DEVICE_OK; - } - - return DEVICE_ERR; -} - -/** - * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz） - */ -int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu){ - if (imu == NULL) { - return DEVICE_ERR_NULL; - } - switch (count) { - case 0: - DM_IMU_Request(imu, RID_ACCL); - break; - case 1: - DM_IMU_Request(imu, RID_GYRO); - break; - case 2: - DM_IMU_Request(imu, RID_EULER); - break; - case 3: - DM_IMU_Request(imu, RID_QUATERNION); - DM_IMU_Update(imu); // 更新所有数据 - break; - } - count++; - if (count >= 4) { - count = 0; // 重置计数器 - return DEVICE_OK; - } - return DEVICE_ERR; // 未完成一轮更新 -} - -/** - * @brief 检查设备是否在线 - */ -bool DM_IMU_IsOnline(DM_IMU_t *imu) { - if (imu == NULL) { - return false; - } - - uint32_t current_time = BSP_TIME_Get_ms(); - return imu->header.online && - (current_time - imu->header.last_online_time < DM_IMU_OFFLINE_TIMEOUT); -} diff --git a/User/device/dm_imu.h b/User/device/dm_imu.h deleted file mode 100644 index 3965980..0000000 --- a/User/device/dm_imu.h +++ /dev/null @@ -1,90 +0,0 @@ -#pragma once - -#ifdef __cplusplus -extern "C" { -#endif - -/* Includes ----------------------------------------------------------------- */ -#include "device/device.h" -#include "component/ahrs.h" -#include "bsp/can.h" -/* Exported constants ------------------------------------------------------- */ - -#define DM_IMU_CAN_ID_DEFAULT 0x6FF -#define DM_IMU_ID_DEFAULT 0x42 -#define DM_IMU_MST_ID_DEFAULT 0x43 - -/* Exported macro ----------------------------------------------------------- */ -/* Exported types ----------------------------------------------------------- */ - -typedef struct { - BSP_CAN_t can; // CAN总线句柄 - uint16_t can_id; // CAN通信ID - uint8_t device_id; // 设备ID - uint8_t master_id; // 主机ID -} DM_IMU_Param_t; -typedef enum { - RID_ACCL = 0x01, // 加速度计 - RID_GYRO = 0x02, // 陀螺仪 - RID_EULER = 0x03, // 欧拉角 - RID_QUATERNION = 0x04, // 四元数 -} DM_IMU_RID_t; - -typedef struct { - AHRS_Accl_t accl; // 加速度计 - AHRS_Gyro_t gyro; // 陀螺仪 - AHRS_Eulr_t euler; // 欧拉角 - AHRS_Quaternion_t quat; // 四元数 - float temp; // 温度 -} DM_IMU_Data_t; - -typedef struct { - DEVICE_Header_t header; - DM_IMU_Param_t param; // IMU参数配置 - DM_IMU_Data_t data; // IMU数据 -} DM_IMU_t; - -/* Exported functions prototypes -------------------------------------------- */ - -/** - * @brief 初始化DM IMU设备 - * @param imu DM IMU设备结构体指针 - * @param param IMU参数配置指针 - * @return DEVICE_OK 成功，其他值失败 - */ -int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param); - -/** - * @brief 请求IMU数据 - * @param imu DM IMU设备结构体指针 - * @param rid 请求的数据类型 - * @return DEVICE_OK 成功，其他值失败 - */ -int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid); - - -/** - * @brief 更新IMU数据（从CAN中获取所有数据并解析） - * @param imu DM IMU设备结构体指针 - * @return DEVICE_OK 成功，其他值失败 - */ -int8_t DM_IMU_Update(DM_IMU_t *imu); - -/** - * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz，运行4次才有完整数据） - * @param imu DM IMU设备结构体指针 - * @return DEVICE_OK 成功，其他值失败 - */ -int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu); - -/** - * @brief 检查设备是否在线 - * @param imu DM IMU设备结构体指针 - * @return true 在线，false 离线 - */ -bool DM_IMU_IsOnline(DM_IMU_t *imu); - - -#ifdef __cplusplus -} -#endif diff --git a/User/module/config.c b/User/module/config.c index 8bb0270..825e910 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -16,13 +16,6 @@ // 机器人参数配置 Config_RobotParam_t robot_config = { - .imu_param = - { - .can = BSP_CAN_2, - .can_id = 0x6FF, - .device_id = 0x42, - .master_id = 0x43, - }, .joint_param = { { diff --git a/User/module/config.h b/User/module/config.h index 0fc9560..d3fbbe7 100644 --- a/User/module/config.h +++ b/User/module/config.h @@ -9,11 +9,9 @@ extern "C" { #endif #include -#include "device/dm_imu.h" #include "device/motor_lz.h" #include "device/motor_lk.h" typedef struct { - DM_IMU_Param_t imu_param; MOTOR_LZ_Param_t joint_param[4]; MOTOR_LK_Param_t wheel_param[2]; } Config_RobotParam_t; diff --git a/User/task/config.yaml b/User/task/config.yaml index a8274e6..66735ad 100644 --- a/User/task/config.yaml +++ b/User/task/config.yaml @@ -8,7 +8,7 @@ - delay: 0 description: '' freq_control: true - frequency: 1000.0 + frequency: 500.0 function: Task_imu name: imu stack: 256 @@ -16,6 +16,6 @@ description: '' freq_control: true frequency: 500.0 - function: Task_ctrl_lz - name: ctrl_lz + function: Task_ctrl_chassis + name: ctrl_chassis stack: 256 diff --git a/User/task/ctrl_chassis.c b/User/task/ctrl_chassis.c new file mode 100644 index 0000000..8f0ba13 --- /dev/null +++ b/User/task/ctrl_chassis.c @@ -0,0 +1,44 @@ +/* + ctrl_chassis Task + +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +void Task_ctrl_chassis(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + + + /* 计算任务运行到指定频率需要等待的tick数 */ + const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_CHASSIS_FREQ; + + osDelay(CTRL_CHASSIS_INIT_DELAY); /* 延时一段时间再开启任务 */ + + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ + /* USER CODE INIT BEGIN */ + + /* USER CODE INIT END */ + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* USER CODE BEGIN */ + + /* USER CODE END */ + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ + } + +} \ No newline at end of file diff --git a/User/task/ctrl_lz.c b/User/task/ctrl_lz.c deleted file mode 100644 index 500da8e..0000000 --- a/User/task/ctrl_lz.c +++ /dev/null @@ -1,134 +0,0 @@ -/* - ctrl_lz Task - -*/ - -/* Includes ----------------------------------------------------------------- */ -#include "task/user_task.h" -/* USER INCLUDE BEGIN */ -#include "bsp/can.h" -#include "device/motor_lz.h" -#include "device/motor_lk.h" -#include "module/config.h" -#include -/* USER INCLUDE END */ - -/* Private typedef ---------------------------------------------------------- */ -/* Private define ----------------------------------------------------------- */ -/* Private macro ------------------------------------------------------------ */ -/* Private variables -------------------------------------------------------- */ -/* USER STRUCT BEGIN */ -static bool command_received = false; -/* USER STRUCT END */ - -/* Private function --------------------------------------------------------- */ -/* Exported functions ------------------------------------------------------- */ -void Task_ctrl_lz(void *argument) { - (void)argument; /* 未使用argument，消除警告 */ - - - /* 计算任务运行到指定频率需要等待的tick数 */ - const uint32_t delay_tick = osKernelGetTickFreq() / CTRL_LZ_FREQ; - - osDelay(CTRL_LZ_INIT_DELAY); /* 延时一段时间再开启任务 */ - - uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ - /* USER CODE INIT BEGIN */ - BSP_CAN_Init(); - - MOTOR_LZ_Init(); - - for (int i = 0; i < 4; i++) { - MOTOR_LZ_Register(&Config_GetRobotParam()->joint_param[i]); - } - - // 注册CAN接收ID - BSP_CAN_RegisterId(BSP_CAN_1, 121, 0); // 使能命令 - BSP_CAN_RegisterId(BSP_CAN_1, 122, 0); // 力矩控制命令 - - for (int i = 0; i < 4; i++) { - MOTOR_LZ_Enable(&Config_GetRobotParam()->joint_param[i]); - } - /* USER CODE INIT END */ - - while (1) { - tick += delay_tick; /* 计算下一个唤醒时刻 */ - /* USER CODE BEGIN */ - MOTOR_LZ_UpdateAll(); - - // 检查CAN接收消息 - BSP_CAN_Message_t rx_msg; - command_received = false; // 重置命令接收标志 - - // 检查ID 121 - 使能4个电机 - if (BSP_CAN_GetMessage(BSP_CAN_1, 121, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { - command_received = true; - for (int i = 0; i < 4; i++) { - MOTOR_LZ_Enable(&Config_GetRobotParam()->joint_param[i]); - } - } - - // 检查ID 122 - 运控模式控制4个电机 - if (BSP_CAN_GetMessage(BSP_CAN_1, 122, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) { - command_received = true; - // 8字节数据分别是4个电机的力矩 (每个电机2字节，有符号整数，精度0.01 Nm) - for (int i = 0; i < 4; i++) { - int16_t torque_raw; - memcpy(&torque_raw, &rx_msg.data[i * 2], sizeof(int16_t)); - float torque = (float)torque_raw / 100.0f; // 转换为浮点数力矩值 - - // 使用运控模式控制电机，只设置力矩，其他参数为0 - MOTOR_LZ_MotionParam_t motion_param = { - .target_angle = 0.0f, - .target_velocity = 0.0f, - .kp = 0.0f, - .kd = 0.0f, - .torque = torque - }; - MOTOR_LZ_MotionControl(&Config_GetRobotParam()->joint_param[i], &motion_param); - } - } - - // 如果没有收到任何控制命令，电机进入relax模式 - if (!command_received) { - for (int i = 0; i < 4; i++) { - MOTOR_LZ_Relax(&Config_GetRobotParam()->joint_param[i]); - } - } - - // 发送4个电机的反馈数据，ID分别为124、125、126、127 - for (int i = 0; i < 4; i++) { - MOTOR_LZ_t* motor = MOTOR_LZ_GetMotor(&Config_GetRobotParam()->joint_param[i]); - if (motor != NULL) { - BSP_CAN_StdDataFrame_t motor_frame = { - .id = 124 + i, // ID: 124, 125, 126, 127 - .dlc = 8, - .data = {0} - }; - - // 数据重构：转矩电流(2字节) + 位置(3字节) + 速度(3字节) = 8字节 - // 转矩电流 - 转换为16位整数 (精度0.01 Nm) - int16_t torque_int = (int16_t)(motor->lz_feedback.current_torque * 100); - memcpy(&motor_frame.data[0], &torque_int, sizeof(int16_t)); - - // 位置 - 转换为24位整数，使用3字节 (精度0.0001 rad) - int32_t angle_int = (int32_t)(motor->lz_feedback.current_angle * 10000) & 0xFFFFFF; - motor_frame.data[2] = (angle_int >> 16) & 0xFF; - motor_frame.data[3] = (angle_int >> 8) & 0xFF; - motor_frame.data[4] = angle_int & 0xFF; - - // 速度 - 转换为24位整数，使用3字节 (精度0.001 rad/s) - int32_t velocity_int = (int32_t)(motor->lz_feedback.current_velocity * 1000) & 0xFFFFFF; - motor_frame.data[5] = (velocity_int >> 16) & 0xFF; - motor_frame.data[6] = (velocity_int >> 8) & 0xFF; - motor_frame.data[7] = velocity_int & 0xFF; - - BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &motor_frame); - } - } - - /* USER CODE END */ - osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ - } - -} \ No newline at end of file diff --git a/User/task/imu.c b/User/task/imu.c index 1d782fe..6d6f714 100644 --- a/User/task/imu.c +++ b/User/task/imu.c @@ -1,15 +1,18 @@ /* imu Task - + */ /* Includes ----------------------------------------------------------------- */ #include "task/user_task.h" /* USER INCLUDE BEGIN */ -#include "bsp/can.h" -#include "device/dm_imu.h" -#include "module/config.h" -#include +#include "bsp/mm.h" +#include "bsp/pwm.h" +#include "bsp/gpio.h" +#include "component/ahrs.h" +#include "component/pid.h" +#include "device/bmi088.h" +#include "task/user_task.h" /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -17,15 +20,61 @@ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ -DM_IMU_t dm_imu; -int i = 0; +BMI088_t bmi088; + +AHRS_t gimbal_ahrs; +AHRS_Magn_t magn; +AHRS_Eulr_t eulr_to_send; + +KPID_t imu_temp_ctrl_pid; + +BMI088_Cali_t cali_bmi088= { + .gyro_offset = {0.0f,0.0f,0.0f}, +}; + +static const KPID_Params_t imu_temp_ctrl_pid_param = { + .k = 0.2f, + .p = 1.0f, + .i = 0.01f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, +}; + +/* 校准相关变量 */ +typedef enum { + CALIB_IDLE, // 空闲状态 + CALIB_RUNNING, // 正在校准 + CALIB_DONE // 校准完成 +} CalibState_t; + +static CalibState_t calib_state = CALIB_IDLE; +static uint16_t calib_count = 0; +static struct { + float x_sum; + float y_sum; + float z_sum; +} gyro_sum = {0.00341676874f, 0.000505680335f, -0.00134522165f}; + +#define CALIB_SAMPLES 5000 // 校准采样数量 /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ +/* 按钮回调函数：开始陀螺仪校准 */ +static void start_gyro_calibration(void) { + if (calib_state == CALIB_IDLE) { + calib_state = CALIB_RUNNING; + calib_count = 0; + gyro_sum.x_sum = 0.0f; + gyro_sum.y_sum = 0.0f; + gyro_sum.z_sum = 0.0f; + } +} /* Exported functions ------------------------------------------------------- */ void Task_imu(void *argument) { (void)argument; /* 未使用argument，消除警告 */ + /* 计算任务运行到指定频率需要等待的tick数 */ const uint32_t delay_tick = osKernelGetTickFreq() / IMU_FREQ; @@ -33,105 +82,73 @@ void Task_imu(void *argument) { uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ - BSP_CAN_Init(); - DM_IMU_Init(&dm_imu, &Config_GetRobotParam()->imu_param); + BMI088_Init(&bmi088,&cali_bmi088); + AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088)); + PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D, + 1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param); + BSP_PWM_Start(BSP_PWM_IMU_HEAT_PWM); + + /* 注册按钮回调函数并启用中断 */ + BSP_GPIO_RegisterCallback(BSP_GPIO_USER_KEY, start_gyro_calibration); + BSP_GPIO_EnableIRQ(BSP_GPIO_USER_KEY); /* USER CODE INIT END */ - + while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ - i++; - /* USER CODE BEGIN */ - DM_IMU_AutoUpdateAll(&dm_imu); - switch (i) { - case 0: { - // 发送加速度计数据 (ID: 0x66) - 三轴压缩到一帧，每轴2字节，精度0.01g - BSP_CAN_StdDataFrame_t accl_frame = {.id = 150, .dlc = 8, .data = {0}}; + BMI088_WaitNew(); + BMI088_AcclStartDmaRecv(); + BMI088_AcclWaitDmaCplt(); - // 转换为16位整数发送 (精度0.01g，范围±327.67g) - int16_t accl_x_int = (int16_t)(dm_imu.data.accl.x * 100.0f); - int16_t accl_y_int = (int16_t)(dm_imu.data.accl.y * 100.0f); - int16_t accl_z_int = (int16_t)(dm_imu.data.accl.z * 100.0f); + BMI088_GyroStartDmaRecv(); + BMI088_GyroWaitDmaCplt(); - // 打包数据：x(2字节) + y(2字节) + z(2字节) + 2字节保留 - memcpy(&accl_frame.data[0], &accl_x_int, 2); - memcpy(&accl_frame.data[2], &accl_y_int, 2); - memcpy(&accl_frame.data[4], &accl_z_int, 2); + /* 锁住RTOS内核防止数据解析过程中断，造成错误 */ + osKernelLock(); + /* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */ + BMI088_ParseAccl(&bmi088); + BMI088_ParseGyro(&bmi088); + // IST8310_Parse(&ist8310); - BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &accl_frame); + /* 陀螺仪校准处理 */ + if (calib_state == CALIB_RUNNING) { + /* 累加陀螺仪数据用于计算零偏 */ + gyro_sum.x_sum += bmi088.gyro.x; + gyro_sum.y_sum += bmi088.gyro.y; + gyro_sum.z_sum += bmi088.gyro.z; + calib_count++; + /* 达到采样数量后计算平均值并更新零偏 */ + if (calib_count >= CALIB_SAMPLES) { + /* 计算平均值作为零偏 */ + cali_bmi088.gyro_offset.x = gyro_sum.x_sum / CALIB_SAMPLES; + cali_bmi088.gyro_offset.y = gyro_sum.y_sum / CALIB_SAMPLES; + cali_bmi088.gyro_offset.z = gyro_sum.z_sum / CALIB_SAMPLES; + + /* 校准完成，重置为空闲状态以便下次校准 */ + calib_state = CALIB_IDLE; + + /* 重新初始化BMI088以应用新的校准数据 */ + BMI088_Init(&bmi088, &cali_bmi088); + AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088)); + } } - break; - case 1: { - // 发送陀螺仪数据 (ID: 0x67) - 三轴压缩到一帧，每轴2字节，精度0.01°/s - BSP_CAN_StdDataFrame_t gyro_frame = {.id = 151, .dlc = 8, .data = {0}}; + /* 只有在非校准状态下才进行正常的姿态解析 */ + if (calib_state != CALIB_RUNNING) { + /* 根据设备接收到的数据进行姿态解析 */ + AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn); - // 转换为16位整数发送 (精度0.01°/s，范围±327.67°/s) - int16_t gyro_x_int = (int16_t)(dm_imu.data.gyro.x * 57.2958f * - 100.0f); // 弧度/s转角度/s*100 - int16_t gyro_y_int = (int16_t)(dm_imu.data.gyro.y * 57.2958f * 100.0f); - int16_t gyro_z_int = (int16_t)(dm_imu.data.gyro.z * 57.2958f * 100.0f); - - // 打包数据：x(2字节) + y(2字节) + z(2字节) + 2字节保留 - memcpy(&gyro_frame.data[0], &gyro_x_int, 2); - memcpy(&gyro_frame.data[2], &gyro_y_int, 2); - memcpy(&gyro_frame.data[4], &gyro_z_int, 2); - - BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &gyro_frame); - - break; + /* 根据解析出来的四元数计算欧拉角 */ + AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs); } + + osKernelUnlock(); - case 2: { - // 发送欧拉角数据 (ID: 0x68) - 三轴压缩到一帧，每轴2字节，精度0.01度 - BSP_CAN_StdDataFrame_t euler_frame = {.id = 152, .dlc = 8, .data = {0}}; - - // 转换为16位整数发送 (精度0.01度，范围±327.67度) - int16_t yaw_int = (int16_t)(dm_imu.data.euler.yaw * 57.2958f * - 100.0f); // 弧度转角度*100 - int16_t pit_int = (int16_t)(dm_imu.data.euler.pit * 57.2958f * - 100.0f); // 弧度转角度*100 - int16_t rol_int = (int16_t)(dm_imu.data.euler.rol * 57.2958f * - 100.0f); // 弧度转角度*100 - - // 打包数据：yaw(2字节) + pitch(2字节) + roll(2字节) + 2字节保留 - memcpy(&euler_frame.data[0], &yaw_int, 2); - memcpy(&euler_frame.data[2], &pit_int, 2); - memcpy(&euler_frame.data[4], &rol_int, 2); - - BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &euler_frame); - - } - - break; - case 3: { - // 发送四元数数据 (ID: 0x69) - 四分量压缩到一帧，每分量2字节，精度0.0001 - BSP_CAN_StdDataFrame_t quat_frame = {.id = 153, .dlc = 8, .data = {0}}; - - // 转换为16位整数发送 (精度0.0001，范围±3.2767) - int16_t q0_int = (int16_t)(dm_imu.data.quat.q0 * 10000.0f); - int16_t q1_int = (int16_t)(dm_imu.data.quat.q1 * 10000.0f); - int16_t q2_int = (int16_t)(dm_imu.data.quat.q2 * 10000.0f); - int16_t q3_int = (int16_t)(dm_imu.data.quat.q3 * 10000.0f); - - // 打包数据：q0(2字节) + q1(2字节) + q2(2字节) + q3(2字节) - memcpy(&quat_frame.data[0], &q0_int, 2); - memcpy(&quat_frame.data[2], &q1_int, 2); - memcpy(&quat_frame.data[4], &q2_int, 2); - memcpy(&quat_frame.data[6], &q3_int, 2); - - BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &quat_frame); - } - - i = 0; - break; - default: - i = 0; - break; - } + BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 50.0f, bmi088.temp, 0.0f, 0.0f)); /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } -} \ No newline at end of file + +} diff --git a/User/task/init.c b/User/task/init.c index e261f86..cca7ac9 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -32,7 +32,7 @@ void Task_Init(void *argument) { /* 创建任务线程 */ task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink); task_runtime.thread.imu = osThreadNew(Task_imu, NULL, &attr_imu); - task_runtime.thread.ctrl_lz = osThreadNew(Task_ctrl_lz, NULL, &attr_ctrl_lz); + task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis); // 创建消息队列 /* USER MESSAGE BEGIN */ diff --git a/User/task/user_task.c b/User/task/user_task.c index 5dde688..764f790 100644 --- a/User/task/user_task.c +++ b/User/task/user_task.c @@ -19,8 +19,8 @@ const osThreadAttr_t attr_imu = { .priority = osPriorityNormal, .stack_size = 256 * 4, }; -const osThreadAttr_t attr_ctrl_lz = { - .name = "ctrl_lz", +const osThreadAttr_t attr_ctrl_chassis = { + .name = "ctrl_chassis", .priority = osPriorityNormal, .stack_size = 256 * 4, }; \ No newline at end of file diff --git a/User/task/user_task.h b/User/task/user_task.h index 95c4bc8..69d6e18 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -14,14 +14,14 @@ extern "C" { /* Exported constants ------------------------------------------------------- */ /* 任务运行频率 */ #define BLINK_FREQ (500.0) -#define IMU_FREQ (1000.0) -#define CTRL_LZ_FREQ (500.0) +#define IMU_FREQ (500.0) +#define CTRL_CHASSIS_FREQ (500.0) /* 任务初始化延时ms */ #define TASK_INIT_DELAY (100u) #define BLINK_INIT_DELAY (0) #define IMU_INIT_DELAY (0) -#define CTRL_LZ_INIT_DELAY (0) +#define CTRL_CHASSIS_INIT_DELAY (0) /* Exported defines --------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */ @@ -33,7 +33,7 @@ typedef struct { struct { osThreadId_t blink; osThreadId_t imu; - osThreadId_t ctrl_lz; + osThreadId_t ctrl_chassis; } thread; /* USER MESSAGE BEGIN */ @@ -57,21 +57,21 @@ typedef struct { struct { UBaseType_t blink; UBaseType_t imu; - UBaseType_t ctrl_lz; + UBaseType_t ctrl_chassis; } stack_water_mark; /* 各任务运行频率 */ struct { float blink; float imu; - float ctrl_lz; + float ctrl_chassis; } freq; /* 任务最近运行时间 */ struct { float blink; float imu; - float ctrl_lz; + float ctrl_chassis; } last_up_time; } Task_Runtime_t; @@ -83,13 +83,13 @@ extern Task_Runtime_t task_runtime; extern const osThreadAttr_t attr_init; extern const osThreadAttr_t attr_blink; extern const osThreadAttr_t attr_imu; -extern const osThreadAttr_t attr_ctrl_lz; +extern const osThreadAttr_t attr_ctrl_chassis; /* 任务函数声明 */ void Task_Init(void *argument); void Task_blink(void *argument); void Task_imu(void *argument); -void Task_ctrl_lz(void *argument); +void Task_ctrl_chassis(void *argument); #ifdef __cplusplus }