TuxMonkey/tronone-h7-scaffold
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/tronone-h7-scaffold synced 2026-07-24 03:27:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

imu test failed

Browse Source
This commit is contained in:
TuxMonkey
2025-11-15 22:18:00 +08:00
parent 14a3cbb955
commit de8a0d5117
24 changed files with 2458 additions and 157 deletions
Download Patch File Download Diff File Expand all files Collapse all files

31
User_Code/module/periph/imu/BMI088Middleware.c Normal file
View File

@@ -0,0 +1,31 @@
#include "BMI088Middleware.h"
#include "main.h"
SPI_HandleTypeDef *BMI088_SPI;
void BMI088_ACCEL_NS_L(void)
{
HAL_GPIO_WritePin(ACC_CS_GPIO_Port, ACC_CS_Pin, GPIO_PIN_RESET);
}
void BMI088_ACCEL_NS_H(void)
{
HAL_GPIO_WritePin(ACC_CS_GPIO_Port, ACC_CS_Pin, GPIO_PIN_SET);
}
void BMI088_GYRO_NS_L(void)
{
HAL_GPIO_WritePin(GYRO_CS_GPIO_Port, GYRO_CS_Pin, GPIO_PIN_RESET);
}
void BMI088_GYRO_NS_H(void)
{
HAL_GPIO_WritePin(GYRO_CS_GPIO_Port, GYRO_CS_Pin, GPIO_PIN_SET);
}
uint8_t BMI088_read_write_byte(uint8_t txdata)
{
uint8_t rx_data;
HAL_SPI_TransmitReceive(BMI088_SPI, &txdata, &rx_data, 1, 100); //1000or100?
return rx_data;
}

34
User_Code/module/periph/imu/BMI088Middleware.h Normal file
View File

@@ -0,0 +1,34 @@
#ifndef BMI088MIDDLEWARE_H
#define BMI088MIDDLEWARE_H
#include "main.h"
#define BMI088_USE_SPI
//#define BMI088_USE_IIC
/*
#define CS1_ACCEL_GPIO_Port ACCEL_NSS_GPIO_Port
#define CS1_ACCEL_Pin ACCEL_NSS_Pin
#define CS1_GYRO_GPIO_Port GYRO_NSS_GPIO_Port
#define CS1_GYRO_Pin GYRO_NSS_Pin
*/
#if defined(BMI088_USE_SPI)
extern void BMI088_ACCEL_NS_L(void);
extern void BMI088_ACCEL_NS_H(void);
extern void BMI088_GYRO_NS_L(void);
extern void BMI088_GYRO_NS_H(void);
extern uint8_t BMI088_read_write_byte(uint8_t reg);
extern SPI_HandleTypeDef *BMI088_SPI;
#elif defined(BMI088_USE_IIC)
#endif
#endif

416
User_Code/module/periph/imu/BMI088driver.c Normal file
View File

