374 lines
10 KiB
C
374 lines
10 KiB
C
/* sensor_lsm9ds0_gyro.c - Driver for LSM9DS0 gyroscope sensor */
|
|
|
|
/*
|
|
* 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 <sensor.h>
|
|
#include <nanokernel.h>
|
|
#include <device.h>
|
|
#include <init.h>
|
|
#include <misc/byteorder.h>
|
|
#include <misc/__assert.h>
|
|
|
|
#include <gpio.h>
|
|
|
|
#include "sensor_lsm9ds0_gyro.h"
|
|
|
|
static inline int lsm9ds0_gyro_power_ctrl(struct device *dev, int power,
|
|
int x_en, int y_en, int z_en)
|
|
{
|
|
struct lsm9ds0_gyro_data *data = dev->driver_data;
|
|
struct lsm9ds0_gyro_config *config = dev->config->config_info;
|
|
uint8_t state = (power << LSM9DS0_GYRO_SHIFT_CTRL_REG1_G_PD) |
|
|
(x_en << LSM9DS0_GYRO_SHIFT_CTRL_REG1_G_XEN) |
|
|
(y_en << LSM9DS0_GYRO_SHIFT_CTRL_REG1_G_YEN) |
|
|
(z_en << LSM9DS0_GYRO_SHIFT_CTRL_REG1_G_ZEN);
|
|
|
|
return i2c_reg_update_byte(data->i2c_master, config->i2c_slave_addr,
|
|
LSM9DS0_GYRO_REG_CTRL_REG1_G,
|
|
LSM9DS0_GYRO_MASK_CTRL_REG1_G_PD |
|
|
LSM9DS0_GYRO_MASK_CTRL_REG1_G_XEN |
|
|
LSM9DS0_GYRO_MASK_CTRL_REG1_G_YEN |
|
|
LSM9DS0_GYRO_MASK_CTRL_REG1_G_ZEN,
|
|
state);
|
|
}
|
|
|
|
static int lsm9ds0_gyro_set_fs_raw(struct device *dev, uint8_t fs)
|
|
{
|
|
struct lsm9ds0_gyro_data *data = dev->driver_data;
|
|
struct lsm9ds0_gyro_config *config = dev->config->config_info;
|
|
|
|
if (i2c_reg_update_byte(data->i2c_master, config->i2c_slave_addr,
|
|
LSM9DS0_GYRO_REG_CTRL_REG4_G,
|
|
LSM9DS0_GYRO_MASK_CTRL_REG4_G_FS,
|
|
fs << LSM9DS0_GYRO_SHIFT_CTRL_REG4_G_FS) < 0) {
|
|
return -EIO;
|
|
}
|
|
|
|
#if defined(CONFIG_LSM9DS0_GYRO_FULLSCALE_RUNTIME)
|
|
data->fs = fs;
|
|
#endif
|
|
|
|
return 0;
|
|
}
|
|
|
|
#if defined(CONFIG_LSM9DS0_GYRO_FULLSCALE_RUNTIME)
|
|
static const struct {
|
|
int fs;
|
|
uint8_t reg_val;
|
|
} lsm9ds0_gyro_fs_table[] = { {245, 0},
|
|
{500, 1},
|
|
{2000, 2} };
|
|
|
|
static int lsm9ds0_gyro_set_fs(struct device *dev, int fs)
|
|
{
|
|
int i;
|
|
|
|
for (i = 0; i < ARRAY_SIZE(lsm9ds0_gyro_fs_table); ++i) {
|
|
if (fs <= lsm9ds0_gyro_fs_table[i].fs) {
|
|
return lsm9ds0_gyro_set_fs_raw(dev, lsm9ds0_gyro_fs_table[i].reg_val);
|
|
}
|
|
}
|
|
|
|
return -ENOTSUP;
|
|
}
|
|
#endif
|
|
|
|
static inline int lsm9ds0_gyro_set_odr_raw(struct device *dev, uint8_t odr)
|
|
{
|
|
struct lsm9ds0_gyro_data *data = dev->driver_data;
|
|
struct lsm9ds0_gyro_config *config = dev->config->config_info;
|
|
|
|
return i2c_reg_update_byte(data->i2c_master, config->i2c_slave_addr,
|
|
LSM9DS0_GYRO_REG_CTRL_REG1_G,
|
|
LSM9DS0_GYRO_MASK_CTRL_REG1_G_DR,
|
|
odr << LSM9DS0_GYRO_SHIFT_CTRL_REG1_G_BW);
|
|
}
|
|
|
|
#if defined(CONFIG_LSM9DS0_GYRO_SAMPLING_RATE_RUNTIME)
|
|
static const struct {
|
|
int freq;
|
|
uint8_t reg_val;
|
|
} lsm9ds0_gyro_samp_freq_table[] = { {95, 0},
|
|
{190, 1},
|
|
{380, 2},
|
|
{760, 3} };
|
|
|
|
static int lsm9ds0_gyro_set_odr(struct device *dev, int odr)
|
|
{
|
|
int i;
|
|
|
|
for (i = 0; i < ARRAY_SIZE(lsm9ds0_gyro_samp_freq_table); ++i) {
|
|
if (odr <= lsm9ds0_gyro_samp_freq_table[i].freq) {
|
|
return lsm9ds0_gyro_set_odr_raw(dev,
|
|
lsm9ds0_gyro_samp_freq_table[i].
|
|
reg_val);
|
|
}
|
|
}
|
|
|
|
return -ENOTSUP;
|
|
}
|
|
#endif
|
|
|
|
static int lsm9ds0_gyro_sample_fetch(struct device *dev,
|
|
enum sensor_channel chan)
|
|
{
|
|
struct lsm9ds0_gyro_data *data = dev->driver_data;
|
|
struct lsm9ds0_gyro_config *config = dev->config->config_info;
|
|
uint8_t x_l, x_h, y_l, y_h, z_l, z_h;
|
|
|
|
__ASSERT(chan == SENSOR_CHAN_ALL || chan == SENSOR_CHAN_GYRO_ANY);
|
|
|
|
if (i2c_reg_read_byte(data->i2c_master, config->i2c_slave_addr,
|
|
LSM9DS0_GYRO_REG_OUT_X_L_G, &x_l) < 0 ||
|
|
i2c_reg_read_byte(data->i2c_master, config->i2c_slave_addr,
|
|
LSM9DS0_GYRO_REG_OUT_X_H_G, &x_h) < 0 ||
|
|
i2c_reg_read_byte(data->i2c_master, config->i2c_slave_addr,
|
|
LSM9DS0_GYRO_REG_OUT_Y_L_G, &y_l) < 0 ||
|
|
i2c_reg_read_byte(data->i2c_master, config->i2c_slave_addr,
|
|
LSM9DS0_GYRO_REG_OUT_Y_H_G, &y_h) < 0 ||
|
|
i2c_reg_read_byte(data->i2c_master, config->i2c_slave_addr,
|
|
LSM9DS0_GYRO_REG_OUT_Z_L_G, &z_l) < 0 ||
|
|
i2c_reg_read_byte(data->i2c_master, config->i2c_slave_addr,
|
|
LSM9DS0_GYRO_REG_OUT_Z_H_G, &z_h) < 0) {
|
|
SYS_LOG_DBG("failed to read sample");
|
|
return -EIO;
|
|
}
|
|
|
|
data->sample_x = (int16_t)((uint16_t)(x_l) | ((uint16_t)(x_h) << 8));
|
|
data->sample_y = (int16_t)((uint16_t)(y_l) | ((uint16_t)(y_h) << 8));
|
|
data->sample_z = (int16_t)((uint16_t)(z_l) | ((uint16_t)(z_h) << 8));
|
|
|
|
#if defined(CONFIG_LSM9DS0_GYRO_FULLSCALE_RUNTIME)
|
|
data->sample_fs = data->fs;
|
|
#endif
|
|
|
|
return 0;
|
|
}
|
|
|
|
static inline void lsm9ds0_gyro_convert(struct sensor_value *val, int raw_val,
|
|
float numerator)
|
|
{
|
|
val->type = SENSOR_VALUE_TYPE_DOUBLE;
|
|
val->dval = (double)(raw_val) * numerator / 1000.0 * DEG2RAD;
|
|
}
|
|
|
|
static inline int lsm9ds0_gyro_get_channel(enum sensor_channel chan,
|
|
struct sensor_value *val,
|
|
struct lsm9ds0_gyro_data *data,
|
|
float numerator)
|
|
{
|
|
switch (chan) {
|
|
case SENSOR_CHAN_GYRO_X:
|
|
lsm9ds0_gyro_convert(val, data->sample_x, numerator);
|
|
break;
|
|
case SENSOR_CHAN_GYRO_Y:
|
|
lsm9ds0_gyro_convert(val, data->sample_y, numerator);
|
|
break;
|
|
case SENSOR_CHAN_GYRO_Z:
|
|
lsm9ds0_gyro_convert(val, data->sample_z, numerator);
|
|
break;
|
|
case SENSOR_CHAN_GYRO_ANY:
|
|
lsm9ds0_gyro_convert(val, data->sample_x, numerator);
|
|
lsm9ds0_gyro_convert(val + 1, data->sample_y, numerator);
|
|
lsm9ds0_gyro_convert(val + 2, data->sample_z, numerator);
|
|
break;
|
|
default:
|
|
return -ENOTSUP;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int lsm9ds0_gyro_channel_get(struct device *dev,
|
|
enum sensor_channel chan,
|
|
struct sensor_value *val)
|
|
{
|
|
struct lsm9ds0_gyro_data *data = dev->driver_data;
|
|
|
|
#if defined(CONFIG_LSM9DS0_GYRO_FULLSCALE_RUNTIME)
|
|
switch (data->sample_fs) {
|
|
case 0:
|
|
return lsm9ds0_gyro_get_channel(chan, val, data, 8.75f);
|
|
case 1:
|
|
return lsm9ds0_gyro_get_channel(chan, val, data, 17.50f);
|
|
default:
|
|
return lsm9ds0_gyro_get_channel(chan, val, data, 70.0f);
|
|
}
|
|
#elif defined(CONFIG_LSM9DS0_GYRO_FULLSCALE_245)
|
|
return lsm9ds0_gyro_get_channel(chan, val, data, 8.75f);
|
|
#elif defined(CONFIG_LSM9DS0_GYRO_FULLSCALE_500)
|
|
return lsm9ds0_gyro_get_channel(chan, val, data, 17.50f);
|
|
#elif defined(CONFIG_LSM9DS0_GYRO_FULLSCALE_2000)
|
|
return lsm9ds0_gyro_get_channel(chan, val, data, 70.0f);
|
|
#endif
|
|
return 0;
|
|
}
|
|
|
|
#if defined(LSM9DS0_GYRO_SET_ATTR)
|
|
static int lsm9ds0_gyro_attr_set(struct device *dev,
|
|
enum sensor_channel chan,
|
|
enum sensor_attribute attr,
|
|
const struct sensor_value *val)
|
|
{
|
|
switch (attr) {
|
|
#if defined(CONFIG_LSM9DS0_GYRO_FULLSCALE_RUNTIME)
|
|
case SENSOR_ATTR_FULL_SCALE:
|
|
if (val->type != SENSOR_VALUE_TYPE_INT &&
|
|
val->type != SENSOR_VALUE_TYPE_INT_PLUS_MICRO) {
|
|
return -ENOTSUP;
|
|
}
|
|
|
|
if (lsm9ds0_gyro_set_fs(dev, sensor_rad_to_degrees(val)) < 0) {
|
|
SYS_LOG_DBG("full-scale value not supported");
|
|
return -EIO;
|
|
}
|
|
break;
|
|
#endif
|
|
#if defined(CONFIG_LSM9DS0_GYRO_SAMPLING_RATE_RUNTIME)
|
|
case SENSOR_ATTR_SAMPLING_FREQUENCY:
|
|
if (val->type != SENSOR_VALUE_TYPE_INT) {
|
|
return -ENOTSUP;
|
|
}
|
|
|
|
if (lsm9ds0_gyro_set_odr(dev, val->val1) < 0) {
|
|
SYS_LOG_DBG("sampling frequency value not supported");
|
|
return -EIO;
|
|
}
|
|
break;
|
|
#endif
|
|
default:
|
|
return -ENOTSUP;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
#endif
|
|
|
|
static struct sensor_driver_api lsm9ds0_gyro_api_funcs = {
|
|
.sample_fetch = lsm9ds0_gyro_sample_fetch,
|
|
.channel_get = lsm9ds0_gyro_channel_get,
|
|
#if defined(LSM9DS0_GYRO_SET_ATTR)
|
|
.attr_set = lsm9ds0_gyro_attr_set,
|
|
#endif
|
|
#if defined(CONFIG_LSM9DS0_GYRO_TRIGGER_DRDY)
|
|
.trigger_set = lsm9ds0_gyro_trigger_set,
|
|
#endif
|
|
};
|
|
|
|
static int lsm9ds0_gyro_init_chip(struct device *dev)
|
|
{
|
|
struct lsm9ds0_gyro_data *data = dev->driver_data;
|
|
struct lsm9ds0_gyro_config *config = dev->config->config_info;
|
|
uint8_t chip_id;
|
|
|
|
if (lsm9ds0_gyro_power_ctrl(dev, 0, 0, 0, 0) < 0) {
|
|
SYS_LOG_DBG("failed to power off device");
|
|
return -EIO;
|
|
}
|
|
|
|
if (lsm9ds0_gyro_power_ctrl(dev, 1, 1, 1, 1) < 0) {
|
|
SYS_LOG_DBG("failed to power on device");
|
|
return -EIO;
|
|
}
|
|
|
|
if (i2c_reg_read_byte(data->i2c_master, config->i2c_slave_addr,
|
|
LSM9DS0_GYRO_REG_WHO_AM_I_G, &chip_id) < 0) {
|
|
SYS_LOG_DBG("failed reading chip id");
|
|
goto err_poweroff;
|
|
}
|
|
if (chip_id != LSM9DS0_GYRO_VAL_WHO_AM_I_G) {
|
|
SYS_LOG_DBG("invalid chip id 0x%x", chip_id);
|
|
goto err_poweroff;
|
|
}
|
|
SYS_LOG_DBG("chip id 0x%x", chip_id);
|
|
|
|
if (lsm9ds0_gyro_set_fs_raw(dev, LSM9DS0_GYRO_DEFAULT_FULLSCALE) < 0) {
|
|
SYS_LOG_DBG("failed to set full-scale");
|
|
goto err_poweroff;
|
|
}
|
|
|
|
if (lsm9ds0_gyro_set_odr_raw(dev, LSM9DS0_GYRO_DEFAULT_SAMPLING_RATE)
|
|
< 0) {
|
|
SYS_LOG_DBG("failed to set sampling rate");
|
|
goto err_poweroff;
|
|
}
|
|
|
|
if (i2c_reg_update_byte(data->i2c_master, config->i2c_slave_addr,
|
|
LSM9DS0_GYRO_REG_CTRL_REG4_G,
|
|
LSM9DS0_GYRO_MASK_CTRL_REG4_G_BDU |
|
|
LSM9DS0_GYRO_MASK_CTRL_REG4_G_BLE,
|
|
(1 << LSM9DS0_GYRO_SHIFT_CTRL_REG4_G_BDU) |
|
|
(0 << LSM9DS0_GYRO_SHIFT_CTRL_REG4_G_BLE))
|
|
< 0) {
|
|
SYS_LOG_DBG("failed to set BDU and BLE");
|
|
goto err_poweroff;
|
|
}
|
|
|
|
return 0;
|
|
|
|
err_poweroff:
|
|
lsm9ds0_gyro_power_ctrl(dev, 0, 0, 0, 0);
|
|
return -EIO;
|
|
}
|
|
|
|
int lsm9ds0_gyro_init(struct device *dev)
|
|
{
|
|
const struct lsm9ds0_gyro_config * const config =
|
|
dev->config->config_info;
|
|
struct lsm9ds0_gyro_data *data = dev->driver_data;
|
|
|
|
data->i2c_master = device_get_binding(config->i2c_master_dev_name);
|
|
if (!data->i2c_master) {
|
|
SYS_LOG_DBG("i2c master not found: %s",
|
|
config->i2c_master_dev_name);
|
|
return -EINVAL;
|
|
}
|
|
|
|
if (lsm9ds0_gyro_init_chip(dev) < 0) {
|
|
SYS_LOG_DBG("failed to initialize chip");
|
|
return -EIO;
|
|
}
|
|
|
|
#if defined(CONFIG_LSM9DS0_GYRO_TRIGGER_DRDY)
|
|
if (lsm9ds0_gyro_init_interrupt(dev) < 0) {
|
|
SYS_LOG_DBG("failed to initialize interrupts");
|
|
return -EIO;
|
|
}
|
|
|
|
data->dev = dev;
|
|
#endif
|
|
|
|
dev->driver_api = &lsm9ds0_gyro_api_funcs;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static struct lsm9ds0_gyro_config lsm9ds0_gyro_config = {
|
|
.i2c_master_dev_name = CONFIG_LSM9DS0_GYRO_I2C_MASTER_DEV_NAME,
|
|
.i2c_slave_addr = LSM9DS0_GYRO_I2C_ADDR,
|
|
#if defined(CONFIG_LSM9DS0_GYRO_TRIGGER_DRDY)
|
|
.gpio_drdy_dev_name = CONFIG_LSM9DS0_GYRO_GPIO_DRDY_DEV_NAME,
|
|
.gpio_drdy_int_pin = CONFIG_LSM9DS0_GYRO_GPIO_DRDY_INT_PIN,
|
|
#endif
|
|
};
|
|
|
|
struct lsm9ds0_gyro_data lsm9ds0_gyro_data;
|
|
|
|
DEVICE_INIT(lsm9ds0_gyro, CONFIG_LSM9DS0_GYRO_DEV_NAME, lsm9ds0_gyro_init,
|
|
&lsm9ds0_gyro_data, &lsm9ds0_gyro_config, SECONDARY,
|
|
CONFIG_LSM9DS0_GYRO_INIT_PRIORITY);
|