TuxMonkey/bf_original_balance_chassis
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/bf_original_balance_chassis synced 2026-07-23 19:25:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

init commit

Browse Source
This commit is contained in:
NeoZng
2022-10-20 17:13:02 +08:00
commit 78f85b7fda
518 changed files with 409143 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
modules/imu/BMI088Middleware.c Normal file
View File

@@ -0,0 +1,29 @@
#include "BMI088Middleware.h"
#include "main.h"
SPI_HandleTypeDef *BMI088_SPI;
void BMI088_ACCEL_NS_L(void)
{
HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, GPIO_PIN_RESET);
}
void BMI088_ACCEL_NS_H(void)
{
HAL_GPIO_WritePin(CS1_ACCEL_GPIO_Port, CS1_ACCEL_Pin, GPIO_PIN_SET);
}
void BMI088_GYRO_NS_L(void)
{
HAL_GPIO_WritePin(CS1_GYRO_GPIO_Port, CS1_GYRO_Pin, GPIO_PIN_RESET);
}
void BMI088_GYRO_NS_H(void)
{
HAL_GPIO_WritePin(CS1_GYRO_GPIO_Port, CS1_GYRO_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, 1000);
return rx_data;
}

32
modules/imu/BMI088Middleware.h Normal file
View File

@@ -0,0 +1,32 @@
#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

418
modules/imu/BMI088driver.c Normal file
View File

@@ -0,0 +1,418 @@
/**
******************************************************************************
* @file BMI088driver.c
* @author
* @version V1.2.0
* @date 2022/3/8
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#include "BMI088driver.h"
#include "BMI088reg.h"
#include "BMI088Middleware.h"
#include "bsp_dwt.h"
#include <math.h>
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;
static uint32_t offset_cal_DWT_Count = 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 write_BMI088_accel_reg_data_error[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_800_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 write_BMI088_gyro_reg_data_error[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 BMI088_init(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, gNormMax, gNormMin;
startTime = DWT_GetTimeline_s();
do
{
if (DWT_GetTimeline_s() - startTime > 10)
{
// <20><>????
bmi088->GyroOffset[0] = GxOFFSET;
bmi088->GyroOffset[1] = GyOFFSET;
bmi088->GyroOffset[2] = GzOFFSET;
bmi088->gNorm = gNORM;
bmi088->TempWhenCali = 40;
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.15f ||
gyroDiff[1] > 0.15f ||
gyroDiff[2] > 0.15f)
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);
HAL_Delay(1);
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
HAL_Delay(1);
// accel software reset
BMI088_accel_write_single_reg(BMI088_ACC_SOFTRESET, BMI088_ACC_SOFTRESET_VALUE);
HAL_Delay(BMI088_LONG_DELAY_TIME);
// check commiunication is normal after reset
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
HAL_Delay(1);
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
HAL_Delay(1);
// check the "who am I"
if (res != BMI088_ACC_CHIP_ID_VALUE)
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(write_BMI088_accel_reg_data_error[write_reg_num][0], write_BMI088_accel_reg_data_error[write_reg_num][1]);
HAL_Delay(1);
BMI088_accel_read_single_reg(write_BMI088_accel_reg_data_error[write_reg_num][0], res);
HAL_Delay(1);
if (res != write_BMI088_accel_reg_data_error[write_reg_num][1])
{
// write_reg_num--;
// return write_BMI088_accel_reg_data_error[write_reg_num][2];
error |= write_BMI088_accel_reg_data_error[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(1);
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
HAL_Delay(1);
// reset the gyro sensor
BMI088_gyro_write_single_reg(BMI088_GYRO_SOFTRESET, BMI088_GYRO_SOFTRESET_VALUE);
HAL_Delay(BMI088_LONG_DELAY_TIME);
// check commiunication is normal after reset
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
HAL_Delay(1);
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
HAL_Delay(1);
// check the "who am I"
if (res != BMI088_GYRO_CHIP_ID_VALUE)
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(write_BMI088_gyro_reg_data_error[write_reg_num][0], write_BMI088_gyro_reg_data_error[write_reg_num][1]);
HAL_Delay(1);
BMI088_gyro_read_single_reg(write_BMI088_gyro_reg_data_error[write_reg_num][0], res);
HAL_Delay(1);
if (res != write_BMI088_gyro_reg_data_error[write_reg_num][1])
{
write_reg_num--;
// return write_BMI088_gyro_reg_data_error[write_reg_num][2];
error |= write_BMI088_accel_reg_data_error[write_reg_num][2];
}
}
return BMI088_NO_ERROR;
}
void BMI088_Read(IMU_Data_t *bmi088)
{
static uint8_t buf[8] = {0, 0, 0, 0, 0, 0};
static int16_t bmi088_raw_temp;
static float dt=1.0;
static uint8_t first_read_flag=0;
if(!first_read_flag)
{
first_read_flag=1;
DWT_GetDeltaT(&offset_cal_DWT_Count);
}
else
{
dt=DWT_GetDeltaT(&offset_cal_DWT_Count)*1000.0;
}
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]*dt;
bmi088_raw_temp = (int16_t)((buf[5]) << 8) | buf[4];
bmi088->Gyro[1] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[1]*dt;
bmi088_raw_temp = (int16_t)((buf[7]) << 8) | buf[6];
bmi088->Gyro[2] = bmi088_raw_temp * BMI088_GYRO_SEN - bmi088->GyroOffset[2]*dt;
}
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

121
modules/imu/BMI088driver.h Normal file
View File

@@ -0,0 +1,121 @@
/**
******************************************************************************
* @file BMI088driver.h
* @author
* @version V1.1.2
* @version V1.2.0
* @date 2022/3/8
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#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
typedef struct
{
float Accel[3];
float Gyro[3];
float TempWhenCali;
float Temperature;
float AccelScale;
float GyroOffset[3];
float gNorm;
} IMU_Data_t;
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,
};
void BMI088_Init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate);
extern uint8_t BMI088_init(SPI_HandleTypeDef *bmi088_SPI, uint8_t calibrate);
extern uint8_t bmi088_accel_init(void);
extern uint8_t bmi088_gyro_init(void);
extern IMU_Data_t BMI088;
extern void BMI088_Read(IMU_Data_t *bmi088);
#endif

180
modules/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

296
modules/imu/ins_task.c Normal file
View File

@@ -0,0 +1,296 @@
/**
******************************************************************************
* @file ins_task.c
* @author Wang Hongxi
* @version V2.0.0
* @date 2022/2/23
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#include "ins_task.h"
#include "controller.h"
#include "QuaternionEKF.h"
#include "bsp_temperature.h"
#include "spi.h"
INS_t INS;
IMU_Param_t IMU_Param;
PID_t TempCtrl = {0};
const float xb[3] = {1, 0, 0};
const float yb[3] = {0, 1, 0};
const float zb[3] = {0, 0, 1};
uint32_t INS_DWT_Count = 0;
static float dt = 0, t = 0;
uint8_t ins_debug_mode = 0;
float RefTemp = 40;
static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[3]);
void INS_Init(void)
{
// while (BMI088_init(&hspi1, 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;
IMU_QuaternionEKF_Init(10, 0.001, 10000000, 1, 0);
// imu heat init
PID_Init(&TempCtrl, 2000, 300, 0, 1000, 20, 0, 0, 0, 0, 0, 0, 0);
// noise of accel is relatively big and of high freq,thus lpf is used
INS.AccelLPF = 0.0085;
}
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;
}
// temperature control
if ((count % 2) == 0)
{
// 500hz
IMU_Temperature_Ctrl();
}
if ((count % 1000) == 0)
{
// 200hz
}
count++;
}
/**
* @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;
}
/**
* @brief 温度控制
*
*/
void IMU_Temperature_Ctrl(void)
{
PID_Calculate(&TempCtrl, BMI088.Temperature, RefTemp);
imu_pwm_set(float_constrain(float_rounding(TempCtrl.Output), 0, UINT32_MAX));
}
//------------------------------------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;
}