@@ -0,0 +1,416 @@
#include "BMI088driver.h"
#include "BMI088reg.h"
#include "BMI088Middleware.h"
#include "bsp_dwt.h"
#include "bsp_log.h"
#include <math.h>
#pragma message "this is a legacy support. test the new BMI088 module as soon as possible."
float BMI088_ACCEL_SEN = BMI088_ACCEL_6G_SEN;
float BMI088_GYRO_SEN = BMI088_GYRO_2000_SEN;
static uint8_t res = 0;
static uint8_t write_reg_num = 0;
static uint8_t error = BMI088_NO_ERROR;
float gyroDiff[3], gNormDiff;
uint8_t caliOffset = 1;
int16_t caliCount = 0;
IMU_Data_t BMI088;
#if defined(BMI088_USE_SPI)
#define BMI088_accel_write_single_reg(reg, data) \
{ \
BMI088_ACCEL_NS_L(); \
BMI088_write_single_reg((reg), (data)); \
BMI088_ACCEL_NS_H(); \
}
#define BMI088_accel_read_single_reg(reg, data) \
{ \
BMI088_ACCEL_NS_L(); \
BMI088_read_write_byte((reg) | 0x80); \
BMI088_read_write_byte(0x55); \
(data) = BMI088_read_write_byte(0x55); \
BMI088_ACCEL_NS_H(); \
}
#define BMI088_accel_read_muli_reg(reg, data, len) \
{ \
BMI088_ACCEL_NS_L(); \
BMI088_read_write_byte((reg) | 0x80); \
BMI088_read_muli_reg(reg, data, len); \
BMI088_ACCEL_NS_H(); \
}
#define BMI088_gyro_write_single_reg(reg, data) \
{ \
BMI088_GYRO_NS_L(); \
BMI088_write_single_reg((reg), (data)); \
BMI088_GYRO_NS_H(); \
}
#define BMI088_gyro_read_single_reg(reg, data) \
{ \
BMI088_GYRO_NS_L(); \
BMI088_read_single_reg((reg), &(data)); \
BMI088_GYRO_NS_H(); \
}
#define BMI088_gyro_read_muli_reg(reg, data, len) \
{ \
BMI088_GYRO_NS_L(); \
BMI088_read_muli_reg((reg), (data), (len)); \
BMI088_GYRO_NS_H(); \
}
static void BMI088_write_single_reg(uint8_t reg, uint8_t data);
static void BMI088_read_single_reg(uint8_t reg, uint8_t *return_data);
static void BMI088_read_muli_reg(uint8_t reg, uint8_t *buf, uint8_t len);
#elif defined(BMI088_USE_IIC)
#endif
static uint8_t BMI088_Accel_Init_Table[BMI088_WRITE_ACCEL_REG_NUM][3] =
{
{BMI088_ACC_PWR_CTRL, BMI088_ACC_ENABLE_ACC_ON, BMI088_ACC_PWR_CTRL_ERROR},
{BMI088_ACC_PWR_CONF, BMI088_ACC_PWR_ACTIVE_MODE, BMI088_ACC_PWR_CONF_ERROR},
{BMI088_ACC_CONF, BMI088_ACC_NORMAL | BMI088_ACC_1600_HZ | BMI088_ACC_CONF_MUST_Set, BMI088_ACC_CONF_ERROR},
{BMI088_ACC_RANGE, BMI088_ACC_RANGE_6G, BMI088_ACC_RANGE_ERROR},
{BMI088_INT1_IO_CTRL, BMI088_ACC_INT1_IO_ENABLE | BMI088_ACC_INT1_GPIO_PP | BMI088_ACC_INT1_GPIO_LOW,
BMI088_INT1_IO_CTRL_ERROR},
{BMI088_INT_MAP_DATA, BMI088_ACC_INT1_DRDY_INTERRUPT, BMI088_INT_MAP_DATA_ERROR}
};
static uint8_t BMI088_Gyro_Init_Table[BMI088_WRITE_GYRO_REG_NUM][3] =
{
{BMI088_GYRO_RANGE, BMI088_GYRO_2000, BMI088_GYRO_RANGE_ERROR},
{BMI088_GYRO_BANDWIDTH, BMI088_GYRO_2000_230_HZ | BMI088_GYRO_BANDWIDTH_MUST_Set, BMI088_GYRO_BANDWIDTH_ERROR},
{BMI088_GYRO_LPM1, BMI088_GYRO_NORMAL_MODE, BMI088_GYRO_LPM1_ERROR},
{BMI088_GYRO_CTRL, BMI088_DRDY_ON, BMI088_GYRO_CTRL_ERROR},
{BMI088_GYRO_INT3_INT4_IO_CONF, BMI088_GYRO_INT3_GPIO_PP | BMI088_GYRO_INT3_GPIO_LOW,
BMI088_GYRO_INT3_INT4_IO_CONF_ERROR},
{BMI088_GYRO_INT3_INT4_IO_MAP, BMI088_GYRO_DRDY_IO_INT3, BMI088_GYRO_INT3_INT4_IO_MAP_ERROR}
};
static void Calibrate_MPU_Offset(IMU_Data_t *bmi088);
uint8_t BMI088Init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate)
{
BMI088_SPI = bmi088_SPI;
error = BMI088_NO_ERROR;
error |= bmi088_accel_init();
error |= bmi088_gyro_init();
if (calibrate)
Calibrate_MPU_Offset(&BMI088);
else
{
BMI088.GyroOffset[0] = GxOFFSET;
BMI088.GyroOffset[1] = GyOFFSET;
BMI088.GyroOffset[2] = GzOFFSET;
BMI088.gNorm = gNORM;
BMI088.AccelScale = 9.81f / BMI088.gNorm;
BMI088.TempWhenCali = 40;
}
return error;
}
void Calibrate_MPU_Offset(IMU_Data_t *bmi088)
{
static float startTime;
static uint16_t CaliTimes = 6000;
uint8_t buf[8] = {0, 0, 0, 0, 0, 0};
int16_t bmi088_raw_temp;
float gyroMax[3], gyroMin[3];
float gNormTemp = 0.0f, gNormMax = 0.0f, gNormMin = 0.0f;
startTime = DWT_GetTimeline_s();
do
{
if (DWT_GetTimeline_s() - startTime > 12)
{
// <20><>????
bmi088->GyroOffset[0] = GxOFFSET;
bmi088->GyroOffset[1] = GyOFFSET;
bmi088->GyroOffset[2] = GzOFFSET;
bmi088->gNorm = gNORM;
bmi088->TempWhenCali = 40;
LOGERROR("[BMI088] Calibrate Failed! Use offline params");
break;
}
DWT_Delay(0.005);
bmi088->gNorm = 0;
bmi088->GyroOffset[0] = 0;
bmi088->GyroOffset[1] = 0;
bmi088->GyroOffset[2] = 0;
for (uint16_t i = 0; i < CaliTimes; ++i)
{
BMI088_accel_read_muli_reg(BMI088_ACCEL_XOUT_L, buf, 6);
bmi088_raw_temp = (int16_t) ((buf[1]) << 8) | buf[0];
bmi088->Accel[0] = bmi088_raw_temp * BMI088_ACCEL_SEN;
bmi088_raw_temp = (int16_t) ((buf[3]) << 8) | buf[2];
bmi088->Accel[1] = bmi088_raw_temp * BMI088_ACCEL_SEN;
bmi088_raw_temp = (int16_t) ((buf[5]) << 8) | buf[4];
bmi088->Accel[2] = bmi088_raw_temp * BMI088_ACCEL_SEN;
gNormTemp = sqrtf(bmi088->Accel[0] * bmi088->Accel[0] +
bmi088->Accel[1] * bmi088->Accel[1] +
bmi088->Accel[2] * bmi088->Accel[2]);
bmi088->gNorm += gNormTemp;
BMI088_gyro_read_muli_reg(BMI088_GYRO_CHIP_ID, buf, 8);
if (buf[0] == BMI088_GYRO_CHIP_ID_VALUE)
{
bmi088_raw_temp = (int16_t) ((buf[3]) << 8) | buf[2];
bmi088->Gyro[0] = bmi088_raw_temp * BMI088_GYRO_SEN;
bmi088->GyroOffset[0] += bmi088->Gyro[0];
bmi088_raw_temp = (int16_t) ((buf[5]) << 8) | buf[4];
bmi088->Gyro[1] = bmi088_raw_temp * BMI088_GYRO_SEN;
bmi088->GyroOffset[1] += bmi088->Gyro[1];
bmi088_raw_temp = (int16_t) ((buf[7]) << 8) | buf[6];
bmi088->Gyro[2] = bmi088_raw_temp * BMI088_GYRO_SEN;
bmi088->GyroOffset[2] += bmi088->Gyro[2];
}
if (i == 0)
{
gNormMax = gNormTemp;
gNormMin = gNormTemp;
for (uint8_t j = 0; j < 3; ++j)
{
gyroMax[j] = bmi088->Gyro[j];
gyroMin[j] = bmi088->Gyro[j];
}
}
else
{
if (gNormTemp > gNormMax)
gNormMax = gNormTemp;
if (gNormTemp < gNormMin)
gNormMin = gNormTemp;
for (uint8_t j = 0; j < 3; ++j)
{
if (bmi088->Gyro[j] > gyroMax[j])
gyroMax[j] = bmi088->Gyro[j];
if (bmi088->Gyro[j] < gyroMin[j])
gyroMin[j] = bmi088->Gyro[j];
}
}
gNormDiff = gNormMax - gNormMin;
for (uint8_t j = 0; j < 3; ++j)
gyroDiff[j] = gyroMax[j] - gyroMin[j];
if (gNormDiff > 0.5f ||
gyroDiff[0] > 0.5f || //0.15
gyroDiff[1] > 0.5f || //0.15
gyroDiff[2] > 0.5f) //0.15
{
LOGWARNING("[bmi088] calibration was interrupted\n");
break;
}
DWT_Delay(0.0005);
}
bmi088->gNorm /= (float) CaliTimes;
for (uint8_t i = 0; i < 3; ++i)
bmi088->GyroOffset[i] /= (float) CaliTimes;
BMI088_accel_read_muli_reg(BMI088_TEMP_M, buf, 2);
bmi088_raw_temp = (int16_t) ((buf[0] << 3) | (buf[1] >> 5));
if (bmi088_raw_temp > 1023)
bmi088_raw_temp -= 2048;
bmi088->TempWhenCali = bmi088_raw_temp * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET;
caliCount++;
}
while (gNormDiff > 0.5f ||
fabsf(bmi088->gNorm - 9.8f) > 0.5f ||
gyroDiff[0] > 0.15f ||
gyroDiff[1] > 0.15f ||
gyroDiff[2] > 0.15f ||
fabsf(bmi088->GyroOffset[0]) > 0.01f ||
fabsf(bmi088->GyroOffset[1]) > 0.01f ||
fabsf(bmi088->GyroOffset[2]) > 0.01f);
bmi088->AccelScale = 9.81f / bmi088->gNorm;
}
uint8_t bmi088_accel_init(void)
{
// check commiunication
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
// DWT_Delay(0.001);
HAL_Delay(BMI088_LONG_DELAY_TIME);
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
// DWT_Delay(0.001);
HAL_Delay(BMI088_LONG_DELAY_TIME);
// accel software reset
BMI088_accel_write_single_reg(BMI088_ACC_SOFTRESET, BMI088_ACC_SOFTRESET_VALUE);
// HAL_Delay(BMI088_LONG_DELAY_TIME);
HAL_Delay(BMI088_COM_WAIT_SENSOR_TIME);
// check commiunication is normal after reset
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
// DWT_Delay(0.001);
HAL_Delay(BMI088_LONG_DELAY_TIME);
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
// DWT_Delay(0.001);
HAL_Delay(BMI088_LONG_DELAY_TIME);
// check the "who am I"
if (res != BMI088_ACC_CHIP_ID_VALUE)
{
LOGERROR("[bmi088] Can not read bmi088 acc chip id");
return BMI088_NO_SENSOR;
}
// set accel sonsor config and check
for (write_reg_num = 0; write_reg_num < BMI088_WRITE_ACCEL_REG_NUM; write_reg_num++)
{
BMI088_accel_write_single_reg(BMI088_Accel_Init_Table[write_reg_num][0],
BMI088_Accel_Init_Table[write_reg_num][1]);
// DWT_Delay(0.001);
HAL_Delay(BMI088_LONG_DELAY_TIME);
BMI088_accel_read_single_reg(BMI088_Accel_Init_Table[write_reg_num][0], res);
// DWT_Delay(0.001);
HAL_Delay(BMI088_LONG_DELAY_TIME);
if (res != BMI088_Accel_Init_Table[write_reg_num][1])
{
// write_reg_num--;
// return BMI088_Accel_Init_Table[write_reg_num][2];
error |= BMI088_Accel_Init_Table[write_reg_num][2];
}
}
return BMI088_NO_ERROR;
}
uint8_t bmi088_gyro_init(void)
{
// check commiunication
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
HAL_Delay(BMI088_LONG_DELAY_TIME);
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
HAL_Delay(BMI088_LONG_DELAY_TIME);
// reset the gyro sensor
BMI088_gyro_write_single_reg(BMI088_GYRO_SOFTRESET, BMI088_GYRO_SOFTRESET_VALUE);
// HAL_Delay(BMI088_LONG_DELAY_TIME);
HAL_Delay(BMI088_COM_WAIT_SENSOR_TIME);
// check commiunication is normal after reset
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
HAL_Delay(BMI088_LONG_DELAY_TIME);
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
HAL_Delay(BMI088_LONG_DELAY_TIME);
// check the "who am I"
if (res != BMI088_GYRO_CHIP_ID_VALUE)
{
LOGERROR("[bmi088] Can not read bmi088 gyro chip id");
return BMI088_NO_SENSOR;
}
// set gyro sonsor config and check
for (write_reg_num = 0; write_reg_num < BMI088_WRITE_GYRO_REG_NUM; write_reg_num++)
{
BMI088_gyro_write_single_reg(BMI088_Gyro_Init_Table[write_reg_num][0],
BMI088_Gyro_Init_Table[write_reg_num][1]);
HAL_Delay(BMI088_LONG_DELAY_TIME);
BMI088_gyro_read_single_reg(BMI088_Gyro_Init_Table[write_reg_num][0], res);
HAL_Delay(BMI088_LONG_DELAY_TIME); //但是不知道为什么还是yaw pitch还是nan
if (res != BMI088_Gyro_Init_Table[write_reg_num][1])
{
write_reg_num--;
// return BMI088_Gyro_Init_Table[write_reg_num][2];
error |= BMI088_Accel_Init_Table[write_reg_num][2];
}
}
return BMI088_NO_ERROR;
}
void BMI088_Read(IMU_Data_t *bmi088)
{
static uint8_t buf[8] = {0};
static int16_t bmi088_raw_temp;
BMI088_accel_read_muli_reg(BMI088_ACCEL_XOUT_L, buf, 6);
bmi088_raw_temp = (int16_t) ((buf[1]) << 8) | buf[0];
bmi088->Accel[0] = bmi088_raw_temp * BMI088_ACCEL_SEN * bmi088->AccelScale;
bmi088_raw_temp = (int16_t) ((buf[3]) << 8) | buf[2];
bmi088->Accel[1] = bmi088_raw_temp * BMI088_ACCEL_SEN * bmi088->AccelScale;
bmi088_raw_temp = (int16_t) ((buf[5]) << 8) | buf[4];
bmi088->Accel[2] = bmi088_raw_temp * BMI088_ACCEL_SEN * bmi088->AccelScale;
BMI088_gyro_read_muli_reg(BMI088_GYRO_CHIP_ID, buf, 8);
if (buf[0] == BMI088_GYRO_CHIP_ID_VALUE)
{
if (caliOffset)
{
bmi088_raw_temp = (int16_t) ((buf[3]) << 8) | buf[2];
bmi088->Gyro[0] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[0];
bmi088_raw_temp = (int16_t) ((buf[5]) << 8) | buf[4];
bmi088->Gyro[1] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[1];
bmi088_raw_temp = (int16_t) ((buf[7]) << 8) | buf[6];
bmi088->Gyro[2] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[2];
}
else
{
bmi088_raw_temp = (int16_t) ((buf[3]) << 8) | buf[2];
bmi088->Gyro[0] = bmi088_raw_temp * BMI088_GYRO_SEN;
bmi088_raw_temp = (int16_t) ((buf[5]) << 8) | buf[4];
bmi088->Gyro[1] = bmi088_raw_temp * BMI088_GYRO_SEN;
bmi088_raw_temp = (int16_t) ((buf[7]) << 8) | buf[6];
bmi088->Gyro[2] = bmi088_raw_temp * BMI088_GYRO_SEN;
}
}
BMI088_accel_read_muli_reg(BMI088_TEMP_M, buf, 2);
bmi088_raw_temp = (int16_t) ((buf[0] << 3) | (buf[1] >> 5));
if (bmi088_raw_temp > 1023)
{
bmi088_raw_temp -= 2048;
}
bmi088->Temperature = bmi088_raw_temp * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET;
}
#if defined(BMI088_USE_SPI)
static void BMI088_write_single_reg(uint8_t reg, uint8_t data)
{
BMI088_read_write_byte(reg);
BMI088_read_write_byte(data);
}
static void BMI088_read_single_reg(uint8_t reg, uint8_t *return_data)
{
BMI088_read_write_byte(reg | 0x80);
*return_data = BMI088_read_write_byte(0x55);
}
static void BMI088_read_muli_reg(uint8_t reg, uint8_t *buf, uint8_t len)
{
BMI088_read_write_byte(reg | 0x80);
while (len != 0)
{
*buf = BMI088_read_write_byte(0x55);
buf++;
len--;
}
}
#elif defined(BMI088_USE_IIC)
#endif

