robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

添加imu

Browse Source
This commit is contained in:
Robofish 2025-10-05 03:03:27 +08:00
parent 84c8d72e38
commit 4f4e485912
28 changed files with 1959 additions and 614 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
CMakeLists.txt
View File

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

114
User/bsp/bsp_config.yaml
View File

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

130
User/bsp/gpio.c Normal file
View File

@ -0,0 +1,130 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp/gpio.h"
#include <gpio.h>
#include <main.h>
/* 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;
}

57
User/bsp/gpio.h Normal file
View File

@ -0,0 +1,57 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
#include "bsp/bsp.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported types ----------------------------------------------------------- */
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

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

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

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

@ -0,0 +1,59 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include "tim.h"
#include "bsp.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported types ----------------------------------------------------------- */
/* PWM通道 */
typedef enum {
BSP_PWM_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

181
User/bsp/spi.c Normal file
View File

@ -0,0 +1,181 @@
/* Includes ----------------------------------------------------------------- */
#include <spi.h>
#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, &reg, 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, &reg, 1u, true);
return BSP_SPI_Transmit(spi, data, size, true);
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

70
User/bsp/spi.h Normal file
View File

@ -0,0 +1,70 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <spi.h>
#include <stdint.h>
#include <stdbool.h>
#include "bsp/bsp.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported types ----------------------------------------------------------- */
/* 要添加使用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

8
User/component/component_config.yaml
View File

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

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

@ -0,0 +1,185 @@
/*
各类滤波器。
*/
#include "filter.h"
#include "user_math.h"
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param cutoff_freq 截止频率
*/
void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
float cutoff_freq) {
if (f == NULL) return;
f->cutoff_freq = cutoff_freq;
f->delay_element_1 = 0.0f;
f->delay_element_2 = 0.0f;
if (f->cutoff_freq <= 0.0f) {
/* no filtering */
f->b0 = 1.0f;
f->b1 = 0.0f;
f->b2 = 0.0f;
f->a1 = 0.0f;
f->a2 = 0.0f;
return;
}
const float fr = sample_freq / f->cutoff_freq;
const float ohm = tanf(M_PI / fr);
const float c = 1.0f + 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm;
f->b0 = ohm * ohm / c;
f->b1 = 2.0f * f->b0;
f->b2 = f->b0;
f->a1 = 2.0f * (ohm * ohm - 1.0f) / c;
f->a2 = (1.0f - 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm) / c;
}
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample) {
if (f == NULL) return 0.0f;
/* do the filtering */
float delay_element_0 =
sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
if (isinf(delay_element_0)) {
/* don't allow bad values to propagate via the filter */
delay_element_0 = sample;
}
const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
f->delay_element_2 * f->b2;
f->delay_element_2 = f->delay_element_1;
f->delay_element_1 = delay_element_0;
/* return the value. Should be no need to check limits */
return output;
}
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample) {
if (f == NULL) return 0.0f;
const float dval = sample / (f->b0 + f->b1 + f->b2);
if (isfinite(dval)) {
f->delay_element_1 = dval;
f->delay_element_2 = dval;
} else {
f->delay_element_1 = sample;
f->delay_element_2 = sample;
}
return LowPassFilter2p_Apply(f, sample);
}
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param notch_freq 中心频率
* @param bandwidth 带宽
*/
void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
float bandwidth) {
if (f == NULL) return;
f->notch_freq = notch_freq;
f->bandwidth = bandwidth;
f->delay_element_1 = 0.0f;
f->delay_element_2 = 0.0f;
if (notch_freq <= 0.0f) {
/* no filtering */
f->b0 = 1.0f;
f->b1 = 0.0f;
f->b2 = 0.0f;
f->a1 = 0.0f;
f->a2 = 0.0f;
return;
}
const float alpha = tanf(M_PI * bandwidth / sample_freq);
const float beta = -cosf(M_2PI * notch_freq / sample_freq);
const float a0_inv = 1.0f / (alpha + 1.0f);
f->b0 = a0_inv;
f->b1 = 2.0f * beta * a0_inv;
f->b2 = a0_inv;
f->a1 = f->b1;
f->a2 = (1.0f - alpha) * a0_inv;
}
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
inline float NotchFilter_Apply(NotchFilter_t *f, float sample) {
if (f == NULL) return 0.0f;
/* Direct Form II implementation */
const float delay_element_0 =
sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
f->delay_element_2 * f->b2;
f->delay_element_2 = f->delay_element_1;
f->delay_element_1 = delay_element_0;
return output;
}
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float NotchFilter_Reset(NotchFilter_t *f, float sample) {
if (f == NULL) return 0.0f;
float dval = sample;
if (fabsf(f->b0 + f->b1 + f->b2) > FLT_EPSILON) {
dval = dval / (f->b0 + f->b1 + f->b2);
}
f->delay_element_1 = dval;
f->delay_element_2 = dval;
return NotchFilter_Apply(f, sample);
}

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

