mirror of
https://github.com/RIOT-OS/RIOT.git
synced 2024-12-29 04:50:03 +01:00
343 lines
8.9 KiB
C
343 lines
8.9 KiB
C
/*
|
|
* Copyright (C) 2018 Gunar Schorcht
|
|
*
|
|
* This file is subject to the terms and conditions of the GNU Lesser
|
|
* General Public License v2.1. See the file LICENSE in the top level
|
|
* directory for more details.
|
|
*/
|
|
|
|
/**
|
|
* @ingroup drivers_itg320x
|
|
* @brief Device driver for the InvenSense ITG320X 3-axis gyroscope
|
|
* @author Gunar Schorcht <gunar@schorcht.net>
|
|
* @file
|
|
* @{
|
|
*/
|
|
|
|
#include <assert.h>
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
#include <errno.h>
|
|
|
|
#include "itg320x_regs.h"
|
|
#include "itg320x.h"
|
|
|
|
#include "log.h"
|
|
#include "xtimer.h"
|
|
|
|
#define ENABLE_DEBUG 0
|
|
#include "debug.h"
|
|
|
|
#define DEBUG_DEV(f, d, ...) \
|
|
DEBUG("[itg320x] %s i2c dev=%d addr=%02x: " f "\n", \
|
|
__func__, d->params.dev, d->params.addr, ## __VA_ARGS__);
|
|
|
|
#define ERROR_DEV(f, d, ...) \
|
|
do { \
|
|
LOG_ERROR("[itg320x] %s i2c dev=%d addr=%02x: " f "\n", \
|
|
__func__, d->params.dev, d->params.addr, ## __VA_ARGS__); \
|
|
} while (0)
|
|
|
|
#define EXEC_RET(f) \
|
|
do { \
|
|
int _r; \
|
|
if ((_r = f) != ITG320X_OK) { \
|
|
DEBUG("[itg320x] %s: error code %d\n", __func__, _r); \
|
|
return _r; \
|
|
} \
|
|
} while (0)
|
|
|
|
/** Forward declaration of functions for internal use */
|
|
|
|
static int _is_available(const itg320x_t *dev);
|
|
static int _reset(itg320x_t *dev);
|
|
|
|
static int _reg_read(const itg320x_t *dev, uint8_t reg, uint8_t *data, uint16_t len);
|
|
static int _reg_write(const itg320x_t *dev, uint8_t reg, uint8_t data);
|
|
static int _update_reg(const itg320x_t *dev, uint8_t reg, uint8_t mask, uint8_t val);
|
|
|
|
int itg320x_init(itg320x_t *dev, const itg320x_params_t *params)
|
|
{
|
|
assert(dev != NULL);
|
|
assert(params != NULL);
|
|
|
|
DEBUG_DEV("params=%p", dev, params);
|
|
|
|
/* init sensor data structure */
|
|
dev->params = *params;
|
|
|
|
/* check availability of the sensor */
|
|
EXEC_RET(_is_available(dev));
|
|
|
|
/* reset the sensor */
|
|
EXEC_RET(_reset(dev));
|
|
|
|
/* set internal sample rate divider (ISR) from parameters */
|
|
EXEC_RET(_reg_write(dev, ITG320X_REG_SMPLRT_DIV, params->isr_div));
|
|
|
|
/* set full scale always to +-2000 and LPF bandwidth from parameters */
|
|
EXEC_RET(_reg_write(dev, ITG320X_REG_DLPFS,
|
|
params->lpf_bw | ITG320X_REG_DLPFS_FS_SEL_VAL));
|
|
|
|
/* set clock source selection from parameters */
|
|
EXEC_RET(_reg_write(dev, ITG320X_REG_PWR_MGM, params->clk_sel));
|
|
|
|
return ITG320X_OK;
|
|
}
|
|
|
|
#ifdef MODULE_ITG320X_INT
|
|
|
|
int itg320x_init_int(const itg320x_t *dev, itg320x_drdy_int_cb_t cb, void *arg)
|
|
{
|
|
assert(dev != NULL);
|
|
assert(gpio_is_valid(dev->params.int_pin));
|
|
|
|
DEBUG_DEV("cb=%p, arg=%p", dev, cb, arg);
|
|
|
|
if (dev->params.int_level == ITG320X_INT_HIGH) {
|
|
/* for high active interrupt signal (default) */
|
|
gpio_init_int(dev->params.int_pin, GPIO_IN, GPIO_RISING, cb, arg);
|
|
}
|
|
else {
|
|
/* for low active interrupt signal (default) */
|
|
gpio_init_int(dev->params.int_pin, GPIO_IN, GPIO_FALLING, cb, arg);
|
|
}
|
|
|
|
/*
|
|
* Set interrupt configuration as following
|
|
* - Logic level and drive type used from parameters
|
|
* - Latching interrupt is enabled
|
|
* - Latch clear method is reading the status register
|
|
* - RAW data ready interrupt is enabled
|
|
*/
|
|
EXEC_RET(_reg_write(dev, ITG320X_REG_INT_CFG,
|
|
dev->params.int_level | dev->params.int_drive |
|
|
ITG320X_REG_INT_CFG_LATCH_INT |
|
|
ITG320X_REG_INT_STATUS_RAW_RDY |
|
|
ITG320X_REG_INT_CFG_RAW_RDY_EN));
|
|
|
|
return ITG320X_OK;
|
|
}
|
|
|
|
#endif /* MODULE_ITG320X_INT */
|
|
|
|
int itg320x_data_ready(const itg320x_t *dev)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
DEBUG_DEV("", dev);
|
|
|
|
uint8_t reg;
|
|
EXEC_RET(_reg_read(dev, ITG320X_REG_INT_STATUS, ®, 1));
|
|
|
|
return (reg & ITG320X_REG_INT_STATUS_RAW_RDY) ? ITG320X_OK
|
|
: ITG320X_ERROR_NO_DATA;
|
|
}
|
|
|
|
int itg320x_read(const itg320x_t *dev, itg320x_data_t *data)
|
|
{
|
|
assert(dev != NULL);
|
|
assert(data != NULL);
|
|
|
|
DEBUG_DEV("data=%p", dev, data);
|
|
|
|
itg320x_raw_data_t raw;
|
|
|
|
EXEC_RET(itg320x_read_raw(dev, &raw));
|
|
|
|
/*
|
|
* The sensitivity of the sensor is 1/14.375 degree/seconds per LSB
|
|
* with a tolerance of the scale factor of +-6 %. Scale raw values to
|
|
* tenths of a degree/seconds according to sensors sensitivity
|
|
* which corresponds to 8/115 degrees/seconds per LSB.
|
|
*/
|
|
data->x = (uint32_t)(raw.x * 80 / 115);
|
|
data->y = (uint32_t)(raw.y * 80 / 115);
|
|
data->z = (uint32_t)(raw.z * 80 / 115);
|
|
|
|
return ITG320X_OK;
|
|
}
|
|
|
|
int itg320x_read_raw(const itg320x_t *dev, itg320x_raw_data_t *raw)
|
|
{
|
|
assert(dev != NULL);
|
|
assert(raw != NULL);
|
|
|
|
DEBUG_DEV("raw=%p", dev, raw);
|
|
|
|
uint8_t data[6];
|
|
|
|
/* read raw data sample */
|
|
EXEC_RET(_reg_read(dev, ITG320X_REG_GYRO_XOUT_H, data, 6));
|
|
|
|
/* data MSB @ lower address */
|
|
raw->x = (data[0] << 8) | data[1];
|
|
raw->y = (data[2] << 8) | data[3];
|
|
raw->z = (data[4] << 8) | data[5];
|
|
|
|
#ifdef MODULE_ITG320X_INT
|
|
/* read status register to clear the interrupt */
|
|
EXEC_RET(_reg_read(dev, ITG320X_REG_INT_STATUS, data, 1));
|
|
#endif
|
|
|
|
return ITG320X_OK;
|
|
}
|
|
|
|
int itg320x_read_temp(const itg320x_t *dev, int16_t *temp)
|
|
{
|
|
assert(dev != NULL);
|
|
assert(temp != NULL);
|
|
|
|
DEBUG_DEV("temp=%p", dev, temp);
|
|
|
|
uint8_t data[2];
|
|
|
|
/* read raw temperature */
|
|
EXEC_RET(_reg_read(dev, ITG320X_REG_TEMP_OUT_H, data, 2));
|
|
|
|
/* data MSB @ lower address */
|
|
*temp = (data[0] << 8) | data[1];
|
|
/* convert raw temperature data to tenths of a degree Celsius */
|
|
*temp = (*temp + 13200) / 28 + 350;
|
|
|
|
return ITG320X_OK;
|
|
}
|
|
|
|
int itg320x_power_down(itg320x_t *dev)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
DEBUG_DEV("", dev);
|
|
|
|
return _update_reg(dev, ITG320X_REG_PWR_MGM, ITG320X_REG_PWR_MGM_SLEEP, 1);
|
|
}
|
|
|
|
int itg320x_power_up(itg320x_t *dev)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
DEBUG_DEV("", dev);
|
|
|
|
EXEC_RET(_update_reg(dev, ITG320X_REG_PWR_MGM, ITG320X_REG_PWR_MGM_SLEEP, 0));
|
|
|
|
/* wait 20 ms after power-up */
|
|
xtimer_msleep(20);
|
|
|
|
return ITG320X_OK;
|
|
}
|
|
|
|
/** Functions for internal use only */
|
|
|
|
static int _reset(itg320x_t *dev)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
DEBUG_DEV("", dev);
|
|
|
|
/* set the reset flag, it automatically reset by the device */
|
|
EXEC_RET(_update_reg(dev, ITG320X_REG_PWR_MGM, ITG320X_REG_PWR_MGM_H_RESET, 1));
|
|
|
|
/* wait 20 ms after reset */
|
|
xtimer_msleep(20);
|
|
|
|
return ITG320X_OK;
|
|
}
|
|
|
|
/**
|
|
* @brief Check the chip ID to test whether sensor is available
|
|
*/
|
|
static int _is_available(const itg320x_t *dev)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
DEBUG_DEV("", dev);
|
|
|
|
uint8_t reg;
|
|
|
|
/* read the chip id from ITG320X_REG_ID_X */
|
|
EXEC_RET(_reg_read(dev, ITG320X_REG_WHO_AM_I, ®, 1));
|
|
|
|
if (reg != ITG320X_ID) {
|
|
DEBUG_DEV("sensor is not available, wrong id %02x, should be %02x",
|
|
dev, reg, ITG320X_ID);
|
|
return ITG320X_ERROR_WRONG_ID;
|
|
}
|
|
|
|
return ITG320X_OK;
|
|
}
|
|
|
|
static int _update_reg(const itg320x_t *dev, uint8_t reg, uint8_t mask, uint8_t val)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
DEBUG_DEV("reg=%02x mask=%02x val=%02x", dev, reg, mask, val);
|
|
|
|
uint8_t reg_val;
|
|
uint8_t shift = 0;
|
|
|
|
while (!((mask >> shift) & 0x01)) {
|
|
shift++;
|
|
}
|
|
|
|
/* read current register value */
|
|
EXEC_RET(_reg_read(dev, reg, ®_val, 1));
|
|
|
|
/* set masked bits to the given value */
|
|
reg_val = (reg_val & ~mask) | ((val << shift) & mask);
|
|
|
|
/* write back new register value */
|
|
EXEC_RET(_reg_write(dev, reg, reg_val));
|
|
|
|
return ITG320X_OK;
|
|
}
|
|
|
|
static int _reg_read(const itg320x_t *dev, uint8_t reg, uint8_t *data, uint16_t len)
|
|
{
|
|
assert(dev != NULL);
|
|
assert(data != NULL);
|
|
assert(len != 0);
|
|
|
|
DEBUG_DEV("read %d bytes from reg 0x%02x", dev, len, reg);
|
|
|
|
i2c_acquire(dev->params.dev);
|
|
int res = i2c_read_regs(dev->params.dev, dev->params.addr, reg, data, len, 0);
|
|
i2c_release(dev->params.dev);
|
|
|
|
if (res != 0) {
|
|
DEBUG_DEV("could not read %d bytes from sensor registers "
|
|
"starting at addr %02x, reason %d (%s)",
|
|
dev, len, reg, res, strerror(res * -1));
|
|
return ITG320X_ERROR_I2C;
|
|
}
|
|
|
|
if (IS_ACTIVE(ENABLE_DEBUG)) {
|
|
printf("[itg320x] %s i2c dev=%d addr=%02x: read %d bytes from reg 0x%02x: ",
|
|
__func__, dev->params.dev, dev->params.addr, len, reg);
|
|
for (unsigned i = 0; i < len; i++) {
|
|
printf("%02x ", data[i]);
|
|
}
|
|
printf("\n");
|
|
}
|
|
|
|
return ITG320X_OK;
|
|
}
|
|
|
|
static int _reg_write(const itg320x_t *dev, uint8_t reg, uint8_t data)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
DEBUG_DEV("write 1 byte to reg 0x%02x: 0x%02x", dev, reg, data);
|
|
|
|
i2c_acquire(dev->params.dev);
|
|
int res = i2c_write_regs(dev->params.dev, dev->params.addr, reg, &data, 1, 0);
|
|
i2c_release(dev->params.dev);
|
|
|
|
if (res != 0) {
|
|
DEBUG_DEV("could not write to sensor register 0x%02x, reason %d (%s)",
|
|
dev, reg, res, strerror(res * -1));
|
|
return ITG320X_ERROR_I2C;
|
|
}
|
|
|
|
return ITG320X_OK;
|
|
}
|