406 lines
14 KiB
C
406 lines
14 KiB
C
#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_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 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, gNormMax, gNormMin;
|
||
|
||
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.15f ||
|
||
gyroDiff[1] > 0.15f ||
|
||
gyroDiff[2] > 0.15f)
|
||
{
|
||
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);
|
||
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
|
||
DWT_Delay(0.001);
|
||
// accel software reset
|
||
BMI088_accel_write_single_reg(BMI088_ACC_SOFTRESET, BMI088_ACC_SOFTRESET_VALUE);
|
||
// HAL_Delay(BMI088_LONG_DELAY_TIME);
|
||
DWT_Delay(0.08);
|
||
// check commiunication is normal after reset
|
||
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
|
||
DWT_Delay(0.001);
|
||
BMI088_accel_read_single_reg(BMI088_ACC_CHIP_ID, res);
|
||
DWT_Delay(0.001);
|
||
|
||
// 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);
|
||
|
||
BMI088_accel_read_single_reg(BMI088_Accel_Init_Table[write_reg_num][0], res);
|
||
DWT_Delay(0.001);
|
||
|
||
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);
|
||
DWT_Delay(0.001);
|
||
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
|
||
DWT_Delay(0.001);
|
||
|
||
// reset the gyro sensor
|
||
BMI088_gyro_write_single_reg(BMI088_GYRO_SOFTRESET, BMI088_GYRO_SOFTRESET_VALUE);
|
||
// HAL_Delay(BMI088_LONG_DELAY_TIME);
|
||
DWT_Delay(0.08);
|
||
// check commiunication is normal after reset
|
||
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
|
||
DWT_Delay(0.001);
|
||
BMI088_gyro_read_single_reg(BMI088_GYRO_CHIP_ID, res);
|
||
DWT_Delay(0.001);
|
||
|
||
// 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]);
|
||
DWT_Delay(0.001);
|
||
|
||
BMI088_gyro_read_single_reg(BMI088_Gyro_Init_Table[write_reg_num][0], res);
|
||
DWT_Delay(0.001);
|
||
|
||
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
|