@ -0,0 +1,120 @@
/*
各类滤波器。
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* 二阶低通滤波器 */
typedef struct {
float cutoff_freq; /* 截止频率 */
float a1;
float a2;
float b0;
float b1;
float b2;
float delay_element_1;
float delay_element_2;
} LowPassFilter2p_t;
/* 带阻滤波器 */
typedef struct {
float notch_freq; /* 阻止频率 */
float bandwidth; /* 带宽 */
float a1;
float a2;
float b0;
float b1;
float b2;
float delay_element_1;
float delay_element_2;
} NotchFilter_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param cutoff_freq 截止频率
*/
void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
float cutoff_freq);
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample);
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample);
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param notch_freq 中心频率
* @param bandwidth 带宽
*/
void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
float bandwidth);
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float NotchFilter_Apply(NotchFilter_t *f, float sample);
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float NotchFilter_Reset(NotchFilter_t *f, float sample);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

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

@ -0,0 +1,158 @@
/*
Modified from
https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.cpp
参考资料：
https://github.com/PX4/Firmware/issues/12362
https://dev.px4.io/master/en/flight_stack/controller_diagrams.html
https://docs.px4.io/master/en/config_mc/pid_tuning_guide_multicopter.html#standard_form
https://www.controleng.com/articles/not-all-pid-controllers-are-the-same/
https://en.wikipedia.org/wiki/PID_controller
http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
*/
#include "pid.h"
#define SIGMA 0.000001f
/**
* @brief 初始化PID
*
* @param pid PID结构体
* @param mode PID模式
* @param sample_freq 采样频率
* @param param PID参数
* @return int8_t 0对应没有错误
*/
int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq,
const KPID_Params_t *param) {
if (pid == NULL) return -1;
if (!isfinite(param->p)) return -1;
if (!isfinite(param->i)) return -1;
if (!isfinite(param->d)) return -1;
if (!isfinite(param->i_limit)) return -1;
if (!isfinite(param->out_limit)) return -1;
pid->param = param;
float dt_min = 1.0f / sample_freq;
if (isfinite(dt_min))
pid->dt_min = dt_min;
else
return -1;
LowPassFilter2p_Init(&(pid->dfilter), sample_freq, pid->param->d_cutoff_freq);
pid->mode = mode;
PID_Reset(pid);
return 0;
}
/**
* @brief PID计算
*
* @param pid PID结构体
* @param sp 设定值
* @param fb 反馈值
* @param fb_dot 反馈值微分
* @param dt 间隔时间
* @return float 计算的输出
*/
float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt) {
if (!isfinite(sp) || !isfinite(fb) || !isfinite(fb_dot) || !isfinite(dt)) {
return pid->last.out;
}
/* 计算误差值 */
const float err = CircleError(sp, fb, pid->param->range);
/* 计算P项 */
const float k_err = err * pid->param->k;
/* 计算D项 */
const float k_fb = pid->param->k * fb;
const float filtered_k_fb = LowPassFilter2p_Apply(&(pid->dfilter), k_fb);
float d;
switch (pid->mode) {
case KPID_MODE_CALC_D:
/* 通过fb计算D，避免了由于sp变化导致err突变的问题 */
/* 当sp不变时，err的微分等于负的fb的微分 */
d = (filtered_k_fb - pid->last.k_fb) / fmaxf(dt, pid->dt_min);
break;
case KPID_MODE_SET_D:
d = fb_dot;
break;
case KPID_MODE_NO_D:
d = 0.0f;
break;
}
pid->last.err = err;
pid->last.k_fb = filtered_k_fb;
if (!isfinite(d)) d = 0.0f;
/* 计算PD输出 */
float output = (k_err * pid->param->p) - (d * pid->param->d);
/* 计算I项 */
const float i = pid->i + (k_err * dt);
const float i_out = i * pid->param->i;
if (pid->param->i > SIGMA) {
/* 检查是否饱和 */
if (isfinite(i)) {
if ((fabsf(output + i_out) <= pid->param->out_limit) &&
(fabsf(i) <= pid->param->i_limit)) {
/* 未饱和，使用新积分 */
pid->i = i;
}
}
}
/* 计算PID输出 */
output += i_out;
/* 限制输出 */
if (isfinite(output)) {
if (pid->param->out_limit > SIGMA) {
output = AbsClip(output, pid->param->out_limit);
}
pid->last.out = output;
}
return pid->last.out;
}
/**
* @brief 重置微分项
*
* @param pid PID结构体
* @return int8_t 0对应没有错误
*/
int8_t PID_ResetIntegral(KPID_t *pid) {
if (pid == NULL) return -1;
pid->i = 0.0f;
return 0;
}
/**
* @brief 重置PID
*
* @param pid PID结构体
* @return int8_t 0对应没有错误
*/
int8_t PID_Reset(KPID_t *pid) {
if (pid == NULL) return -1;
pid->i = 0.0f;
pid->last.err = 0.0f;
pid->last.k_fb = 0.0f;
pid->last.out = 0.0f;
LowPassFilter2p_Reset(&(pid->dfilter), 0.0f);
return 0;
}

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