133
User_Code/module/periph/imu/BMI088driver.h Normal file
View File

@@ -0,0 +1,133 @@
#ifndef BMI088DRIVER_H
#define BMI088DRIVER_H
#include "stdint.h"
#include "main.h"
#define BMI088_TEMP_FACTOR 0.125f
#define BMI088_TEMP_OFFSET 23.0f
#define BMI088_WRITE_ACCEL_REG_NUM 6
#define BMI088_WRITE_GYRO_REG_NUM 6
#define BMI088_GYRO_DATA_READY_BIT 0
#define BMI088_ACCEL_DATA_READY_BIT 1
#define BMI088_ACCEL_TEMP_DATA_READY_BIT 2
#define BMI088_LONG_DELAY_TIME 80
#define BMI088_COM_WAIT_SENSOR_TIME 150
#define BMI088_ACCEL_IIC_ADDRESSE (0x18 << 1)
#define BMI088_GYRO_IIC_ADDRESSE (0x68 << 1)
#define BMI088_ACCEL_3G_SEN 0.0008974358974f
#define BMI088_ACCEL_6G_SEN 0.00179443359375f
#define BMI088_ACCEL_12G_SEN 0.0035888671875f
#define BMI088_ACCEL_24G_SEN 0.007177734375f
#define BMI088_GYRO_2000_SEN 0.00106526443603169529841533860381f
#define BMI088_GYRO_1000_SEN 0.00053263221801584764920766930190693f
#define BMI088_GYRO_500_SEN 0.00026631610900792382460383465095346f
#define BMI088_GYRO_250_SEN 0.00013315805450396191230191732547673f
#define BMI088_GYRO_125_SEN 0.000066579027251980956150958662738366f
// <20><><EFBFBD>ֶ<EFBFBD><D6B6>޸<EFBFBD>
#if INFANTRY_ID == 0
#define GxOFFSET 0.00247530174f
#define GyOFFSET 0.000393082853f
#define GzOFFSET 0.000393082853f
#define gNORM 9.69293118f
#elif INFANTRY_ID == 1
#define GxOFFSET 0.0007222f
#define GyOFFSET -0.001786f
#define GzOFFSET 0.0004346f
#define gNORM 9.876785f
#elif INFANTRY_ID == 2
#define GxOFFSET 0.0007222f
#define GyOFFSET -0.001786f
#define GzOFFSET 0.0004346f
#define gNORM 9.876785f
#elif INFANTRY_ID == 3
#define GxOFFSET 0.00270364084f
#define GyOFFSET -0.000532632112f
#define GzOFFSET 0.00478090625f
#define gNORM 9.73574924f
#elif INFANTRY_ID == 4
#define GxOFFSET 0.0007222f
#define GyOFFSET -0.001786f
#define GzOFFSET 0.0004346f
#define gNORM 9.876785f
#endif
/* IMU数据结构体 */
typedef struct
{
float Accel[3];
float Gyro[3];
float TempWhenCali;
float Temperature;
float AccelScale;
float GyroOffset[3];
float gNorm;
} IMU_Data_t;
/* BMI088错误码枚举 */
enum
{
BMI088_NO_ERROR = 0x00,
BMI088_ACC_PWR_CTRL_ERROR = 0x01,
BMI088_ACC_PWR_CONF_ERROR = 0x02,
BMI088_ACC_CONF_ERROR = 0x03,
BMI088_ACC_SELF_TEST_ERROR = 0x04,
BMI088_ACC_RANGE_ERROR = 0x05,
BMI088_INT1_IO_CTRL_ERROR = 0x06,
BMI088_INT_MAP_DATA_ERROR = 0x07,
BMI088_GYRO_RANGE_ERROR = 0x08,
BMI088_GYRO_BANDWIDTH_ERROR = 0x09,
BMI088_GYRO_LPM1_ERROR = 0x0A,
BMI088_GYRO_CTRL_ERROR = 0x0B,
BMI088_GYRO_INT3_INT4_IO_CONF_ERROR = 0x0C,
BMI088_GYRO_INT3_INT4_IO_MAP_ERROR = 0x0D,
BMI088_SELF_TEST_ACCEL_ERROR = 0x80,
BMI088_SELF_TEST_GYRO_ERROR = 0x40,
BMI088_NO_SENSOR = 0xFF,
};
extern IMU_Data_t BMI088;
/**
* @brief 初始化BMI088,传入连接的SPI总线handle,以及是否进行在线标定
*
* @param bmi088_SPI handle
* @param calibrate 1为进行在线标定,0使用离线数据
* @return uint8_t 成功则返回BMI088_NO_ERROR
*/
extern uint8_t BMI088Init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate);
/**
* @brief 加速计初始化
*
* @return uint8_t
*/
extern uint8_t bmi088_accel_init(void);
/**
* @brief 陀螺仪初始化
*
* @return uint8_t
*/
extern uint8_t bmi088_gyro_init(void);
/**
* @brief 读取一次BMI088的数据,包括gyro和accel
*
* @param bmi088 传入BMI088实例(结构体)
*/
extern void BMI088_Read(IMU_Data_t *bmi088);
#endif

