718 lines
19 KiB
C
718 lines
19 KiB
C
/****************************************************************************
|
|
* drivers/sensors/lsm9ds1_base.c
|
|
*
|
|
* Licensed to the Apache Software Foundation (ASF) under one or more
|
|
* contributor license agreements. See the NOTICE file distributed with
|
|
* this work for additional information regarding copyright ownership. The
|
|
* ASF licenses this file to you 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.
|
|
*
|
|
****************************************************************************/
|
|
|
|
/****************************************************************************
|
|
* Included Files
|
|
****************************************************************************/
|
|
|
|
#include "lsm9ds1_base.h"
|
|
|
|
/****************************************************************************
|
|
* Pre-processor Definitions
|
|
****************************************************************************/
|
|
|
|
/****************************************************************************
|
|
* Private Function Prototypes
|
|
****************************************************************************/
|
|
|
|
/* Accelerometer Operations */
|
|
|
|
static int lsm9ds1accelgyro_config(FAR struct lsm9ds1_dev_s *priv);
|
|
static int lsm9ds1accel_start(FAR struct lsm9ds1_dev_s *priv);
|
|
static int lsm9ds1accel_stop(FAR struct lsm9ds1_dev_s *priv);
|
|
static int lsm9ds1accelgyro_setsamplerate(FAR struct lsm9ds1_dev_s *priv,
|
|
uint32_t samplerate);
|
|
static int lsm9ds1accel_setfullscale(FAR struct lsm9ds1_dev_s *priv,
|
|
uint32_t fullscale);
|
|
|
|
/* Gyroscope Operations */
|
|
|
|
static int lsm9ds1gyro_start(FAR struct lsm9ds1_dev_s *priv);
|
|
static int lsm9ds1gyro_stop(FAR struct lsm9ds1_dev_s *priv);
|
|
static int lsm9ds1gyro_setfullscale(FAR struct lsm9ds1_dev_s *priv,
|
|
uint32_t fullscale);
|
|
|
|
/* Magnetometer Operations */
|
|
|
|
static int lsm9ds1mag_config(FAR struct lsm9ds1_dev_s *priv);
|
|
static int lsm9ds1mag_start(FAR struct lsm9ds1_dev_s *priv);
|
|
static int lsm9ds1mag_stop(FAR struct lsm9ds1_dev_s *priv);
|
|
static int lsm9ds1mag_setfullscale(FAR struct lsm9ds1_dev_s *priv,
|
|
uint32_t fullscale);
|
|
static int lsm9ds1mag_setsamplerate(FAR struct lsm9ds1_dev_s *priv,
|
|
uint32_t samplerate);
|
|
|
|
/****************************************************************************
|
|
* Private Types
|
|
****************************************************************************/
|
|
|
|
const struct lsm9ds1_ops_s g_lsm9ds1accel_ops =
|
|
{
|
|
lsm9ds1accelgyro_config,
|
|
lsm9ds1accel_start,
|
|
lsm9ds1accel_stop,
|
|
lsm9ds1accelgyro_setsamplerate,
|
|
lsm9ds1accel_setfullscale,
|
|
};
|
|
|
|
const struct lsm9ds1_ops_s g_lsm9ds1gyro_ops =
|
|
{
|
|
lsm9ds1accelgyro_config,
|
|
lsm9ds1gyro_start,
|
|
lsm9ds1gyro_stop,
|
|
lsm9ds1accelgyro_setsamplerate,
|
|
lsm9ds1gyro_setfullscale,
|
|
};
|
|
|
|
const struct lsm9ds1_ops_s g_lsm9ds1mag_ops =
|
|
{
|
|
lsm9ds1mag_config,
|
|
lsm9ds1mag_start,
|
|
lsm9ds1mag_stop,
|
|
lsm9ds1mag_setsamplerate,
|
|
lsm9ds1mag_setfullscale,
|
|
};
|
|
|
|
/****************************************************************************
|
|
* Private Functions
|
|
****************************************************************************/
|
|
|
|
/****************************************************************************
|
|
* Name: lsm9ds1accelgyro_config
|
|
*
|
|
* Description:
|
|
* Configure the accelerometer and gyroscope.
|
|
*
|
|
****************************************************************************/
|
|
|
|
static int lsm9ds1accelgyro_config(FAR struct lsm9ds1_dev_s *priv)
|
|
{
|
|
uint8_t regval;
|
|
int ret;
|
|
|
|
/* Sanity check */
|
|
|
|
DEBUGASSERT(priv != NULL);
|
|
|
|
/* Get the device identification */
|
|
|
|
ret = lsm9ds1_readreg8(priv, LSM9DS1_WHO_AM_I, ®val);
|
|
if (ret < 0)
|
|
{
|
|
snerr("ERROR: lsm9ds1_readreg8 failed: %d\n", ret);
|
|
return ret;
|
|
}
|
|
|
|
if (regval != LSM9DS1_WHO_AM_I_VALUE)
|
|
{
|
|
snerr("ERROR: Invalid device identification %02x\n", regval);
|
|
return -ENODEV;
|
|
}
|
|
|
|
return OK;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: lsm9ds1accel_start
|
|
*
|
|
* Description:
|
|
* Start the accelerometer.
|
|
*
|
|
****************************************************************************/
|
|
|
|
static int lsm9ds1accel_start(FAR struct lsm9ds1_dev_s *priv)
|
|
{
|
|
uint8_t setbits;
|
|
|
|
/* Sanity check */
|
|
|
|
DEBUGASSERT(priv != NULL);
|
|
|
|
if (priv->samplerate < lsm9ds1_midpoint(10, 50))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG6_XL_ODR_XL_10HZ;
|
|
}
|
|
else if (priv->samplerate < lsm9ds1_midpoint(50, 119))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG6_XL_ODR_XL_50HZ;
|
|
}
|
|
else if (priv->samplerate < lsm9ds1_midpoint(119, 238))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG6_XL_ODR_XL_119HZ;
|
|
}
|
|
else if (priv->samplerate < lsm9ds1_midpoint(238, 476))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG6_XL_ODR_XL_238HZ;
|
|
}
|
|
else if (priv->samplerate < lsm9ds1_midpoint(476, 952))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG6_XL_ODR_XL_476HZ;
|
|
}
|
|
else
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG6_XL_ODR_XL_952HZ;
|
|
}
|
|
|
|
return lsm9ds1_modifyreg8(priv, LSM9DS1_CTRL_REG6_XL,
|
|
LSM9DS1_CTRL_REG6_XL_ODR_XL_MASK, setbits);
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: lsm9ds1accel_stop
|
|
*
|
|
* Description:
|
|
* Stop the accelerometer.
|
|
*
|
|
****************************************************************************/
|
|
|
|
static int lsm9ds1accel_stop(FAR struct lsm9ds1_dev_s *priv)
|
|
{
|
|
/* Sanity check */
|
|
|
|
DEBUGASSERT(priv != NULL);
|
|
|
|
return lsm9ds1_modifyreg8(priv, LSM9DS1_CTRL_REG6_XL,
|
|
LSM9DS1_CTRL_REG6_XL_ODR_XL_MASK,
|
|
LSM9DS1_CTRL_REG6_XL_ODR_XL_POWERDOWN);
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: lsm9ds1accelgyro_setsamplerate
|
|
*
|
|
* Description:
|
|
* Set the accelerometer or gyroscope's sample rate.
|
|
*
|
|
****************************************************************************/
|
|
|
|
static int lsm9ds1accelgyro_setsamplerate(FAR struct lsm9ds1_dev_s *priv,
|
|
uint32_t samplerate)
|
|
{
|
|
/* Sanity check */
|
|
|
|
DEBUGASSERT(priv != NULL);
|
|
|
|
priv->samplerate = samplerate;
|
|
return OK;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: lsm9ds1accel_setfullscale
|
|
*
|
|
* Description:
|
|
* Set the accelerometer's full-scale range.
|
|
*
|
|
****************************************************************************/
|
|
|
|
static int lsm9ds1accel_setfullscale(FAR struct lsm9ds1_dev_s *priv,
|
|
uint32_t fullscale)
|
|
{
|
|
uint8_t setbits;
|
|
|
|
/* Sanity check */
|
|
|
|
DEBUGASSERT(priv != NULL);
|
|
|
|
if (fullscale < lsm9ds1_midpoint(2, 4))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG6_XL_FS_XL_2G;
|
|
}
|
|
else if (fullscale < lsm9ds1_midpoint(4, 8))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG6_XL_FS_XL_4G;
|
|
}
|
|
else if (fullscale < lsm9ds1_midpoint(8, 16))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG6_XL_FS_XL_8G;
|
|
}
|
|
else
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG6_XL_FS_XL_16G;
|
|
}
|
|
|
|
return lsm9ds1_modifyreg8(priv, LSM9DS1_CTRL_REG6_XL,
|
|
LSM9DS1_CTRL_REG6_XL_FS_XL_MASK, setbits);
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: lsm9ds1gyro_start
|
|
*
|
|
* Description:
|
|
* Start the gyroscope.
|
|
*
|
|
****************************************************************************/
|
|
|
|
static int lsm9ds1gyro_start(FAR struct lsm9ds1_dev_s *priv)
|
|
{
|
|
uint8_t setbits;
|
|
|
|
/* Sanity check */
|
|
|
|
DEBUGASSERT(priv != NULL);
|
|
|
|
if (priv->samplerate < lsm9ds1_midpoint(14, 59))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG1_G_ODR_G_14p9HZ;
|
|
}
|
|
else if (priv->samplerate < lsm9ds1_midpoint(59, 119))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG1_G_ODR_G_59p5HZ;
|
|
}
|
|
else if (priv->samplerate < lsm9ds1_midpoint(119, 238))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG1_G_ODR_G_119HZ;
|
|
}
|
|
else if (priv->samplerate < lsm9ds1_midpoint(238, 476))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG1_G_ODR_G_238HZ;
|
|
}
|
|
else if (priv->samplerate < lsm9ds1_midpoint(476, 952))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG1_G_ODR_G_476HZ;
|
|
}
|
|
else
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG1_G_ODR_G_952HZ;
|
|
}
|
|
|
|
return lsm9ds1_modifyreg8(priv, LSM9DS1_CTRL_REG1_G,
|
|
LSM9DS1_CTRL_REG1_G_ODR_G_MASK, setbits);
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: lsm9ds1gyro_stop
|
|
*
|
|
* Description:
|
|
* Stop the gyroscope.
|
|
*
|
|
****************************************************************************/
|
|
|
|
static int lsm9ds1gyro_stop(FAR struct lsm9ds1_dev_s *priv)
|
|
{
|
|
/* Sanity check */
|
|
|
|
DEBUGASSERT(priv != NULL);
|
|
|
|
return lsm9ds1_modifyreg8(priv, LSM9DS1_CTRL_REG1_G,
|
|
LSM9DS1_CTRL_REG1_G_ODR_G_MASK,
|
|
LSM9DS1_CTRL_REG1_G_ODR_G_POWERDOWN);
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: lsm9ds1gyro_setfullscale
|
|
*
|
|
* Description:
|
|
* Set the gyroscope's full-scale range.
|
|
*
|
|
****************************************************************************/
|
|
|
|
static int lsm9ds1gyro_setfullscale(FAR struct lsm9ds1_dev_s *priv,
|
|
uint32_t fullscale)
|
|
{
|
|
uint8_t setbits;
|
|
|
|
/* Sanity check */
|
|
|
|
DEBUGASSERT(priv != NULL);
|
|
|
|
if (fullscale < lsm9ds1_midpoint(245, 500))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG1_G_FS_G_245DPS;
|
|
}
|
|
else if (fullscale < lsm9ds1_midpoint(500, 2000))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG1_G_FS_G_500DPS;
|
|
}
|
|
else
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG1_G_FS_G_2000DPS;
|
|
}
|
|
|
|
return lsm9ds1_modifyreg8(priv, LSM9DS1_CTRL_REG1_G,
|
|
LSM9DS1_CTRL_REG1_G_FS_G_MASK, setbits);
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: lsm9ds1mag_config
|
|
*
|
|
* Description:
|
|
* Configure the magnetometer.
|
|
*
|
|
****************************************************************************/
|
|
|
|
static int lsm9ds1mag_config(FAR struct lsm9ds1_dev_s *priv)
|
|
{
|
|
uint8_t regval;
|
|
int ret;
|
|
|
|
/* Sanity check */
|
|
|
|
DEBUGASSERT(priv != NULL);
|
|
|
|
/* Get the device identification */
|
|
|
|
ret = lsm9ds1_readreg8(priv, LSM9DS1_WHO_AM_I_M, ®val);
|
|
if (ret < 0)
|
|
{
|
|
snerr("ERROR: lsm9ds1_readreg8 failed: %d\n", ret);
|
|
return ret;
|
|
}
|
|
|
|
if (regval != LSM9DS1_WHO_AM_I_M_VALUE)
|
|
{
|
|
snerr("ERROR: Invalid device identification %02x\n", regval);
|
|
return -ENODEV;
|
|
}
|
|
|
|
return OK;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: lsm9ds1mag_start
|
|
*
|
|
* Description:
|
|
* Start the magnetometer.
|
|
*
|
|
****************************************************************************/
|
|
|
|
static int lsm9ds1mag_start(FAR struct lsm9ds1_dev_s *priv)
|
|
{
|
|
/* Sanity check */
|
|
|
|
DEBUGASSERT(priv != NULL);
|
|
|
|
return lsm9ds1_modifyreg8(priv, LSM9DS1_CTRL_REG3_M,
|
|
LSM9DS1_CTRL_REG3_M_MD_MASK,
|
|
LSM9DS1_CTRL_REG3_M_MD_CONT);
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: lsm9ds1mag_stop
|
|
*
|
|
* Description:
|
|
* Stop the magnetometer.
|
|
*
|
|
****************************************************************************/
|
|
|
|
static int lsm9ds1mag_stop(FAR struct lsm9ds1_dev_s *priv)
|
|
{
|
|
/* Sanity check */
|
|
|
|
DEBUGASSERT(priv != NULL);
|
|
|
|
return lsm9ds1_modifyreg8(priv, LSM9DS1_CTRL_REG3_M,
|
|
LSM9DS1_CTRL_REG3_M_MD_MASK,
|
|
LSM9DS1_CTRL_REG3_M_MD_POWERDOWN2);
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: lsm9ds1mag_setfullscale
|
|
*
|
|
* Description:
|
|
* Set the magnetometer's full-scale range.
|
|
*
|
|
****************************************************************************/
|
|
|
|
static int lsm9ds1mag_setfullscale(FAR struct lsm9ds1_dev_s *priv,
|
|
uint32_t fullscale)
|
|
{
|
|
uint8_t setbits;
|
|
|
|
/* Sanity check */
|
|
|
|
DEBUGASSERT(priv != NULL);
|
|
|
|
if (fullscale < lsm9ds1_midpoint(4, 8))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG2_M_FS_4GAUSS;
|
|
}
|
|
else if (fullscale < lsm9ds1_midpoint(8, 12))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG2_M_FS_8GAUSS;
|
|
}
|
|
else if (fullscale < lsm9ds1_midpoint(12, 16))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG2_M_FS_12GAUSS;
|
|
}
|
|
else
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG2_M_FS_16GAUSS;
|
|
}
|
|
|
|
return lsm9ds1_modifyreg8(priv, LSM9DS1_CTRL_REG2_M,
|
|
LSM9DS1_CTRL_REG2_M_FS_MASK, setbits);
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: lsm9ds1mag_setsamplerate
|
|
*
|
|
* Description:
|
|
* Set the magnetometer's sample rate.
|
|
*
|
|
****************************************************************************/
|
|
|
|
static int lsm9ds1mag_setsamplerate(FAR struct lsm9ds1_dev_s *priv,
|
|
uint32_t samplerate)
|
|
{
|
|
uint8_t setbits;
|
|
|
|
/* Sanity check */
|
|
|
|
DEBUGASSERT(priv != NULL);
|
|
|
|
/* The magnetometer can change its sample rate without exiting
|
|
* power-down mode, so we don't need to save the value for later,
|
|
* unlike the accelerometer and gyroscope.
|
|
*/
|
|
|
|
if (samplerate < lsm9ds1_midpoint(0, 1))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG1_M_DO_0p625HZ;
|
|
}
|
|
else if (samplerate < lsm9ds1_midpoint(1, 2))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG1_M_DO_1p25HZ;
|
|
}
|
|
else if (samplerate < lsm9ds1_midpoint(2, 5))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG1_M_DO_2p5HZ;
|
|
}
|
|
else if (samplerate < lsm9ds1_midpoint(5, 10))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG1_M_DO_5HZ;
|
|
}
|
|
else if (samplerate < lsm9ds1_midpoint(10, 20))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG1_M_DO_10HZ;
|
|
}
|
|
else if (samplerate < lsm9ds1_midpoint(20, 40))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG1_M_DO_20HZ;
|
|
}
|
|
else if (samplerate < lsm9ds1_midpoint(40, 80))
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG1_M_DO_40HZ;
|
|
}
|
|
else
|
|
{
|
|
setbits = LSM9DS1_CTRL_REG1_M_DO_80HZ;
|
|
}
|
|
|
|
return lsm9ds1_modifyreg8(priv, LSM9DS1_CTRL_REG1_M,
|
|
LSM9DS1_CTRL_REG1_M_DO_MASK, setbits);
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Private Data
|
|
****************************************************************************/
|
|
|
|
/****************************************************************************
|
|
* Public Functions
|
|
****************************************************************************/
|
|
|
|
/****************************************************************************
|
|
* Name: lsm9ds1_readreg8
|
|
*
|
|
* Description:
|
|
* Read from an 8-bit register.
|
|
*
|
|
****************************************************************************/
|
|
|
|
int lsm9ds1_readreg8(FAR struct lsm9ds1_dev_s *priv,
|
|
uint8_t regaddr, FAR uint8_t *regval)
|
|
{
|
|
struct i2c_config_s config;
|
|
int ret;
|
|
|
|
/* Sanity check */
|
|
|
|
DEBUGASSERT(priv != NULL);
|
|
DEBUGASSERT(regval != NULL);
|
|
|
|
/* Set up the I2C configuration */
|
|
|
|
config.frequency = CONFIG_LSM9DS1_I2C_FREQUENCY;
|
|
config.address = priv->addr;
|
|
config.addrlen = 7;
|
|
|
|
/* Write the register address */
|
|
|
|
ret = i2c_write(priv->i2c, &config, ®addr, sizeof(regaddr));
|
|
if (ret < 0)
|
|
{
|
|
snerr("ERROR: i2c_write failed: %d\n", ret);
|
|
return ret;
|
|
}
|
|
|
|
/* Restart and read 8 bits from the register */
|
|
|
|
ret = i2c_read(priv->i2c, &config, regval, sizeof(*regval));
|
|
if (ret < 0)
|
|
{
|
|
snerr("ERROR: i2c_read failed: %d\n", ret);
|
|
return ret;
|
|
}
|
|
|
|
sninfo("addr: %02x value: %02x\n", regaddr, *regval);
|
|
return OK;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: lsm9ds1_readreg
|
|
*
|
|
* Description:
|
|
* Read bytes from registers
|
|
*
|
|
****************************************************************************/
|
|
|
|
int lsm9ds1_readreg(FAR struct lsm9ds1_dev_s *priv,
|
|
uint8_t regaddr, FAR uint8_t *regval, uint8_t len)
|
|
{
|
|
struct i2c_config_s config;
|
|
int ret;
|
|
|
|
/* Sanity check */
|
|
|
|
DEBUGASSERT(priv != NULL);
|
|
DEBUGASSERT(regval != NULL);
|
|
|
|
/* Set up the I2C configuration */
|
|
|
|
config.frequency = CONFIG_LSM9DS1_I2C_FREQUENCY;
|
|
config.address = priv->addr;
|
|
config.addrlen = 7;
|
|
|
|
/* Write the register address */
|
|
|
|
ret = i2c_write(priv->i2c, &config, ®addr, sizeof(regaddr));
|
|
if (ret < 0)
|
|
{
|
|
snerr("ERROR: i2c_write failed: %d\n", ret);
|
|
return ret;
|
|
}
|
|
|
|
/* Restart and read 8 bits from the register */
|
|
|
|
ret = i2c_read(priv->i2c, &config, regval, sizeof(*regval) * len);
|
|
if (ret < 0)
|
|
{
|
|
snerr("ERROR: i2c_read failed: %d\n", ret);
|
|
return ret;
|
|
}
|
|
|
|
return OK;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: lsm9ds1_writereg8
|
|
*
|
|
* Description:
|
|
* Write to an 8-bit register.
|
|
*
|
|
****************************************************************************/
|
|
|
|
int lsm9ds1_writereg8(FAR struct lsm9ds1_dev_s *priv,
|
|
uint8_t regaddr, uint8_t regval)
|
|
{
|
|
struct i2c_config_s config;
|
|
uint8_t buffer[2];
|
|
int ret;
|
|
|
|
/* Sanity check */
|
|
|
|
DEBUGASSERT(priv != NULL);
|
|
|
|
/* Set up a 2-byte message to send */
|
|
|
|
buffer[0] = regaddr;
|
|
buffer[1] = regval;
|
|
|
|
/* Set up the I2C configuration */
|
|
|
|
config.frequency = CONFIG_LSM9DS1_I2C_FREQUENCY;
|
|
config.address = priv->addr;
|
|
config.addrlen = 7;
|
|
|
|
/* Write the register address followed by the data (no RESTART) */
|
|
|
|
ret = i2c_write(priv->i2c, &config, buffer, sizeof(buffer));
|
|
if (ret < 0)
|
|
{
|
|
snerr("ERROR: i2c_write failed: %d\n", ret);
|
|
return ret;
|
|
}
|
|
|
|
sninfo("addr: %02x value: %02x\n", regaddr, regval);
|
|
return OK;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: lsm9ds1_modifyreg8
|
|
*
|
|
* Description:
|
|
* Modify an 8-bit register.
|
|
*
|
|
****************************************************************************/
|
|
|
|
int lsm9ds1_modifyreg8(FAR struct lsm9ds1_dev_s *priv,
|
|
uint8_t regaddr, uint8_t clearbits,
|
|
uint8_t setbits)
|
|
{
|
|
uint8_t regval;
|
|
int ret;
|
|
|
|
/* Sanity check */
|
|
|
|
DEBUGASSERT(priv != NULL);
|
|
|
|
ret = lsm9ds1_readreg8(priv, regaddr, ®val);
|
|
if (ret < 0)
|
|
{
|
|
snerr("ERROR: lsm9ds1_readreg8 failed: %d\n", ret);
|
|
return ret;
|
|
}
|
|
|
|
regval &= ~clearbits;
|
|
regval |= setbits;
|
|
|
|
ret = lsm9ds1_writereg8(priv, regaddr, regval);
|
|
if (ret < 0)
|
|
{
|
|
snerr("ERROR: lsm9ds1_writereg8 failed: %d\n", ret);
|
|
return ret;
|
|
}
|
|
|
|
return OK;
|
|
}
|
|
|
|
/****************************************************************************
|
|
* Name: lsm9ds1_midpoint
|
|
*
|
|
* Description:
|
|
* Find the midpoint between two numbers.
|
|
*
|
|
****************************************************************************/
|
|
|
|
uint32_t lsm9ds1_midpoint(uint32_t a, uint32_t b)
|
|
{
|
|
return (uint32_t)(((uint64_t)a +
|
|
(uint64_t)b + (uint64_t)1) / (uint64_t)2);
|
|
}
|
|
|