[DeviceDrivers] Add MPU6050 and BMI055 sensor.
This commit is contained in:
parent
2c88533659
commit
94e5d1e5e6
|
@ -3,10 +3,15 @@
|
|||
from building import *
|
||||
|
||||
cwd = GetCurrentDir()
|
||||
src = Glob('*.c') + Glob('*.cpp')
|
||||
src = ['sensor.cpp']
|
||||
CPPPATH = [cwd, cwd + '/../include']
|
||||
|
||||
if GetDepend('SENSOR_USING_MPU6050'):
|
||||
src += ['mpu6050_sensor.cpp'];
|
||||
|
||||
if GetDepend('SENSOR_USING_BMI055'):
|
||||
src += ['bmi055_sensor.cpp']
|
||||
|
||||
group = DefineGroup('Sensors', src, depend = ['RT_USING_SENSOR', 'RT_USING_DEVICE'], CPPPATH = CPPPATH)
|
||||
|
||||
Return('group')
|
||||
|
||||
|
|
|
@ -0,0 +1,455 @@
|
|||
/*
|
||||
* File : bmi055_sensor.cpp
|
||||
* This file is part of RT-Thread RTOS
|
||||
* COPYRIGHT (C) 2014, RT-Thread Development Team
|
||||
*
|
||||
* The license and distribution terms for this file may be
|
||||
* found in the file LICENSE in this distribution or at
|
||||
* http://www.rt-thread.org/license/LICENSE
|
||||
*
|
||||
* 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));
|
||||
}
|
||||
}
|
|
@ -0,0 +1,338 @@
|
|||
/*
|
||||
* File : bmi055_sensor.h
|
||||
* This file is part of RT-Thread RTOS
|
||||
* COPYRIGHT (C) 2014, RT-Thread Development Team
|
||||
*
|
||||
* The license and distribution terms for this file may be
|
||||
* found in the file LICENSE in this distribution or at
|
||||
* http://www.rt-thread.org/license/LICENSE
|
||||
*
|
||||
* Change Logs:
|
||||
* Date Author Notes
|
||||
* 2015-1-11 RT_learning the first version
|
||||
*/
|
||||
#ifndef __BMI055_H__
|
||||
#define __BMI055_H__
|
||||
|
||||
#include <sensor.h>
|
||||
|
||||
/**************************************************************************************************/
|
||||
/************************************Register map accelerometer************************************/
|
||||
|
||||
#define BMI055_ACC_I2C_ADDR1 0x18 //SDO is low(GND)
|
||||
#define BMI055_ACC_I2C_ADDR2 0x19 //SDO is high(VCC)
|
||||
|
||||
#define BMI055_ACC_DEFAULT_ADDRESS BMI055_ACC_I2C_ADDR2 //in the LPC54102 SPM-S
|
||||
|
||||
#define BMI055_ACC_BGW_CHIPID_VALUE 0xFA
|
||||
|
||||
#define BMI055_ACC_BGW_CHIPID 0x00
|
||||
/**<Address of ACC Chip ID Register */
|
||||
#define BMI055_ACCD_X_LSB 0x02
|
||||
/**< Address of X axis ACC LSB Register */
|
||||
#define BMI055_ACCD_X_MSB 0x03
|
||||
/**< Address of X axis ACC MSB Register */
|
||||
#define BMI055_ACCD_Y_LSB 0x04
|
||||
/**< Address of Y axis ACC LSB Register */
|
||||
#define BMI055_ACCD_Y_MSB 0x05
|
||||
/**< Address of Y axis ACC MSB Register */
|
||||
#define BMI055_ACCD_Z_LSB 0x06
|
||||
/**< Address of Z axis ACC LSB Register */
|
||||
#define BMI055_ACCD_Z_MSB 0x07
|
||||
/**< Address of Z axis ACC MSB Register */
|
||||
#define BMI055_ACCD_TEMP 0x08
|
||||
/**< Address of Temperature Data Register */
|
||||
|
||||
/* Status Register */
|
||||
#define BMI055_INT_STATUS_0 0x09
|
||||
/**< Address of Interrupt status Register 0 */
|
||||
#define BMI055_INT_STATUS_1 0x0A
|
||||
/**< Address of Interrupt status Register 1 */
|
||||
#define BMI055_INT_STATUS_2 0x0B
|
||||
/**< Address of Interrupt status Register 2 */
|
||||
#define BMI055_INT_STATUS_3 0x0C
|
||||
/**< Address of Interrupt status Register 3 */
|
||||
#define BMI055_FIFO_STATUS 0x0E
|
||||
/**< Address of FIFO status Register */
|
||||
|
||||
/* Control Register */
|
||||
#define BMI055_PMU_RANGE 0x0F
|
||||
/**< Address of Range address Register */
|
||||
#define BMI055_PMU_BW 0x10
|
||||
/**< Address of Bandwidth Register */
|
||||
#define BMI055_PMU_LPW 0x11
|
||||
/**< Address of PMU LPW */
|
||||
#define BMI055_PMU_LOW_POWER 0x12
|
||||
/**< Address of PMU LOW POWER */
|
||||
#define BMI055_ACCD_HBW 0x13
|
||||
/**< Address of ACCD HBW */
|
||||
#define BMI055_BGW_SOFTRESET 0x14
|
||||
/**< Address of BGW SOFTRESET */
|
||||
#define BMI055_INT_EN_0 0x16
|
||||
/**< Address of interrupt engines in group 0 */
|
||||
#define BMI055_INT_EN_1 0x17
|
||||
/**< Address of interrupt engines in group 1 */
|
||||
#define BMI055_INT_EN_2 0x18
|
||||
/**< Address of interrupt engines in group 2 */
|
||||
#define BMI055_INT_MAP_0 0x19
|
||||
/**< Address of Interrupt MAP 0 */
|
||||
#define BMI055_INT_MAP_1 0x1A
|
||||
/**< Address of Interrupt MAP 1 */
|
||||
#define BMI055_INT_MAP_2 0x1B
|
||||
/**< Address of Interrupt MAP 2 */
|
||||
#define BMI055_INT_SRC 0x1E
|
||||
/**< Address of Interrupt source */
|
||||
#define BMI055_INT_OUT_CTRL 0x20
|
||||
/**< Address of Interrupt Register */
|
||||
#define BMI055_INT_RST_LATCH 0x21
|
||||
/**< Address of Interrupt reset and mode Register */
|
||||
#define BMI055_INT_0 0x22
|
||||
/**< Address of low-g Interrupt delay time Register */
|
||||
#define BMI055_INT_1 0x23
|
||||
/**< Address of low-g Interrupt threshold Register */
|
||||
#define BMI055_INT_2 0x24
|
||||
/**< Address of Interrupt 2 Register */
|
||||
#define BMI055_INT_3 0x25
|
||||
/**< Address of high-g Interrupt delay time Register */
|
||||
#define BMI055_INT_4 0x26
|
||||
/**< Address of high-g Interrupt threshold Register */
|
||||
#define BMI055_INT_5 0x27
|
||||
/**< Address of high-g Interrupt 5 Register */
|
||||
#define BMI055_INT_6 0x28
|
||||
/**< Address of any-motion Interrupt threshold Register */
|
||||
#define BMI055_INT_7 0x29
|
||||
/**< Address of slow/no-motion interrupt threshold Register */
|
||||
#define BMI055_INT_8 0x2A
|
||||
/**< Address of high-g Interrupt 8 Register */
|
||||
#define BMI055_INT_9 0x2B
|
||||
/**< Address of high-g Interrupt 9 Register */
|
||||
#define BMI055_INT_A 0x2C
|
||||
/**< Address of Interrupt A Register */
|
||||
#define BMI055_INT_B 0x2D
|
||||
/**< Address of Interrupt B Register */
|
||||
#define BMI055_INT_C 0x2E
|
||||
/**< Address of Interrupt C Register */
|
||||
#define BMI055_INT_D 0x2F
|
||||
/**< Address of Interrupt D Register */
|
||||
#define BMI055_FIFO_CONFIG_0 0x30
|
||||
/**< Address of FIFO CONFIG 0 Register */
|
||||
#define BMI055_PMU_SELF_TEST 0x32
|
||||
/**< Address of PMU SELF TEST Register */
|
||||
#define BMI055_TRIM_NVM_CTRL 0x33
|
||||
/**< Address of TRIM NVM CTRL Register */
|
||||
#define BMI055_BGW_SPI3_WDT 0x34
|
||||
/**< Address of BGW SPI3 WDT Register */
|
||||
#define BMI055_OFC_CTRL 0x36
|
||||
/**< Address of OFC CTRL Register */
|
||||
#define BMI055_OFC_SETTING 0x37
|
||||
/**< Address of OFC SETTING Register */
|
||||
#define BMI055_OFC_OFFSET_X 0x38
|
||||
/**< Address of OFC OFFSET X Register */
|
||||
#define BMI055_OFC_OFFSET_Y 0x39
|
||||
/**< Address of OFC OFFSET Y Register */
|
||||
#define BMI055_OFC_OFFSET_Z 0x3A
|
||||
/**< Address of OFC OFFSET Z Register */
|
||||
|
||||
/* Trim Register */
|
||||
#define BMI055_TRIM_GP0 0x3B
|
||||
/**< Address of TRIM GP0 Register */
|
||||
#define BMI055_TRIM_GP1 0x3C
|
||||
/**< Address of TRIM GP1 Register */
|
||||
|
||||
/* Control Register */
|
||||
#define BMI055_FIFO_CONFIG_1 0x3E
|
||||
/**< Address of FIFO CONFIG 1 Register */
|
||||
|
||||
/* Data Register */
|
||||
#define BMI055_FIFO_DATA 0x3F
|
||||
/**< Address of FIFO DATA Register */
|
||||
|
||||
|
||||
/**************************************************************************************************/
|
||||
|
||||
|
||||
|
||||
|
||||
/**************************************************************************************************/
|
||||
/************************************Register map gyroscope****************************************/
|
||||
|
||||
|
||||
/**< This refers BMI055 return type as signed */
|
||||
|
||||
// #define BMI055_I2C_ADDR1 0x68 //SDO is low(GND)
|
||||
// #define BMI055_I2C_ADDR2 0x69 //SDO is high(VCC)
|
||||
#define BMI055_GYRO_I2C_ADDR1 0x68 //SDO is low(GND)
|
||||
#define BMI055_GYRO_I2C_ADDR2 0x69 //SDO is high(VCC)
|
||||
|
||||
#define BMI055_GYRO_DEFAULT_ADDRESS BMI055_GYRO_I2C_ADDR2
|
||||
|
||||
#define BMI055_GRRO_CHIP_ID 0x0F
|
||||
|
||||
/*Define of registers*/
|
||||
|
||||
/* Hard Wired */
|
||||
#define BMI055_CHIP_ID_ADDR 0x00
|
||||
/**<Address of Chip ID Register*/
|
||||
|
||||
/* Data Register */
|
||||
#define BMI055_RATE_X_LSB_ADDR 0x02
|
||||
/**< Address of X axis Rate LSB Register */
|
||||
#define BMI055_RATE_X_MSB_ADDR 0x03
|
||||
/**< Address of X axis Rate MSB Register */
|
||||
#define BMI055_RATE_Y_LSB_ADDR 0x04
|
||||
/**< Address of Y axis Rate LSB Register */
|
||||
#define BMI055_RATE_Y_MSB_ADDR 0x05
|
||||
/**< Address of Y axis Rate MSB Register */
|
||||
#define BMI055_RATE_Z_LSB_ADDR 0x06
|
||||
/**< Address of Z axis Rate LSB Register */
|
||||
#define BMI055_RATE_Z_MSB_ADDR 0x07
|
||||
/**< Address of Z axis Rate MSB Register */
|
||||
#define BMI055_TEMP_ADDR 0x08
|
||||
/**< Address of Temperature Data LSB Register */
|
||||
|
||||
/* Status Register */
|
||||
#define BMI055_INTR_STAT0_ADDR 0x09
|
||||
/**< Address of Interrupt status Register 0 */
|
||||
#define BMI055_INTR_STAT1_ADDR 0x0A
|
||||
/**< Address of Interrupt status Register 1 */
|
||||
#define BMI055_INTR_STAT2_ADDR 0x0B
|
||||
/**< Address of Interrupt status Register 2 */
|
||||
#define BMI055_INTR_STAT3_ADDR 0x0C
|
||||
/**< Address of Interrupt status Register 3 */
|
||||
#define BMI055_FIFO_STAT_ADDR 0x0E
|
||||
/**< Address of FIFO status Register */
|
||||
|
||||
/* Control Register */
|
||||
#define BMI055_RANGE_ADDR 0x0F
|
||||
/**< Address of Range address Register */
|
||||
#define BMI055_BW_ADDR 0x10
|
||||
/**< Address of Bandwidth Register */
|
||||
#define BMI055_MODE_LPM1_ADDR 0x11
|
||||
/**< Address of Mode LPM1 Register */
|
||||
#define BMI055_MODE_LPM2_ADDR 0x12
|
||||
/**< Address of Mode LPM2 Register */
|
||||
#define BMI055_HIGH_BW_ADDR 0x13
|
||||
/**< Address of Rate HIGH_BW Register */
|
||||
#define BMI055_BGW_SOFT_RST_ADDR 0x14
|
||||
/**< Address of BGW Softreset Register */
|
||||
#define BMI055_INTR_ENABLE0_ADDR 0x15
|
||||
/**< Address of Interrupt Enable 0 */
|
||||
#define BMI055_INTR_ENABLE1_ADDR 0x16
|
||||
/**< Address of Interrupt Enable 1 */
|
||||
#define BMI055_INTR_MAP_ZERO_ADDR 0x17
|
||||
/**< Address of Interrupt MAP 0 */
|
||||
#define BMI055_INTR_MAP_ONE_ADDR 0x18
|
||||
/**< Address of Interrupt MAP 1 */
|
||||
#define BMI055_INTR_MAP_TWO_ADDR 0x19
|
||||
/**< Address of Interrupt MAP 2 */
|
||||
#define BMI055_INTR_ZERO_ADDR 0x1A
|
||||
/**< Address of Interrupt 0 register */
|
||||
#define BMI055_INTR_ONE_ADDR 0x1B
|
||||
/**< Address of Interrupt 1 register */
|
||||
#define BMI055_INTR_TWO_ADDR 0x1C
|
||||
/**< Address of Interrupt 2 register */
|
||||
#define BMI055_INTR_4_ADDR 0x1E
|
||||
/**< Address of Interrupt 4 register */
|
||||
#define BMI055_RST_LATCH_ADDR 0x21
|
||||
/**< Address of Reset Latch Register */
|
||||
#define BMI055_HIGHRATE_THRES_X_ADDR 0x22
|
||||
/**< Address of High Th x Address register */
|
||||
#define BMI055_HIGHRATE_DURN_X_ADDR 0x23
|
||||
/**< Address of High Dur x Address register */
|
||||
#define BMI055_HIGHRATE_THRES_Y_ADDR 0x24
|
||||
/**< Address of High Th y Address register */
|
||||
#define BMI055_HIGHRATE_DURN_Y_ADDR 0x25
|
||||
/**< Address of High Dur y Address register */
|
||||
#define BMI055_HIGHRATE_THRES_Z_ADDR 0x26
|
||||
/**< Address of High Th z Address register */
|
||||
#define BMI055_HIGHRATE_DURN_Z_ADDR 0x27
|
||||
/**< Address of High Dur z Address register */
|
||||
#define BMI055_SOC_ADDR 0x31
|
||||
/**< Address of SOC register */
|
||||
#define BMI055_A_FOC_ADDR 0x32
|
||||
/**< Address of A_FOC Register */
|
||||
#define BMI055_TRIM_NVM_CTRL_ADDR 0x33
|
||||
/**< Address of Trim NVM control register */
|
||||
#define BMI055_BGW_SPI3_WDT_ADDR 0x34
|
||||
/**< Address of BGW SPI3,WDT Register */
|
||||
|
||||
/* Trim Register */
|
||||
#define BMI055_OFC1_ADDR 0x36
|
||||
/**< Address of OFC1 Register */
|
||||
#define BMI055_OFC2_ADDR 0x37
|
||||
/**< Address of OFC2 Register */
|
||||
#define BMI055_OFC3_ADDR 0x38
|
||||
/**< Address of OFC3 Register */
|
||||
#define BMI055_OFC4_ADDR 0x39
|
||||
/**< Address of OFC4 Register */
|
||||
#define BMI055_TRIM_GP0_ADDR 0x3A
|
||||
/**< Address of Trim GP0 Register */
|
||||
#define BMI055_TRIM_GP1_ADDR 0x3B
|
||||
/**< Address of Trim GP1 Register */
|
||||
#define BMI055_SELECTF_TEST_ADDR 0x3C
|
||||
/**< Address of BGW Self test Register */
|
||||
|
||||
/* Control Register */
|
||||
#define BMI055_FIFO_CGF1_ADDR 0x3D
|
||||
/**< Address of FIFO CGF0 Register */
|
||||
#define BMI055_FIFO_CGF0_ADDR 0x3E
|
||||
/**< Address of FIFO CGF1 Register */
|
||||
|
||||
/* Data Register */
|
||||
#define BMI055_FIFO_DATA_ADDR 0x3F
|
||||
/**< Address of FIFO Data Register */
|
||||
|
||||
|
||||
/**************************************************************************************************/
|
||||
|
||||
class BMI055 :public SensorBase
|
||||
{
|
||||
public:
|
||||
BMI055(int sensor_type, const char* iic_bus, int addr);
|
||||
|
||||
int read_reg(rt_uint8_t reg, rt_uint8_t* value);
|
||||
int write_reg(rt_uint8_t reg, rt_uint8_t value);
|
||||
int read_buffer(rt_uint8_t reg, rt_uint8_t* value, rt_size_t size);
|
||||
|
||||
private:
|
||||
struct rt_i2c_bus_device *i2c_bus;
|
||||
int i2c_addr;
|
||||
};
|
||||
|
||||
class BMI055_Accelerometer:public BMI055
|
||||
{
|
||||
public:
|
||||
BMI055_Accelerometer(const char* iic_name, int addr);
|
||||
|
||||
virtual int configure(SensorConfig *config);
|
||||
virtual int activate(int enable);
|
||||
|
||||
virtual int poll(sensors_event_t *event);
|
||||
virtual void getSensor(sensor_t *sensor);
|
||||
|
||||
private:
|
||||
rt_int16_t x_offset, y_offset, z_offset;
|
||||
|
||||
rt_bool_t enable;
|
||||
float sensitivity;
|
||||
};
|
||||
|
||||
class BMI055_Gyroscope:public BMI055
|
||||
{
|
||||
public:
|
||||
BMI055_Gyroscope(const char* iic_name, int addr);
|
||||
|
||||
virtual int configure(SensorConfig *config);
|
||||
virtual int activate(int enable);
|
||||
|
||||
virtual int poll(sensors_event_t *event);
|
||||
virtual void getSensor(sensor_t *sensor);
|
||||
|
||||
private:
|
||||
rt_int16_t x_offset, y_offset, z_offset;
|
||||
|
||||
rt_bool_t enable;
|
||||
float sensitivity;
|
||||
};
|
||||
|
||||
#endif
|
|
@ -0,0 +1,462 @@
|
|||
/*
|
||||
* File : MPU6050_sensor.cpp
|
||||
* This file is part of RT-Thread RTOS
|
||||
* COPYRIGHT (C) 2014, RT-Thread Development Team
|
||||
*
|
||||
* The license and distribution terms for this file may be
|
||||
* found in the file LICENSE in this distribution or at
|
||||
* http://www.rt-thread.org/license/LICENSE
|
||||
*
|
||||
* Change Logs:
|
||||
* Date Author Notes
|
||||
* 2014-12-20 Bernard the first version
|
||||
* 2015-1-11 RT_learning modify the mpu6050 initialize
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
#include <stdio.h>
|
||||
#include <rtdevice.h>
|
||||
|
||||
#include "mpu6050_sensor.h"
|
||||
|
||||
|
||||
const static sensor_t _MPU6050_sensor[] =
|
||||
{
|
||||
{
|
||||
.name = "Accelerometer",
|
||||
.vendor = "Invensense",
|
||||
.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 = "Invensense",
|
||||
.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,
|
||||
}
|
||||
};
|
||||
|
||||
MPU6050::MPU6050(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("MPU6050: No IIC device:%s\n", iic_bus);
|
||||
return;
|
||||
}
|
||||
|
||||
this->i2c_addr = addr;
|
||||
|
||||
/* register to sensor manager */
|
||||
SensorManager::registerSensor(this);
|
||||
}
|
||||
|
||||
int MPU6050::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 MPU6050::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 MPU6050::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;
|
||||
}
|
||||
|
||||
|
||||
MPU6050_Accelerometer::MPU6050_Accelerometer(const char* iic_name, int addr)
|
||||
: MPU6050(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};
|
||||
|
||||
/* initialize MPU6050 */
|
||||
write_reg(MPU6050_PWR_MGMT_1, 0x80); /* reset mpu6050 device */
|
||||
write_reg(MPU6050_SMPLRT_DIV, 0x00); /* Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) */
|
||||
write_reg(MPU6050_PWR_MGMT_1, 0x03); /* Wake up device , set device clock Z axis gyroscope */
|
||||
write_reg(MPU6050_CONFIG, 0x03); /* set DLPF_CFG 42Hz */
|
||||
write_reg(MPU6050_GYRO_CONFIG, 0x18); /* set gyro 2000deg/s */
|
||||
write_reg(MPU6050_ACCEL_CONFIG, 0x08); /* set acc +-4g/s */
|
||||
|
||||
|
||||
x_offset = y_offset = z_offset = 0;
|
||||
x = y = z = 0;
|
||||
|
||||
/* read MPU6050 id */
|
||||
read_buffer(MPU6050_WHOAMI, &id, 1);
|
||||
if (id != MPU6050_ID)
|
||||
{
|
||||
printf("Warning: not found MPU6050 id: %02x\n", id);
|
||||
}
|
||||
|
||||
/* get offset */
|
||||
for (index = 0; index < 200; index ++)
|
||||
{
|
||||
read_buffer(MPU6050_ACCEL_XOUT_H, value, 6);
|
||||
|
||||
x += (((rt_int16_t)value[0] << 8) | value[1]);
|
||||
y += (((rt_int16_t)value[2] << 8) | value[3]);
|
||||
z += (((rt_int16_t)value[4] << 8) | value[5]);
|
||||
}
|
||||
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
|
||||
MPU6050_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 = 0;
|
||||
break;
|
||||
case SENSOR_ACCEL_RANGE_4G:
|
||||
this->sensitivity = SENSOR_ACCEL_SENSITIVITY_4G;
|
||||
range = 0x01 << 2;
|
||||
break;
|
||||
case SENSOR_ACCEL_RANGE_8G:
|
||||
this->sensitivity = SENSOR_ACCEL_SENSITIVITY_8G;
|
||||
range = 0x02 << 2;
|
||||
break;
|
||||
case SENSOR_ACCEL_RANGE_16G:
|
||||
this->sensitivity = SENSOR_ACCEL_SENSITIVITY_16G;
|
||||
range = 0x03 << 2;
|
||||
break;
|
||||
default:
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* set range to sensor */
|
||||
read_reg(MPU6050_ACCEL_CONFIG, &value);
|
||||
value &= ~(0x3 << 2);
|
||||
value |= range;
|
||||
write_reg(MPU6050_ACCEL_CONFIG, value);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
MPU6050_Accelerometer::activate(int enable)
|
||||
{
|
||||
uint8_t value;
|
||||
|
||||
if (enable && this->enable == RT_FALSE)
|
||||
{
|
||||
/* enable accelerometer */
|
||||
read_reg(MPU6050_PWR_MGMT_2, &value);
|
||||
value &= ~(0x07 << 2);
|
||||
write_reg(MPU6050_PWR_MGMT_2, value);
|
||||
}
|
||||
|
||||
if (!enable && this->enable == RT_TRUE)
|
||||
{
|
||||
/* disable accelerometer */
|
||||
read_reg(MPU6050_PWR_MGMT_2, &value);
|
||||
value |= (0x07 << 2);
|
||||
write_reg(MPU6050_PWR_MGMT_2, value);
|
||||
}
|
||||
|
||||
if (enable) this->enable = RT_TRUE;
|
||||
else this->enable = RT_FALSE;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
MPU6050_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(MPU6050_ACCEL_XOUT_H, value, 6);
|
||||
|
||||
/* get raw data */
|
||||
x = (((rt_int16_t)value[0] << 8) | value[1]);
|
||||
y = (((rt_int16_t)value[2] << 8) | value[3]);
|
||||
z = (((rt_int16_t)value[4] << 8) | value[5]);
|
||||
|
||||
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
|
||||
MPU6050_Accelerometer::getSensor(sensor_t *sensor)
|
||||
{
|
||||
/* get sensor description */
|
||||
if (sensor)
|
||||
{
|
||||
memcpy(sensor, &_MPU6050_sensor[0], sizeof(sensor_t));
|
||||
}
|
||||
}
|
||||
|
||||
MPU6050_Gyroscope::MPU6050_Gyroscope(const char* iic_name, int addr)
|
||||
: MPU6050(SENSOR_TYPE_GYROSCOPE, iic_name, addr)
|
||||
{
|
||||
int index;
|
||||
uint8_t id;
|
||||
rt_uint8_t value[6];
|
||||
rt_int32_t x, y, z;
|
||||
|
||||
/* initialize MPU6050 */
|
||||
write_reg(MPU6050_PWR_MGMT_1, 0x80); /* reset mpu6050 device */
|
||||
write_reg(MPU6050_SMPLRT_DIV, 0x00); /* Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV) */
|
||||
write_reg(MPU6050_PWR_MGMT_1, 0x03); /* Wake up device , set device clock Z axis gyroscope */
|
||||
write_reg(MPU6050_CONFIG, 0x03); /* set DLPF_CFG 42Hz */
|
||||
write_reg(MPU6050_GYRO_CONFIG, 0x18); /* set gyro 2000deg/s */
|
||||
write_reg(MPU6050_ACCEL_CONFIG, 0x08); /* set acc +-4g/s */
|
||||
|
||||
x_offset = y_offset = z_offset = 0;
|
||||
x = y = z = 0;
|
||||
|
||||
/* read MPU6050 id */
|
||||
read_reg(MPU6050_WHOAMI, &id);
|
||||
if (id != MPU6050_ID)
|
||||
{
|
||||
printf("Warning: not found MPU6050 id: %02x\n", id);
|
||||
}
|
||||
|
||||
/* get offset */
|
||||
for (index = 0; index < 200; index ++)
|
||||
{
|
||||
read_buffer(MPU6050_GYRO_XOUT_H, value, 6);
|
||||
|
||||
x += (((rt_int16_t)value[0] << 8) | value[1]);
|
||||
y += (((rt_int16_t)value[2] << 8) | value[3]);
|
||||
z += (((rt_int16_t)value[4] << 8) | value[5]);
|
||||
}
|
||||
x_offset = x / 200;
|
||||
y_offset = y / 200;
|
||||
z_offset = z / 200;
|
||||
|
||||
this->enable = RT_FALSE;
|
||||
this->sensitivity = SENSOR_GYRO_SENSITIVITY_250DPS;
|
||||
}
|
||||
|
||||
int
|
||||
MPU6050_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)
|
||||
{
|
||||
case SENSOR_GYRO_RANGE_250DPS:
|
||||
this->sensitivity = SENSOR_GYRO_SENSITIVITY_250DPS;
|
||||
range = 0;
|
||||
break;
|
||||
case SENSOR_GYRO_RANGE_500DPS:
|
||||
this->sensitivity = SENSOR_GYRO_SENSITIVITY_500DPS;
|
||||
range = 0x01 << 2;
|
||||
break;
|
||||
case SENSOR_GYRO_RANGE_1000DPS:
|
||||
this->sensitivity = SENSOR_GYRO_SENSITIVITY_1000DPS;
|
||||
range = 0x02 << 2;
|
||||
break;
|
||||
case SENSOR_GYRO_RANGE_2000DPS:
|
||||
this->sensitivity = SENSOR_GYRO_SENSITIVITY_2000DPS;
|
||||
range = 0x03 << 2;
|
||||
break;
|
||||
default:
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* set range to sensor */
|
||||
read_reg(MPU6050_GYRO_CONFIG, &value);
|
||||
value &= ~(0x3 << 2);
|
||||
value |= range;
|
||||
write_reg(MPU6050_GYRO_CONFIG, value);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
MPU6050_Gyroscope::activate(int enable)
|
||||
{
|
||||
uint8_t value;
|
||||
|
||||
if (enable && this->enable == RT_FALSE)
|
||||
{
|
||||
/* enable gyroscope */
|
||||
read_reg(MPU6050_PWR_MGMT_1, &value);
|
||||
value &= ~(0x01 << 4);
|
||||
write_reg(MPU6050_PWR_MGMT_1, value);
|
||||
|
||||
read_reg(MPU6050_PWR_MGMT_2, &value);
|
||||
value &= ~(0x07 << 0);
|
||||
write_reg(MPU6050_PWR_MGMT_2, value);
|
||||
}
|
||||
|
||||
if (!enable && this->enable == RT_TRUE)
|
||||
{
|
||||
/* disable gyroscope */
|
||||
read_reg(MPU6050_PWR_MGMT_2, &value);
|
||||
value |= (0x07 << 0);
|
||||
write_reg(MPU6050_PWR_MGMT_2, value);
|
||||
}
|
||||
|
||||
if (enable) this->enable = RT_TRUE;
|
||||
else this->enable = RT_FALSE;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int
|
||||
MPU6050_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(MPU6050_GYRO_XOUT_H, value, 6);
|
||||
|
||||
/* get raw data */
|
||||
x = (((rt_int16_t)value[0] << 8) | value[1]);
|
||||
y = (((rt_int16_t)value[2] << 8) | value[3]);
|
||||
z = (((rt_int16_t)value[4] << 8) | value[5]);
|
||||
|
||||
|
||||
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
|
||||
MPU6050_Gyroscope::getSensor(sensor_t *sensor)
|
||||
{
|
||||
/* get sensor description */
|
||||
if (sensor)
|
||||
{
|
||||
memcpy(sensor, &_MPU6050_sensor[1], sizeof(sensor_t));
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,193 @@
|
|||
/*
|
||||
* File : MPU6050_sensor.h
|
||||
* This file is part of RT-Thread RTOS
|
||||
* COPYRIGHT (C) 2014, RT-Thread Development Team
|
||||
*
|
||||
* The license and distribution terms for this file may be
|
||||
* found in the file LICENSE in this distribution or at
|
||||
* http://www.rt-thread.org/license/LICENSE
|
||||
*
|
||||
* Change Logs:
|
||||
* Date Author Notes
|
||||
* 2014-12-20 Bernard the first version
|
||||
* 2015-1-11 RT_learning modify the mpu6050 ID
|
||||
*/
|
||||
|
||||
#ifndef MPU6050_SENSOR_H__
|
||||
#define MPU6050_SENSOR_H__
|
||||
|
||||
#include <sensor.h>
|
||||
|
||||
#define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board
|
||||
#define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC)
|
||||
#define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW
|
||||
|
||||
#define MPU6050_XG_OFFS_TC 0x00
|
||||
#define MPU6050_YG_OFFS_TC 0x01
|
||||
#define MPU6050_ZG_OFFS_TC 0x02
|
||||
#define MPU6050_X_FINE_GAIN 0x03
|
||||
#define MPU6050_Y_FINE_GAIN 0x04
|
||||
#define MPU6050_Z_FINE_GAIN 0x05
|
||||
#define MPU6050_XA_OFFS_H 0x06
|
||||
#define MPU6050_XA_OFFS_L 0x07
|
||||
#define MPU6050_YA_OFFS_H 0x08
|
||||
#define MPU6050_YA_OFFS_L 0x09
|
||||
#define MPU6050_ZA_OFFS_H 0x0A
|
||||
#define MPU6050_ZA_OFFS_L 0x0B
|
||||
#define MPU6050_PRODUCT_ID 0x0C
|
||||
#define MPU6050_SELF_TEST_X 0x0D
|
||||
#define MPU6050_SELF_TEST_Y 0x0E
|
||||
#define MPU6050_SELF_TEST_Z 0x0F
|
||||
#define MPU6050_SELF_TEST_A 0x10
|
||||
#define MPU6050_XG_OFFS_USRH 0x13
|
||||
#define MPU6050_XG_OFFS_USRL 0x14
|
||||
#define MPU6050_YG_OFFS_USRH 0x15
|
||||
#define MPU6050_YG_OFFS_USRL 0x16
|
||||
#define MPU6050_ZG_OFFS_USRH 0x17
|
||||
#define MPU6050_ZG_OFFS_USRL 0x18
|
||||
#define MPU6050_SMPLRT_DIV 0x19
|
||||
#define MPU6050_CONFIG 0x1A
|
||||
#define MPU6050_GYRO_CONFIG 0x1B
|
||||
#define MPU6050_ACCEL_CONFIG 0x1C
|
||||
#define MPU6050_ACCEL_CONFIG_2 0x1D
|
||||
#define MPU6050_LP_ACCEL_ODR 0x1E
|
||||
#define MPU6050_MOT_THR 0x1F
|
||||
#define MPU6050_FIFO_EN 0x23
|
||||
#define MPU6050_I2C_MST_CTRL 0x24
|
||||
#define MPU6050_I2C_SLV0_ADDR 0x25
|
||||
#define MPU6050_I2C_SLV0_REG 0x26
|
||||
#define MPU6050_I2C_SLV0_CTRL 0x27
|
||||
#define MPU6050_I2C_SLV1_ADDR 0x28
|
||||
#define MPU6050_I2C_SLV1_REG 0x29
|
||||
#define MPU6050_I2C_SLV1_CTRL 0x2A
|
||||
#define MPU6050_I2C_SLV2_ADDR 0x2B
|
||||
#define MPU6050_I2C_SLV2_REG 0x2C
|
||||
#define MPU6050_I2C_SLV2_CTRL 0x2D
|
||||
#define MPU6050_I2C_SLV3_ADDR 0x2E
|
||||
#define MPU6050_I2C_SLV3_REG 0x2F
|
||||
#define MPU6050_I2C_SLV3_CTRL 0x30
|
||||
#define MPU6050_I2C_SLV4_ADDR 0x31
|
||||
#define MPU6050_I2C_SLV4_REG 0x32
|
||||
#define MPU6050_I2C_SLV4_DO 0x33
|
||||
#define MPU6050_I2C_SLV4_CTRL 0x34
|
||||
#define MPU6050_I2C_SLV4_DI 0x35
|
||||
#define MPU6050_I2C_MST_STATUS 0x36
|
||||
#define MPU6050_INT_PIN_CFG 0x37
|
||||
#define MPU6050_INT_ENABLE 0x38
|
||||
#define MPU6050_ACCEL_XOUT_H 0x3B
|
||||
#define MPU6050_ACCEL_XOUT_L 0x3C
|
||||
#define MPU6050_ACCEL_YOUT_H 0x3D
|
||||
#define MPU6050_ACCEL_YOUT_L 0x3E
|
||||
#define MPU6050_ACCEL_ZOUT_H 0x3F
|
||||
#define MPU6050_ACCEL_ZOUT_L 0x40
|
||||
#define MPU6050_TEMP_OUT_H 0x41
|
||||
#define MPU6050_TEMP_OUT_L 0x42
|
||||
#define MPU6050_GYRO_XOUT_H 0x43
|
||||
#define MPU6050_GYRO_XOUT_L 0x44
|
||||
#define MPU6050_GYRO_YOUT_H 0x45
|
||||
#define MPU6050_GYRO_YOUT_L 0x46
|
||||
#define MPU6050_GYRO_ZOUT_H 0x47
|
||||
#define MPU6050_GYRO_ZOUT_L 0x48
|
||||
#define MPU6050_EXT_SENS_DATA_00 0x49
|
||||
#define MPU6050_EXT_SENS_DATA_01 0x4A
|
||||
#define MPU6050_EXT_SENS_DATA_02 0x4B
|
||||
#define MPU6050_EXT_SENS_DATA_03 0x4C
|
||||
#define MPU6050_EXT_SENS_DATA_04 0x4D
|
||||
#define MPU6050_EXT_SENS_DATA_05 0x4E
|
||||
#define MPU6050_EXT_SENS_DATA_06 0x4F
|
||||
#define MPU6050_EXT_SENS_DATA_07 0x50
|
||||
#define MPU6050_EXT_SENS_DATA_08 0x51
|
||||
#define MPU6050_EXT_SENS_DATA_09 0x52
|
||||
#define MPU6050_EXT_SENS_DATA_10 0x53
|
||||
#define MPU6050_EXT_SENS_DATA_11 0x54
|
||||
#define MPU6050_EXT_SENS_DATA_12 0x55
|
||||
#define MPU6050_EXT_SENS_DATA_13 0x56
|
||||
#define MPU6050_EXT_SENS_DATA_14 0x57
|
||||
#define MPU6050_EXT_SENS_DATA_15 0x58
|
||||
#define MPU6050_EXT_SENS_DATA_16 0x59
|
||||
#define MPU6050_EXT_SENS_DATA_17 0x5A
|
||||
#define MPU6050_EXT_SENS_DATA_18 0x5B
|
||||
#define MPU6050_EXT_SENS_DATA_19 0x5C
|
||||
#define MPU6050_EXT_SENS_DATA_20 0x5D
|
||||
#define MPU6050_EXT_SENS_DATA_21 0x5E
|
||||
#define MPU6050_EXT_SENS_DATA_22 0x5F
|
||||
#define MPU6050_EXT_SENS_DATA_23 0x60
|
||||
#define MPU6050_I2C_SLV0_DO 0x63
|
||||
#define MPU6050_I2C_SLV1_DO 0x64
|
||||
#define MPU6050_I2C_SLV2_DO 0x65
|
||||
#define MPU6050_I2C_SLV3_DO 0x66
|
||||
#define MPU6050_I2C_MST_DELAY_CTRL 0x67
|
||||
#define MPU6050_SIGNAL_PATH_RESET 0x68
|
||||
#define MPU6050_MOT_DETECT_CTRL 0x69
|
||||
#define MPU6050_USER_CTRL 0x6A
|
||||
#define MPU6050_PWR_MGMT_1 0x6B
|
||||
#define MPU6050_PWR_MGMT_2 0x6C
|
||||
#define MPU6050_BANK_SEL 0x6D
|
||||
#define MPU6050_MEM_START_ADDR 0x6E
|
||||
#define MPU6050_MEM_R_W 0x6F
|
||||
#define MPU6050_DMP_CFG_1 0x70
|
||||
#define MPU6050_DMP_CFG_2 0x71
|
||||
#define MPU6050_FIFO_COUNTH 0x72
|
||||
#define MPU6050_FIFO_COUNTL 0x73
|
||||
#define MPU6050_FIFO_R_W 0x74
|
||||
#define MPU6050_WHOAMI 0x75
|
||||
#define MPU6050_XA_OFFSET_H 0x77
|
||||
#define MPU6050_XA_OFFSET_L 0x78
|
||||
#define MPU6050_YA_OFFSET_H 0x7A
|
||||
#define MPU6050_YA_OFFSET_L 0x7B
|
||||
#define MPU6050_ZA_OFFSET_H 0x7D
|
||||
#define MPU6050_ZA_OFFSET_L 0x7E
|
||||
|
||||
#define MPU6050_ID 0x68
|
||||
|
||||
class MPU6050 :public SensorBase
|
||||
{
|
||||
public:
|
||||
MPU6050(int sensor_type, const char* iic_bus, int addr);
|
||||
|
||||
int read_reg(rt_uint8_t reg, rt_uint8_t* value);
|
||||
int write_reg(rt_uint8_t reg, rt_uint8_t value);
|
||||
int read_buffer(rt_uint8_t reg, rt_uint8_t* value, rt_size_t size);
|
||||
|
||||
private:
|
||||
struct rt_i2c_bus_device *i2c_bus;
|
||||
int i2c_addr;
|
||||
};
|
||||
|
||||
class MPU6050_Accelerometer:public MPU6050
|
||||
{
|
||||
public:
|
||||
MPU6050_Accelerometer(const char* iic_name, int addr);
|
||||
|
||||
virtual int configure(SensorConfig *config);
|
||||
virtual int activate(int enable);
|
||||
|
||||
virtual int poll(sensors_event_t *event);
|
||||
virtual void getSensor(sensor_t *sensor);
|
||||
|
||||
private:
|
||||
rt_int16_t x_offset, y_offset, z_offset;
|
||||
|
||||
rt_bool_t enable;
|
||||
float sensitivity;
|
||||
};
|
||||
|
||||
class MPU6050_Gyroscope:public MPU6050
|
||||
{
|
||||
public:
|
||||
MPU6050_Gyroscope(const char* iic_name, int addr);
|
||||
|
||||
virtual int configure(SensorConfig *config);
|
||||
virtual int activate(int enable);
|
||||
|
||||
virtual int poll(sensors_event_t *event);
|
||||
virtual void getSensor(sensor_t *sensor);
|
||||
|
||||
private:
|
||||
rt_int16_t x_offset, y_offset, z_offset;
|
||||
|
||||
rt_bool_t enable;
|
||||
float sensitivity;
|
||||
};
|
||||
|
||||
#endif
|
Loading…
Reference in New Issue