mirror of
https://github.com/eclipse/upm.git
synced 2025-03-17 14:07:30 +03:00
671 lines
17 KiB
C
671 lines
17 KiB
C
/*
|
|
* Author: Jon Trulson <jtrulson@ics.com>
|
|
* Copyright (c) 2017 Intel Corporation.
|
|
*
|
|
* The MIT License
|
|
*
|
|
* Permission is hereby granted, free of charge, to any person obtaining
|
|
* a copy of this software and associated documentation files (the
|
|
* "Software"), to deal in the Software without restriction, including
|
|
* without limitation the rights to use, copy, modify, merge, publish,
|
|
* distribute, sublicense, and/or sell copies of the Software, and to
|
|
* permit persons to whom the Software is furnished to do so, subject to
|
|
* the following conditions:
|
|
*
|
|
* The above copyright notice and this permission notice shall be
|
|
* included in all copies or substantial portions of the Software.
|
|
*
|
|
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
|
|
* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
|
|
* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
|
|
* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
|
|
* LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
|
|
* OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
|
|
* WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
|
|
*/
|
|
|
|
#include <unistd.h>
|
|
#include <assert.h>
|
|
|
|
#include "upm_utilities.h"
|
|
#include "lsm6dsl.h"
|
|
|
|
// macro for converting a uint8_t low/high pair into a float
|
|
#define INT16_TO_FLOAT(h, l) \
|
|
(float)( (int16_t)( (l) | ((h) << 8) ) )
|
|
|
|
// some useful macros to save on typing and text wrapping
|
|
#undef _SHIFT
|
|
#define _SHIFT(x) (_LSM6DSL_##x##_SHIFT)
|
|
|
|
#undef _MASK
|
|
#define _MASK(x) (_LSM6DSL_##x##_MASK)
|
|
|
|
#undef _SHIFTMASK
|
|
#define _SHIFTMASK(x) (_MASK(x) << _SHIFT(x))
|
|
|
|
// SPI CS on and off functions
|
|
static void _csOn(const lsm6dsl_context dev)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
if (dev->gpioCS)
|
|
mraa_gpio_write(dev->gpioCS, 0);
|
|
}
|
|
|
|
static void _csOff(const lsm6dsl_context dev)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
if (dev->gpioCS)
|
|
mraa_gpio_write(dev->gpioCS, 1);
|
|
}
|
|
|
|
// init
|
|
lsm6dsl_context lsm6dsl_init(int bus, int addr, int cs)
|
|
{
|
|
lsm6dsl_context dev =
|
|
(lsm6dsl_context)malloc(sizeof(struct _lsm6dsl_context));
|
|
|
|
if (!dev)
|
|
return NULL;
|
|
|
|
// zero out context
|
|
memset((void *)dev, 0, sizeof(struct _lsm6dsl_context));
|
|
|
|
// make sure MRAA is initialized
|
|
if (mraa_init() != MRAA_SUCCESS)
|
|
{
|
|
printf("%s: mraa_init() failed.\n", __FUNCTION__);
|
|
lsm6dsl_close(dev);
|
|
return NULL;
|
|
}
|
|
|
|
if (addr < 0)
|
|
{
|
|
// SPI
|
|
if (!(dev->spi = mraa_spi_init(bus)))
|
|
{
|
|
printf("%s: mraa_spi_init() failed.\n", __FUNCTION__);
|
|
lsm6dsl_close(dev);
|
|
return NULL;
|
|
}
|
|
|
|
// Only create cs context if we are actually using a valid pin.
|
|
// A hardware controlled pin should specify cs as -1.
|
|
if (cs >= 0)
|
|
{
|
|
if (!(dev->gpioCS = mraa_gpio_init(cs)))
|
|
{
|
|
printf("%s: mraa_gpio_init() failed.\n", __FUNCTION__);
|
|
lsm6dsl_close(dev);
|
|
return NULL;
|
|
}
|
|
mraa_gpio_dir(dev->gpioCS, MRAA_GPIO_OUT);
|
|
}
|
|
|
|
mraa_spi_mode(dev->spi, MRAA_SPI_MODE0);
|
|
if (mraa_spi_frequency(dev->spi, 5000000))
|
|
{
|
|
printf("%s: mraa_spi_frequency() failed.\n", __FUNCTION__);
|
|
lsm6dsl_close(dev);
|
|
return NULL;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// I2C
|
|
if (!(dev->i2c = mraa_i2c_init(bus)))
|
|
{
|
|
printf("%s: mraa_i2c_init() failed.\n", __FUNCTION__);
|
|
lsm6dsl_close(dev);
|
|
return NULL;
|
|
}
|
|
|
|
if (mraa_i2c_address(dev->i2c, addr))
|
|
{
|
|
printf("%s: mraa_i2c_address() failed.\n", __FUNCTION__);
|
|
lsm6dsl_close(dev);
|
|
return NULL;
|
|
}
|
|
}
|
|
|
|
// check the chip id
|
|
|
|
uint8_t chipID = lsm6dsl_get_chip_id(dev);
|
|
if (chipID != LSM6DSL_CHIPID)
|
|
{
|
|
printf("%s: invalid chip id: %02x. Expected %02x\n",
|
|
__FUNCTION__, chipID, LSM6DSL_CHIPID);
|
|
lsm6dsl_close(dev);
|
|
return NULL;
|
|
}
|
|
|
|
// reset
|
|
if (lsm6dsl_reset(dev))
|
|
{
|
|
printf("%s: lsm6dsl_reset() failed.\n", __FUNCTION__);
|
|
lsm6dsl_close(dev);
|
|
return NULL;
|
|
}
|
|
|
|
// call devinit with default options
|
|
if (lsm6dsl_devinit(dev, LSM6DSL_XL_ODR_104HZ, LSM6DSL_XL_FS_2G,
|
|
LSM6DSL_G_ODR_104HZ, LSM6DSL_G_FS_245DPS))
|
|
{
|
|
printf("%s: lsm6dsl_devinit() failed.\n", __FUNCTION__);
|
|
lsm6dsl_close(dev);
|
|
return NULL;
|
|
}
|
|
|
|
return dev;
|
|
}
|
|
|
|
void lsm6dsl_close(lsm6dsl_context dev)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
lsm6dsl_uninstall_isr(dev, LSM6DSL_INTERRUPT_INT1);
|
|
lsm6dsl_uninstall_isr(dev, LSM6DSL_INTERRUPT_INT2);
|
|
|
|
if (dev->i2c)
|
|
mraa_i2c_stop(dev->i2c);
|
|
if (dev->spi)
|
|
mraa_spi_stop(dev->spi);
|
|
if (dev->gpioCS)
|
|
mraa_gpio_close(dev->gpioCS);
|
|
|
|
free(dev);
|
|
}
|
|
|
|
upm_result_t lsm6dsl_devinit(const lsm6dsl_context dev,
|
|
LSM6DSL_XL_ODR_T acc_odr,
|
|
LSM6DSL_XL_FS_T acc_fs,
|
|
LSM6DSL_G_ODR_T gyr_odr,
|
|
LSM6DSL_G_FS_T gyr_fs)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
// enable register auto-increment and BDU (block data update)
|
|
uint8_t reg = lsm6dsl_read_reg(dev, LSM6DSL_REG_CTRL3_C);
|
|
reg |= (LSM6DSL_CTRL3_IF_INC | LSM6DSL_CTRL3_BDU);
|
|
|
|
if (lsm6dsl_write_reg(dev, LSM6DSL_REG_CTRL3_C, reg))
|
|
return UPM_ERROR_OPERATION_FAILED;
|
|
|
|
// set our ODR, FS, and HP mode
|
|
if (lsm6dsl_set_acc_odr(dev, acc_odr)
|
|
|| lsm6dsl_set_acc_full_scale(dev, acc_fs)
|
|
|| lsm6dsl_set_gyr_odr(dev, gyr_odr)
|
|
|| lsm6dsl_set_gyr_full_scale(dev, gyr_fs)
|
|
|| lsm6dsl_high_performance(dev, true))
|
|
{
|
|
printf("%s: failed to set configuration parameters.\n",
|
|
__FUNCTION__);
|
|
return UPM_ERROR_OPERATION_FAILED;
|
|
}
|
|
|
|
// settle
|
|
upm_delay_ms(50);
|
|
|
|
return UPM_SUCCESS;
|
|
}
|
|
|
|
upm_result_t lsm6dsl_set_acc_odr(const lsm6dsl_context dev,
|
|
LSM6DSL_XL_ODR_T odr)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
uint8_t reg = lsm6dsl_read_reg(dev, LSM6DSL_REG_CTRL1_XL);
|
|
|
|
// mask out ODR bits, add our own
|
|
reg &= ~_SHIFTMASK(CTRL1_XL_ODR);
|
|
reg |= (odr << _SHIFT(CTRL1_XL_ODR));
|
|
|
|
if (lsm6dsl_write_reg(dev, LSM6DSL_REG_CTRL1_XL, reg))
|
|
return UPM_ERROR_OPERATION_FAILED;
|
|
|
|
return UPM_SUCCESS;
|
|
}
|
|
|
|
upm_result_t lsm6dsl_set_gyr_odr(const lsm6dsl_context dev,
|
|
LSM6DSL_G_ODR_T odr)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
uint8_t reg = lsm6dsl_read_reg(dev, LSM6DSL_REG_CTRL2_G);
|
|
|
|
// mask out ODR bits, add our own
|
|
reg &= ~_SHIFTMASK(CTRL2_G_ODR);
|
|
reg |= (odr << _SHIFT(CTRL2_G_ODR));
|
|
|
|
if (lsm6dsl_write_reg(dev, LSM6DSL_REG_CTRL2_G, reg))
|
|
return UPM_ERROR_OPERATION_FAILED;
|
|
|
|
return UPM_SUCCESS;
|
|
}
|
|
|
|
upm_result_t lsm6dsl_set_acc_full_scale(const lsm6dsl_context dev,
|
|
LSM6DSL_XL_FS_T fs)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
uint8_t reg = lsm6dsl_read_reg(dev, LSM6DSL_REG_CTRL1_XL);
|
|
|
|
// mask out FS bits, add our own
|
|
reg &= ~_SHIFTMASK(CTRL1_XL_FS);
|
|
reg |= (fs << _SHIFT(CTRL1_XL_FS));
|
|
|
|
if (lsm6dsl_write_reg(dev, LSM6DSL_REG_CTRL1_XL, reg))
|
|
return UPM_ERROR_OPERATION_FAILED;
|
|
|
|
// 16b resolution
|
|
switch(fs)
|
|
{
|
|
case LSM6DSL_XL_FS_2G:
|
|
dev->accScale = 0.061;
|
|
break;
|
|
|
|
case LSM6DSL_XL_FS_4G:
|
|
dev->accScale = 0.122;
|
|
break;
|
|
|
|
case LSM6DSL_XL_FS_8G:
|
|
dev->accScale = 0.244;
|
|
break;
|
|
|
|
case LSM6DSL_XL_FS_16G:
|
|
dev->accScale = 0.488;
|
|
break;
|
|
}
|
|
|
|
return UPM_SUCCESS;
|
|
}
|
|
|
|
upm_result_t lsm6dsl_set_gyr_full_scale(const lsm6dsl_context dev,
|
|
LSM6DSL_G_FS_T fs)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
uint8_t reg = lsm6dsl_read_reg(dev, LSM6DSL_REG_CTRL2_G);
|
|
|
|
// mask out FS bits, add our own
|
|
reg &= ~_SHIFTMASK(CTRL2_G_FS);
|
|
|
|
if ((int)fs > (int)_MASK(CTRL2_G_FS))
|
|
reg |= LSM6DSL_CTRL2_G_FS_125;
|
|
else
|
|
reg &= ~LSM6DSL_CTRL2_G_FS_125;
|
|
|
|
// mask off the virtual bit. The end result will be 245dps (0),
|
|
// if it was tagged with the virtual bit.
|
|
fs &= _MASK(CTRL2_G_FS);
|
|
// add it to the register
|
|
reg |= (fs << _SHIFT(CTRL2_G_FS));
|
|
|
|
if (lsm6dsl_write_reg(dev, LSM6DSL_REG_CTRL2_G, reg))
|
|
return UPM_ERROR_OPERATION_FAILED;
|
|
|
|
return UPM_SUCCESS;
|
|
}
|
|
|
|
upm_result_t lsm6dsl_update(const lsm6dsl_context dev)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
int bufLen = 14;
|
|
uint8_t buf[bufLen];
|
|
|
|
if (lsm6dsl_read_regs(dev, LSM6DSL_REG_OUT_TEMP_L,
|
|
buf, bufLen) != bufLen)
|
|
{
|
|
printf("%s: lsm6dsl_read_regs() failed to read %d bytes\n",
|
|
__FUNCTION__, bufLen);
|
|
return UPM_ERROR_OPERATION_FAILED;
|
|
}
|
|
|
|
// temperature is first msb lsb
|
|
dev->temperature = INT16_TO_FLOAT(buf[1], buf[0]);
|
|
|
|
// gyroscope
|
|
dev->gyrX = INT16_TO_FLOAT(buf[3], buf[2]);
|
|
dev->gyrY = INT16_TO_FLOAT(buf[5], buf[4]);
|
|
dev->gyrZ = INT16_TO_FLOAT(buf[7], buf[6]);
|
|
|
|
// accelerometer
|
|
dev->accX = INT16_TO_FLOAT(buf[9], buf[8]);
|
|
dev->accY = INT16_TO_FLOAT(buf[11], buf[10]);
|
|
dev->accZ = INT16_TO_FLOAT(buf[13], buf[12]);
|
|
|
|
return UPM_SUCCESS;
|
|
}
|
|
|
|
uint8_t lsm6dsl_read_reg(const lsm6dsl_context dev, uint8_t reg)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
if (dev->spi)
|
|
{
|
|
reg |= 0x80; // needed for read
|
|
uint8_t pkt[2] = {reg, 0};
|
|
|
|
_csOn(dev);
|
|
if (mraa_spi_transfer_buf(dev->spi, pkt, pkt, 2))
|
|
{
|
|
_csOff(dev);
|
|
printf("%s: mraa_spi_transfer_buf() failed.\n", __FUNCTION__);
|
|
return 0xff;
|
|
}
|
|
_csOff(dev);
|
|
|
|
return pkt[1];
|
|
}
|
|
else
|
|
return (uint8_t)mraa_i2c_read_byte_data(dev->i2c, reg);
|
|
}
|
|
|
|
int lsm6dsl_read_regs(const lsm6dsl_context dev, uint8_t reg,
|
|
uint8_t *buffer, int len)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
if (dev->spi)
|
|
{
|
|
reg |= 0x80; // needed for read
|
|
|
|
uint8_t sbuf[len + 1];
|
|
memset((char *)sbuf, 0, len + 1);
|
|
sbuf[0] = reg;
|
|
|
|
_csOn(dev);
|
|
if (mraa_spi_transfer_buf(dev->spi, sbuf, sbuf, len + 1))
|
|
{
|
|
_csOff(dev);
|
|
printf("%s: mraa_spi_transfer_buf() failed.\n", __FUNCTION__);
|
|
return -1;
|
|
}
|
|
_csOff(dev);
|
|
|
|
// now copy it into user buffer
|
|
for (int i=0; i<len; i++)
|
|
buffer[i] = sbuf[i + 1];
|
|
}
|
|
else
|
|
{
|
|
if (mraa_i2c_read_bytes_data(dev->i2c, reg, buffer, len) != len)
|
|
return -1;
|
|
}
|
|
|
|
return len;
|
|
}
|
|
|
|
upm_result_t lsm6dsl_write_reg(const lsm6dsl_context dev,
|
|
uint8_t reg, uint8_t val)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
if (dev->spi)
|
|
{
|
|
reg &= 0x7f; // mask off 0x80 for writing
|
|
uint8_t pkt[2] = {reg, val};
|
|
|
|
_csOn(dev);
|
|
if (mraa_spi_transfer_buf(dev->spi, pkt, NULL, 2))
|
|
{
|
|
_csOff(dev);
|
|
printf("%s: mraa_spi_transfer_buf() failed.",
|
|
__FUNCTION__);
|
|
|
|
return UPM_ERROR_OPERATION_FAILED;
|
|
}
|
|
_csOff(dev);
|
|
}
|
|
else
|
|
{
|
|
if (mraa_i2c_write_byte_data(dev->i2c, val, reg))
|
|
{
|
|
printf("%s: mraa_i2c_write_byte_data() failed.",
|
|
__FUNCTION__);
|
|
return UPM_ERROR_OPERATION_FAILED;
|
|
}
|
|
}
|
|
|
|
return UPM_SUCCESS;
|
|
}
|
|
|
|
uint8_t lsm6dsl_get_chip_id(const lsm6dsl_context dev)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
return lsm6dsl_read_reg(dev, LSM6DSL_REG_WHO_AM_I);
|
|
}
|
|
|
|
void lsm6dsl_get_accelerometer(const lsm6dsl_context dev,
|
|
float *x, float *y, float *z)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
if (x)
|
|
*x = dev->accX * dev->accScale / 1000.00;
|
|
|
|
if (y)
|
|
*y = dev->accY * dev->accScale / 1000.00;
|
|
|
|
if (z)
|
|
*z = dev->accZ * dev->accScale / 1000.00;
|
|
}
|
|
|
|
void lsm6dsl_get_gyroscope(const lsm6dsl_context dev,
|
|
float *x, float *y, float *z)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
if (x)
|
|
*x = dev->gyrX / 1000.00;
|
|
|
|
if (y)
|
|
*y = dev->gyrY / 1000.00;
|
|
|
|
if (z)
|
|
*z = dev->gyrZ / 1000.00;
|
|
}
|
|
|
|
float lsm6dsl_get_temperature(const lsm6dsl_context dev)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
// this seems to work, but I sure wish they would document this in
|
|
// their DS's
|
|
return (dev->temperature / 65536.0) + 25.0;
|
|
}
|
|
|
|
upm_result_t lsm6dsl_high_performance(const lsm6dsl_context dev,
|
|
bool enable)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
uint8_t reg = lsm6dsl_read_reg(dev, LSM6DSL_REG_CTRL6_C);
|
|
|
|
// acc
|
|
if (enable)
|
|
reg &= ~LSM6DSL_CTRL6_XL_HM_MODE;
|
|
else
|
|
reg |= LSM6DSL_CTRL6_XL_HM_MODE;
|
|
|
|
if (lsm6dsl_write_reg(dev, LSM6DSL_REG_CTRL6_C, reg))
|
|
return UPM_ERROR_OPERATION_FAILED;
|
|
|
|
// gyr
|
|
reg = lsm6dsl_read_reg(dev, LSM6DSL_REG_CTRL7_G);
|
|
|
|
if (enable)
|
|
reg &= ~LSM6DSL_CTRL7_G_HM_MODE;
|
|
else
|
|
reg |= LSM6DSL_CTRL7_G_HM_MODE;
|
|
|
|
if (lsm6dsl_write_reg(dev, LSM6DSL_REG_CTRL7_G, reg))
|
|
return UPM_ERROR_OPERATION_FAILED;
|
|
|
|
upm_delay_ms(100);
|
|
|
|
return UPM_SUCCESS;
|
|
}
|
|
|
|
upm_result_t lsm6dsl_reset(const lsm6dsl_context dev)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
uint8_t reg = lsm6dsl_read_reg(dev, LSM6DSL_REG_CTRL3_C);
|
|
|
|
reg |= LSM6DSL_CTRL3_SW_RESET;
|
|
|
|
if (lsm6dsl_write_reg(dev, LSM6DSL_REG_CTRL3_C, reg))
|
|
return UPM_ERROR_OPERATION_FAILED;
|
|
|
|
upm_delay_ms(100);
|
|
|
|
return UPM_SUCCESS;
|
|
}
|
|
|
|
upm_result_t lsm6dsl_set_interrupt_active_high(const lsm6dsl_context dev,
|
|
bool high)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
uint8_t reg = lsm6dsl_read_reg(dev, LSM6DSL_REG_CTRL3_C);
|
|
|
|
if (high)
|
|
reg &= ~LSM6DSL_CTRL3_H_LACTIVE;
|
|
else
|
|
reg |= LSM6DSL_CTRL3_H_LACTIVE;
|
|
|
|
if (lsm6dsl_write_reg(dev, LSM6DSL_REG_CTRL3_C, reg))
|
|
return UPM_ERROR_OPERATION_FAILED;
|
|
|
|
return UPM_SUCCESS;
|
|
}
|
|
|
|
upm_result_t lsm6dsl_set_interrupt_push_pull(const lsm6dsl_context dev,
|
|
bool pp)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
uint8_t reg = lsm6dsl_read_reg(dev, LSM6DSL_REG_CTRL3_C);
|
|
|
|
if (pp)
|
|
reg &= ~LSM6DSL_CTRL3_PP_OD;
|
|
else
|
|
reg |= LSM6DSL_CTRL3_PP_OD;
|
|
|
|
if (lsm6dsl_write_reg(dev, LSM6DSL_REG_CTRL3_C, reg))
|
|
return UPM_ERROR_OPERATION_FAILED;
|
|
|
|
return UPM_SUCCESS;
|
|
}
|
|
|
|
uint8_t lsm6dsl_get_status(const lsm6dsl_context dev)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
return lsm6dsl_read_reg(dev, LSM6DSL_REG_STATUS);
|
|
}
|
|
|
|
upm_result_t lsm6dsl_set_acc_offsets(const lsm6dsl_context dev,
|
|
int x, int y, int z, bool weight)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
// narrowing conversion to int8_t
|
|
int8_t ix = (int8_t)x;
|
|
int8_t iy = (int8_t)y;
|
|
int8_t iz = (int8_t)z;
|
|
|
|
if (lsm6dsl_write_reg(dev, LSM6DSL_REG_X_OFS_USR, (uint8_t)ix))
|
|
return UPM_ERROR_OPERATION_FAILED;
|
|
if (lsm6dsl_write_reg(dev, LSM6DSL_REG_Y_OFS_USR, (uint8_t)iy))
|
|
return UPM_ERROR_OPERATION_FAILED;
|
|
if (lsm6dsl_write_reg(dev, LSM6DSL_REG_Z_OFS_USR, (uint8_t)iz))
|
|
return UPM_ERROR_OPERATION_FAILED;
|
|
|
|
// now set the weighting...
|
|
uint8_t reg = lsm6dsl_read_reg(dev, LSM6DSL_REG_CTRL6_C);
|
|
if (weight)
|
|
reg |= LSM6DSL_CTRL6_USR_OFF_W;
|
|
else
|
|
reg &= ~LSM6DSL_CTRL6_USR_OFF_W;
|
|
|
|
if (lsm6dsl_write_reg(dev, LSM6DSL_REG_CTRL6_C, reg))
|
|
return UPM_ERROR_OPERATION_FAILED;
|
|
|
|
return UPM_SUCCESS;
|
|
}
|
|
|
|
upm_result_t lsm6dsl_install_isr(const lsm6dsl_context dev,
|
|
LSM6DSL_INTERRUPT_PINS_T intr, int gpio,
|
|
mraa_gpio_edge_t level,
|
|
void (*isr)(void *), void *arg)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
// delete any existing ISR and GPIO context for this interrupt
|
|
lsm6dsl_uninstall_isr(dev, intr);
|
|
|
|
mraa_gpio_context gpio_isr = NULL;
|
|
|
|
// create gpio context
|
|
if (!(gpio_isr = mraa_gpio_init(gpio)))
|
|
{
|
|
printf("%s: mraa_gpio_init() failed.\n", __FUNCTION__);
|
|
return UPM_ERROR_OPERATION_FAILED;
|
|
}
|
|
|
|
mraa_gpio_dir(gpio_isr, MRAA_GPIO_IN);
|
|
|
|
if (mraa_gpio_isr(gpio_isr, level, isr, arg))
|
|
{
|
|
mraa_gpio_close(gpio_isr);
|
|
printf("%s: mraa_gpio_isr() failed.\n", __FUNCTION__);
|
|
return UPM_ERROR_OPERATION_FAILED;
|
|
}
|
|
|
|
switch (intr)
|
|
{
|
|
case LSM6DSL_INTERRUPT_INT1:
|
|
dev->gpioINT1 = gpio_isr;
|
|
break;
|
|
|
|
case LSM6DSL_INTERRUPT_INT2:
|
|
dev->gpioINT2 = gpio_isr;
|
|
break;
|
|
}
|
|
|
|
return UPM_SUCCESS;
|
|
}
|
|
|
|
void lsm6dsl_uninstall_isr(const lsm6dsl_context dev,
|
|
LSM6DSL_INTERRUPT_PINS_T intr)
|
|
{
|
|
assert(dev != NULL);
|
|
|
|
switch (intr)
|
|
{
|
|
case LSM6DSL_INTERRUPT_INT1:
|
|
if (dev->gpioINT1)
|
|
{
|
|
mraa_gpio_isr_exit(dev->gpioINT1);
|
|
mraa_gpio_close(dev->gpioINT1);
|
|
dev->gpioINT1 = NULL;
|
|
}
|
|
break;
|
|
|
|
case LSM6DSL_INTERRUPT_INT2:
|
|
if (dev->gpioINT2)
|
|
{
|
|
mraa_gpio_isr_exit(dev->gpioINT2);
|
|
mraa_gpio_close(dev->gpioINT2);
|
|
dev->gpioINT2 = NULL;
|
|
}
|
|
break;
|
|
}
|
|
}
|