patrick/zephyr
1
0
Fork 0
Code Issues Pull requests Actions 8 Packages Projects Releases Wiki Activity

sensor: add driver for MPU6050 sensor

Browse source 
Add driver to MPU6050 six-axis motion tracking device. The driver has support
for accelerometer, gyroscope and temperature channels.

Datasheet:
	http://43zrtwysvxb2gf29r5o0athu.wpengine.netdna-cdn.com/wp-content/uploads/2015/02/MPU-6000-Datasheet1.pdf

Register Map:
	http://43zrtwysvxb2gf29r5o0athu.wpengine.netdna-cdn.com/wp-content/uploads/2015/02/MPU-6000-Register-Map1.pdf

Change-Id: Id3930fc666195051093fee80c15cf2deb0eaedff
Origin: Original
Signed-off-by: Bogdan Davidoaia <bogdan.m.davidoaia@intel.com>
This commit is contained in:
Bogdan Davidoaia 2016-05-31 10:53:22 +03:00
parent d9033386bf
commit 01062d97aa
6 changed files with 644 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

2
drivers/sensor/Kconfig
View file

@ -51,6 +51,8 @@ source "drivers/sensor/Kconfig.lsm9ds0_mfd"
source "drivers/sensor/Kconfig.mcp9808"
source "drivers/sensor/Kconfig.mpu6050"
source "drivers/sensor/Kconfig.sht3xd"
source "drivers/sensor/Kconfig.sx9500"

158
drivers/sensor/Kconfig.mpu6050 Normal file
View file

@ -0,0 +1,158 @@
# Kconfig.mpu6050 - MPU6050 Six-Axis Motion Tracking device configuration options
#
# Copyright (c) 2016 Intel Corporation
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
#
menuconfig MPU6050
bool
prompt "MPU6050 Six-Axis Motion Tracking Device"
depends on SENSOR && I2C
default n
help
Enable driver for MPU6050 I2C-based six-axis motion tracking device.
config MPU6050_SYS_LOG_LEVEL
int "MPU6050 Log level"
depends on SYS_LOG && MPU6050
default 0
range 0 4
help
Sets log level for MPU6050 driver.
Levels are:
0 OFF, do not write
1 ERROR, only write SYS_LOG_ERR
2 WARNING, write SYS_LOG_WRN in addition to previous level
3 INFO, write SYS_LOG_INF in addition to previous levels
4 DEBUG, write SYS_LOG_DBG in addition to previous levels
config MPU6050_NAME
string
prompt "Driver name"
default "MPU6050"
depends on MPU6050
help
Device name with which the MPU6050 sensor is identified.
config MPU6050_INIT_PRIORITY
int
prompt "Init priority"
depends on MPU6050
default 70
help
Device driver initialization priority.
config MPU6050_I2C_ADDR
hex
prompt "I2C address"
depends on MPU6050
default 0x68
range 0x68 0x69
help
I2C address of the MPU6050 sensor.
Choose 0x68 if the AD0 pin is pulled to GND or 0x69 if the AD0 pin
is pulled to VDD.
config MPU6050_I2C_MASTER_DEV_NAME
string
prompt "I2C master where MPU6050 is connected"
depends on MPU6050
default "I2C_0"
help
Specify the device name of the I2C master device to which MPU6050 is
connected.
choice
prompt "Trigger mode"
depends on MPU6050
default MPU6050_TRIGGER_GLOBAL_FIBER
help
Specify the type of triggering to be used by the driver.
config MPU6050_TRIGGER_NONE
bool
prompt "No trigger"
config MPU6050_TRIGGER_GLOBAL_FIBER
bool
prompt "Use global fiber"
depends on GPIO && SYSTEM_WORKQUEUE
select MPU6050_TRIGGER
config MPU6050_TRIGGER_OWN_FIBER
bool
prompt "Use own fiber"
depends on GPIO
select MPU6050_TRIGGER
endchoice
config MPU6050_TRIGGER
bool
depends on MPU6050
config MPU6050_GPIO_DEV_NAME
string
prompt "GPIO device"
default "GPIO_0"
depends on MPU6050 && MPU6050_TRIGGER
help
The device name of the GPIO device to which the MPU6050 interrupt pin
is connected.
config MPU6050_GPIO_PIN_NUM
int
prompt "Interrupt GPIO pin number"
default 0
depends on MPU6050 && MPU6050_TRIGGER
help
The number of the GPIO on which the interrupt signal from the MPU6050
chip will be received.
config MPU6050_FIBER_PRIORITY
int
prompt "Fiber priority"
depends on MPU6050 && MPU6050_TRIGGER_OWN_FIBER
default 10
help
Priority of fiber used by the driver to handle interrupts.
config MPU6050_FIBER_STACK_SIZE
int
prompt "Fiber stack size"
depends on MPU6050 && MPU6050_TRIGGER_OWN_FIBER
default 1024
help
Stack size of fiber used by the driver to handle interrupts.
config MPU6050_ACCEL_FS
int
prompt "Accelerometer full-scale range"
depends on MPU6050
default 2
help
Magnetometer full-scale range.
An X value for the config represents a range of +/- X g. Valid
values are 2, 4, 8 and 16.
config MPU6050_GYRO_FS
int
prompt "Gyroscope full-scale range"
depends on MPU6050
default 250
help
Gyroscope full-scale range.
An X value for the config represents a range of +/- X degrees/second.
Valid values are 250, 500, 1000, 2000.