180
User_Code/module/periph/imu/BMI088reg.h Normal file
View File

@@ -0,0 +1,180 @@
#ifndef BMI088REG_H
#define BMI088REG_H
#define BMI088_ACC_CHIP_ID 0x00 // the register is " Who am I "
#define BMI088_ACC_CHIP_ID_VALUE 0x1E
#define BMI088_ACC_ERR_REG 0x02
#define BMI088_ACCEL_CONGIF_ERROR_SHFITS 0x2
#define BMI088_ACCEL_CONGIF_ERROR (1 << BMI088_ACCEL_CONGIF_ERROR_SHFITS)
#define BMI088_FATAL_ERROR_SHFITS 0x0
#define BMI088_FATAL_ERROR (1 << BMI088_FATAL_ERROR)
#define BMI088_ACC_STATUS 0x03
#define BMI088_ACCEL_DRDY_SHFITS 0x7
#define BMI088_ACCEL_DRDY (1 << BMI088_ACCEL_DRDY_SHFITS)
#define BMI088_ACCEL_XOUT_L 0x12
#define BMI088_ACCEL_XOUT_M 0x13
#define BMI088_ACCEL_YOUT_L 0x14
#define BMI088_ACCEL_YOUT_M 0x15
#define BMI088_ACCEL_ZOUT_L 0x16
#define BMI088_ACCEL_ZOUT_M 0x17
#define BMI088_SENSORTIME_DATA_L 0x18
#define BMI088_SENSORTIME_DATA_M 0x19
#define BMI088_SENSORTIME_DATA_H 0x1A
#define BMI088_ACC_INT_STAT_1 0x1D
#define BMI088_ACCEL_DRDY_INTERRUPT_SHFITS 0x7
#define BMI088_ACCEL_DRDY_INTERRUPT (1 << BMI088_ACCEL_DRDY_INTERRUPT_SHFITS)
#define BMI088_TEMP_M 0x22
#define BMI088_TEMP_L 0x23
#define BMI088_ACC_CONF 0x40
#define BMI088_ACC_CONF_MUST_Set 0x80
#define BMI088_ACC_BWP_SHFITS 0x4
#define BMI088_ACC_OSR4 (0x0 << BMI088_ACC_BWP_SHFITS)
#define BMI088_ACC_OSR2 (0x1 << BMI088_ACC_BWP_SHFITS)
#define BMI088_ACC_NORMAL (0x2 << BMI088_ACC_BWP_SHFITS)
#define BMI088_ACC_ODR_SHFITS 0x0
#define BMI088_ACC_12_5_HZ (0x5 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_25_HZ (0x6 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_50_HZ (0x7 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_100_HZ (0x8 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_200_HZ (0x9 << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_400_HZ (0xA << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_800_HZ (0xB << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_1600_HZ (0xC << BMI088_ACC_ODR_SHFITS)
#define BMI088_ACC_RANGE 0x41
#define BMI088_ACC_RANGE_SHFITS 0x0
#define BMI088_ACC_RANGE_3G (0x0 << BMI088_ACC_RANGE_SHFITS)
#define BMI088_ACC_RANGE_6G (0x1 << BMI088_ACC_RANGE_SHFITS)
#define BMI088_ACC_RANGE_12G (0x2 << BMI088_ACC_RANGE_SHFITS)
#define BMI088_ACC_RANGE_24G (0x3 << BMI088_ACC_RANGE_SHFITS)
#define BMI088_INT1_IO_CTRL 0x53
#define BMI088_ACC_INT1_IO_ENABLE_SHFITS 0x3
#define BMI088_ACC_INT1_IO_ENABLE (0x1 << BMI088_ACC_INT1_IO_ENABLE_SHFITS)
#define BMI088_ACC_INT1_GPIO_MODE_SHFITS 0x2
#define BMI088_ACC_INT1_GPIO_PP (0x0 << BMI088_ACC_INT1_GPIO_MODE_SHFITS)
#define BMI088_ACC_INT1_GPIO_OD (0x1 << BMI088_ACC_INT1_GPIO_MODE_SHFITS)
#define BMI088_ACC_INT1_GPIO_LVL_SHFITS 0x1
#define BMI088_ACC_INT1_GPIO_LOW (0x0 << BMI088_ACC_INT1_GPIO_LVL_SHFITS)
#define BMI088_ACC_INT1_GPIO_HIGH (0x1 << BMI088_ACC_INT1_GPIO_LVL_SHFITS)
#define BMI088_INT2_IO_CTRL 0x54
#define BMI088_ACC_INT2_IO_ENABLE_SHFITS 0x3
#define BMI088_ACC_INT2_IO_ENABLE (0x1 << BMI088_ACC_INT2_IO_ENABLE_SHFITS)
#define BMI088_ACC_INT2_GPIO_MODE_SHFITS 0x2
#define BMI088_ACC_INT2_GPIO_PP (0x0 << BMI088_ACC_INT2_GPIO_MODE_SHFITS)
#define BMI088_ACC_INT2_GPIO_OD (0x1 << BMI088_ACC_INT2_GPIO_MODE_SHFITS)
#define BMI088_ACC_INT2_GPIO_LVL_SHFITS 0x1
#define BMI088_ACC_INT2_GPIO_LOW (0x0 << BMI088_ACC_INT2_GPIO_LVL_SHFITS)
#define BMI088_ACC_INT2_GPIO_HIGH (0x1 << BMI088_ACC_INT2_GPIO_LVL_SHFITS)
#define BMI088_INT_MAP_DATA 0x58
#define BMI088_ACC_INT2_DRDY_INTERRUPT_SHFITS 0x6
#define BMI088_ACC_INT2_DRDY_INTERRUPT (0x1 << BMI088_ACC_INT2_DRDY_INTERRUPT_SHFITS)
#define BMI088_ACC_INT1_DRDY_INTERRUPT_SHFITS 0x2
#define BMI088_ACC_INT1_DRDY_INTERRUPT (0x1 << BMI088_ACC_INT1_DRDY_INTERRUPT_SHFITS)
#define BMI088_ACC_SELF_TEST 0x6D
#define BMI088_ACC_SELF_TEST_OFF 0x00
#define BMI088_ACC_SELF_TEST_POSITIVE_SIGNAL 0x0D
#define BMI088_ACC_SELF_TEST_NEGATIVE_SIGNAL 0x09
#define BMI088_ACC_PWR_CONF 0x7C
#define BMI088_ACC_PWR_SUSPEND_MODE 0x03
#define BMI088_ACC_PWR_ACTIVE_MODE 0x00
#define BMI088_ACC_PWR_CTRL 0x7D
#define BMI088_ACC_ENABLE_ACC_OFF 0x00
#define BMI088_ACC_ENABLE_ACC_ON 0x04
#define BMI088_ACC_SOFTRESET 0x7E
#define BMI088_ACC_SOFTRESET_VALUE 0xB6
#define BMI088_GYRO_CHIP_ID 0x00
#define BMI088_GYRO_CHIP_ID_VALUE 0x0F
#define BMI088_GYRO_X_L 0x02
#define BMI088_GYRO_X_H 0x03
#define BMI088_GYRO_Y_L 0x04
#define BMI088_GYRO_Y_H 0x05
#define BMI088_GYRO_Z_L 0x06
#define BMI088_GYRO_Z_H 0x07
#define BMI088_GYRO_INT_STAT_1 0x0A
#define BMI088_GYRO_DYDR_SHFITS 0x7
#define BMI088_GYRO_DYDR (0x1 << BMI088_GYRO_DYDR_SHFITS)
#define BMI088_GYRO_RANGE 0x0F
#define BMI088_GYRO_RANGE_SHFITS 0x0
#define BMI088_GYRO_2000 (0x0 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_1000 (0x1 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_500 (0x2 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_250 (0x3 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_125 (0x4 << BMI088_GYRO_RANGE_SHFITS)
#define BMI088_GYRO_BANDWIDTH 0x10
// the first num means Output data rate, the second num means bandwidth
#define BMI088_GYRO_BANDWIDTH_MUST_Set 0x80
#define BMI088_GYRO_2000_532_HZ 0x00
#define BMI088_GYRO_2000_230_HZ 0x01
#define BMI088_GYRO_1000_116_HZ 0x02
#define BMI088_GYRO_400_47_HZ 0x03
#define BMI088_GYRO_200_23_HZ 0x04
#define BMI088_GYRO_100_12_HZ 0x05
#define BMI088_GYRO_200_64_HZ 0x06
#define BMI088_GYRO_100_32_HZ 0x07
#define BMI088_GYRO_LPM1 0x11
#define BMI088_GYRO_NORMAL_MODE 0x00
#define BMI088_GYRO_SUSPEND_MODE 0x80
#define BMI088_GYRO_DEEP_SUSPEND_MODE 0x20
#define BMI088_GYRO_SOFTRESET 0x14
#define BMI088_GYRO_SOFTRESET_VALUE 0xB6
#define BMI088_GYRO_CTRL 0x15
#define BMI088_DRDY_OFF 0x00
#define BMI088_DRDY_ON 0x80
#define BMI088_GYRO_INT3_INT4_IO_CONF 0x16
#define BMI088_GYRO_INT4_GPIO_MODE_SHFITS 0x3
#define BMI088_GYRO_INT4_GPIO_PP (0x0 << BMI088_GYRO_INT4_GPIO_MODE_SHFITS)
#define BMI088_GYRO_INT4_GPIO_OD (0x1 << BMI088_GYRO_INT4_GPIO_MODE_SHFITS)
#define BMI088_GYRO_INT4_GPIO_LVL_SHFITS 0x2
#define BMI088_GYRO_INT4_GPIO_LOW (0x0 << BMI088_GYRO_INT4_GPIO_LVL_SHFITS)
#define BMI088_GYRO_INT4_GPIO_HIGH (0x1 << BMI088_GYRO_INT4_GPIO_LVL_SHFITS)
#define BMI088_GYRO_INT3_GPIO_MODE_SHFITS 0x1
#define BMI088_GYRO_INT3_GPIO_PP (0x0 << BMI088_GYRO_INT3_GPIO_MODE_SHFITS)
#define BMI088_GYRO_INT3_GPIO_OD (0x1 << BMI088_GYRO_INT3_GPIO_MODE_SHFITS)
#define BMI088_GYRO_INT3_GPIO_LVL_SHFITS 0x0
#define BMI088_GYRO_INT3_GPIO_LOW (0x0 << BMI088_GYRO_INT3_GPIO_LVL_SHFITS)
#define BMI088_GYRO_INT3_GPIO_HIGH (0x1 << BMI088_GYRO_INT3_GPIO_LVL_SHFITS)
#define BMI088_GYRO_INT3_INT4_IO_MAP 0x18
#define BMI088_GYRO_DRDY_IO_OFF 0x00
#define BMI088_GYRO_DRDY_IO_INT3 0x01
#define BMI088_GYRO_DRDY_IO_INT4 0x80
#define BMI088_GYRO_DRDY_IO_BOTH (BMI088_GYRO_DRDY_IO_INT3 | BMI088_GYRO_DRDY_IO_INT4)
#define BMI088_GYRO_SELF_TEST 0x3C
#define BMI088_GYRO_RATE_OK_SHFITS 0x4
#define BMI088_GYRO_RATE_OK (0x1 << BMI088_GYRO_RATE_OK_SHFITS)
#define BMI088_GYRO_BIST_FAIL_SHFITS 0x2
#define BMI088_GYRO_BIST_FAIL (0x1 << BMI088_GYRO_BIST_FAIL_SHFITS)
#define BMI088_GYRO_BIST_RDY_SHFITS 0x1
#define BMI088_GYRO_BIST_RDY (0x1 << BMI088_GYRO_BIST_RDY_SHFITS)
#define BMI088_GYRO_TRIG_BIST_SHFITS 0x0
#define BMI088_GYRO_TRIG_BIST (0x1 << BMI088_GYRO_TRIG_BIST_SHFITS)
#endif

