459 lines
11 KiB
C++
459 lines
11 KiB
C++
/*
|
|
* Copyright (c) 2006-2018, RT-Thread Development Team
|
|
*
|
|
* SPDX-License-Identifier: Apache-2.0
|
|
*
|
|
* 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));
|
|
}
|
|
}
|
|
|