2
drivers/sensor/Makefile
View file

@ -24,6 +24,8 @@ obj-$(CONFIG_LSM9DS0_GYRO_TRIGGER_DRDY) += sensor_lsm9ds0_gyro_trigger.o
obj-$(CONFIG_LSM9DS0_MFD) += sensor_lsm9ds0_mfd.o
obj-$(CONFIG_MCP9808) += sensor_mcp9808.o
obj-$(CONFIG_MCP9808_TRIGGER) += sensor_mcp9808_trigger.o
obj-$(CONFIG_MPU6050) += sensor_mpu6050.o
obj-$(CONFIG_MPU6050_TRIGGER) += sensor_mpu6050_trigger.o
obj-$(CONFIG_SHT3XD) += sensor_sht3xd.o
obj-$(CONFIG_SHT3XD_TRIGGER) += sensor_sht3xd_trigger.o
obj-$(CONFIG_SX9500) += sensor_sx9500.o

239
drivers/sensor/sensor_mpu6050.c Normal file
View file

@ -0,0 +1,239 @@
/*
* Copyright (c) 2016 Intel Corporation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include <i2c.h>
#include <init.h>
#include <misc/byteorder.h>
#include <sensor.h>
#include "sensor_mpu6050.h"
/* see "Accelerometer Measurements" section from register map description */
static void mpu6050_convert_accel(struct sensor_value *val, int16_t raw_val,
uint16_t sensitivity_shift)
{
int64_t conv_val;
conv_val = ((int64_t)raw_val * SENSOR_G) >> sensitivity_shift;
val->type = SENSOR_VALUE_TYPE_INT_PLUS_MICRO;
val->val1 = conv_val / 1000000;
val->val2 = conv_val % 1000000;
}
/* see "Gyroscope Measurements" section from register map description */
static void mpu6050_convert_gyro(struct sensor_value *val, int16_t raw_val,
uint16_t sensitivity_x10)
{
int64_t conv_val;
conv_val = ((int64_t)raw_val * SENSOR_PI * 10) /
(180 * sensitivity_x10);
val->type = SENSOR_VALUE_TYPE_INT_PLUS_MICRO;
val->val1 = conv_val / 1000000;
val->val2 = conv_val % 1000000;
}
/* see "Temperature Measurement" section from register map description */
static inline void mpu6050_convert_temp(struct sensor_value *val,
int16_t raw_val)
{
val->type = SENSOR_VALUE_TYPE_INT_PLUS_MICRO;
val->val1 = raw_val / 340 + 36;
val->val2 = ((int64_t)(raw_val % 340) * 1000000) / 340 + 530000;
if (val->val2 < 0) {
val->val1--;
val->val2 += 1000000;
} else if (val->val2 >= 1000000) {
val->val1++;
val->val2 -= 1000000;
}
}
static int mpu6050_channel_get(struct device *dev,
enum sensor_channel chan,
struct sensor_value *val)
{
struct mpu6050_data *drv_data = dev->driver_data;
switch (chan) {
case SENSOR_CHAN_ACCEL_ANY:
mpu6050_convert_accel(val, drv_data->accel_x,
drv_data->accel_sensitivity_shift);
mpu6050_convert_accel(val + 1, drv_data->accel_y,
drv_data->accel_sensitivity_shift);
mpu6050_convert_accel(val + 2, drv_data->accel_z,
drv_data->accel_sensitivity_shift);
break;
case SENSOR_CHAN_ACCEL_X:
mpu6050_convert_accel(val, drv_data->accel_x,
drv_data->accel_sensitivity_shift);
break;
case SENSOR_CHAN_ACCEL_Y:
mpu6050_convert_accel(val, drv_data->accel_y,
drv_data->accel_sensitivity_shift);
break;
case SENSOR_CHAN_ACCEL_Z:
mpu6050_convert_accel(val, drv_data->accel_z,
drv_data->accel_sensitivity_shift);
break;
case SENSOR_CHAN_GYRO_ANY:
mpu6050_convert_gyro(val, drv_data->gyro_x,
drv_data->gyro_sensitivity_x10);
mpu6050_convert_gyro(val + 1, drv_data->gyro_y,
drv_data->gyro_sensitivity_x10);
mpu6050_convert_gyro(val + 2, drv_data->gyro_z,
drv_data->gyro_sensitivity_x10);
break;
case SENSOR_CHAN_GYRO_X:
mpu6050_convert_gyro(val, drv_data->gyro_x,
drv_data->gyro_sensitivity_x10);
break;
case SENSOR_CHAN_GYRO_Y:
mpu6050_convert_gyro(val, drv_data->gyro_y,
drv_data->gyro_sensitivity_x10);
break;
case SENSOR_CHAN_GYRO_Z:
mpu6050_convert_gyro(val, drv_data->gyro_z,
drv_data->gyro_sensitivity_x10);
break;
default: /* chan == SENSOR_CHAN_TEMP */
mpu6050_convert_temp(val, drv_data->temp);
}
return 0;
}
static int mpu6050_sample_fetch(struct device *dev, enum sensor_channel chan)
{
struct mpu6050_data *drv_data = dev->driver_data;
int16_t buf[7];
if (i2c_burst_read(drv_data->i2c, CONFIG_MPU6050_I2C_ADDR,
MPU6050_REG_DATA_START, (uint8_t *)buf, 14) < 0) {
SYS_LOG_ERR("Failed to read data sample.");
return -EIO;
}
drv_data->accel_x = sys_be16_to_cpu(buf[0]);
drv_data->accel_y = sys_be16_to_cpu(buf[1]);
drv_data->accel_z = sys_be16_to_cpu(buf[2]);
drv_data->temp = sys_be16_to_cpu(buf[3]);
drv_data->gyro_x = sys_be16_to_cpu(buf[4]);
drv_data->gyro_y = sys_be16_to_cpu(buf[5]);
drv_data->gyro_z = sys_be16_to_cpu(buf[6]);
return 0;
}
static struct sensor_driver_api mpu6050_driver_api = {
#if CONFIG_MPU6050_TRIGGER
.trigger_set = mpu6050_trigger_set,
#endif
.sample_fetch = mpu6050_sample_fetch,
.channel_get = mpu6050_channel_get,
};
int mpu6050_init(struct device *dev)
{
struct mpu6050_data *drv_data = dev->driver_data;
uint8_t id, i;
drv_data->i2c = device_get_binding(CONFIG_MPU6050_I2C_MASTER_DEV_NAME);
if (drv_data->i2c == NULL) {
SYS_LOG_ERR("Failed to get pointer to %s device",
CONFIG_MPU6050_I2C_MASTER_DEV_NAME);
return -EINVAL;
}
/* check chip ID */
if (i2c_reg_read_byte(drv_data->i2c, CONFIG_MPU6050_I2C_ADDR,
MPU6050_REG_CHIP_ID, &id) < 0) {
SYS_LOG_ERR("Failed to read chip ID.");
return -EIO;
}
if (id != MPU6050_CHIP_ID) {
SYS_LOG_ERR("Invalid chip ID.");
return -EINVAL;
}
/* wake up chip */
if (i2c_reg_update_byte(drv_data->i2c, CONFIG_MPU6050_I2C_ADDR,
MPU6050_REG_PWR_MGMT1, MPU6050_SLEEP_EN,
0) < 0) {
SYS_LOG_ERR("Failed to wake up chip.");
return -EIO;
}
/* set accelerometer full-scale range */
for (i = 0; i < 4; i++) {
if (BIT(i+1) == CONFIG_MPU6050_ACCEL_FS) {
break;
}
}
if (i == 4) {
SYS_LOG_ERR("Invalid value for accel full-scale range.");
return -EINVAL;
}
if (i2c_reg_write_byte(drv_data->i2c, CONFIG_MPU6050_I2C_ADDR,
MPU6050_REG_ACCEL_CFG,
i << MPU6050_ACCEL_FS_SHIFT) < 0) {
SYS_LOG_ERR("Failed to write accel full-scale range.");
return -EIO;
}
drv_data->accel_sensitivity_shift = 14 - i;
/* set gyroscope full-scale range */
for (i = 0; i < 4; i++) {
if (BIT(i) * 250 == CONFIG_MPU6050_GYRO_FS) {
break;
}
}
if (i == 4) {
SYS_LOG_ERR("Invalid value for gyro full-scale range.");
return -EINVAL;
}
if (i2c_reg_write_byte(drv_data->i2c, CONFIG_MPU6050_I2C_ADDR,
MPU6050_REG_GYRO_CFG,
i << MPU6050_GYRO_FS_SHIFT) < 0) {
SYS_LOG_ERR("Failed to write gyro full-scale range.");
return -EIO;
}
drv_data->gyro_sensitivity_x10 = mpu6050_gyro_sensitivity_x10[i];
#ifdef CONFIG_MPU6050_TRIGGER
if (mpu6050_init_interrupt(dev) < 0) {
SYS_LOG_DBG("Failed to initialize interrupts.");
return -EIO;
}
#endif
dev->driver_api = &mpu6050_driver_api;
return 0;
}
struct mpu6050_data mpu6050_driver;
DEVICE_INIT(mpu6050, CONFIG_MPU6050_NAME, mpu6050_init, &mpu6050_driver,
NULL, SECONDARY, CONFIG_MPU6050_INIT_PRIORITY);

