串口ok
This commit is contained in:
@@ -0,0 +1,297 @@
|
||||
|
||||
#include "InertialSensor_BMI055.h"
|
||||
#include <stdio.h>
|
||||
|
||||
#define GRAVITY_MSS 9.80665f
|
||||
|
||||
/*
|
||||
device registers, names follow datasheet conventions, with REGA_
|
||||
prefix for accel, and REGG_ prefix for gyro
|
||||
*/
|
||||
#define REGA_BGW_CHIPID 0x00
|
||||
#define REGA_ACCD_X_LSB 0x02
|
||||
#define REGA_ACCD_TEMP 0x08
|
||||
#define REGA_INT_STATUS_0 0x09
|
||||
#define REGA_INT_STATUS_1 0x0A
|
||||
#define REGA_INT_STATUS_2 0x0B
|
||||
#define REGA_INT_STATUS_3 0x0C
|
||||
#define REGA_FIFO_STATUS 0x0E
|
||||
#define REGA_PMU_RANGE 0x0F
|
||||
#define REGA_PMU_BW 0x10
|
||||
#define REGA_PMU_LPW 0x11
|
||||
#define REGA_ACCD_HBW 0x13
|
||||
#define REGA_BGW_SOFTRESET 0x14
|
||||
#define REGA_OUT_CTRL 0x20
|
||||
#define REGA_EST_LATCH 0x21
|
||||
#define REGA_FIFO_CONFIG_0 0x30
|
||||
#define REGA_PMU_SELF_TEST 0x32
|
||||
#define REGA_FIFO_CONFIG_1 0x3E
|
||||
#define REGA_FIFO_DATA 0x3F
|
||||
|
||||
#define REGG_CHIPID 0x00
|
||||
#define REGA_RATE_X_LSB 0x02
|
||||
#define REGG_INT_STATUS_0 0x09
|
||||
#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 int16_val(v, idx) ((int16_t)(((uint16_t)v[2*idx] << 8) | v[2*idx+1]))
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
/*
|
||||
probe and initialise accelerometer
|
||||
*/
|
||||
static bool _accel_init(InertialSensor_BMI055_t *imu)
|
||||
{
|
||||
SPI_DEV_begin(imu->dev_accel, 1000);
|
||||
|
||||
uint8_t v;
|
||||
if (!_read_registers(imu->dev_accel, REGA_BGW_CHIPID, &v, 1) || v != 0xFA) {
|
||||
goto failed;
|
||||
}
|
||||
|
||||
if (!_write_register(imu->dev_accel, REGA_BGW_SOFTRESET, 0xB6)) {
|
||||
goto failed;
|
||||
}
|
||||
vTaskDelay(10);
|
||||
|
||||
// setup 16g range
|
||||
if (!_write_register(imu->dev_accel, REGA_PMU_RANGE, 0x0C)) {
|
||||
goto failed;
|
||||
}
|
||||
|
||||
// setup filter bandwidth 1kHz
|
||||
if (!_write_register(imu->dev_accel, REGA_PMU_BW, 0x0F)) {
|
||||
goto failed;
|
||||
}
|
||||
|
||||
// disable low-power mode
|
||||
if (!_write_register(imu->dev_accel, REGA_PMU_LPW, 0)) {
|
||||
goto failed;
|
||||
}
|
||||
|
||||
// setup for unfiltered data
|
||||
if (!_write_register(imu->dev_accel, REGA_ACCD_HBW, 0x80)) {
|
||||
goto failed;
|
||||
}
|
||||
|
||||
// setup FIFO for streaming X,Y,Z
|
||||
if (!_write_register(imu->dev_accel, REGA_FIFO_CONFIG_1, 0x80)) {
|
||||
goto failed;
|
||||
}
|
||||
|
||||
|
||||
printf("BMI055: found accel\n");
|
||||
|
||||
SPI_DEV_end(imu->dev_accel);
|
||||
return true;
|
||||
|
||||
failed:
|
||||
SPI_DEV_end(imu->dev_accel);
|
||||
return false;
|
||||
}
|
||||
|
||||
/*
|
||||
probe and initialise gyro
|
||||
*/
|
||||
static bool _gyro_init(InertialSensor_BMI055_t *imu)
|
||||
{
|
||||
SPI_DEV_begin(imu->dev_gyro, 1000);
|
||||
|
||||
uint8_t v;
|
||||
if (!_read_registers(imu->dev_gyro, REGG_CHIPID, &v, 1) || v != 0x0F) {
|
||||
goto failed;
|
||||
}
|
||||
|
||||
if (!_write_register(imu->dev_gyro, REGG_BGW_SOFTRESET, 0xB6)) {
|
||||
goto failed;
|
||||
}
|
||||
vTaskDelay(10);
|
||||
|
||||
// setup 2000dps range
|
||||
if (!_write_register(imu->dev_gyro, REGG_RANGE, 0x00)) {
|
||||
goto failed;
|
||||
}
|
||||
|
||||
// setup filter bandwidth 230Hz, no decimation
|
||||
if (!_write_register(imu->dev_gyro, REGG_BW, 0x81)) {
|
||||
goto failed;
|
||||
}
|
||||
|
||||
// disable low-power mode
|
||||
if (!_write_register(imu->dev_gyro, REGG_LPM1, 0)) {
|
||||
goto failed;
|
||||
}
|
||||
|
||||
// setup for filtered data
|
||||
if (!_write_register(imu->dev_gyro, REGG_RATE_HBW, 0x00)) {
|
||||
goto failed;
|
||||
}
|
||||
|
||||
// setup FIFO for streaming X,Y,Z
|
||||
if (!_write_register(imu->dev_gyro, REGG_FIFO_CONFIG_1, 0x80)) {
|
||||
goto failed;
|
||||
}
|
||||
|
||||
printf("BMI055: found gyro\n");
|
||||
|
||||
SPI_DEV_end(imu->dev_gyro);
|
||||
return true;
|
||||
|
||||
failed:
|
||||
SPI_DEV_end(imu->dev_gyro);
|
||||
return false;
|
||||
}
|
||||
|
||||
bool InertialSensor_BMI055_init(InertialSensor_BMI055_t *imu, const char *name, SPI_DEV_t* dev_accel, SPI_DEV_t* dev_gyro)
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
/*
|
||||
read accel fifo
|
||||
*/
|
||||
static void _read_fifo_accel(InertialSensor_BMI055_t *imu)
|
||||
{
|
||||
uint8_t num_frames;
|
||||
|
||||
if (!_read_registers(imu->dev_accel, REGA_FIFO_STATUS, &num_frames, 1)) {
|
||||
++imu->accel_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_accel, REGA_FIFO_DATA, data, num_frames*6)) {
|
||||
++imu->accel_error_count;
|
||||
return;
|
||||
}
|
||||
|
||||
if (imu->temperature_counter++ == 100) {
|
||||
imu->temperature_counter = 0;
|
||||
int8_t t;
|
||||
if (!_read_registers(imu->dev_accel, REGA_ACCD_TEMP, (uint8_t *)&t, 1)) {
|
||||
++imu->accel_error_count;
|
||||
} else {
|
||||
imu->temp_degc = (0.5f * t) + 23.0f;
|
||||
}
|
||||
}
|
||||
|
||||
// data is 12 bits with 16g range, 7.81mg/LSB
|
||||
const float scale = 7.81 * 0.001 * GRAVITY_MSS / 16.0f;
|
||||
for (uint8_t i = 0; i < num_frames; i++) {
|
||||
const uint8_t *d = &data[i*6];
|
||||
int16_t xyz[3];
|
||||
xyz[0] = (int16_t)((uint16_t)((d[0]&0xF0) | (d[1]<<8)));
|
||||
xyz[1] = (int16_t)((uint16_t)((d[2]&0xF0) | (d[3]<<8)));
|
||||
xyz[2] = (int16_t)((uint16_t)((d[4]&0xF0) | (d[5]<<8)));
|
||||
|
||||
imu->accel[0] = xyz[0] * scale;
|
||||
imu->accel[1] = xyz[1] * scale;
|
||||
imu->accel[2] = xyz[2] * scale;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
read gyro fifo
|
||||
*/
|
||||
static void _read_fifo_gyro(InertialSensor_BMI055_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 / 57.3f) / 32767.0f;
|
||||
for (uint8_t i = 0; i < num_frames; i++) {
|
||||
const uint8_t *d = &data[i*6];
|
||||
int16_t xyz[3];
|
||||
xyz[0] = (int16_t)((uint16_t)((d[0]) | (d[1]<<8)));
|
||||
xyz[1] = (int16_t)((uint16_t)((d[2]) | (d[3]<<8)));
|
||||
xyz[2] = (int16_t)((uint16_t)((d[4]) | (d[5]<<8)));
|
||||
|
||||
imu->gyro[0] = xyz[0] * scale;
|
||||
imu->gyro[1] = xyz[1] * scale;
|
||||
imu->gyro[2] = xyz[2] * scale;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
void InertialSensor_BMI055_update(InertialSensor_BMI055_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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user