452 lines
10 KiB
C++
452 lines
10 KiB
C++
/*
|
|
* Copyright (c) 2006-2018, RT-Thread Development Team
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*
|
|
* Change Logs:
|
|
* Date Author Notes
|
|
* 2015-1-11 RT_learning the first version
|
|
*/
|
|
|
|
#include <string.h>
|
|
#include <stdio.h>
|
|
#include <rtdevice.h>
|
|
#include "bmi055_sensor.h"
|
|
|
|
|
|
const static sensor_t _BMI055_sensor[] =
|
|
{
|
|
{
|
|
.name = "Accelerometer",
|
|
.vendor = "Bosch",
|
|
.version = sizeof(sensor_t),
|
|
.handle = 0,
|
|
.type = SENSOR_TYPE_ACCELEROMETER,
|
|
.maxRange = SENSOR_ACCEL_RANGE_16G,
|
|
.resolution = 1.0f,
|
|
.power = 0.5f,
|
|
.minDelay = 10000,
|
|
.fifoReservedEventCount = 0,
|
|
.fifoMaxEventCount = 64,
|
|
},
|
|
{
|
|
.name = "Gyroscope",
|
|
.vendor = "Bosch",
|
|
.version = sizeof(sensor_t),
|
|
.handle = 0,
|
|
.type = SENSOR_TYPE_GYROSCOPE,
|
|
.maxRange = SENSOR_GYRO_RANGE_2000DPS,
|
|
.resolution = 1.0f,
|
|
.power = 0.5f,
|
|
.minDelay = 10000,
|
|
.fifoReservedEventCount = 0,
|
|
.fifoMaxEventCount = 64,
|
|
}
|
|
};
|
|
|
|
BMI055::BMI055(int sensor_type, const char* iic_bus, int addr)
|
|
: SensorBase(sensor_type)
|
|
{
|
|
this->i2c_bus = (struct rt_i2c_bus_device *)rt_device_find(iic_bus);
|
|
if (this->i2c_bus == NULL)
|
|
{
|
|
printf("BMI055: No IIC device:%s\n", iic_bus);
|
|
return;
|
|
}
|
|
|
|
this->i2c_addr = addr;
|
|
|
|
/* register to sensor manager */
|
|
SensorManager::registerSensor(this);
|
|
}
|
|
|
|
int BMI055::read_reg(rt_uint8_t reg, rt_uint8_t *value)
|
|
{
|
|
struct rt_i2c_msg msgs[2];
|
|
|
|
msgs[0].addr = this->i2c_addr;
|
|
msgs[0].flags = RT_I2C_WR;
|
|
msgs[0].buf = ®
|
|
msgs[0].len = 1;
|
|
|
|
msgs[1].addr = this->i2c_addr;
|
|
msgs[1].flags = RT_I2C_RD; /* Read from slave */
|
|
msgs[1].buf = (rt_uint8_t *)value;
|
|
msgs[1].len = 1;
|
|
|
|
if (rt_i2c_transfer(this->i2c_bus, msgs, 2) == 2)
|
|
return RT_EOK;
|
|
|
|
return -RT_ERROR;
|
|
}
|
|
|
|
int BMI055::read_buffer(rt_uint8_t reg, rt_uint8_t* value, rt_size_t size)
|
|
{
|
|
struct rt_i2c_msg msgs[2];
|
|
|
|
msgs[0].addr = this->i2c_addr;
|
|
msgs[0].flags = RT_I2C_WR;
|
|
msgs[0].buf = ®
|
|
msgs[0].len = 1;
|
|
|
|
msgs[1].addr = this->i2c_addr;
|
|
msgs[1].flags = RT_I2C_RD; /* Read from slave */
|
|
msgs[1].buf = (rt_uint8_t *)value;
|
|
msgs[1].len = size;
|
|
|
|
if (rt_i2c_transfer(this->i2c_bus, msgs, 2) == 2)
|
|
return RT_EOK;
|
|
|
|
return -RT_ERROR;
|
|
}
|
|
|
|
int BMI055::write_reg(rt_uint8_t reg, rt_uint8_t value)
|
|
{
|
|
struct rt_i2c_msg msgs[2];
|
|
|
|
msgs[0].addr = this->i2c_addr;
|
|
msgs[0].flags = RT_I2C_WR;
|
|
msgs[0].buf = ®
|
|
msgs[0].len = 1;
|
|
|
|
msgs[1].addr = this->i2c_addr;
|
|
msgs[1].flags = RT_I2C_WR | RT_I2C_NO_START;
|
|
msgs[1].buf = (rt_uint8_t *)&value;
|
|
msgs[1].len = 1;
|
|
|
|
if (rt_i2c_transfer(this->i2c_bus, msgs, 2) == 2)
|
|
return RT_EOK;
|
|
|
|
return -RT_ERROR;
|
|
}
|
|
|
|
|
|
BMI055_Accelerometer::BMI055_Accelerometer(const char* iic_name, int addr)
|
|
: BMI055(SENSOR_TYPE_ACCELEROMETER, iic_name, addr)
|
|
{
|
|
int index;
|
|
uint8_t id;
|
|
rt_uint8_t value[6] = {0};
|
|
rt_int32_t x, y, z;
|
|
SensorConfig config = {SENSOR_MODE_NORMAL, SENSOR_DATARATE_400HZ, SENSOR_ACCEL_RANGE_2G};
|
|
|
|
write_reg(BMI055_BGW_SOFTRESET, 0xB6); /* reset of the sensor P57 */
|
|
write_reg(BMI055_PMU_LPW, 0x00); /* PMU_LPW NORMAL mode P55 */
|
|
write_reg(BMI055_PMU_BW, 0x0A); /* 01010b 31.25 Hz P55 */
|
|
write_reg(BMI055_PMU_RANGE, 0x05); /* 0101b 卤4g range PMU_RANGE set acc +-4g/s P54 */
|
|
|
|
|
|
x_offset = y_offset = z_offset = 0;
|
|
x = y = z = 0;
|
|
|
|
/* read BMI055 id */
|
|
read_buffer(BMI055_ACC_BGW_CHIPID, &id, 1); /* BGW_CHIPID P47*/
|
|
if (id != BMI055_ACC_BGW_CHIPID_VALUE)
|
|
{
|
|
printf("Warning: not found BMI055 id: %02x\n", id);
|
|
}
|
|
|
|
/* get offset */
|
|
for (index = 0; index < 200; index ++)
|
|
{
|
|
read_buffer(BMI055_ACCD_X_LSB, value, 6); /*ACCD_X_LSB P47 */
|
|
|
|
x += (((rt_int16_t)value[1] << 8) | value[0]);
|
|
y += (((rt_int16_t)value[3] << 8) | value[2]);
|
|
z += (((rt_int16_t)value[5] << 8) | value[4]);
|
|
}
|
|
x_offset = x / 200;
|
|
y_offset = y / 200;
|
|
z_offset = z / 200;
|
|
|
|
this->enable = RT_FALSE;
|
|
this->sensitivity = SENSOR_ACCEL_SENSITIVITY_2G;
|
|
this->config = config;
|
|
}
|
|
|
|
int
|
|
BMI055_Accelerometer::configure(SensorConfig *config)
|
|
{
|
|
int range;
|
|
uint8_t value;
|
|
|
|
if (config == RT_NULL) return -1;
|
|
|
|
/* TODO: set datarate */
|
|
|
|
/* get range and calc the sensitivity */
|
|
range = config->range.accel_range;
|
|
switch (range)
|
|
{
|
|
case SENSOR_ACCEL_RANGE_2G:
|
|
this->sensitivity = SENSOR_ACCEL_SENSITIVITY_2G;
|
|
range = 0x03; //0011b
|
|
break;
|
|
case SENSOR_ACCEL_RANGE_4G:
|
|
this->sensitivity = SENSOR_ACCEL_SENSITIVITY_4G;
|
|
range = 0x05; //0101b
|
|
break;
|
|
case SENSOR_ACCEL_RANGE_8G:
|
|
this->sensitivity = SENSOR_ACCEL_SENSITIVITY_8G;
|
|
range = 0x01 << 3; //1000b
|
|
break;
|
|
case SENSOR_ACCEL_RANGE_16G:
|
|
this->sensitivity = SENSOR_ACCEL_SENSITIVITY_16G;
|
|
range = 0x03 << 2; //1100b
|
|
break;
|
|
default:
|
|
return -1;
|
|
}
|
|
|
|
/* set range to sensor */
|
|
read_reg(BMI055_PMU_RANGE, &value); /* PMU_RANGE P54 */
|
|
value &= 0xF0;
|
|
value |= range;
|
|
write_reg(BMI055_PMU_RANGE, value);
|
|
|
|
return 0;
|
|
}
|
|
|
|
int
|
|
BMI055_Accelerometer::activate(int enable)
|
|
{
|
|
uint8_t value;
|
|
|
|
if (enable && this->enable == RT_FALSE)
|
|
{
|
|
/* enable accelerometer */
|
|
read_reg(BMI055_PMU_LPW, &value); /* P55 */
|
|
value &= ~(0x07 << 7);
|
|
write_reg(BMI055_PMU_LPW, value);
|
|
}
|
|
|
|
if (!enable && this->enable == RT_TRUE)
|
|
{
|
|
/* disable accelerometer */
|
|
read_reg(BMI055_PMU_LPW, &value);
|
|
value &= ~(0x07 << 7);
|
|
value |= (0x01 << 7);
|
|
write_reg(BMI055_PMU_LPW, value);
|
|
}
|
|
|
|
if (enable) this->enable = RT_TRUE;
|
|
else this->enable = RT_FALSE;
|
|
|
|
return 0;
|
|
}
|
|
|
|
int
|
|
BMI055_Accelerometer::poll(sensors_event_t *event)
|
|
{
|
|
rt_uint8_t value[6];
|
|
rt_int16_t x, y, z;
|
|
|
|
/* parameters check */
|
|
if (event == NULL) return -1;
|
|
|
|
/* get event data */
|
|
event->version = sizeof(sensors_event_t);
|
|
event->sensor = (int32_t) this;
|
|
event->timestamp = rt_tick_get();
|
|
event->type = SENSOR_TYPE_ACCELEROMETER;
|
|
|
|
read_buffer(0x02, value, 6);
|
|
|
|
/* get raw data */
|
|
x = (((rt_int16_t)value[1] << 8) | value[0]);
|
|
y = (((rt_int16_t)value[3] << 8) | value[2]);
|
|
z = (((rt_int16_t)value[5] << 8) | value[4]);
|
|
|
|
if (config.mode == SENSOR_MODE_RAW)
|
|
{
|
|
event->raw_acceleration.x = x;
|
|
event->raw_acceleration.y = y;
|
|
event->raw_acceleration.z = z;
|
|
}
|
|
else
|
|
{
|
|
|
|
x -= x_offset; y -= y_offset; z -= z_offset;
|
|
event->acceleration.x = x * this->sensitivity * SENSORS_GRAVITY_STANDARD;
|
|
event->acceleration.y = y * this->sensitivity * SENSORS_GRAVITY_STANDARD;
|
|
event->acceleration.z = z * this->sensitivity * SENSORS_GRAVITY_STANDARD;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
void
|
|
BMI055_Accelerometer::getSensor(sensor_t *sensor)
|
|
{
|
|
/* get sensor description */
|
|
if (sensor)
|
|
{
|
|
memcpy(sensor, &_BMI055_sensor[0], sizeof(sensor_t));
|
|
}
|
|
}
|
|
|
|
BMI055_Gyroscope::BMI055_Gyroscope(const char* iic_name, int addr)
|
|
: BMI055(SENSOR_TYPE_GYROSCOPE, iic_name, addr)
|
|
{
|
|
int index;
|
|
uint8_t id;
|
|
rt_uint8_t value[6];
|
|
rt_int32_t x, y, z;
|
|
|
|
/* initialize BMI055 */
|
|
write_reg(BMI055_MODE_LPM1_ADDR, 0x00); /* normal mode */
|
|
write_reg(BMI055_MODE_LPM2_ADDR, 0x80); /* fast powerup */
|
|
write_reg(BMI055_BW_ADDR, 0x03); /* ODR:400Hz Filter Bandwidth:47Hz */
|
|
write_reg(BMI055_RANGE_ADDR, 0x00); /* 2000dps */
|
|
|
|
|
|
x_offset = y_offset = z_offset = 0;
|
|
x = y = z = 0;
|
|
|
|
/* read BMI055 id */
|
|
read_buffer(BMI055_CHIP_ID_ADDR, &id, 1);
|
|
if (id != BMI055_GRRO_CHIP_ID)
|
|
{
|
|
printf("Warning: not found BMI055 id: %02x\n", id);
|
|
}
|
|
|
|
/* get offset */
|
|
for (index = 0; index < 200; index ++)
|
|
{
|
|
read_buffer(BMI055_RATE_X_LSB_ADDR, value, 6);
|
|
|
|
x += (((rt_int16_t)value[1] << 8) | value[0]);
|
|
y += (((rt_int16_t)value[3] << 8) | value[2]);
|
|
z += (((rt_int16_t)value[5] << 8) | value[4]);
|
|
}
|
|
x_offset = x / 200;
|
|
y_offset = y / 200;
|
|
z_offset = z / 200;
|
|
|
|
this->enable = RT_FALSE;
|
|
this->sensitivity = SENSOR_GYRO_SENSITIVITY_250DPS;
|
|
}
|
|
|
|
int
|
|
BMI055_Gyroscope::configure(SensorConfig *config)
|
|
{
|
|
int range;
|
|
uint8_t value;
|
|
|
|
if (config == RT_NULL) return -1;
|
|
|
|
/* TODO: set datarate */
|
|
|
|
/* get range and calc the sensitivity */
|
|
range = config->range.gyro_range;
|
|
switch (range)
|
|
{
|
|
//to do add more range e.g 125DPS
|
|
//case
|
|
case SENSOR_GYRO_RANGE_250DPS:
|
|
this->sensitivity = SENSOR_GYRO_SENSITIVITY_250DPS;
|
|
range = 0x11;
|
|
break;
|
|
case SENSOR_GYRO_RANGE_500DPS:
|
|
this->sensitivity = SENSOR_GYRO_SENSITIVITY_500DPS;
|
|
range = 0x10;
|
|
break;
|
|
case SENSOR_GYRO_RANGE_1000DPS:
|
|
this->sensitivity = SENSOR_GYRO_SENSITIVITY_1000DPS;
|
|
range = 0x01;
|
|
break;
|
|
case SENSOR_GYRO_RANGE_2000DPS:
|
|
this->sensitivity = SENSOR_GYRO_SENSITIVITY_2000DPS;
|
|
range = 0x00;
|
|
break;
|
|
default:
|
|
return -1;
|
|
}
|
|
|
|
/* set range to sensor */
|
|
read_reg(BMI055_RANGE_ADDR, &value);
|
|
value &= ~0x07;
|
|
value |= range;
|
|
write_reg(BMI055_RANGE_ADDR, value);
|
|
|
|
return 0;
|
|
}
|
|
|
|
int
|
|
BMI055_Gyroscope::activate(int enable)
|
|
{
|
|
uint8_t value;
|
|
|
|
if (enable && this->enable == RT_FALSE)
|
|
{
|
|
/* enable gyroscope */
|
|
read_reg(BMI055_MODE_LPM1_ADDR, &value);
|
|
value &= ~(0x1010 << 4); //{0; 0} NORMAL mode
|
|
write_reg(BMI055_MODE_LPM1_ADDR, value); //P101 NORMAL mode
|
|
}
|
|
|
|
if (!enable && this->enable == RT_TRUE)
|
|
{
|
|
/* disable gyroscope */
|
|
read_reg(BMI055_MODE_LPM1_ADDR, &value);
|
|
value &= ~(0x01 << 5); //set bit5 deep_suspend 0
|
|
value |= (0x01 << 7); //set bit1 suspend 1
|
|
write_reg(BMI055_MODE_LPM1_ADDR, value); //{1; 0} SUSPEND mode
|
|
}
|
|
|
|
if (enable) this->enable = RT_TRUE;
|
|
else this->enable = RT_FALSE;
|
|
|
|
return 0;
|
|
}
|
|
|
|
int
|
|
BMI055_Gyroscope::poll(sensors_event_t *event)
|
|
{
|
|
rt_uint8_t value[6];
|
|
rt_int16_t x, y, z;
|
|
|
|
/* parameters check */
|
|
if (event == NULL) return -1;
|
|
|
|
/* get event data */
|
|
event->version = sizeof(sensors_event_t);
|
|
event->sensor = (int32_t) this;
|
|
event->timestamp = rt_tick_get();
|
|
event->type = SENSOR_TYPE_GYROSCOPE;
|
|
|
|
read_buffer(BMI055_RATE_X_LSB_ADDR, value, 6);
|
|
|
|
/* get raw data */
|
|
x = (((rt_int16_t)value[1] << 8) | value[0]);
|
|
y = (((rt_int16_t)value[3] << 8) | value[2]);
|
|
z = (((rt_int16_t)value[5] << 8) | value[4]);
|
|
|
|
|
|
if (config.mode == SENSOR_MODE_RAW)
|
|
{
|
|
event->raw_gyro.x = x;
|
|
event->raw_gyro.y = y;
|
|
event->raw_gyro.z = z;
|
|
}
|
|
else
|
|
{
|
|
x -= x_offset; y -= y_offset; z -= z_offset;
|
|
event->gyro.x = x * this->sensitivity * SENSORS_DPS_TO_RADS;
|
|
event->gyro.y = y * this->sensitivity * SENSORS_DPS_TO_RADS;
|
|
event->gyro.z = z * this->sensitivity * SENSORS_DPS_TO_RADS;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
void
|
|
BMI055_Gyroscope::getSensor(sensor_t *sensor)
|
|
{
|
|
/* get sensor description */
|
|
if (sensor)
|
|
{
|
|
memcpy(sensor, &_BMI055_sensor[1], sizeof(sensor_t));
|
|
}
|
|
}
|