93
drivers/sensor/sensor_mpu6050.h Normal file
View file

@ -0,0 +1,93 @@
/*
* Copyright (c) 2016 Intel Corporation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#ifndef __SENSOR_MPU6050_H__
#define __SENSOR_MPU6050_H__
#include <device.h>
#include <gpio.h>
#include <misc/nano_work.h>
#include <misc/util.h>
#include <stdint.h>
#define SYS_LOG_DOMAIN "MPU6050"
#define SYS_LOG_LEVEL CONFIG_MPU6050_SYS_LOG_LEVEL
#include <misc/sys_log.h>
#define MPU6050_REG_CHIP_ID 0x75
#define MPU6050_CHIP_ID 0x68
#define MPU6050_REG_GYRO_CFG 0x1B
#define MPU6050_GYRO_FS_SHIFT 3
#define MPU6050_REG_ACCEL_CFG 0x1C
#define MPU6050_ACCEL_FS_SHIFT 3
#define MPU6050_REG_INT_EN 0x38
#define MPU6050_DRDY_EN BIT(0)
#define MPU6050_REG_DATA_START 0x3B
#define MPU6050_REG_PWR_MGMT1 0x6B
#define MPU6050_SLEEP_EN BIT(6)
/* measured in degrees/sec x10 to avoid floating point */
static const uint16_t mpu6050_gyro_sensitivity_x10[] = {
1310, 655, 328, 164
};
struct mpu6050_data {
struct device *i2c;
int16_t accel_x;
int16_t accel_y;
int16_t accel_z;
uint16_t accel_sensitivity_shift;
int16_t temp;
int16_t gyro_x;
int16_t gyro_y;
int16_t gyro_z;
uint16_t gyro_sensitivity_x10;
#ifdef CONFIG_MPU6050_TRIGGER
struct device *gpio;
struct gpio_callback gpio_cb;
struct sensor_trigger data_ready_trigger;
sensor_trigger_handler_t data_ready_handler;
#if defined(CONFIG_MPU6050_TRIGGER_OWN_FIBER)
char __stack fiber_stack[CONFIG_MPU6050_FIBER_STACK_SIZE];
struct nano_sem gpio_sem;
#elif defined(CONFIG_MPU6050_TRIGGER_GLOBAL_FIBER)
struct nano_work work;
struct device *dev;
#endif
#endif /* CONFIG_MPU6050_TRIGGER */
};
#ifdef CONFIG_MPU6050_TRIGGER
int mpu6050_trigger_set(struct device *dev,
const struct sensor_trigger *trig,
sensor_trigger_handler_t handler);
int mpu6050_init_interrupt(struct device *dev);
#endif
#endif /* __SENSOR_MPU6050__ */

