465 lines
11 KiB
C
465 lines
11 KiB
C
|
|
#include "InertialSensor_BMI088.h"
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
#define GRAVITY_MSS 9.80665f
|
|
|
|
/*
|
|
device registers, names follow datasheet conventions, with REGA_
|
|
prefix for accel, and REGG_ prefix for gyro
|
|
*/
|
|
#define REGA_CHIPID 0x00
|
|
#define REGA_ERR_REG 0x02
|
|
#define REGA_STATUS 0x03
|
|
#define REGA_X_LSB 0x12
|
|
#define REGA_INT_STATUS_1 0x1D
|
|
#define REGA_TEMP_LSB 0x22
|
|
#define REGA_TEMP_MSB 0x23
|
|
#define REGA_CONF 0x40
|
|
#define REGA_RANGE 0x41
|
|
#define REGA_PWR_CONF 0x7C
|
|
#define REGA_PWR_CTRL 0x7D
|
|
#define REGA_SOFTRESET 0x7E
|
|
#define REGA_FIFO_CONFIG0 0x48
|
|
#define REGA_FIFO_CONFIG1 0x49
|
|
#define REGA_FIFO_DOWNS 0x45
|
|
#define REGA_FIFO_DATA 0x26
|
|
#define REGA_FIFO_LEN0 0x24
|
|
#define REGA_FIFO_LEN1 0x25
|
|
|
|
#define REGG_CHIPID 0x00
|
|
#define REGA_RATE_X_LSB 0x02
|
|
#define REGG_INT_STATUS_1 0x0A
|
|
#define REGG_INT_STATUS_2 0x0B
|
|
#define REGG_INT_STATUS_3 0x0C
|
|
#define REGG_FIFO_STATUS 0x0E
|
|
#define REGG_RANGE 0x0F
|
|
#define REGG_BW 0x10
|
|
#define REGG_LPM1 0x11
|
|
#define REGG_RATE_HBW 0x13
|
|
#define REGG_BGW_SOFTRESET 0x14
|
|
#define REGG_FIFO_CONFIG_1 0x3E
|
|
#define REGG_FIFO_DATA 0x3F
|
|
|
|
#define ACCEL_BACKEND_SAMPLE_RATE 1600
|
|
#define GYRO_BACKEND_SAMPLE_RATE 2000
|
|
|
|
static bool read_registers(SPI_DEV_t *dev, uint8_t reg, uint8_t *buf,
|
|
uint32_t size)
|
|
{
|
|
SPI_DEV_select(dev);
|
|
bool rslt = SPI_DEV_read_registers(dev, reg, buf, size);
|
|
SPI_DEV_unselect(dev);
|
|
return rslt;
|
|
}
|
|
|
|
bool write_register(SPI_DEV_t *dev, uint8_t reg, uint8_t val)
|
|
{
|
|
SPI_DEV_select(dev);
|
|
bool rslt = SPI_DEV_write_register(dev, reg, val);
|
|
SPI_DEV_unselect(dev);
|
|
return rslt;
|
|
}
|
|
|
|
/*
|
|
read from accelerometer registers, special SPI handling needed
|
|
*/
|
|
static bool read_accel_registers(InertialSensor_BMI088_t *imu, uint8_t reg, uint8_t *data, uint8_t len)
|
|
{
|
|
// for SPI we need to discard the first returned byte. See
|
|
// datasheet for explanation
|
|
uint8_t b[len + 2];
|
|
SPI_DEV_select(imu->dev_accel);
|
|
b[0] = reg | 0x80;
|
|
memset(&b[1], 0, len + 1);
|
|
if (!SPI_DEV_transfer(imu->dev_accel, b, len + 2, b, len + 2, 2))
|
|
{
|
|
SPI_DEV_unselect(imu->dev_accel);
|
|
return false;
|
|
}
|
|
memcpy(data, &b[2], len);
|
|
SPI_DEV_unselect(imu->dev_accel);
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
write to accel registers with retries. The SPI sensor may take
|
|
several tries to correctly write a register
|
|
*/
|
|
static bool write_accel_register(InertialSensor_BMI088_t *imu, uint8_t reg, uint8_t v)
|
|
{
|
|
for (uint8_t i = 0; i < 8; i++)
|
|
{
|
|
write_register(imu->dev_accel, reg, v);
|
|
uint8_t v2 = 0;
|
|
if (read_accel_registers(imu, reg, &v2, 1) && v2 == v)
|
|
{
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
static const struct
|
|
{
|
|
uint8_t reg;
|
|
uint8_t value;
|
|
} accel_config[] = {
|
|
{REGA_CONF, 0xAC},
|
|
// setup 24g range
|
|
{REGA_RANGE, 0x03},
|
|
// disable low-power mode
|
|
{REGA_PWR_CONF, 0},
|
|
{REGA_PWR_CTRL, 0x04},
|
|
// setup FIFO for streaming X,Y,Z
|
|
{REGA_FIFO_CONFIG0, 0x00},
|
|
{REGA_FIFO_CONFIG1, 0x50},
|
|
};
|
|
|
|
#ifndef ARRAY_SIZE
|
|
#define ARRAY_SIZE(x) (sizeof(x) / sizeof((x)[0]))
|
|
#endif
|
|
|
|
static bool setup_accel_config(InertialSensor_BMI088_t *imu)
|
|
{
|
|
if (imu->done_accel_config)
|
|
{
|
|
return true;
|
|
}
|
|
imu->accel_config_count++;
|
|
for (uint8_t i = 0; i < ARRAY_SIZE(accel_config); i++)
|
|
{
|
|
uint8_t v;
|
|
if (!read_accel_registers(imu, accel_config[i].reg, &v, 1))
|
|
{
|
|
return false;
|
|
}
|
|
if (v == accel_config[i].value)
|
|
{
|
|
continue;
|
|
}
|
|
if (!write_accel_register(imu, accel_config[i].reg, accel_config[i].value))
|
|
{
|
|
return false;
|
|
}
|
|
}
|
|
imu->done_accel_config = true;
|
|
printf("BMI088: accel config OK (%u tries)\n", (unsigned)imu->accel_config_count);
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
probe and initialise accelerometer
|
|
*/
|
|
static bool accel_init(InertialSensor_BMI088_t *imu)
|
|
{
|
|
uint8_t v;
|
|
|
|
// dummy ready on accel ChipID to init accel (see section 3 of datasheet)
|
|
read_accel_registers(imu, REGA_CHIPID, &v, 1);
|
|
|
|
if (!read_accel_registers(imu, REGA_CHIPID, &v, 1) || v != 0x1E)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
if (!setup_accel_config(imu))
|
|
{
|
|
printf("BMI088: delaying accel config\n");
|
|
}
|
|
|
|
printf("BMI088: found accel\n");
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
probe and initialise gyro
|
|
*/
|
|
static bool gyro_init(InertialSensor_BMI088_t *imu)
|
|
{
|
|
uint8_t v;
|
|
if (!read_registers(imu->dev_gyro, REGG_CHIPID, &v, 1) || v != 0x0F)
|
|
{
|
|
return false;
|
|
}
|
|
|
|
/* Soft-reset gyro
|
|
Return value of 'write_register()' is not checked.
|
|
This commands has the tendency to fail upon soft-reset.
|
|
*/
|
|
write_register(imu->dev_gyro, REGG_BGW_SOFTRESET, 0xB6);
|
|
osDelay(30);
|
|
|
|
//dev_gyro->setup_checked_registers(5, 20);
|
|
|
|
// setup 2000dps range
|
|
if (!write_register(imu->dev_gyro, REGG_RANGE, 0x00))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// setup filter bandwidth 230Hz, no decimation
|
|
if (!write_register(imu->dev_gyro, REGG_BW, 0x81))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// disable low-power mode
|
|
if (!write_register(imu->dev_gyro, REGG_LPM1, 0))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// setup for filtered data
|
|
if (!write_register(imu->dev_gyro, REGG_RATE_HBW, 0x00))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// setup FIFO for streaming X,Y,Z
|
|
if (!write_register(imu->dev_gyro, REGG_FIFO_CONFIG_1, 0x80))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
printf("BMI088: found gyro\n");
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
read accel fifo
|
|
*/
|
|
static void read_fifo_accel(InertialSensor_BMI088_t *imu)
|
|
{
|
|
if (!setup_accel_config(imu))
|
|
{
|
|
return;
|
|
}
|
|
uint8_t len[2];
|
|
if (!read_accel_registers(imu, REGA_FIFO_LEN0, len, 2))
|
|
{
|
|
imu->accel_error_count++;
|
|
return;
|
|
}
|
|
uint16_t fifo_length = len[0] + (len[1] << 8);
|
|
if (fifo_length & 0x8000)
|
|
{
|
|
// empty
|
|
return;
|
|
}
|
|
|
|
// don't read more than 8 frames at a time
|
|
if (fifo_length > 8 * 7)
|
|
{
|
|
fifo_length = 8 * 7;
|
|
}
|
|
if (fifo_length == 0)
|
|
{
|
|
return;
|
|
}
|
|
|
|
uint8_t data[fifo_length];
|
|
if (!read_accel_registers(imu, REGA_FIFO_DATA, data, fifo_length))
|
|
{
|
|
imu->accel_error_count++;
|
|
return;
|
|
}
|
|
// assume configured for 24g range
|
|
const float scale = (1.0 / 32768.0) * GRAVITY_MSS * 24.0;
|
|
const uint8_t *p = &data[0];
|
|
int32_t xyz[3];
|
|
int32_t tmp;
|
|
int32_t nn;
|
|
xyz[0] = 0;
|
|
xyz[1] = 0;
|
|
xyz[2] = 0;
|
|
nn = 0;
|
|
while (fifo_length >= 7)
|
|
{
|
|
/*
|
|
the fifo frames are variable length, with the frame type in the first byte
|
|
*/
|
|
uint8_t frame_len = 2;
|
|
switch (p[0] & 0xFC)
|
|
{
|
|
case 0x84:
|
|
{
|
|
// accel frame
|
|
frame_len = 7;
|
|
const uint8_t *d = p + 1;
|
|
{
|
|
tmp = (int32_t)(int16_t)((uint16_t)(d[0] | (d[1] << 8)));
|
|
xyz[0] += tmp;
|
|
tmp = (int32_t)(int16_t)((uint16_t)(d[2] | (d[3] << 8)));
|
|
xyz[1] += tmp;
|
|
tmp = (int32_t)(int16_t)((uint16_t)(d[4] | (d[5] << 8)));
|
|
xyz[2] += tmp;
|
|
nn++;
|
|
};
|
|
imu->accel_count++;
|
|
imu->accel_seq++;
|
|
|
|
break;
|
|
}
|
|
case 0x40:
|
|
// skip frame
|
|
frame_len = 2;
|
|
break;
|
|
case 0x44:
|
|
// sensortime frame
|
|
frame_len = 4;
|
|
break;
|
|
case 0x48:
|
|
// fifo config frame
|
|
frame_len = 2;
|
|
break;
|
|
case 0x50:
|
|
// sample drop frame
|
|
frame_len = 2;
|
|
break;
|
|
}
|
|
p += frame_len;
|
|
fifo_length -= frame_len;
|
|
}
|
|
if (nn > 0)
|
|
{
|
|
float nscale;
|
|
nscale = scale/nn;
|
|
imu->accel[0] = xyz[1] * nscale;
|
|
imu->accel[1] = xyz[0] * nscale;
|
|
imu->accel[2] = -xyz[2] * nscale;
|
|
}
|
|
|
|
|
|
if (imu->temperature_counter++ == 100)
|
|
{
|
|
imu->temperature_counter = 0;
|
|
uint8_t tbuf[2];
|
|
if (!read_accel_registers(imu, REGA_TEMP_LSB, tbuf, 2))
|
|
{
|
|
imu->accel_error_count++;
|
|
}
|
|
else
|
|
{
|
|
uint16_t temp_uint11 = (tbuf[0] << 3) | (tbuf[1] >> 5);
|
|
int16_t temp_int11 = temp_uint11 > 1023 ? temp_uint11 - 2048 : temp_uint11;
|
|
imu->temp_degc = temp_int11 * 0.125f + 23;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
read gyro fifo
|
|
*/
|
|
static void read_fifo_gyro(InertialSensor_BMI088_t *imu)
|
|
{
|
|
uint8_t num_frames;
|
|
if (!read_registers(imu->dev_gyro, REGG_FIFO_STATUS, &num_frames, 1))
|
|
{
|
|
imu->gyro_error_count++;
|
|
return;
|
|
}
|
|
num_frames &= 0x7F;
|
|
|
|
// don't read more than 8 frames at a time
|
|
if (num_frames > 8)
|
|
{
|
|
num_frames = 8;
|
|
}
|
|
if (num_frames == 0)
|
|
{
|
|
return;
|
|
}
|
|
uint8_t data[6 * num_frames];
|
|
if (!read_registers(imu->dev_gyro, REGG_FIFO_DATA, data, num_frames * 6))
|
|
{
|
|
imu->gyro_error_count++;
|
|
return;
|
|
}
|
|
|
|
// data is 16 bits with 2000dps range
|
|
const float scale = (2000.0f / 180.0f * M_PI) / 32767.0f;
|
|
imu->gyro_count += num_frames;
|
|
imu->gyro_seq++;
|
|
int32_t xyz[3];
|
|
int32_t tmp;
|
|
xyz[0] = 0;
|
|
xyz[1] = 0;
|
|
xyz[2] = 0;
|
|
for (uint8_t i = 0; i < num_frames; i++)
|
|
{
|
|
const uint8_t *d = &data[i * 6];
|
|
{
|
|
tmp = (int32_t)(int16_t)((uint16_t)(d[0] | d[1] << 8));
|
|
xyz[0] += tmp;
|
|
tmp = (int32_t)(int16_t)((uint16_t)(d[2] | d[3] << 8));
|
|
xyz[1] += tmp;
|
|
tmp = (int32_t)(int16_t)((uint16_t)(d[4] | d[5] << 8));
|
|
xyz[2] += tmp;
|
|
}
|
|
}
|
|
float nscale = scale/num_frames;
|
|
imu->gyro[0] = xyz[1] * nscale;
|
|
imu->gyro[1] = xyz[0] * nscale;
|
|
imu->gyro[2] = -xyz[2] * nscale;
|
|
}
|
|
|
|
bool InertialSensor_BMI088_init(InertialSensor_BMI088_t *imu, const char *name, SPI_DEV_t *dev_accel, SPI_DEV_t *dev_gyro)
|
|
{
|
|
imu->done_accel_config = false;
|
|
imu->accel_config_count = 0;
|
|
|
|
imu->accel_error_count = 0;
|
|
imu->gyro_error_count = 0;
|
|
imu->temperature_counter = 0;
|
|
|
|
imu->name = name;
|
|
imu->dev_accel = dev_accel;
|
|
imu->dev_gyro = dev_gyro;
|
|
|
|
SPI_DEV_set_read_flag(dev_accel, 0x80);
|
|
SPI_DEV_set_read_flag(dev_gyro, 0x80);
|
|
|
|
imu->success = (accel_init(imu) && gyro_init(imu));
|
|
return imu->success;
|
|
}
|
|
|
|
void InertialSensor_BMI088_update(InertialSensor_BMI088_t *imu)
|
|
{
|
|
if (imu->success)
|
|
{
|
|
if (SPI_DEV_begin(imu->dev_accel, 0))
|
|
{
|
|
read_fifo_accel(imu);
|
|
read_fifo_gyro(imu);
|
|
SPI_DEV_end(imu->dev_accel);
|
|
}
|
|
}
|
|
}
|
|
|
|
void InertialSensor_BMI088_loop(InertialSensor_BMI088_t *imu)
|
|
{
|
|
while (true)
|
|
{
|
|
osDelay(2);
|
|
if (SPI_DEV_begin(imu->dev_accel, 0))
|
|
{
|
|
read_fifo_accel(imu);
|
|
read_fifo_gyro(imu);
|
|
SPI_DEV_end(imu->dev_accel);
|
|
}
|
|
}
|
|
}
|
|
|
|
void InertialSensor_BMI088_monitor(InertialSensor_BMI088_t *imu)
|
|
{
|
|
imu->accel_pps = imu->accel_count;
|
|
imu->gyro_pps = imu->gyro_count;
|
|
|
|
imu->accel_count = 0;
|
|
imu->gyro_count = 0;
|
|
}
|