Merge pull request #425 from BernardXiong/masterv2.0.0

[DeviceDrivers] Add more sensor drivers.

1 files changed, 455 insertions, 0 deletions

diff --git a/components/drivers/sensors/bmi055_sensor.cpp b/components/drivers/sensors/bmi055_sensor.cpp
new file mode 100644
index 0000000000..705895f368
--- /dev/null
+++ b/components/drivers/sensors/bmi055_sensor.cpp
@@ -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   = &reg;
+    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   = &reg;
+    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   = &reg;
+    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));
+    }
+}