351
User_Code/module/periph/imu/ins_task.c Normal file
View File

@@ -0,0 +1,351 @@
/**
******************************************************************************
* @file ins_task.c
* @author Wang Hongxi
* @author annotation and modificaiton by neozng
* @version V2.0.0
* @date 2022/2/23
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#include "ins_task.h"
#include "controller.h"
#include "QuaternionEKF.h"
#include "spi.h"
#include "tim.h"
#include "user_lib.h"
#include "general_def.h"
// #include "master_process.h"
static INS_t INS;
static IMU_Param_t IMU_Param;
static PIDInstance TempCtrl = {0};
//旋转矩阵
const float xb[3] = {1, 0, 0};
const float yb[3] = {0, 1, 0};
const float zb[3] = {0, 0, 1};
// 用于获取两次采样之间的时间间隔
static uint32_t INS_DWT_Count = 0;
static float dt = 0, t = 0;
static float RefTemp = 40; // 恒温设定温度
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]);
static void IMUPWMSet(uint16_t pwm)
{
__HAL_TIM_SetCompare(&htim3, TIM_CHANNEL_4, pwm);
}
/**
* @brief 温度控制
*
*/
static void IMU_Temperature_Ctrl(void)
{
PIDCalculate(&TempCtrl, BMI088.Temperature, RefTemp);
IMUPWMSet(float_constrain(float_rounding(TempCtrl.Output), 0, UINT32_MAX));
}
// 使用加速度计的数据初始化Roll和Pitch,而Yaw置0,这样可以避免在初始时候的姿态估计误差
static void InitQuaternion(float *init_q4)
{
float acc_init[3] = {0};
float gravity_norm[3] = {0, 0, 1}; // 导航系重力加速度矢量,归一化后为(0,0,1)
float axis_rot[3] = {0}; // 旋转轴
// 读取100次加速度计数据,取平均值作为初始值
for (uint8_t i = 0; i < 100; ++i)
{
BMI088_Read(&BMI088);
acc_init[X] += BMI088.Accel[X];
acc_init[Y] += BMI088.Accel[Y];
acc_init[Z] += BMI088.Accel[Z];
DWT_Delay(0.001);
}
for (uint8_t i = 0; i < 3; ++i)
acc_init[i] /= 100;
Norm3d(acc_init);
// 计算原始加速度矢量和导航系重力加速度矢量的夹角
float angle = acosf(Dot3d(acc_init, gravity_norm));
Cross3d(acc_init, gravity_norm, axis_rot);
Norm3d(axis_rot);
init_q4[0] = cosf(angle / 2.0f);
for (uint8_t i = 0; i < 2; ++i)
init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量)
}
attitude_t *INS_Init(void)
{
if (!INS.init)
INS.init = 1;
else
return (attitude_t *) &INS.Gyro;
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_4);
while (BMI088Init(&hspi2, 1) != BMI088_NO_ERROR);
IMU_Param.scale[X] = 1;
IMU_Param.scale[Y] = 1;
IMU_Param.scale[Z] = 1;
IMU_Param.Yaw = 0;
IMU_Param.Pitch = 0;
IMU_Param.Roll = 0;
IMU_Param.flag = 1;
float init_quaternion[4] = {0};
InitQuaternion(init_quaternion);
IMU_QuaternionEKF_Init(init_quaternion, 10, 0.001, 1000000, 1, 0);
// imu heat init
PID_Init_Config_s config = {.MaxOut = 2000,
.IntegralLimit = 300,
.DeadBand = 0,
.Kp = 1000,
.Ki = 20,
.Kd = 0,
.Improve = 0x01}; // enable integratiaon limit
PIDInit(&TempCtrl, &config);
// noise of accel is relatively big and of high freq,thus lpf is used
INS.AccelLPF = 0.0085;
DWT_GetDeltaT(&INS_DWT_Count);
return (attitude_t *) &INS.Gyro; // @todo: 这里偷懒了,不要这样做! 修改INT_t结构体可能会导致异常,待修复.
}
/* 注意以1kHz的频率运行此任务 */
void INS_Task(void)
{
static uint32_t count = 0;
const float gravity[3] = {0, 0, 9.81f};
dt = DWT_GetDeltaT(&INS_DWT_Count);
t += dt;
// ins update
if ((count % 1) == 0)
{
BMI088_Read(&BMI088);
INS.Accel[X] = BMI088.Accel[X];
INS.Accel[Y] = BMI088.Accel[Y];
INS.Accel[Z] = BMI088.Accel[Z];
INS.Gyro[X] = BMI088.Gyro[X];
INS.Gyro[Y] = BMI088.Gyro[Y];
INS.Gyro[Z] = BMI088.Gyro[Z];
// demo function,用于修正安装误差,可以不管,本demo暂时没用
IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);
// 计算重力加速度矢量和b系的XY两轴的夹角,可用作功能扩展,本demo暂时没用
// INS.atanxz = -atan2f(INS.Accel[X], INS.Accel[Z]) * 180 / PI;
// INS.atanyz = atan2f(INS.Accel[Y], INS.Accel[Z]) * 180 / PI;
// 核心函数,EKF更新四元数
IMU_QuaternionEKF_Update(INS.Gyro[X], INS.Gyro[Y], INS.Gyro[Z], INS.Accel[X], INS.Accel[Y], INS.Accel[Z], dt);
memcpy(INS.q, QEKF_INS.q, sizeof(QEKF_INS.q));
// 机体系基向量转换到导航坐标系，本例选取惯性系为导航系
BodyFrameToEarthFrame(xb, INS.xn, INS.q);
BodyFrameToEarthFrame(yb, INS.yn, INS.q);
BodyFrameToEarthFrame(zb, INS.zn, INS.q);
// 将重力从导航坐标系n转换到机体系b,随后根据加速度计数据计算运动加速度
float gravity_b[3];
EarthFrameToBodyFrame(gravity, gravity_b, INS.q);
for (uint8_t i = 0; i < 3; ++i) // 同样过一个低通滤波
{
INS.MotionAccel_b[i] = (INS.Accel[i] - gravity_b[i]) * dt / (INS.AccelLPF + dt) + INS.MotionAccel_b[i] * INS
.AccelLPF / (INS.AccelLPF + dt);
}
BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n
INS.Yaw = QEKF_INS.Yaw;
INS.Pitch = QEKF_INS.Pitch;
INS.Roll = QEKF_INS.Roll;
INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
// VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
}
// temperature control
if ((count % 2) == 0)
{
// 500hz
IMU_Temperature_Ctrl();
}
if ((count++ % 1000) == 0)
{
// 1Hz 可以加入monitor函数,检查IMU是否正常运行/离线
}
}
/**
* @brief Transform 3dvector from BodyFrame to EarthFrame
* @param[1] vector in BodyFrame
* @param[2] vector in EarthFrame
* @param[3] quaternion
*/
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q)
{
vecEF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecBF[0] +
(q[1] * q[2] - q[0] * q[3]) * vecBF[1] +
(q[1] * q[3] + q[0] * q[2]) * vecBF[2]);
vecEF[1] = 2.0f * ((q[1] * q[2] + q[0] * q[3]) * vecBF[0] +
(0.5f - q[1] * q[1] - q[3] * q[3]) * vecBF[1] +
(q[2] * q[3] - q[0] * q[1]) * vecBF[2]);
vecEF[2] = 2.0f * ((q[1] * q[3] - q[0] * q[2]) * vecBF[0] +
(q[2] * q[3] + q[0] * q[1]) * vecBF[1] +
(0.5f - q[1] * q[1] - q[2] * q[2]) * vecBF[2]);
}
/**
* @brief Transform 3dvector from EarthFrame to BodyFrame
* @param[1] vector in EarthFrame
* @param[2] vector in BodyFrame
* @param[3] quaternion
*/
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q)
{
vecBF[0] = 2.0f * ((0.5f - q[2] * q[2] - q[3] * q[3]) * vecEF[0] +
(q[1] * q[2] + q[0] * q[3]) * vecEF[1] +
(q[1] * q[3] - q[0] * q[2]) * vecEF[2]);
vecBF[1] = 2.0f * ((q[1] * q[2] - q[0] * q[3]) * vecEF[0] +
(0.5f - q[1] * q[1] - q[3] * q[3]) * vecEF[1] +
(q[2] * q[3] + q[0] * q[1]) * vecEF[2]);
vecBF[2] = 2.0f * ((q[1] * q[3] + q[0] * q[2]) * vecEF[0] +
(q[2] * q[3] - q[0] * q[1]) * vecEF[1] +
(0.5f - q[1] * q[1] - q[2] * q[2]) * vecEF[2]);
}
/**
* @brief reserved.用于修正IMU安装误差与标度因数误差,即陀螺仪轴和云台轴的安装偏移
*
*
* @param param IMU参数
* @param gyro 角速度
* @param accel 加速度
*/
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3])
{
static float lastYawOffset, lastPitchOffset, lastRollOffset;
static float c_11, c_12, c_13, c_21, c_22, c_23, c_31, c_32, c_33;
float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll;
if (fabsf(param->Yaw - lastYawOffset) > 0.001f ||
fabsf(param->Pitch - lastPitchOffset) > 0.001f ||
fabsf(param->Roll - lastRollOffset) > 0.001f || param->flag)
{
cosYaw = arm_cos_f32(param->Yaw / 57.295779513f);
cosPitch = arm_cos_f32(param->Pitch / 57.295779513f);
cosRoll = arm_cos_f32(param->Roll / 57.295779513f);
sinYaw = arm_sin_f32(param->Yaw / 57.295779513f);
sinPitch = arm_sin_f32(param->Pitch / 57.295779513f);
sinRoll = arm_sin_f32(param->Roll / 57.295779513f);
// 1.yaw(alpha) 2.pitch(beta) 3.roll(gamma)
c_11 = cosYaw * cosRoll + sinYaw * sinPitch * sinRoll;
c_12 = cosPitch * sinYaw;
c_13 = cosYaw * sinRoll - cosRoll * sinYaw * sinPitch;
c_21 = cosYaw * sinPitch * sinRoll - cosRoll * sinYaw;
c_22 = cosYaw * cosPitch;
c_23 = -sinYaw * sinRoll - cosYaw * cosRoll * sinPitch;
c_31 = -cosPitch * sinRoll;
c_32 = sinPitch;
c_33 = cosPitch * cosRoll;
param->flag = 0;
}
float gyro_temp[3];
for (uint8_t i = 0; i < 3; ++i)
gyro_temp[i] = gyro[i] * param->scale[i];
gyro[X] = c_11 * gyro_temp[X] +
c_12 * gyro_temp[Y] +
c_13 * gyro_temp[Z];
gyro[Y] = c_21 * gyro_temp[X] +
c_22 * gyro_temp[Y] +
c_23 * gyro_temp[Z];
gyro[Z] = c_31 * gyro_temp[X] +
c_32 * gyro_temp[Y] +
c_33 * gyro_temp[Z];
float accel_temp[3];
for (uint8_t i = 0; i < 3; ++i)
accel_temp[i] = accel[i];
accel[X] = c_11 * accel_temp[X] +
c_12 * accel_temp[Y] +
c_13 * accel_temp[Z];
accel[Y] = c_21 * accel_temp[X] +
c_22 * accel_temp[Y] +
c_23 * accel_temp[Z];
accel[Z] = c_31 * accel_temp[X] +
c_32 * accel_temp[Y] +
c_33 * accel_temp[Z];
lastYawOffset = param->Yaw;
lastPitchOffset = param->Pitch;
lastRollOffset = param->Roll;
}
//------------------------------------functions below are not used in this demo-------------------------------------------------
//----------------------------------you can read them for learning or programming-----------------------------------------------
//----------------------------------they could also be helpful for further design-----------------------------------------------
/**
* @brief Update quaternion
*/
void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt)
{
float qa, qb, qc;
gx *= 0.5f * dt;
gy *= 0.5f * dt;
gz *= 0.5f * dt;
qa = q[0];
qb = q[1];
qc = q[2];
q[0] += (-qb * gx - qc * gy - q[3] * gz);
q[1] += (qa * gx + qc * gz - q[3] * gy);
q[2] += (qa * gy - qb * gz + q[3] * gx);
q[3] += (qa * gz + qb * gy - qc * gx);
}
/**
* @brief Convert quaternion to eular angle
*/
void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll)
{
*Yaw = atan2f(2.0f * (q[0] * q[3] + q[1] * q[2]), 2.0f * (q[0] * q[0] + q[1] * q[1]) - 1.0f) * 57.295779513f;
*Pitch = atan2f(2.0f * (q[0] * q[1] + q[2] * q[3]), 2.0f * (q[0] * q[0] + q[3] * q[3]) - 1.0f) * 57.295779513f;
*Roll = asinf(2.0f * (q[0] * q[2] - q[1] * q[3])) * 57.295779513f;
}
/**
* @brief Convert eular angle to quaternion
*/
void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q)
{
float cosPitch, cosYaw, cosRoll, sinPitch, sinYaw, sinRoll;
Yaw /= 57.295779513f;
Pitch /= 57.295779513f;
Roll /= 57.295779513f;
cosPitch = arm_cos_f32(Pitch / 2);
cosYaw = arm_cos_f32(Yaw / 2);
cosRoll = arm_cos_f32(Roll / 2);
sinPitch = arm_sin_f32(Pitch / 2);
sinYaw = arm_sin_f32(Yaw / 2);
sinRoll = arm_sin_f32(Roll / 2);
q[0] = cosPitch * cosRoll * cosYaw + sinPitch * sinRoll * sinYaw;
q[1] = sinPitch * cosRoll * cosYaw - cosPitch * sinRoll * sinYaw;
q[2] = sinPitch * cosRoll * sinYaw + cosPitch * sinRoll * cosYaw;
q[3] = cosPitch * cosRoll * sinYaw - sinPitch * sinRoll * cosYaw;
}

