RIOT-OS/RIOT
RIOT-OS/RIOT
1
0
Fork 0
mirror of https://github.com/RIOT-OS/RIOT.git synced 2024-12-29 04:50:03 +01:00
Code Releases Activity
630b14b322
RIOT/drivers/l3gxxxx/l3gxxxx.c
Benjamin Valentin fcece44394 drivers/l3gxxxx: fix check for l3g4200d_ng
2023-04-28 17:24:15 +02:00

1144 lines
33 KiB
C
Raw Blame History

/*
* 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_l3gxxxx
* @brief Device Driver for ST L3Gxxxx 3-axis gyroscope family
* @author Gunar Schorcht <gunar@schorcht.net>
* @file
* @{
*/
#include <string.h>
#include <stdlib.h>
#include "l3gxxxx_regs.h"
#include "l3gxxxx.h"
#include "byteorder.h"
#include "log.h"
#define ENABLE_DEBUG 0
#include "debug.h"
#if ENABLE_DEBUG
#define ASSERT_PARAM(cond) \
if (!(cond)) { \
DEBUG("[l3gxxxx] %s: %s\n", \
__func__, "parameter condition (" # cond ") not fulfilled"); \
assert(cond); \
}
#define DEBUG_DEV(m, d, ...) \
DEBUG("[l3gxxxx] %s dev=%" PRIxPTR ": " m "\n", \
__func__, (unsigned int)d, ## __VA_ARGS__)
#else /* ENABLE_DEBUG */
#define ASSERT_PARAM(cond) assert(cond);
#define DEBUG_DEV(m, d, ...)
#endif /* ENABLE_DEBUG */
/* set a single bit defined by mask `m` in 8-bit register value `r` to value `v` */
#define _SET_REG_BIT(r, m, v) r = ((v) & ~m) ? r | m : r & ~m
/* get the value of a single bit defined by mask `m` from 8-bit register value `r` */
#define _GET_REG_BIT(r, m) (r & m) ? 1 : 0
/* set the bits defined by mask `m` in 8-bit register value `r` to value `v` */
#define _SET_REG_VALUE(r, m, v) r = (r & ~m) | (((v) << m ## _S) & m)
/* get the value the bits defined by mask `m` from 8-bit register value `r` */
#define _GET_REG_VALUE(r, m) ((r & ~m) >> m ## _S)
#if IS_USED(MODULE_L3GXXXX_SPI)
#define _SPI_DEV (dev->params.if_params.spi.dev)
#define _SPI_CS (dev->params.if_params.spi.cs)
#define _SPI_CLK (dev->params.if_params.spi.clk)
#define _IS_DEV_SPI (dev->params.if_params.type == L3GXXXX_SPI)
#endif
#if IS_USED(MODULE_L3GXXXX_I2C)
#define _I2C_DEV (dev->params.if_params.i2c.dev)
#define _I2C_ADDR (dev->params.if_params.i2c.addr)
#define _IS_DEV_I2C (dev->params.if_params.type == L3GXXXX_I2C)
#endif
#define L3GXXXX_INT_FIFO (L3GXXXX_INT_FIFO_WATERMARK | \
L3GXXXX_INT_FIFO_OVERRUN)
int l3gxxxx_reg_update(const l3gxxxx_t *dev,
uint8_t reg, uint8_t mask, uint8_t val);
/** Forward declaration of functions for internal use */
static int _is_available(l3gxxxx_t *dev);
static void _acquire(const l3gxxxx_t *dev);
static void _release(const l3gxxxx_t *dev);
static int _read(const l3gxxxx_t *dev, uint8_t reg, uint8_t *data, uint8_t len);
static int _write(const l3gxxxx_t *dev, uint8_t reg, const uint8_t *data, uint8_t len);
static int _update(const l3gxxxx_t *dev, uint8_t reg, uint8_t mask, uint8_t val);
#if IS_USED(MODULE_L3GXXXX_IRQ_EVENT)
static int _get_int_event_src(const l3gxxxx_t *dev, l3gxxxx_int_event_src_t *src);
#endif
#if IS_USED(MODULE_L3GXXXX_IRQ_DATA)
static int _get_int_data_src(const l3gxxxx_t *dev, l3gxxxx_int_data_src_t *src);
#endif
#define msb_lsb_to_type(t, b, o) (t)(((t)b[o] << 8) | b[o + 1])
int l3gxxxx_init(l3gxxxx_t *dev, const l3gxxxx_params_t *params)
{
int res = L3GXXXX_OK;
ASSERT_PARAM(dev != NULL);
ASSERT_PARAM(params != NULL);
DEBUG_DEV("params=%p", dev, params);
/* init sensor data structure */
dev->params = *params;
mutex_init(&dev->int_lock);
#if IS_USED(MODULE_L3GXXXX_SPI)
ASSERT_PARAM(gpio_is_valid(_SPI_CS));
/* for SPI, we only need to initialize the chip select pin */
if (spi_init_cs(_SPI_DEV, _SPI_CS) != SPI_OK) {
return -L3GXXXX_ERROR_SPI;
}
#endif /* IS_USED(MODULE_L3GXXXX_SPI) */
_acquire(dev);
/* check availability of the sensor */
if ((res = _is_available(dev)) != L3GXXXX_OK) {
_release(dev);
return res;
}
#if IS_USED(MODULE_A3G4250D)
ASSERT_PARAM((dev->params.scale == L3GXXXX_SCALE_245_DPS) ||
(dev->sensor != X3G42XXD));
#endif
uint8_t reg[8] = { };
res |= _write(dev, L3GXXXX_REG_CTRL1, reg, 6);
res |= _write(dev, L3GXXXX_REG_FIFO_CTRL, reg, 1);
res |= _write(dev, L3GXXXX_REG_IG_CFG, reg, 1);
res |= _write(dev, L3GXXXX_REG_IG_THS_XH, reg, IS_USED(MODULE_L3GD20H) ? 8 : 7);
reg[0] = (dev->params.odr << L3GXXXX_ODR_BW_S) | L3GXXXX_POWER_MODE
| L3GXXXX_XYZ_ENABLED;
reg[1] = (dev->params.hpf_mode << L3GXXXX_HPF_MODE_S) |
(dev->params.hpf_cutoff & L3GXXXX_HPF_CUTOFF);
reg[3] = (dev->params.scale << L3GXXXX_FULL_SCALE_S) | L3GXXXX_BLOCK_DATA_UPDATE;
reg[4] = (dev->params.filter_sel & L3GXXXX_OUT_SEL);
/* HPF has to be enabled by a separate bit if filter mode is 1 or 3 */
reg[4] |= (dev->params.filter_sel & 1) ? L3GXXXX_HP_ENABLED : 0;
/* initialize sensor completely including setting in power down mode */
res |= _write(dev, L3GXXXX_REG_CTRL1, reg, 6);
#if IS_USED(MODULE_L3GD20H) && IS_USED(MODULE_L3GXXXX_LOW_ODR)
/* in case of low ODR, low data rate flag has to be set */
if (dev->params.odr >= L3GXXXX_ODR_12) {
ASSERT_PARAM(dev->sensor == L3GD20H);
res |= _update(dev, L3GXXXX_REG_LOW_ODR, L3GXXXX_LOW_ODR, 1);
}
#endif
#if IS_USED(MODULE_L3GXXXX_FIFO)
uint8_t fifo_ctrl = (dev->params.fifo_mode << L3GXXXX_FIFO_MODE_S)
| (dev->params.fifo_watermark & L3GXXXX_FIFO_WATERMARK);
res |= _write(dev, L3GXXXX_REG_FIFO_CTRL, &fifo_ctrl, 1);
reg[4] |= L3GXXXX_FIFO_EN;
#endif /* IS_USED(MODULE_L3GXXXX_FIFO) */
res |= _write(dev, L3GXXXX_REG_CTRL1, reg, 5);
_release(dev);
if (res != L3GXXXX_OK) {
return res;
}
#if IS_USED(MODULE_L3GXXXX_IRQ_DATA)
/* enable FIFO interrupts if FIFO is used, DRDY interrupt otherwise */
if ((res = l3gxxxx_enable_int(dev, IS_USED(MODULE_L3GXXXX_FIFO)
? L3GXXXX_INT_FIFO
: L3GXXXX_INT_DATA_READY, true))) {
return res;
}
#endif /* IS_USED(MODULE_L3GXXXX_IRQ_DATA) */
#if IS_USED(MODULE_L3GXXXX_IRQ_EVENT)
if ((res = l3gxxxx_set_int_event_cfg(dev, &dev->params.int1_cfg)) ||
(res = l3gxxxx_enable_int(dev, L3GXXXX_INT_EVENT, true), res)) {
return res;
}
#endif /* IS_USED(MODULE_L3GXXXX_IRQ_EVENT) */
#if IS_USED(MODULE_L3GXXXX_I2C)
if (_IS_DEV_I2C) {
DEBUG_DEV("I2C device initialized: bus %d, addr=%02x",
dev, _I2C_DEV, _I2C_ADDR);
}
#endif /* IS_USED(MODULE_L3GXXXX_I2C) */
#if IS_USED(MODULE_L3GXXXX_SPI)
if (_IS_DEV_SPI) {
DEBUG_DEV("SPI device initialized: bus %d, cs=%u",
dev, _SPI_DEV, (unsigned)_SPI_CS);
}
#endif /* IS_USED(MODULE_L3GXXXX_SPI) */
return res;
}
int l3gxxxx_data_ready(const l3gxxxx_t *dev)
{
ASSERT_PARAM(dev != NULL);
DEBUG_DEV("", dev);
int res = L3GXXXX_OK;
uint8_t reg;
#if IS_USED(MODULE_L3GXXXX_FIFO)
if (dev->params.fifo_mode == L3GXXXX_BYPASS) {
res = l3gxxxx_reg_read(dev, L3GXXXX_REG_STATUS, &reg, 1);
return res ? res : _GET_REG_BIT(reg, L3GXXXX_ANY_DATA_READY);
}
else {
res = l3gxxxx_reg_read(dev, L3GXXXX_REG_FIFO_SRC, &reg, 1);
return res ? res : _GET_REG_VALUE(reg, L3GXXXX_FIFO_FFS);
}
#else
res = l3gxxxx_reg_read(dev, L3GXXXX_REG_STATUS, &reg, 1);
return res ? res : _GET_REG_BIT(reg, L3GXXXX_ANY_DATA_READY);
#endif
}
/*
* scale factors for conversion of raw sensor data to degree for possible
* sensitivities according to mechanical characteristics in datasheet
* multiplied by 4,000
*/
static const uint16_t L3GXXXX_SCALES[3] = { 35, 70, 280 };
int l3gxxxx_read(const l3gxxxx_t *dev, l3gxxxx_data_t *data)
{
ASSERT_PARAM(dev != NULL);
ASSERT_PARAM(data != NULL);
DEBUG_DEV("data=%p", dev, data);
int res = L3GXXXX_OK;
l3gxxxx_raw_data_t raw;
if ((res = l3gxxxx_read_raw(dev, &raw)) != L3GXXXX_OK) {
return res;
}
data->x = (raw.x * L3GXXXX_SCALES[dev->params.scale]) >> 2;
data->y = (raw.y * L3GXXXX_SCALES[dev->params.scale]) >> 2;
data->z = (raw.z * L3GXXXX_SCALES[dev->params.scale]) >> 2;
return res;
}
int l3gxxxx_read_raw(const l3gxxxx_t *dev, l3gxxxx_raw_data_t *raw)
{
ASSERT_PARAM(dev != NULL);
ASSERT_PARAM(raw != NULL);
DEBUG_DEV("raw=%p", dev, raw);
int res = L3GXXXX_OK;
#if IS_USED(MODULE_L3GXXXX_FIFO)
/* if not in bypass mode we read out the FIFO and return newest data sample */
if (dev->params.fifo_mode != L3GXXXX_BYPASS) {
l3gxxxx_raw_data_fifo_t raw_fifo = {};
int num;
/* read raw data samples from FIFO */
if ((num = l3gxxxx_read_raw_fifo(dev, raw_fifo)) < 0) {
DEBUG_DEV("reading raw data samples from FIFO failed", dev);
return num;
}
if (num == 0) {
DEBUG_DEV("no raw data in FIFO", dev);
return -L3GXXXX_ERROR_NO_NEW_DATA;
}
*raw = raw_fifo[num - 1];
return res;
}
#endif /* IS_USED(MODULE_L3GXXXX_FIFO) */
uint8_t data[6];
/* read raw data sample and clear interrupt signal INT2/DRDY if used */
if (l3gxxxx_reg_read(dev, L3GXXXX_REG_OUT_X_L, data, 6)) {
DEBUG_DEV("could not get raw data", dev);
return -L3GXXXX_ERROR_RAW_DATA;
}
/* L3GXXXX_REG_CTRL4.BLE = 0, Data LSB @ lower address */
raw->x = byteorder_lebuftohs(&data[0]);
raw->y = byteorder_lebuftohs(&data[2]);
raw->z = byteorder_lebuftohs(&data[4]);
return res;
}
#if IS_USED(MODULE_L3GXXXX_FIFO)
int l3gxxxx_read_fifo(const l3gxxxx_t *dev, l3gxxxx_data_fifo_t data)
{
ASSERT_PARAM(dev != NULL);
ASSERT_PARAM(data != NULL);
DEBUG_DEV("", dev);
l3gxxxx_raw_data_fifo_t raw = {};
int num;
if ((num = l3gxxxx_read_raw_fifo(dev, raw)) < 0) {
DEBUG_DEV("reading raw data samples from FIFO failed", dev);
return num;
}
for (int i = 0; i < num; i++) {
data[i].x = (raw[i].x * L3GXXXX_SCALES[dev->params.scale]) >> 2;
data[i].y = (raw[i].y * L3GXXXX_SCALES[dev->params.scale]) >> 2;
data[i].z = (raw[i].z * L3GXXXX_SCALES[dev->params.scale]) >> 2;
}
return num;
}
int l3gxxxx_read_raw_fifo(const l3gxxxx_t *dev, l3gxxxx_raw_data_fifo_t raw)
{
ASSERT_PARAM(dev != NULL);
ASSERT_PARAM(raw != NULL);
DEBUG_DEV("", dev);
int res = L3GXXXX_OK;
/* in bypass mode, we use l3gxxxx_read_raw to return one sample */
if (dev->params.fifo_mode == L3GXXXX_BYPASS) {
res = l3gxxxx_read_raw(dev, raw);
return (res == L3GXXXX_OK) ? 1 : res;
}
uint8_t reg;
_acquire(dev);
/* read FIFO state */
if (_read(dev, L3GXXXX_REG_FIFO_SRC, &reg, 1) != L3GXXXX_OK) {
_release(dev);
return res;
}
/* if nothing is in the FIFO, just return with 0 */
if (reg & L3GXXXX_FIFO_EMPTY) {
_release(dev);
return 0;
}
/* read samples from FIFO */
int samples = (reg & L3GXXXX_FIFO_FFS) + ((reg & L3GXXXX_FIFO_OVR) ? 1 : 0);
/* read samples from FIFO */
for (int i = 0; i < samples; i++) {
uint8_t data[6];
/* read raw data sample */
res |= _read(dev, L3GXXXX_REG_OUT_X_L, data, 6);
/* L3GXXXX_REG_CTRL4.BLE = 0, Data LSB @ lower address */
raw[i].x = byteorder_lebuftohs(&data[0]);
raw[i].y = byteorder_lebuftohs(&data[2]);
raw[i].z = byteorder_lebuftohs(&data[4]);
}
res |= _read(dev, L3GXXXX_REG_FIFO_SRC, &reg, 1);
if (reg & L3GXXXX_FIFO_FFS) {
DEBUG_DEV("New samples stored in FIFO while reading out the FIFO, "
"output data rate (ODR) is too high", dev);
}
if (dev->params.fifo_mode == L3GXXXX_FIFO && samples == 32) {
/* clean FIFO (see app note) */
res |= _update(dev, L3GXXXX_REG_FIFO_CTRL, L3GXXXX_FIFO_MODE, L3GXXXX_BYPASS);
res |= _update(dev, L3GXXXX_REG_FIFO_CTRL, L3GXXXX_FIFO_MODE, L3GXXXX_FIFO);
}
_release(dev);
return (res == L3GXXXX_OK) ? samples : res;
}
#endif /* IS_USED(MODULE_L3GXXXX_FIFO) */
int l3gxxxx_power_down(l3gxxxx_t *dev)
{
ASSERT_PARAM(dev != NULL);
DEBUG_DEV("", dev);
return l3gxxxx_reg_update(dev, L3GXXXX_REG_CTRL1, L3GXXXX_POWER_MODE, 0);
}
int l3gxxxx_power_up(l3gxxxx_t *dev)
{
ASSERT_PARAM(dev != NULL);
DEBUG_DEV("", dev);
int res = l3gxxxx_reg_update(dev, L3GXXXX_REG_CTRL1, L3GXXXX_POWER_MODE, 1);
return res;
}
#if IS_USED(MODULE_L3GXXXX_SLEEP)
int l3gxxxx_sleep(l3gxxxx_t *dev)
{
ASSERT_PARAM(dev != NULL);
DEBUG_DEV("", dev);
return l3gxxxx_reg_update(dev, L3GXXXX_REG_CTRL1, L3GXXXX_XYZ_ENABLED, 0);
}
int l3gxxxx_wake_up(l3gxxxx_t *dev)
{
ASSERT_PARAM(dev != NULL);
DEBUG_DEV("", dev);
int res = l3gxxxx_reg_update(dev, L3GXXXX_REG_CTRL1, L3GXXXX_XYZ_ENABLED,
L3GXXXX_XYZ_ENABLED);
return res;
}
#endif /* IS_USED(MODULE_L3GXXXX_SLEEP) */
#if IS_USED(MODULE_L3GXXXX_IRQ)
int l3gxxxx_enable_int(const l3gxxxx_t *dev,
l3gxxxx_int_types_t mask, bool enable)
{
ASSERT_PARAM(dev != NULL);
DEBUG_DEV("mask=%02x enable=%d", dev, mask, enable);
#if IS_USED(MODULE_L3GXXXX_IRQ_EVENT)
if (enable && (mask & L3GXXXX_INT_EVENT)) {
ASSERT_PARAM(gpio_is_valid(dev->params.int1_pin));
}
#endif
#if IS_USED(MODULE_L3GXXXX_IRQ_DATA)
if (enable && (mask & L3GXXXX_INT_DATA)) {
ASSERT_PARAM(gpio_is_valid(dev->params.int2_pin));
}
#endif
return l3gxxxx_reg_update(dev, L3GXXXX_REG_CTRL3, mask, enable ? 0xff : 0x00);
}
#if IS_USED(MODULE_L3GXXXX_IRQ_EVENT)
static void _cb_int1(void *arg)
{
l3gxxxx_t *dev = (l3gxxxx_t *)arg;
dev->int_type |= L3GXXXX_INT_EVENT;
mutex_unlock(&dev->int_lock);
}
#endif /* IS_USED(MODULE_L3GXXXX_IRQ_EVENT) */
#if IS_USED(MODULE_L3GXXXX_IRQ_DATA)
static void _cb_int2(void *arg)
{
l3gxxxx_t *dev = (l3gxxxx_t *)arg;
dev->int_type |= L3GXXXX_INT_DATA;
mutex_unlock(&dev->int_lock);
}
#endif /* IS_USED(L3GXXXX_IRQ_DRDY) */
l3gxxxx_int_src_t l3gxxxx_wait_int(l3gxxxx_t *dev)
{
ASSERT_PARAM(dev != NULL);
DEBUG_DEV("", dev);
dev->int_type = 0;
#if IS_USED(MODULE_L3GXXXX_IRQ_EVENT)
ASSERT_PARAM(gpio_is_valid(dev->params.int1_pin));
/* init INT1 signal pin and enable the interrupt */
gpio_init_int(dev->params.int1_pin, GPIO_IN, GPIO_RISING, _cb_int1, dev);
#endif /* IS_USED(MODULE_L3GXXXX_IRQ_EVENT) */
#if IS_USED(MODULE_L3GXXXX_IRQ_DATA)
ASSERT_PARAM(gpio_is_valid(dev->params.int2_pin));
/* init INT2/DRDY signal pin and enable the interrupt */
gpio_init_int(dev->params.int2_pin, GPIO_IN, GPIO_RISING, _cb_int2, dev);
#endif /* IS_USED(L3GXXXX_IRQ_DRDY) */
/* wait for an interrupt */
mutex_lock(&dev->int_lock);
#if IS_USED(MODULE_L3GXXXX_IRQ_EVENT)
gpio_irq_disable(dev->params.int1_pin);
#endif /* IS_USED(MODULE_L3GXXXX_IRQ_EVENT) */
#if IS_USED(MODULE_L3GXXXX_IRQ_DATA)
gpio_irq_disable(dev->params.int2_pin);
#endif /* IS_USED(L3GXXXX_IRQ_DRDY) */
l3gxxxx_int_src_t int_src = { };
#if IS_USED(MODULE_L3GXXXX_IRQ_EVENT)
if (dev->int_type & L3GXXXX_INT_EVENT) {
_get_int_event_src(dev, &int_src.event);
}
#endif /* IS_USED(MODULE_L3GXXXX_IRQ_EVENT) */
#if IS_USED(MODULE_L3GXXXX_IRQ_DATA)
if (dev->int_type & L3GXXXX_INT_DATA) {
_get_int_data_src(dev, &int_src.data);
}
#endif /* IS_USED(L3GXXXX_IRQ_DRDY) */
return int_src;
}
#endif /* IS_USED(MODULE_L3GXXXX_IRQ) */
#if IS_USED(MODULE_L3GXXXX_IRQ_EVENT)
int l3gxxxx_set_int_event_cfg(const l3gxxxx_t *dev,
const l3gxxxx_int_event_cfg_t *cfg)
{
ASSERT_PARAM(dev != NULL);
ASSERT_PARAM(cfg != NULL);
DEBUG_DEV("config=%p", dev, cfg);
int res = L3GXXXX_OK;
uint8_t ig_cfg = 0;
uint8_t ig_dur = 0;
uint8_t ig_ths[6] = { 0 };
_SET_REG_BIT(ig_cfg, L3GXXXX_INT1_X_LOW, cfg->x_low_enabled);
_SET_REG_BIT(ig_cfg, L3GXXXX_INT1_X_HIGH, cfg->x_high_enabled);
_SET_REG_BIT(ig_cfg, L3GXXXX_INT1_Y_LOW, cfg->y_low_enabled);
_SET_REG_BIT(ig_cfg, L3GXXXX_INT1_Y_HIGH, cfg->y_high_enabled);
_SET_REG_BIT(ig_cfg, L3GXXXX_INT1_Z_LOW, cfg->z_low_enabled);
_SET_REG_BIT(ig_cfg, L3GXXXX_INT1_Z_HIGH, cfg->z_high_enabled);
_SET_REG_BIT(ig_cfg, L3GXXXX_INT1_LATCH, cfg->latch);
_SET_REG_BIT(ig_cfg, L3GXXXX_INT1_AND_OR, cfg->and_or);
_SET_REG_BIT(ig_dur, L3GXXXX_INT1_WAIT, cfg->wait);
_SET_REG_VALUE(ig_dur, L3GXXXX_INT1_DURATION, cfg->duration);
ig_ths[0] = (cfg->x_threshold >> 8) & 0x7f;
ig_ths[1] = (cfg->x_threshold & 0xff);
ig_ths[2] = (cfg->y_threshold >> 8) & 0x7f;
ig_ths[3] = (cfg->y_threshold & 0xff);
ig_ths[4] = (cfg->z_threshold >> 8) & 0x7f;
ig_ths[5] = (cfg->z_threshold & 0xff);
_acquire(dev);
/* write the thresholds to registers IG_THS_* */
res |= _write(dev, L3GXXXX_REG_IG_THS_XH, ig_ths, 6);
/* write duration configuration to IG_DURATION */
res |= _write(dev, L3GXXXX_REG_IG_DURATION, &ig_dur, 1);
/* write INT1 configuration to IG_CFG */
res |= _write(dev, L3GXXXX_REG_IG_CFG, &ig_cfg, 1);
/* filter selection used for threshold comparison for INT1 generation */
res |= _update(dev, L3GXXXX_REG_CTRL5, L3GXXXX_IG_SEL, cfg->filter);
/* try to set HPen in case LPF2 and HPF is used */
if (cfg->filter == L3GXXXX_HPF_AND_LPF2) {
res |= _update(dev, L3GXXXX_REG_CTRL5, L3GXXXX_HP_ENABLED, 1);
}
_release(dev);
return res;
}
static int _get_int_event_src(const l3gxxxx_t *dev,
l3gxxxx_int_event_src_t *src)
{
ASSERT_PARAM(dev != NULL);
ASSERT_PARAM(src != NULL);
DEBUG_DEV("src=%p", dev, src);
int res = L3GXXXX_OK;
uint8_t ig_cfg;
uint8_t ig_src;
_acquire(dev);
res |= _read(dev, L3GXXXX_REG_IG_CFG, (uint8_t *)&ig_cfg, 1);
res |= _read(dev, L3GXXXX_REG_IG_SRC, (uint8_t *)&ig_src, 1);
_release(dev);
src->val = ig_src & ig_cfg & (L3GXXXX_INT1_X_LOW | L3GXXXX_INT1_X_HIGH |
L3GXXXX_INT1_Y_LOW | L3GXXXX_INT1_Y_HIGH |
L3GXXXX_INT1_Z_LOW | L3GXXXX_INT1_Z_HIGH);
src->active = (ig_src & L3GXXXX_INT1_ACTIVE) ? 1 : 0;
return res;
}
#endif /* IS_USED(MODULE_L3GXXXX_IRQ_EVENT) */
#if IS_USED(MODULE_L3GXXXX_IRQ_DATA)
static int _get_int_data_src(const l3gxxxx_t *dev,
l3gxxxx_int_data_src_t *src)
{
ASSERT_PARAM(dev != NULL);
ASSERT_PARAM(src != NULL);
DEBUG_DEV("src=%p", dev, src);
int res = L3GXXXX_OK;
uint8_t fifo_src;
uint8_t status;
_acquire(dev);
res |= _read(dev, L3GXXXX_REG_STATUS, &status, 1);
res |= _read(dev, L3GXXXX_REG_FIFO_SRC, &fifo_src, 1);
_release(dev);
src->data_ready = _GET_REG_BIT(status, L3GXXXX_ANY_DATA_READY);
src->fifo_watermark = _GET_REG_BIT(fifo_src, L3GXXXX_FIFO_WTM);
src->fifo_overrun = _GET_REG_BIT(fifo_src, L3GXXXX_FIFO_OVR);
src->fifo_empty = _GET_REG_BIT(fifo_src, L3GXXXX_FIFO_EMPTY);
return res;
}
#endif /* IS_USED(MODULE_L3GXXXX_IRQ_DATA) */
#if IS_USED(MODULE_L3GXXXX_CONFIG)
int l3gxxxx_set_mode(l3gxxxx_t *dev, l3gxxxx_odr_t odr,
bool x, bool y, bool z)
{
ASSERT_PARAM(dev != NULL);
DEBUG_DEV("odr=%02x x=%d y=%d z=%d", dev, odr, x, y, z);
int res = L3GXXXX_OK;
dev->params.odr = odr;
uint8_t ctrl1;
ctrl1 = (odr << L3GXXXX_ODR_BW_S) & L3GXXXX_ODR_BW;
ctrl1 |= L3GXXXX_POWER_MODE;
ctrl1 |= x ? L3GXXXX_X_ENABLED : 0;
ctrl1 |= y ? L3GXXXX_Y_ENABLED : 0;
ctrl1 |= z ? L3GXXXX_Z_ENABLED : 0;
_acquire(dev);
res |= _write(dev, L3GXXXX_REG_CTRL1, &ctrl1, 1);
#if IS_USED(MODULE_L3GD20H) && IS_USED(MODULE_L3GXXXX_LOW_ODR)
/* in case of low ODR, low data rate flag has to be set */
if (dev->params.odr >= L3GXXXX_ODR_12) {
ASSERT_PARAM(dev->sensor == L3GD20H);
res |= _update(dev, L3GXXXX_REG_LOW_ODR, L3GXXXX_LOW_ODR, 1);
}
#endif
_release(dev);
return res;
}
int l3gxxxx_set_scale(l3gxxxx_t *dev, l3gxxxx_scale_t scale)
{
ASSERT_PARAM(dev != NULL);
#if IS_USED(MODULE_A3G4250)
ASSERT_PARAM((scale == L3GXXXX_SCALE_245_DPS) || (dev->sensor != X3G42XXD));
#endif
DEBUG_DEV("scale=%02x", dev, scale);
dev->params.scale = scale;
/* read CTRL4 register and write scale */
return l3gxxxx_reg_update(dev, L3GXXXX_REG_CTRL4, L3GXXXX_FULL_SCALE, scale);
}
int l3gxxxx_select_output_filter(l3gxxxx_t *dev,
l3gxxxx_filter_sel_t filter)
{
ASSERT_PARAM(dev != NULL);
DEBUG_DEV("filter=%02x", dev, filter);
dev->params.filter_sel = filter;
int res = L3GXXXX_OK;
_acquire(dev);
/* set the register OUT_SEL in any case */
res |= _update(dev, L3GXXXX_REG_CTRL5, L3GXXXX_OUT_SEL, filter);
/* HPF has to be enabled by a separate bit if filter mode is 1 or 3 */
res |= _update(dev, L3GXXXX_REG_CTRL5, L3GXXXX_HP_ENABLED, filter & 1);
_release(dev);
return res;
}
int l3gxxxx_config_hpf(const l3gxxxx_t *dev,
l3gxxxx_hpf_mode_t mode, uint8_t cutoff)
{
ASSERT_PARAM(dev != NULL);
ASSERT_PARAM(cutoff <= 9);
DEBUG_DEV("mode=%02x cutoff=%d", dev, mode, cutoff);
int res = L3GXXXX_OK;
_acquire(dev);
res |= _update(dev, L3GXXXX_REG_CTRL2, L3GXXXX_HPF_MODE, mode);
res |= _update(dev, L3GXXXX_REG_CTRL2, L3GXXXX_HPF_CUTOFF, cutoff);
_release(dev);
return res;
}
int l3gxxxx_set_hpf_ref(const l3gxxxx_t *dev, int8_t ref)
{
ASSERT_PARAM(dev != NULL);
DEBUG_DEV("ref=%d", dev, ref);
return l3gxxxx_reg_write(dev, L3GXXXX_REG_REFERENCE, (uint8_t *)&ref, 1);
}
int l3gxxxx_get_hpf_ref(const l3gxxxx_t *dev, int8_t *ref)
{
ASSERT_PARAM(dev != NULL);
ASSERT_PARAM(ref != NULL);
DEBUG_DEV("ref=%p", dev, ref);
return l3gxxxx_reg_read(dev, L3GXXXX_REG_REFERENCE, (uint8_t *)&ref, 1);
}
#if IS_USED(MODULE_L3GXXXX_FIFO)
int l3gxxxx_set_fifo_mode(l3gxxxx_t *dev, l3gxxxx_fifo_mode_t mode,
uint8_t watermark)
{
ASSERT_PARAM(dev != NULL);
#if !IS_USED(L3GD20H) && !IS_USED(L3GD20)
ASSERT_PARAM(mode <= L3GXXXX_STREAM);
#endif
#if !IS_USED(L3GD20H)
ASSERT_PARAM(mode <= L3GXXXX_DYNAMIC_STREAM);
#endif
DEBUG_DEV("mode=%d watermark=%d", dev, mode, watermark);
int res = L3GXXXX_OK;
dev->params.fifo_mode = mode;
_acquire(dev);
/* read CTRL5 register and write FIFO_EN flag */
res |= _update(dev, L3GXXXX_REG_CTRL5, L3GXXXX_FIFO_EN, (mode != L3GXXXX_BYPASS));
/* read FIFO_CTRL register and write FIFO mode */
res |= _update(dev, L3GXXXX_REG_FIFO_CTRL, L3GXXXX_FIFO_WATERMARK, watermark);
res |= _update(dev, L3GXXXX_REG_FIFO_CTRL, L3GXXXX_FIFO_MODE, mode);
_release(dev);
return res;
}
#endif /* IS_USED(MODULE_L3GXXXX_FIFO) */
int l3gxxxx_get_int_event_cfg(const l3gxxxx_t *dev,
l3gxxxx_int_event_cfg_t *cfg)
{
ASSERT_PARAM(dev != NULL);
ASSERT_PARAM(cfg != NULL);
DEBUG_DEV("config=%p", dev, cfg);
int res = L3GXXXX_OK;
uint8_t ig_cfg;
uint8_t ig_dur;
uint8_t ig_ths[6];
uint8_t ctrl3;
uint8_t ctrl5;
_acquire(dev);
res |= _read(dev, L3GXXXX_REG_IG_THS_XH, ig_ths, 6);
res |= _read(dev, L3GXXXX_REG_IG_CFG, &ig_cfg, 1);
res |= _read(dev, L3GXXXX_REG_IG_DURATION, &ig_dur, 1);
res |= _read(dev, L3GXXXX_REG_CTRL3, &ctrl3, 1);
res |= _read(dev, L3GXXXX_REG_CTRL5, &ctrl5, 1);
_release(dev);
cfg->x_low_enabled = _GET_REG_BIT(ig_cfg, L3GXXXX_INT1_X_LOW);
cfg->x_high_enabled = _GET_REG_BIT(ig_cfg, L3GXXXX_INT1_X_HIGH);
cfg->y_low_enabled = _GET_REG_BIT(ig_cfg, L3GXXXX_INT1_Y_LOW);
cfg->y_high_enabled = _GET_REG_BIT(ig_cfg, L3GXXXX_INT1_Y_HIGH);
cfg->z_low_enabled = _GET_REG_BIT(ig_cfg, L3GXXXX_INT1_Z_LOW);
cfg->z_high_enabled = _GET_REG_BIT(ig_cfg, L3GXXXX_INT1_Z_HIGH);
cfg->x_threshold = msb_lsb_to_type(uint16_t, ig_ths, 0);
cfg->y_threshold = msb_lsb_to_type(uint16_t, ig_ths, 2);
cfg->z_threshold = msb_lsb_to_type(uint16_t, ig_ths, 4);
cfg->filter = _GET_REG_VALUE(ctrl5, L3GXXXX_IG_SEL);
cfg->and_or = _GET_REG_BIT(ig_cfg, L3GXXXX_INT1_AND_OR);
cfg->latch = _GET_REG_BIT(ig_cfg, L3GXXXX_INT1_LATCH);
cfg->wait = _GET_REG_BIT(ig_dur, L3GXXXX_INT1_WAIT);
cfg->duration = _GET_REG_VALUE(ig_dur, L3GXXXX_INT1_DURATION);
cfg->counter_mode = 0;
return res;
}
#endif /* IS_USED(MODULE_L3GXXXX_CONFIG) */
int l3gxxxx_reg_read(const l3gxxxx_t *dev,
uint8_t reg, uint8_t *data, uint8_t len)
{
int res;
_acquire(dev);
res = _read(dev, reg, data, len);
_release(dev);
return res;
}
int l3gxxxx_reg_write(const l3gxxxx_t *dev,
uint8_t reg, const uint8_t *data, uint8_t len)
{
int res;
_acquire(dev);
res = _write(dev, reg, data, len);
_release(dev);
return res;
}
int l3gxxxx_reg_update(const l3gxxxx_t *dev,
uint8_t reg, uint8_t mask, uint8_t val)
{
int res;
_acquire(dev);
res = _update(dev, reg, mask, val);
_release(dev);
return res;
}
/** Functions for internal use only */
#define L3GXXXX_I2C_L3GD20X_ADDR_1 (0x6a) /* L3GD20x I2C address 1 */
#define L3GXXXX_I2C_L3GD20X_ADDR_2 (0x6b) /* L3GD20x I2C address 2 */
#define L3GXXXX_I2C_X3G42XXD_ADDR_1 (0x68) /* x3G42xxD I2C address 1 */
#define L3GXXXX_I2C_X3G42XXD_ADDR_2 (0x69) /* x3G42xxD I2C address 2 */
/**
* @brief Check the chip ID to test whether sensor is available
* @note Communication bus has to be acquired when enter.
*/
static int _is_available(l3gxxxx_t *dev)
{
DEBUG_DEV("", dev);
int res;
uint8_t chip_id, tries = 16;
/* try to read the chip id from L3GXXXX_REG_WHO_AM_I */
do {
res = _read(dev, L3GXXXX_REG_WHO_AM_I, &chip_id, 1);
} while ((chip_id == 0xFF || chip_id == 0x0) && --tries);
#if IS_USED(MODULE_L3GXXXX_I2C)
if ((dev->params.if_params.type == L3GXXXX_I2C) && (res != L3GXXXX_OK)) {
/* if the interface type is I2C but the chip ID couldn't be read
from configured address, we probe possible I2C addresses */
#if IS_USED(MODULE_L3GD20H) || IS_USED(MODULE_L3GD20)
if (res != L3GXXXX_OK) {
dev->params.if_params.i2c.addr = L3GXXXX_I2C_L3GD20X_ADDR_1;
res = _read(dev, L3GXXXX_REG_WHO_AM_I, &chip_id, 1);
}
if (res != L3GXXXX_OK) {
dev->params.if_params.i2c.addr = L3GXXXX_I2C_L3GD20X_ADDR_2;
res = _read(dev, L3GXXXX_REG_WHO_AM_I, &chip_id, 1);
}
#endif /* IS_USED(MODULE_L3GD20H) || IS_USED(MODULE_L3GD20) */
#if IS_USED(MODULE_L3G4200D_NG) || IS_USED(MODULE_I3G4250D) || IS_USED(MODULE_A3G4250D)
if (res != L3GXXXX_OK) {
dev->params.if_params.i2c.addr = L3GXXXX_I2C_X3G42XXD_ADDR_1;
res = _read(dev, L3GXXXX_REG_WHO_AM_I, &chip_id, 1);
}
if (res != L3GXXXX_OK) {
dev->params.if_params.i2c.addr = L3GXXXX_I2C_X3G42XXD_ADDR_2;
res = _read(dev, L3GXXXX_REG_WHO_AM_I, &chip_id, 1);
}
#endif /* IS_USED(MODULE_L3G4200D_NG) || IS_USED(MODULE_I3G4250D) || IS_USED(MODULE_A3G4250D) */
}
#endif /* #if IS_USED(MODULE_L3GXXXX_I2C) */
if (res != L3GXXXX_OK) {
return res;
}
/* determine the sensor type */
switch (chip_id) {
#if IS_USED(MODULE_L3GD20H)
case L3GXXXX_CHIP_ID_L3GD20H: dev->sensor = L3GD20H;
break;
#endif
#if IS_USED(MODULE_L3GD20)
case L3GXXXX_CHIP_ID_L3GD20: dev->sensor = L3GD20;
break;
#endif
#if IS_USED(MODULE_L3G4200D_NG) || IS_USED(MODULE_I3G4250D) || IS_USED(MODULE_A3G4250D)
case L3GXXXX_CHIP_ID_X3G42XXD: dev->sensor = X3G42XXD;
break;
#endif
default: DEBUG_DEV("sensor is not available, wrong chip id %02x",
dev, chip_id);
return -L3GXXXX_ERROR_WRONG_CHIP_ID;
}
return res;
}
static int _update(const l3gxxxx_t *dev,
uint8_t reg, uint8_t mask, uint8_t val)
{
DEBUG_DEV("reg=%02x mask=%02x val=%02x", dev, reg, mask, val);
int res = L3GXXXX_OK;
uint8_t reg_val;
uint8_t shift = 0;
while (!((mask >> shift) & 0x01)) {
shift++;
}
/* read current register value */
res |= _read(dev, reg, &reg_val, 1);
/* set masked bits to the given value */
reg_val = (reg_val & ~mask) | ((val << shift) & mask);
/* write back new register value */
res |= _write(dev, reg, &reg_val, 1);
return res;
}
#define L3GXXXX_SPI_READ_FLAG 0x80
#define L3GXXXX_SPI_WRITE_FLAG 0x00
#define L3GXXXX_SPI_AUTO_INC_FLAG 0x40
#define L3GXXXX_I2C_AUTO_INC_FLAG 0x80
static void _acquire(const l3gxxxx_t *dev)
{
ASSERT_PARAM(dev != NULL);
#if IS_USED(MODULE_L3GXXXX_SPI)
if (dev->params.if_params.type == L3GXXXX_SPI) {
spi_acquire(_SPI_DEV, _SPI_CS, SPI_MODE_3, _SPI_CLK);
return;
}
#endif
#if IS_USED(MODULE_L3GXXXX_I2C)
if (dev->params.if_params.type == L3GXXXX_I2C) {
i2c_acquire(_I2C_DEV);
return;
}
#endif
return;
}
static void _release(const l3gxxxx_t *dev)
{
ASSERT_PARAM(dev != NULL);
#if IS_USED(MODULE_L3GXXXX_SPI)
if (dev->params.if_params.type == L3GXXXX_SPI) {
spi_release(_SPI_DEV);
return;
}
#endif
#if IS_USED(MODULE_L3GXXXX_I2C)
if (dev->params.if_params.type == L3GXXXX_I2C) {
i2c_release(_I2C_DEV);
return;
}
#endif
return;
}
static int _read(const l3gxxxx_t *dev, uint8_t reg, uint8_t *data, uint8_t len)
{
ASSERT_PARAM(dev != NULL);
ASSERT_PARAM(data != NULL);
ASSERT_PARAM(len != 0);
DEBUG_DEV("read %d byte from sensor registers starting at addr 0x%02x",
dev, len, reg);
#if IS_USED(MODULE_L3GXXXX_SPI)
if (dev->params.if_params.type == L3GXXXX_SPI) {
reg &= 0x3f;
reg |= L3GXXXX_SPI_READ_FLAG;
reg |= L3GXXXX_SPI_AUTO_INC_FLAG;
/* the first byte sent is the register address */
spi_transfer_byte(_SPI_DEV, _SPI_CS, true, reg);
spi_transfer_bytes(_SPI_DEV, _SPI_CS, false, 0, data, len);
if (ENABLE_DEBUG) {
printf("[l3gxxxx] %s dev=%" PRIxPTR ": read following bytes: ",
__func__, (unsigned int)dev);
for (uint8_t i = 0; i < len; i++) {
printf("%02x ", data[i]);
}
printf("\n");
}
return L3GXXXX_OK;
}
#endif /* IS_USED(MODULE_L3GXXXX_SPI) */
#if IS_USED(MODULE_L3GXXXX_I2C)
if (dev->params.if_params.type == L3GXXXX_I2C) {
if (len > 1) {
reg |= L3GXXXX_I2C_AUTO_INC_FLAG;
}
int res = i2c_read_regs(_I2C_DEV, _I2C_ADDR, reg, data, len, 0);
if (res != 0) {
DEBUG_DEV("I2C addr=%02x could not read %d bytes from sensor "
"registers starting at addr %02x, reason %d (%s)",
dev, _I2C_ADDR, len, reg, res, strerror(res * -1));
return -L3GXXXX_ERROR_I2C;
}
if (ENABLE_DEBUG) {
printf("[l3gxxxx] %s dev=%" PRIxPTR ": read following bytes: ",
__func__, (unsigned int)dev);
for (uint8_t i = 0; i < len; i++) {
printf("%02x ", data[i]);
}
printf("\n");
}
return L3GXXXX_OK;
}
#endif /* IS_USED(MODULE_L3GXXXX_I2C) */
return -L3GXXXX_ERROR_INV_DEV;
}
static int _write(const l3gxxxx_t *dev,
uint8_t reg, const uint8_t *data, uint8_t len)
{
ASSERT_PARAM(dev != NULL);
ASSERT_PARAM(data != NULL);
ASSERT_PARAM(len != 0);
DEBUG_DEV("write %d bytes to sensor registers starting at addr 0x%02x",
dev, len, reg);
#if IS_USED(MODULE_L3GXXXX_SPI)
if (dev->params.if_params.type == L3GXXXX_SPI) {
reg &= 0x3f;
reg |= L3GXXXX_SPI_WRITE_FLAG;
reg |= L3GXXXX_SPI_AUTO_INC_FLAG;
if (ENABLE_DEBUG) {
printf("[l3gxxxx] %s dev=%" PRIxPTR ": write following bytes: ",
__func__, (unsigned int)dev);
for (uint8_t i = 0; i < len; i++) {
printf("%02x ", data[i]);
}
printf("\n");
}
spi_transfer_byte(_SPI_DEV, _SPI_CS, true, reg);
spi_transfer_bytes(_SPI_DEV, _SPI_CS, false, data, 0, len);
return L3GXXXX_OK;
}
#endif /* IS_USED(MODULE_L3GXXXX_SPI) */
#if IS_USED(MODULE_L3GXXXX_I2C)
if (dev->params.if_params.type == L3GXXXX_I2C) {
if (len > 1) {
reg |= L3GXXXX_I2C_AUTO_INC_FLAG;
}
if (ENABLE_DEBUG) {
printf("[l3gxxxx] %s dev=%" PRIxPTR ": write following bytes: ",
__func__, (unsigned int)dev);
for (uint8_t i = 0; i < len; i++) {
printf("%02x ", data[i]);
}
printf("\n");
}
int res;
if (!data || !len) {
res = i2c_write_byte(_I2C_DEV, _I2C_ADDR, reg, 0);
}
else {
res = i2c_write_regs(_I2C_DEV, _I2C_ADDR, reg, data, len, 0);
}
if (res != L3GXXXX_OK) {
DEBUG_DEV("I2C addr=%02x could not write %d bytes to sensor "
"registers starting at addr 0x%02x, reason %d (%s)",
dev, _I2C_ADDR, len, reg, res, strerror(res * -1));
return -L3GXXXX_ERROR_I2C;
}
return res;
}
#endif /* IS_USED(MODULE_L3GXXXX_I2C) */
return -L3GXXXX_ERROR_INV_DEV;
}
View Git Blame Copy Permalink