80
modules/imu/ins_task.h Normal file
View File

@@ -0,0 +1,80 @@
/**
******************************************************************************
* @file ins_task.h
* @author Wang Hongxi
* @version V2.0.0
* @date 2022/2/23
* @brief
******************************************************************************
* @attention
*
******************************************************************************
*/
#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 q[4]; // 四元数估计值
float Gyro[3]; // 角速度
float Accel[3]; // 加速度
float MotionAccel_b[3]; // 机体坐标加速度
float MotionAccel_n[3]; // 绝对系加速度
float AccelLPF; // 加速度低通滤波系数
// 加速度在绝对系的向量表示
float xn[3];
float yn[3];
float zn[3];
float atanxz;
float atanyz;
// 位姿
float Roll;
float Pitch;
float Yaw;
float YawTotalAngle;
} INS_t;
/**
* @brief 用于修正安装误差的参数,demo中可无视
*
*/
typedef struct
{
uint8_t flag;
float scale[3];
float Yaw;
float Pitch;
float Roll;
} IMU_Param_t;
extern INS_t INS;
void INS_Init(void);
void INS_Task(void);
void IMU_Temperature_Ctrl(void);
void QuaternionUpdate(float *q, float gx, float gy, float gz, float dt);
void QuaternionToEularAngle(float *q, float *Yaw, float *Pitch, float *Roll);
void EularAngleToQuaternion(float Yaw, float Pitch, float Roll, float *q);
void BodyFrameToEarthFrame(const float *vecBF, float *vecEF, float *q);
void EarthFrameToBodyFrame(const float *vecEF, float *vecBF, float *q);
#endif