143
User_Code/module/periph/imu/ins_task.h Normal file
View File

@@ -0,0 +1,143 @@
/**
******************************************************************************
* @file ins_task.h
* @author Wang Hongxi
* @author annotation and modification by NeoZeng
* @version V2.0.0
* @date 2022/2/23
* @brief
******************************************************************************
* @attention INS任务的初始化不要放入实时系统!应该由application拥有实例,随后在
* 应用层调用初始化函数.
*
******************************************************************************
*/
#ifndef __INS_TASK_H
#define __INS_TASK_H
#include "stdint.h"
#include "BMI088driver.h"
#include "QuaternionEKF.h"
#define X 0
#define Y 1
#define Z 2
#define INS_TASK_PERIOD 1
typedef struct
{
float Gyro[3]; // 角速度
float Accel[3]; // 加速度
// 还需要增加角速度数据
float Roll;
float Pitch;
float Yaw;
float YawTotalAngle;
} attitude_t; // 最终解算得到的角度,以及yaw转动的总角度(方便多圈控制)
typedef struct
{
float q[4]; // 四元数估计值
float MotionAccel_b[3]; // 机体坐标加速度
float MotionAccel_n[3]; // 绝对系加速度
float AccelLPF; // 加速度低通滤波系数
// bodyframe在绝对系的向量表示
float xn[3];
float yn[3];
float zn[3];
// 加速度在机体系和XY两轴的夹角
// float atanxz;
// float atanyz;
// IMU量测值
float Gyro[3]; // 角速度
float Accel[3]; // 加速度
// 位姿
float Roll;
float Pitch;
float Yaw;
float YawTotalAngle;
uint8_t init;
} INS_t;
/* 用于修正安装误差的参数 */
typedef struct
{
uint8_t flag;
float scale[3];
float Yaw;
float Pitch;
float Roll;
} IMU_Param_t;
/**
* @brief 初始化惯导解算系统
*
*/
attitude_t *INS_Init(void);
/**
* @brief 此函数放入实时系统中,以1kHz频率运行
* p.s. osDelay(1);
*
*/
void INS_Task(void);
/**
* @brief 四元数更新函数,即实现dq/dt=0.5Ωq
*
* @param q 四元数
* @param gx
* @param gy
* @param gz
* @param dt 距离上次调用的时间间隔
*/
void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt);
/**
* @brief 四元数转换成欧拉角 ZYX
*
* @param q
* @param Yaw
* @param Pitch
* @param Roll
*/
void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll);
/**
* @brief ZYX欧拉角转换为四元数
*
* @param Yaw
* @param Pitch
* @param Roll
* @param q
*/
void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q);
/**
* @brief 机体系到惯性系的变换函数
*
* @param vecBF body frame
* @param vecEF earth frame
* @param q
*/
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q);
/**
* @brief 惯性系转换到机体系
*
* @param vecEF
* @param vecBF
* @param q
*/
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q);
#endif