@ -0,0 +1,107 @@
/*
Modified from
https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.h
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include "filter.h"
#include "user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* PID模式 */
typedef enum {
KPID_MODE_NO_D = 0, /* 不使用微分项，PI控制器 */
KPID_MODE_CALC_D, /* 根据反馈的值计算离散微分，忽略PID_Calc中的fb_dot */
KPID_MODE_SET_D /* 直接提供微分值，PID_Calc中的fb_dot将被使用，(Gyros) */
} KPID_Mode_t;
/* PID参数 */
typedef struct {
float k; /* 控制器增益，设置为1用于并行模式 */
float p; /* 比例项增益，设置为1用于标准形式 */
float i; /* 积分项增益 */
float d; /* 微分项增益 */
float i_limit; /* 积分项上限 */
float out_limit; /* 输出绝对值限制 */
float d_cutoff_freq; /* D项低通截止频率 */
float range; /* 计算循环误差时使用，大于0时启用 */
} KPID_Params_t;
/* PID主结构体 */
typedef struct {
KPID_Mode_t mode;
const KPID_Params_t *param;
float dt_min; /* 最小PID_Calc调用间隔 */
float i; /* 积分 */
struct {
float err; /* 上次误差 */
float k_fb; /* 上次反馈值 */
float out; /* 上次输出 */
} last;
LowPassFilter2p_t dfilter; /* D项低通滤波器 */
} KPID_t;
/**
* @brief 初始化PID
*
* @param pid PID结构体
* @param mode PID模式
* @param sample_freq 采样频率
* @param param PID参数
* @return int8_t 0对应没有错误
*/
int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq,
const KPID_Params_t *param);
/**
* @brief PID计算
*
* @param pid PID结构体
* @param sp 设定值
* @param fb 反馈值
* @param fb_dot 反馈值微分
* @param dt 间隔时间
* @return float 计算的输出
*/
float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt);
/**
* @brief 重置微分项
*
* @param pid PID结构体
* @return int8_t 0对应没有错误
*/
int8_t PID_ResetIntegral(KPID_t *pid);
/**
* @brief 重置PID
*
* @param pid PID结构体
* @return int8_t 0对应没有错误
*/
int8_t PID_Reset(KPID_t *pid);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

381
User/device/bmi088.c Normal file
View File

@ -0,0 +1,381 @@
/*
BMI088 陀螺仪+加速度计传感器。
*/
/* Includes ----------------------------------------------------------------- */
#include "bmi088.h"
#include <cmsis_os2.h>
#include <gpio.h>
#include <stdbool.h>
#include <string.h>
#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;
}

81
User/device/bmi088.h Normal file
View File

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

5
User/device/device.h
View File

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

8
User/device/device_config.yaml
View File

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

271
User/device/dm_imu.c
View File

@ -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 <string.h>
/* 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<<bits)-1)) + offset;
}
/**
* @brief 解析加速度计数据
*/
static int8_t DM_IMU_ParseAccelData(DM_IMU_t *imu, uint8_t *data, uint8_t len) {
if (imu == NULL || data == NULL || len < 8) {
return DEVICE_ERR;
}
int8_t temp = data[1];
uint16_t acc_x_raw = (data[3] << 8) | data[2];
uint16_t acc_y_raw = (data[5] << 8) | data[4];
uint16_t acc_z_raw = (data[7] << 8) | data[6];
imu->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);
}

90
User/device/dm_imu.h
View File

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

7
User/module/config.c
View File

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

2
User/module/config.h
View File

@ -9,11 +9,9 @@ extern "C" {
#endif
#include <stdint.h>
#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;

6
User/task/config.yaml
View File

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

44
User/task/ctrl_chassis.c Normal file
View File

@ -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); /* 运行结束，等待下一次唤醒 */
}
}

134
User/task/ctrl_lz.c
View File

@ -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 <string.h>
/* 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); /* 运行结束，等待下一次唤醒 */
}
}

201
User/task/imu.c
View File

@ -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 <string.h>
#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); /* 运行结束，等待下一次唤醒 */
}
}
}

2
User/task/init.c
View File

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

4
User/task/user_task.c
View File

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

18
User/task/user_task.h
View File

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