Google Git
Sign in
apache / mynewt-documentation / 94c47ae2bc67c54a68d227abbb47c27ae97cc232 / . / versions / v1_4_0 / mynewt-core / hw / drivers / sensors / lis2dh12 / src / lis2dh12.c
blob: 169631827578e548b3a3d12a503fa28e97ee1d46 [file] [log] [blame]
/*
* 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
* resarding 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.
*/
#include <assert.h>
#include <stdio.h>
#include <errno.h>
#include <string.h>
#include "os/mynewt.h"
#include "hal/hal_spi.h"
#include "hal/hal_i2c.h"
#include "i2cn/i2cn.h"
#include "sensor/sensor.h"
#include "sensor/accel.h"
#include "lis2dh12/lis2dh12.h"
#include "lis2dh12_priv.h"
#include "hal/hal_gpio.h"
#include "modlog/modlog.h"
#include "stats/stats.h"
#include <syscfg/syscfg.h>
static struct hal_spi_settings spi_lis2dh12_settings = {
.data_order = HAL_SPI_MSB_FIRST,
.data_mode = HAL_SPI_MODE3,
.baudrate = 4000,
.word_size = HAL_SPI_WORD_SIZE_8BIT,
};
/* Define the stats section and records */
STATS_SECT_START(lis2dh12_stat_section)
STATS_SECT_ENTRY(write_errors)
STATS_SECT_ENTRY(read_errors)
STATS_SECT_END
/* Define stat names for querying */
STATS_NAME_START(lis2dh12_stat_section)
STATS_NAME(lis2dh12_stat_section, write_errors)
STATS_NAME(lis2dh12_stat_section, read_errors)
STATS_NAME_END(lis2dh12_stat_section)
/* Global variable used to hold stats data */
STATS_SECT_DECL(lis2dh12_stat_section) g_lis2dh12stats;
#define LIS2DH12_LOG(lvl_, ...) \
MODLOG_ ## lvl_(MYNEWT_VAL(LIS2DH12_LOG_MODULE), __VA_ARGS__)
/* Exports for the sensor API */
static int lis2dh12_sensor_read(struct sensor *, sensor_type_t,
sensor_data_func_t, void *, uint32_t);
static int lis2dh12_sensor_get_config(struct sensor *, sensor_type_t,
struct sensor_cfg *);
static int
lis2dh12_sensor_set_trigger_thresh(struct sensor *, sensor_type_t,
struct sensor_type_traits *);
static int
lis2dh12_sensor_clear_low_thresh(struct sensor *, sensor_type_t);
static int
lis2dh12_sensor_clear_high_thresh(struct sensor *, sensor_type_t);
static const struct sensor_driver g_lis2dh12_sensor_driver = {
.sd_read = lis2dh12_sensor_read,
.sd_get_config = lis2dh12_sensor_get_config,
/* Setting trigger threshold is optional */
.sd_set_trigger_thresh = lis2dh12_sensor_set_trigger_thresh,
.sd_clear_low_trigger_thresh = lis2dh12_sensor_clear_low_thresh,
.sd_clear_high_trigger_thresh = lis2dh12_sensor_clear_high_thresh
};
/**
* Read multiple length data from LIS2DH12 sensor over I2C
*
* @param The sensor interface
* @param register address
* @param variable length buffer
* @param length of the payload to read
*
* @return 0 on success, non-zero on failure
*/
static int
lis2dh12_i2c_readlen(struct sensor_itf *itf, uint8_t addr, uint8_t *buffer,
uint8_t len)
{
int rc;
uint8_t payload[20] = { addr, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0};
struct hal_i2c_master_data data_struct = {
.address = itf->si_addr,
.len = 1,
.buffer = payload
};
/* Clear the supplied buffer */
memset(buffer, 0, len);
/* Register write */
rc = i2cn_master_write(itf->si_num, &data_struct, MYNEWT_VAL(LIS2DH12_I2C_TIMEOUT_TICKS), 1,
MYNEWT_VAL(LIS2DH12_I2C_RETRIES));
if (rc) {
LIS2DH12_LOG(ERROR, "I2C access failed at address 0x%02X\n",
data_struct.address);
STATS_INC(g_lis2dh12stats, read_errors);
goto err;
}
/* Read len bytes back */
memset(payload, 0, sizeof(payload));
data_struct.len = len;
rc = i2cn_master_read(itf->si_num, &data_struct, MYNEWT_VAL(LIS2DH12_I2C_TIMEOUT_TICKS), 1,
MYNEWT_VAL(LIS2DH12_I2C_RETRIES));
if (rc) {
LIS2DH12_LOG(ERROR, "Failed to read from 0x%02X:0x%02X\n",
data_struct.address, addr);
STATS_INC(g_lis2dh12stats, read_errors);
goto err;
}
/* Copy the I2C results into the supplied buffer */
memcpy(buffer, payload, len);
return 0;
err:
return rc;
}
/**
* Read multiple length data from LIS2DH12 sensor over SPI
*
* @param The sensor interface
* @param register address
* @param variable length payload
* @param length of the payload to read
*
* @return 0 on success, non-zero on failure
*/
static int
lis2dh12_spi_readlen(struct sensor_itf *itf, uint8_t addr, uint8_t *payload,
uint8_t len)
{
int i;
uint16_t retval;
int rc;
rc = 0;
addr |= LIS2DH12_SPI_READ_CMD_BIT;
/*
* Auto register address increment is needed if the length
* requested is more than 1
*/
if (len > 1) {
addr |= LIS2DH12_SPI_ADR_INC;
}
/* Select the device */
hal_gpio_write(itf->si_cs_pin, 0);
/* Send the address */
retval = hal_spi_tx_val(itf->si_num, addr);
if (retval == 0xFFFF) {
rc = SYS_EINVAL;
LIS2DH12_LOG(ERROR, "SPI_%u register write failed addr:0x%02X\n",
itf->si_num, addr);
STATS_INC(g_lis2dh12stats, read_errors);
goto err;
}
for (i = 0; i < len; i++) {
/* Read data */
retval = hal_spi_tx_val(itf->si_num, 0x55);
if (retval == 0xFFFF) {
rc = SYS_EINVAL;
LIS2DH12_LOG(ERROR, "SPI_%u read failed addr:0x%02X\n",
itf->si_num, addr);
STATS_INC(g_lis2dh12stats, read_errors);
goto err;
}
payload[i] = retval;
}
rc = 0;
err:
/* De-select the device */
hal_gpio_write(itf->si_cs_pin, 1);
return rc;
}
/**
* Write multiple length data to LIS2DH12 sensor over I2C (MAX: 19 bytes)
*
* @param The sensor interface
* @param register address
* @param variable length payload
* @param length of the payload to write
*
* @return 0 on success, non-zero on failure
*/
static int
lis2dh12_i2c_writelen(struct sensor_itf *itf, uint8_t addr, uint8_t *buffer,
uint8_t len)
{
int rc;
uint8_t payload[20] = { addr, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0};
struct hal_i2c_master_data data_struct = {
.address = itf->si_addr,
.len = len + 1,
.buffer = payload
};
if (len > (sizeof(payload) - 1)) {
rc = OS_EINVAL;
goto err;
}
memcpy(&payload[1], buffer, len);
/* Register write */
rc = i2cn_master_write(itf->si_num, &data_struct, MYNEWT_VAL(LIS2DH12_I2C_TIMEOUT_TICKS), 1,
MYNEWT_VAL(LIS2DH12_I2C_RETRIES));
if (rc) {
LIS2DH12_LOG(ERROR, "I2C access failed at address 0x%02X\n",
data_struct.address);
STATS_INC(g_lis2dh12stats, write_errors);
goto err;
}
return 0;
err:
return rc;
}
/**
* Write multiple length data to LIS2DH12 sensor over SPI
*
* @param The sensor interface
* @param register address
* @param variable length payload
* @param length of the payload to write
*
* @return 0 on success, non-zero on failure
*/
static int
lis2dh12_spi_writelen(struct sensor_itf *itf, uint8_t addr, uint8_t *payload,
uint8_t len)
{
int i;
int rc;
/*
* Auto register address increment is needed if the length
* requested is moret than 1
*/
if (len > 1) {
addr |= LIS2DH12_SPI_ADR_INC;
}
/* Select the device */
hal_gpio_write(itf->si_cs_pin, 0);
/* Send the address */
rc = hal_spi_tx_val(itf->si_num, addr);
if (rc == 0xFFFF) {
rc = SYS_EINVAL;
LIS2DH12_LOG(ERROR, "SPI_%u register write failed addr:0x%02X\n",
itf->si_num, addr);
STATS_INC(g_lis2dh12stats, write_errors);
goto err;
}
for (i = 0; i < len; i++) {
/* Read data */
rc = hal_spi_tx_val(itf->si_num, payload[i]);
if (rc == 0xFFFF) {
rc = SYS_EINVAL;
LIS2DH12_LOG(ERROR, "SPI_%u write failed addr:0x%02X\n",
itf->si_num, addr);
STATS_INC(g_lis2dh12stats, write_errors);
goto err;
}
}
rc = 0;
err:
/* De-select the device */
hal_gpio_write(itf->si_cs_pin, 1);
return rc;
}
/**
* Write multiple length data to LIS2DH12 sensor over different interfaces
*
* @param The sensor interface
* @param register address
* @param variable length payload
* @param length of the payload to write
*
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_writelen(struct sensor_itf *itf, uint8_t addr, uint8_t *payload,
uint8_t len)
{
int rc;
rc = sensor_itf_lock(itf, MYNEWT_VAL(LIS2DH12_ITF_LOCK_TMO));
if (rc) {
return rc;
}
if (itf->si_type == SENSOR_ITF_I2C) {
rc = lis2dh12_i2c_writelen(itf, addr, payload, len);
} else {
rc = lis2dh12_spi_writelen(itf, addr, payload, len);
}
sensor_itf_unlock(itf);
return rc;
}
/**
* Read multiple length data from LIS2DH12 sensor over different interfaces
*
* @param register address
* @param variable length payload
* @param length of the payload to read
*
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_readlen(struct sensor_itf *itf, uint8_t addr, uint8_t *payload,
uint8_t len)
{
int rc;
rc = sensor_itf_lock(itf, MYNEWT_VAL(LIS2DH12_ITF_LOCK_TMO));
if (rc) {
return rc;
}
if (itf->si_type == SENSOR_ITF_I2C) {
rc = lis2dh12_i2c_readlen(itf, addr, payload, len);
} else {
rc = lis2dh12_spi_readlen(itf, addr, payload, len);
}
sensor_itf_unlock(itf);
return rc;
}
/**
* Reset lis2dh12
*
* @param The sensor interface
*/
int
lis2dh12_reset(struct sensor_itf *itf)
{
int rc;
uint8_t reg;
rc = lis2dh12_readlen(itf, LIS2DH12_REG_CTRL_REG5, &reg, 1);
if (rc) {
goto err;
}
reg |= LIS2DH12_CTRL_REG5_BOOT;
rc = lis2dh12_writelen(itf, LIS2DH12_REG_CTRL_REG5, &reg, 1);
if (rc) {
goto err;
}
os_time_delay((OS_TICKS_PER_SEC * 6/1000) + 1);
err:
return rc;
}
/**
* Pull up disconnect
*
* @param The sensor interface
* @param disconnect pull up
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_pull_up_disc(struct sensor_itf *itf, uint8_t disconnect)
{
uint8_t reg;
reg = 0;
reg |= ((disconnect ? LIS2DH12_CTRL_REG0_SPD : 0) |
LIS2DH12_CTRL_REG0_CORR_OP);
return lis2dh12_writelen(itf, LIS2DH12_REG_CTRL_REG0, &reg, 1);
}
/**
* Enable channels
*
* @param sensor interface
* @param chan
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_chan_enable(struct sensor_itf *itf, uint8_t chan)
{
uint8_t reg;
int rc;
rc = lis2dh12_readlen(itf, LIS2DH12_REG_CTRL_REG1, &reg, 1);
if (rc) {
goto err;
}
reg &= 0xF0;
reg |= chan;
rc = lis2dh12_writelen(itf, LIS2DH12_REG_CTRL_REG1, &reg, 1);
err:
return rc;
}
/**
* Get chip ID
*
* @param sensor interface
* @param ptr to chip id to be filled up
*/
int
lis2dh12_get_chip_id(struct sensor_itf *itf, uint8_t *chip_id)
{
uint8_t reg;
int rc;
rc = lis2dh12_readlen(itf, LIS2DH12_REG_WHO_AM_I, &reg, 1);
if (rc) {
goto err;
}
*chip_id = reg;
err:
return rc;
}
/**
* Sets the full scale selection
*
* @param The sensor interface
* @param The fs setting
*
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_set_full_scale(struct sensor_itf *itf, uint8_t fs)
{
int rc;
uint8_t reg;
if (fs > LIS2DH12_FS_16G) {
LIS2DH12_LOG(ERROR, "Invalid full scale value\n");
rc = SYS_EINVAL;
goto err;
}
rc = lis2dh12_readlen(itf, LIS2DH12_REG_CTRL_REG4,
&reg, 1);
if (rc) {
goto err;
}
reg = (reg & ~LIS2DH12_CTRL_REG4_FS) | fs;
rc = lis2dh12_writelen(itf, LIS2DH12_REG_CTRL_REG4,
&reg, 1);
if (rc) {
goto err;
}
return 0;
err:
return rc;
}
/**
* Gets the full scale selection
*
* @param The sensor interface
* @param ptr to fs
*
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_get_full_scale(struct sensor_itf *itf, uint8_t *fs)
{
int rc;
uint8_t reg;
rc = lis2dh12_readlen(itf, LIS2DH12_REG_CTRL_REG4,
&reg, 1);
if (rc) {
goto err;
}
*fs = (reg & LIS2DH12_CTRL_REG4_FS) >> 4;
return 0;
err:
return rc;
}
/**
* Calculates the acceleration in m/s^2 from mg
*
* @param acc value in mg
* @param float ptr to return calculated value in ms2
*/
void
lis2dh12_calc_acc_ms2(int16_t acc_mg, float *acc_ms2)
{
*acc_ms2 = (acc_mg * STANDARD_ACCEL_GRAVITY)/1000;
}
/**
* Calculates the acceleration in mg from m/s^2
*
* @param acc value in m/s^2
* @param int16 ptr to return calculated value in mg
*/
void
lis2dh12_calc_acc_mg(float acc_ms2, int16_t *acc_mg)
{
*acc_mg = (acc_ms2 * 1000)/STANDARD_ACCEL_GRAVITY;
}
/**
* Sets the rate
*
* @param The sensor interface
* @param The rate
*
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_set_rate(struct sensor_itf *itf, uint8_t rate)
{
int rc;
uint8_t reg;
if (rate > LIS2DH12_DATA_RATE_HN_1344HZ_L_5376HZ) {
LIS2DH12_LOG(ERROR, "Invalid rate value\n");
rc = SYS_EINVAL;
goto err;
}
/*
* As per the datasheet, REFERENCE(26h) needs to be read
* for a reset of the filter block before switching to
* normal/high-performance mode from power down mode
*/
if (rate != LIS2DH12_DATA_RATE_0HZ || rate != LIS2DH12_DATA_RATE_L_1620HZ) {
rc = lis2dh12_readlen(itf, LIS2DH12_REG_REFERENCE, &reg, 1);
if (rc) {
goto err;
}
}
rc = lis2dh12_readlen(itf, LIS2DH12_REG_CTRL_REG1,
&reg, 1);
if (rc) {
goto err;
}
reg = (reg & ~LIS2DH12_CTRL_REG1_ODR) | rate;
rc = lis2dh12_writelen(itf, LIS2DH12_REG_CTRL_REG1,
&reg, 1);
if (rc) {
goto err;
}
return 0;
err:
return rc;
}
/**
* Gets the rate
*
* @param The sensor ineterface
* @param ptr to the rate
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_get_rate(struct sensor_itf *itf, uint8_t *rate)
{
int rc;
uint8_t reg;
rc = lis2dh12_readlen(itf, LIS2DH12_REG_CTRL_REG1, &reg, 1);
if (rc) {
goto err;
}
*rate = reg & LIS2DH12_CTRL_REG1_ODR;
return 0;
err:
return rc;
}
/**
* Set FIFO mode
*
* @param the sensor interface
* @param mode
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_set_fifo_mode(struct sensor_itf *itf, uint8_t mode)
{
int rc;
uint8_t reg;
rc = lis2dh12_readlen(itf, LIS2DH12_REG_CTRL_REG5, &reg, 1);
if (rc) {
goto err;
}
reg |= LIS2DH12_CTRL_REG5_FIFO_EN;
rc = lis2dh12_writelen(itf, LIS2DH12_REG_CTRL_REG5, &reg, 1);
if (rc) {
goto err;
}
rc = lis2dh12_readlen(itf, LIS2DH12_REG_FIFO_CTRL_REG, &reg, 1);
if (rc) {
goto err;
}
reg |= mode;
rc = lis2dh12_writelen(itf, LIS2DH12_REG_FIFO_CTRL_REG, &reg, 1);
if (rc) {
goto err;
}
rc = lis2dh12_readlen(itf, LIS2DH12_REG_FIFO_SRC_REG, &reg, 1);
if (rc) {
goto err;
}
if (mode == LIS2DH12_FIFO_M_BYPASS && reg != LIS2DH12_FIFO_SRC_EMPTY) {
rc = SYS_EINVAL;
goto err;
}
return 0;
err:
return rc;
}
/**
*
* Get operating mode
*
* @param the sensor interface
* @param ptr to mode
*
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_get_op_mode(struct sensor_itf *itf, uint8_t *mode)
{
int rc;
uint8_t reg1;
uint8_t reg4;
rc = lis2dh12_readlen(itf, LIS2DH12_REG_CTRL_REG1, &reg1, 1);
if (rc) {
goto err;
}
reg1 = (reg1 & LIS2DH12_CTRL_REG1_LPEN) << 4;
rc = lis2dh12_readlen(itf, LIS2DH12_REG_CTRL_REG4, &reg4, 1);
if (rc) {
goto err;
}
reg4 = (reg4 & LIS2DH12_CTRL_REG4_HR);
*mode = reg1 | reg4;
return 0;
err:
return rc;
}
/**
* Set high pass filter cfg
*
* @param the sensor interface
* @param filter register settings
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_hpf_cfg(struct sensor_itf *itf, uint8_t reg)
{
return lis2dh12_writelen(itf, LIS2DH12_REG_CTRL_REG2, &reg, 1);
}
/**
* Set operating mode
*
* @param the sensor interface
* @param mode CTRL_REG1[3:0]:CTRL_REG4[3:0]
*
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_set_op_mode(struct sensor_itf *itf, uint8_t mode)
{
int rc;
uint8_t reg;
rc = lis2dh12_readlen(itf, LIS2DH12_REG_CTRL_REG1, &reg, 1);
if (rc) {
goto err;
}
reg &= ~LIS2DH12_CTRL_REG1_LPEN;
reg |= ((mode & 0x80) >> 4);
rc = lis2dh12_writelen(itf, LIS2DH12_REG_CTRL_REG1, &reg, 1);
if (rc) {
goto err;
}
rc = lis2dh12_readlen(itf, LIS2DH12_REG_CTRL_REG4, &reg, 1);
if (rc) {
goto err;
}
reg &= ~LIS2DH12_CTRL_REG4_HR;
reg |= (mode & 0x08);
rc = lis2dh12_writelen(itf, LIS2DH12_REG_CTRL_REG4, &reg, 1);
if (rc) {
goto err;
}
os_time_delay(OS_TICKS_PER_SEC/1000 + 1);
return 0;
err:
return rc;
}
/**
* Gets a new data sample from the light sensor.
*
* @param The sensor interface
* @param x axis data
* @param y axis data
* @param z axis data
*
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_get_data(struct sensor_itf *itf, int16_t *x, int16_t *y, int16_t *z)
{
int rc;
uint8_t payload[6] = {0};
uint8_t fs;
*x = *y = *z = 0;
rc = lis2dh12_readlen(itf, LIS2DH12_REG_OUT_X_L, payload, 1);
rc |= lis2dh12_readlen(itf, LIS2DH12_REG_OUT_X_H, &payload[1], 1);
rc |= lis2dh12_readlen(itf, LIS2DH12_REG_OUT_Y_L, &payload[2], 1);
rc |= lis2dh12_readlen(itf, LIS2DH12_REG_OUT_Y_H, &payload[3], 1);
rc |= lis2dh12_readlen(itf, LIS2DH12_REG_OUT_Z_L, &payload[4], 1);
rc |= lis2dh12_readlen(itf, LIS2DH12_REG_OUT_Z_H, &payload[5], 1);
if (rc) {
goto err;
}
*x = payload[0] | (payload[1] << 8);
*y = payload[2] | (payload[3] << 8);
*z = payload[4] | (payload[5] << 8);
rc = lis2dh12_get_full_scale(itf, &fs);
if (rc) {
goto err;
}
if (fs == LIS2DH12_FS_2G) {
fs = 2;
} else if (fs == LIS2DH12_FS_4G) {
fs = 4;
} else if (fs == LIS2DH12_FS_8G) {
fs = 8;
} else if (fs == LIS2DH12_FS_16G) {
fs = 16;
} else {
rc = SYS_EINVAL;
goto err;
}
/*
* Since full scale is +/-(fs)g,
* fs should be multiplied by 2 to account for full scale.
* To calculate mg from g we use the 1000 multiple.
* Since the full scale is represented by 16 bit value,
* we use that as a divisor.
* The calculation is based on an example present in AN5005
* application note
*/
*x = (fs * 2 * 1000 * *x)/UINT16_MAX;
*y = (fs * 2 * 1000 * *y)/UINT16_MAX;
*z = (fs * 2 * 1000 * *z)/UINT16_MAX;
return 0;
err:
return rc;
}
/**
* Expects to be called back through os_dev_create().
*
* @param The device object associated with this accelerometer
* @param Argument passed to OS device init, unused
*
* @return 0 on success, non-zero error on failure.
*/
int
lis2dh12_init(struct os_dev *dev, void *arg)
{
struct lis2dh12 *lis2dh12;
struct sensor *sensor;
int rc;
if (!arg || !dev) {
rc = SYS_ENODEV;
goto err;
}
lis2dh12 = (struct lis2dh12 *) dev;
lis2dh12->cfg.lc_s_mask = SENSOR_TYPE_ALL;
sensor = &lis2dh12->sensor;
/* Initialise the stats entry */
rc = stats_init(
STATS_HDR(g_lis2dh12stats),
STATS_SIZE_INIT_PARMS(g_lis2dh12stats, STATS_SIZE_32),
STATS_NAME_INIT_PARMS(lis2dh12_stat_section));
SYSINIT_PANIC_ASSERT(rc == 0);
/* Register the entry with the stats registry */
rc = stats_register(dev->od_name, STATS_HDR(g_lis2dh12stats));
SYSINIT_PANIC_ASSERT(rc == 0);
rc = sensor_init(sensor, dev);
if (rc) {
goto err;
}
/* Add the light driver */
rc = sensor_set_driver(sensor, SENSOR_TYPE_ACCELEROMETER,
(struct sensor_driver *) &g_lis2dh12_sensor_driver);
if (rc) {
goto err;
}
/* Set the interface */
rc = sensor_set_interface(sensor, arg);
if (rc) {
goto err;
}
rc = sensor_mgr_register(sensor);
if (rc) {
goto err;
}
if (sensor->s_itf.si_type == SENSOR_ITF_SPI) {
rc = hal_spi_disable(sensor->s_itf.si_num);
if (rc) {
goto err;
}
rc = hal_spi_config(sensor->s_itf.si_num, &spi_lis2dh12_settings);
if (rc == EINVAL) {
/* If spi is already enabled, for nrf52, it returns -1, We should not
* fail if the spi is already enabled
*/
goto err;
}
rc = hal_spi_enable(sensor->s_itf.si_num);
if (rc) {
goto err;
}
rc = hal_gpio_init_out(sensor->s_itf.si_cs_pin, 1);
if (rc) {
goto err;
}
}
return 0;
err:
return rc;
}
/**
* Self test mode
*
* @param the sensor interface
* @param mode to set
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_set_self_test_mode(struct sensor_itf *itf, uint8_t mode)
{
uint8_t reg;
int rc;
rc = lis2dh12_readlen(itf, LIS2DH12_REG_CTRL_REG4, &reg, 1);
if (rc) {
goto err;
}
reg &= ~LIS2DH12_CTRL_REG4_ST;
reg |= mode;
rc = lis2dh12_writelen(itf, LIS2DH12_REG_CTRL_REG4, &reg, 1);
err:
return rc;
}
static int
lis2dh12_sensor_read(struct sensor *sensor, sensor_type_t type,
sensor_data_func_t data_func, void *data_arg, uint32_t timeout)
{
struct sensor_accel_data sad;
struct sensor_itf *itf;
int16_t x, y ,z;
float fx, fy ,fz;
int rc;
/* If the read isn't looking for accel or mag data, don't do anything. */
if (!(type & SENSOR_TYPE_ACCELEROMETER)) {
rc = SYS_EINVAL;
goto err;
}
itf = SENSOR_GET_ITF(sensor);
x = y = z = 0;
if (itf->si_type == SENSOR_ITF_SPI) {
rc = hal_spi_disable(sensor->s_itf.si_num);
if (rc) {
goto err;
}
rc = hal_spi_config(sensor->s_itf.si_num, &spi_lis2dh12_settings);
if (rc == EINVAL) {
/* If spi is already enabled, for nrf52, it returns -1, We should not
* fail if the spi is already enabled
*/
goto err;
}
rc = hal_spi_enable(sensor->s_itf.si_num);
if (rc) {
goto err;
}
}
rc = lis2dh12_get_data(itf, &x, &y, &z);
if (rc) {
goto err;
}
/* converting values from mg to ms^2 */
lis2dh12_calc_acc_ms2(x, &fx);
lis2dh12_calc_acc_ms2(y, &fy);
lis2dh12_calc_acc_ms2(z, &fz);
sad.sad_x = fx;
sad.sad_y = fy;
sad.sad_z = fz;
sad.sad_x_is_valid = 1;
sad.sad_y_is_valid = 1;
sad.sad_z_is_valid = 1;
/* Call data function */
rc = data_func(sensor, data_arg, &sad, SENSOR_TYPE_ACCELEROMETER);
if (rc != 0) {
goto err;
}
return 0;
err:
return rc;
}
static int
lis2dh12_sensor_get_config(struct sensor *sensor, sensor_type_t type,
struct sensor_cfg *cfg)
{
int rc;
if (type != SENSOR_TYPE_ACCELEROMETER) {
rc = SYS_EINVAL;
goto err;
}
cfg->sc_valtype = SENSOR_VALUE_TYPE_FLOAT_TRIPLET;
return 0;
err:
return rc;
}
/**
* Set interrupt threshold for int 2
*
* @param the sensor interface
* @param threshold
*
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_set_int2_thresh(struct sensor_itf *itf, uint8_t ths)
{
int rc;
rc = lis2dh12_writelen(itf, LIS2DH12_REG_INT2_THS, &ths, 1);
if (rc) {
goto err;
}
return 0;
err:
return rc;
}
/**
* Set interrupt threshold for int 1
*
* @param the sensor interface
* @param threshold
*
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_set_int1_thresh(struct sensor_itf *itf, uint8_t ths)
{
int rc;
rc = lis2dh12_writelen(itf, LIS2DH12_REG_INT1_THS, &ths, 1);
if (rc) {
goto err;
}
return 0;
err:
return rc;
}
/**
* Clear interrupt 2
*
* @param the sensor interface
*/
int
lis2dh12_clear_int2(struct sensor_itf *itf)
{
uint8_t reg;
return lis2dh12_readlen(itf, LIS2DH12_REG_INT2_SRC, &reg, 1);
}
/**
* Clear interrupt 1
*
* @param the sensor interface
*/
int
lis2dh12_clear_int1(struct sensor_itf *itf)
{
uint8_t reg;
return lis2dh12_readlen(itf, LIS2DH12_REG_INT1_SRC, &reg, 1);
}
/**
* Enable interrupt 2
*
* @param the sensor interface
* @param events to enable int for
*/
int
lis2dh12_enable_int2(struct sensor_itf *itf, uint8_t *reg)
{
return lis2dh12_writelen(itf, LIS2DH12_REG_INT2_CFG, reg, 1);
}
/**
* Latch interrupt 1
*
* @param the sensor interface
*/
int
lis2dh12_latch_int1(struct sensor_itf *itf)
{
uint8_t reg;
int rc;
rc = lis2dh12_readlen(itf, LIS2DH12_REG_CTRL_REG5, &reg, 1);
if (rc) {
goto err;
}
reg |= LIS2DH12_CTRL_REG5_LIR_INT1;
rc = lis2dh12_writelen(itf, LIS2DH12_REG_CTRL_REG5, &reg, 1);
if (rc) {
goto err;
}
return 0;
err:
return rc;
}
/**
* Latch interrupt 2
*
* @param the sensor interface
*/
int
lis2dh12_latch_int2(struct sensor_itf *itf)
{
uint8_t reg;
int rc;
rc = lis2dh12_readlen(itf, LIS2DH12_REG_CTRL_REG5, &reg, 1);
if (rc) {
goto err;
}
reg |= LIS2DH12_CTRL_REG5_LIR_INT2;
rc = lis2dh12_writelen(itf, LIS2DH12_REG_CTRL_REG5, &reg, 1);
if (rc) {
goto err;
}
return 0;
err:
return rc;
}
/**
* Set interrupt pin configuration for interrupt 1
*
* @param the sensor interface
* @param config
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_set_int1_pin_cfg(struct sensor_itf *itf, uint8_t cfg)
{
uint8_t reg;
reg = ~0x08 & cfg;
return lis2dh12_writelen(itf, LIS2DH12_REG_CTRL_REG3, &reg, 1);
}
/**
* Set interrupt 1 duration
*
* @param duration in N/ODR units
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_set_int1_duration(struct sensor_itf *itf, uint8_t dur)
{
return lis2dh12_writelen(itf, LIS2DH12_REG_INT1_DURATION, &dur, 1);
}
/**
* Set interrupt 2 duration
*
* @param duration in N/ODR units
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_set_int2_duration(struct sensor_itf *itf, uint8_t dur)
{
return lis2dh12_writelen(itf, LIS2DH12_REG_INT2_DURATION, &dur, 1);
}
/**
* Set interrupt pin configuration for interrupt 2
*
* @param the sensor interface
* @param config
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_set_int2_pin_cfg(struct sensor_itf *itf, uint8_t cfg)
{
return lis2dh12_writelen(itf, LIS2DH12_REG_CTRL_REG6, &cfg, 1);
}
/**
* Disable interrupt 1
*
* @param the sensor interface
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_disable_int1(struct sensor_itf *itf)
{
uint8_t reg;
int rc;
reg = 0;
rc = lis2dh12_clear_int1(itf);
if (rc) {
goto err;
}
os_time_delay((OS_TICKS_PER_SEC * 20)/1000 + 1);
rc = lis2dh12_writelen(itf, LIS2DH12_REG_INT1_CFG, &reg, 1);
err:
return rc;
}
/**
* Disable interrupt 2
*
* @param the sensor interface
* @return 0 on success, non-zero on failure
*/
int
lis2dh12_disable_int2(struct sensor_itf *itf)
{
uint8_t reg;
int rc;
reg = 0;
rc = lis2dh12_clear_int2(itf);
if (rc) {
goto err;
}
os_time_delay((OS_TICKS_PER_SEC * 20)/1000 + 1);
rc = lis2dh12_writelen(itf, LIS2DH12_REG_INT2_CFG, &reg, 1);
err:
return rc;
}
/**
* Enable interrupt 1
*
* @param the sensor interface
* @param events to enable int for
*/
int
lis2dh12_enable_int1(struct sensor_itf *itf, uint8_t *reg)
{
return lis2dh12_writelen(itf, LIS2DH12_REG_INT1_CFG, reg, 1);
}
/**
* IRQ handler for interrupts for high/low threshold
*
* @param arg
*/
static void
lis2dh12_int_irq_handler(void *arg)
{
sensor_mgr_put_read_evt(arg);
}
/**
* Clear the low threshold values and disable interrupt
*
* @param ptr to sensor
* @param the Sensor type
* @param Sensor type traits
*
* @return 0 on success, non-zero on failure
*/
static int
lis2dh12_sensor_clear_low_thresh(struct sensor *sensor,
sensor_type_t type)
{
int rc;
struct sensor_itf *itf;
itf = SENSOR_GET_ITF(sensor);
if (type != SENSOR_TYPE_ACCELEROMETER) {
rc = SYS_EINVAL;
goto err;
}
rc = lis2dh12_disable_int1(itf);
if (rc) {
goto err;
}
hal_gpio_irq_release(itf->si_low_pin);
return 0;
err:
return rc;
}
/**
* Clear the high threshold values and disable interrupt
*
* @param ptr to sensor
* @param the Sensor type
* @param Sensor type traits
*
* @return 0 on success, non-zero on failure
*/
static int
lis2dh12_sensor_clear_high_thresh(struct sensor *sensor,
sensor_type_t type)
{
int rc;
struct sensor_itf *itf;
itf = SENSOR_GET_ITF(sensor);
if (type != SENSOR_TYPE_ACCELEROMETER) {
rc = SYS_EINVAL;
goto err;
}
rc = lis2dh12_disable_int2(itf);
if (rc) {
goto err;
}
hal_gpio_irq_release(itf->si_high_pin);
return 0;
err:
return rc;
}
static int
lis2dh12_set_low_thresh(struct sensor_itf *itf,
sensor_data_t low_thresh,
uint8_t fs,
struct sensor_type_traits *stt)
{
int16_t acc_mg;
uint8_t reg;
int rc;
rc = 0;
if (low_thresh.sad->sad_x_is_valid ||
low_thresh.sad->sad_y_is_valid ||
low_thresh.sad->sad_z_is_valid) {
if (low_thresh.sad->sad_x_is_valid) {
lis2dh12_calc_acc_mg(low_thresh.sad->sad_x, &acc_mg);
reg = acc_mg/fs;
}
if (low_thresh.sad->sad_y_is_valid) {
lis2dh12_calc_acc_mg(low_thresh.sad->sad_y, &acc_mg);
if (reg > acc_mg/fs) {
reg = acc_mg/fs;
}
}
if (low_thresh.sad->sad_z_is_valid) {
lis2dh12_calc_acc_mg(low_thresh.sad->sad_z, &acc_mg);
if (reg > acc_mg/fs) {
reg = acc_mg/fs;
}
}
rc = lis2dh12_set_int1_thresh(itf, reg);
if (rc) {
goto err;
}
reg = LIS2DH12_CTRL_REG3_I1_IA1;
rc = lis2dh12_set_int1_pin_cfg(itf, reg);
if (rc) {
goto err;
}
rc = lis2dh12_set_int1_duration(itf, 3);
if (rc) {
goto err;
}
os_time_delay((OS_TICKS_PER_SEC * 100)/1000 + 1);
hal_gpio_irq_release(itf->si_low_pin);
rc = hal_gpio_irq_init(itf->si_low_pin, lis2dh12_int_irq_handler, stt,
HAL_GPIO_TRIG_FALLING, HAL_GPIO_PULL_NONE);
if (rc) {
goto err;
}
reg = low_thresh.sad->sad_x_is_valid ? LIS2DH12_INT2_CFG_XLIE : 0;
reg |= low_thresh.sad->sad_y_is_valid ? LIS2DH12_INT2_CFG_YLIE : 0;
reg |= low_thresh.sad->sad_z_is_valid ? LIS2DH12_INT2_CFG_ZLIE : 0;
rc = lis2dh12_clear_int1(itf);
if (rc) {
goto err;
}
os_time_delay((OS_TICKS_PER_SEC * 20)/1000 + 1);
hal_gpio_irq_enable(itf->si_low_pin);
rc = lis2dh12_enable_int1(itf, &reg);
if (rc) {
goto err;
}
}
err:
hal_gpio_irq_release(itf->si_low_pin);
return rc;
}
static int
lis2dh12_set_high_thresh(struct sensor_itf *itf,
sensor_data_t high_thresh,
uint8_t fs,
struct sensor_type_traits *stt)
{
int16_t acc_mg;
uint8_t reg;
int rc;
rc = 0;
if (high_thresh.sad->sad_x_is_valid ||
high_thresh.sad->sad_y_is_valid ||
high_thresh.sad->sad_z_is_valid) {
if (high_thresh.sad->sad_x_is_valid) {
lis2dh12_calc_acc_mg(high_thresh.sad->sad_x, &acc_mg);
reg = acc_mg/fs;
}
if (high_thresh.sad->sad_y_is_valid) {
lis2dh12_calc_acc_mg(high_thresh.sad->sad_y, &acc_mg);
if (reg < acc_mg/fs) {
reg = acc_mg/fs;
}
}
if (high_thresh.sad->sad_z_is_valid) {
lis2dh12_calc_acc_mg(high_thresh.sad->sad_z, &acc_mg);
if (reg < acc_mg/fs) {
reg = acc_mg/fs;
}
}
rc = lis2dh12_set_int2_thresh(itf, reg);
if (rc) {
goto err;
}
reg = LIS2DH12_CTRL_REG6_I2_IA2;
rc = lis2dh12_set_int2_pin_cfg(itf, reg);
if (rc) {
goto err;
}
rc = lis2dh12_set_int2_duration(itf, 3);
if (rc) {
goto err;
}
os_time_delay((OS_TICKS_PER_SEC * 100)/1000 + 1);
hal_gpio_irq_release(itf->si_high_pin);
rc = hal_gpio_irq_init(itf->si_high_pin, lis2dh12_int_irq_handler, stt,
HAL_GPIO_TRIG_FALLING, HAL_GPIO_PULL_NONE);
if (rc) {
goto err;
}
reg = high_thresh.sad->sad_x_is_valid ? LIS2DH12_INT2_CFG_XHIE : 0;
reg |= high_thresh.sad->sad_y_is_valid ? LIS2DH12_INT2_CFG_YHIE : 0;
reg |= high_thresh.sad->sad_z_is_valid ? LIS2DH12_INT2_CFG_ZHIE : 0;
rc = lis2dh12_clear_int2(itf);
if (rc) {
goto err;
}
hal_gpio_irq_enable(itf->si_high_pin);
rc = lis2dh12_enable_int2(itf, &reg);
if (rc) {
goto err;
}
}
err:
hal_gpio_irq_release(itf->si_high_pin);
return rc;
}
/**
* Set the trigger threshold values and enable interrupts
*
* @param ptr to sensor
* @param the Sensor type
* @param Sensor type traits
*
* @return 0 on success, non-zero on failure
*/
static int
lis2dh12_sensor_set_trigger_thresh(struct sensor *sensor,
sensor_type_t type,
struct sensor_type_traits *stt)
{
int rc;
uint8_t fs;
struct sensor_itf *itf;
sensor_data_t low_thresh;
sensor_data_t high_thresh;
itf = SENSOR_GET_ITF(sensor);
if (type != SENSOR_TYPE_ACCELEROMETER) {
rc = SYS_EINVAL;
goto err;
}
memcpy(&low_thresh, &stt->stt_low_thresh, sizeof(low_thresh));
memcpy(&high_thresh, &stt->stt_high_thresh, sizeof(high_thresh));
rc = lis2dh12_get_full_scale(itf, &fs);
if (rc) {
goto err;
}
if (fs == LIS2DH12_FS_2G) {
fs = 16;
} else if (fs == LIS2DH12_FS_4G) {
fs = 32;
} else if (fs == LIS2DH12_FS_8G) {
fs = 62;
} else if (fs == LIS2DH12_FS_16G) {
fs = 186;
} else {
rc = SYS_EINVAL;
goto err;
}
/* Set low threshold and enable interrupt */
rc = lis2dh12_set_low_thresh(itf, low_thresh, fs, stt);
if (rc) {
goto err;
}
/* Set high threshold and enable interrupt */
rc = lis2dh12_set_high_thresh(itf, high_thresh, fs, stt);
if (rc) {
goto err;
}
return 0;
err:
return rc;
}
/**
* Configure the sensor
*
* @param ptr to sensor driver
* @param ptr to sensor driver config
*/
int
lis2dh12_config(struct lis2dh12 *lis2dh12, struct lis2dh12_cfg *cfg)
{
int rc;
struct sensor_itf *itf;
uint8_t chip_id;
struct sensor *sensor;
itf = SENSOR_GET_ITF(&(lis2dh12->sensor));
sensor = &(lis2dh12->sensor);
if (itf->si_type == SENSOR_ITF_SPI) {
rc = hal_spi_disable(sensor->s_itf.si_num);
if (rc) {
goto err;
}
rc = hal_spi_config(sensor->s_itf.si_num, &spi_lis2dh12_settings);
if (rc == EINVAL) {
/* If spi is already enabled, for nrf52, it returns -1, We should not
* fail if the spi is already enabled
*/
goto err;
}
rc = hal_spi_enable(sensor->s_itf.si_num);
if (rc) {
goto err;
}
}
rc = lis2dh12_get_chip_id(itf, &chip_id);
if (rc) {
goto err;
}
if (chip_id != LIS2DH12_ID) {
rc = SYS_EINVAL;
goto err;
}
rc = lis2dh12_reset(itf);
if (rc) {
goto err;
}
rc = lis2dh12_pull_up_disc(itf, cfg->lc_pull_up_disc);
if (rc) {
goto err;
}
lis2dh12->cfg.lc_pull_up_disc = cfg->lc_pull_up_disc;
rc = lis2dh12_hpf_cfg(itf, 0x00);
if (rc) {
goto err;
}
rc = lis2dh12_set_full_scale(itf, cfg->lc_fs);
if (rc) {
goto err;
}
lis2dh12->cfg.lc_fs = cfg->lc_fs;
rc = lis2dh12_set_rate(itf, cfg->lc_rate);
if (rc) {
goto err;
}
lis2dh12->cfg.lc_rate = cfg->lc_rate;
rc = lis2dh12_chan_enable(itf, LIS2DH12_CTRL_REG1_XPEN |
LIS2DH12_CTRL_REG1_YPEN |
LIS2DH12_CTRL_REG1_ZPEN);
if (rc) {
goto err;
}
rc = lis2dh12_set_self_test_mode(itf, LIS2DH12_ST_MODE_DISABLE);
if (rc) {
goto err;
}
rc = lis2dh12_set_op_mode(itf, LIS2DH12_OM_HIGH_RESOLUTION);
if (rc) {
goto err;
}
rc = lis2dh12_set_fifo_mode(itf, LIS2DH12_FIFO_M_BYPASS);
if (rc) {
goto err;
}
rc = sensor_set_type_mask(&(lis2dh12->sensor), cfg->lc_s_mask);
if (rc) {
goto err;
}
lis2dh12->cfg.lc_s_mask = cfg->lc_s_mask;
return 0;
err:
return rc;
}