150
drivers/sensor/sensor_mpu6050_trigger.c Normal file
View file

@ -0,0 +1,150 @@
/*
* Copyright (c) 2016 Intel Corporation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include <device.h>
#include <i2c.h>
#include <misc/util.h>
#include <nanokernel.h>
#include <sensor.h>
#include "sensor_mpu6050.h"
int mpu6050_trigger_set(struct device *dev,
const struct sensor_trigger *trig,
sensor_trigger_handler_t handler)
{
struct mpu6050_data *drv_data = dev->driver_data;
if (trig->type != SENSOR_TRIG_DATA_READY) {
return -ENOTSUP;
}
gpio_pin_disable_callback(drv_data->gpio, CONFIG_MPU6050_GPIO_PIN_NUM);
drv_data->data_ready_handler = handler;
if (handler == NULL) {
return 0;
}
drv_data->data_ready_trigger = *trig;
gpio_pin_enable_callback(drv_data->gpio, CONFIG_MPU6050_GPIO_PIN_NUM);
return 0;
}
static void mpu6050_gpio_callback(struct device *dev,
struct gpio_callback *cb, uint32_t pins)
{
struct mpu6050_data *drv_data =
CONTAINER_OF(cb, struct mpu6050_data, gpio_cb);
ARG_UNUSED(pins);
gpio_pin_disable_callback(dev, CONFIG_MPU6050_GPIO_PIN_NUM);
#if defined(CONFIG_MPU6050_TRIGGER_OWN_FIBER)
nano_sem_give(&drv_data->gpio_sem);
#elif defined(CONFIG_MPU6050_TRIGGER_GLOBAL_FIBER)
nano_work_submit(&drv_data->work);
#endif
}
static void mpu6050_fiber_cb(void *arg)
{
struct device *dev = arg;
struct mpu6050_data *drv_data = dev->driver_data;
if (drv_data->data_ready_handler != NULL) {
drv_data->data_ready_handler(dev,
&drv_data->data_ready_trigger);
}
gpio_pin_enable_callback(drv_data->gpio, CONFIG_MPU6050_GPIO_PIN_NUM);
}
#ifdef CONFIG_MPU6050_TRIGGER_OWN_FIBER
static void mpu6050_fiber(int dev_ptr, int unused)
{
struct device *dev = INT_TO_POINTER(dev_ptr);
struct mpu6050_data *drv_data = dev->driver_data;
ARG_UNUSED(unused);
while (1) {
nano_fiber_sem_take(&drv_data->gpio_sem, TICKS_UNLIMITED);
mpu6050_fiber_cb(dev);
}
}
#endif
#ifdef CONFIG_MPU6050_TRIGGER_GLOBAL_FIBER
static void mpu6050_work_cb(struct nano_work *work)
{
struct mpu6050_data *drv_data =
CONTAINER_OF(work, struct mpu6050_data, work);
mpu6050_fiber_cb(drv_data->dev);
}
#endif
int mpu6050_init_interrupt(struct device *dev)
{
struct mpu6050_data *drv_data = dev->driver_data;
/* setup data ready gpio interrupt */
drv_data->gpio = device_get_binding(CONFIG_MPU6050_GPIO_DEV_NAME);
if (drv_data->gpio == NULL) {
SYS_LOG_ERR("Failed to get pointer to %s device",
CONFIG_MPU6050_GPIO_DEV_NAME);
return -EINVAL;
}
gpio_pin_configure(drv_data->gpio, CONFIG_MPU6050_GPIO_PIN_NUM,
GPIO_DIR_IN | GPIO_INT | GPIO_INT_EDGE |
GPIO_INT_ACTIVE_HIGH | GPIO_INT_DEBOUNCE);
gpio_init_callback(&drv_data->gpio_cb,
mpu6050_gpio_callback,
BIT(CONFIG_MPU6050_GPIO_PIN_NUM));
if (gpio_add_callback(drv_data->gpio, &drv_data->gpio_cb) < 0) {
SYS_LOG_ERR("Failed to set gpio callback");
return -EIO;
}
/* enable data ready interrupt */
if (i2c_reg_write_byte(drv_data->i2c, CONFIG_MPU6050_I2C_ADDR,
MPU6050_REG_INT_EN, MPU6050_DRDY_EN) < 0) {
SYS_LOG_ERR("Failed to enable data ready interrupt.");
return -EIO;
}
#if defined(CONFIG_MPU6050_TRIGGER_OWN_FIBER)
nano_sem_init(&drv_data->gpio_sem);
fiber_start(drv_data->fiber_stack, CONFIG_MPU6050_FIBER_STACK_SIZE,
(nano_fiber_entry_t)mpu6050_fiber, POINTER_TO_INT(dev),
0, CONFIG_MPU6050_FIBER_PRIORITY, 0);
#elif defined(CONFIG_MPU6050_TRIGGER_GLOBAL_FIBER)
drv_data->work.handler = mpu6050_work_cb;
drv_data->dev = dev;
#endif
gpio_pin_enable_callback(drv_data->gpio, CONFIG_MPU6050_GPIO_PIN_NUM);
return 0;
}