diff --git a/components/drivers/sensors/SConscript b/components/drivers/sensors/SConscript index 14a7cd2450d121b68b80ea44af0c0cfeac595fe4..79f5c061774a0565df715990493d9717365e3008 100644 --- a/components/drivers/sensors/SConscript +++ b/components/drivers/sensors/SConscript @@ -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') - diff --git a/components/drivers/sensors/bmi055_sensor.cpp b/components/drivers/sensors/bmi055_sensor.cpp new file mode 100644 index 0000000000000000000000000000000000000000..705895f368fb2def312eec61c0b20051170f7fe9 --- /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 +#include +#include +#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)); + } +} diff --git a/components/drivers/sensors/bmi055_sensor.h b/components/drivers/sensors/bmi055_sensor.h new file mode 100644 index 0000000000000000000000000000000000000000..7ec6d9e5cfbae9ceb0d001a50c06c1b43dfdd07c --- /dev/null +++ b/components/drivers/sensors/bmi055_sensor.h @@ -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 + +/**************************************************************************************************/ +/************************************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 +/**
+#include +#include + +#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)); + } +} + diff --git a/components/drivers/sensors/mpu6050_sensor.h b/components/drivers/sensors/mpu6050_sensor.h new file mode 100644 index 0000000000000000000000000000000000000000..c7e28ba28e837764467e2ffd6f42694ad359ac50 --- /dev/null +++ b/components/drivers/sensors/mpu6050_sensor.h @@ -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 + +#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