Google Git
Sign in
apache / mynewt-documentation / 94c47ae2bc67c54a68d227abbb47c27ae97cc232 / . / versions / v1_4_0 / mynewt-core / hw / drivers / sensors / lis2ds12 / src / lis2ds12.c
blob: 198ed8fb9abc94c2c9341fad2248e69f0767cb7d [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 "lis2ds12/lis2ds12.h"
#include "lis2ds12_priv.h"
#include "hal/hal_gpio.h"
#include "modlog/modlog.h"
#include "stats/stats.h"
#include <syscfg/syscfg.h>
/*
* Max time to wait for interrupt.
*/
#define LIS2DS12_MAX_INT_WAIT (4 * OS_TICKS_PER_SEC)
#define LIS2DS12_ST_NUM_READINGS 5
//SLEEP_CHG and SLEEP_STATE interrupts aren't available on int1 or int2 so dont need to be enabled
const struct lis2ds12_notif_cfg dflt_notif_cfg[] = {
{ SENSOR_EVENT_TYPE_SINGLE_TAP, 0, LIS2DS12_INT1_CFG_SINGLE_TAP },
{ SENSOR_EVENT_TYPE_DOUBLE_TAP, 0, LIS2DS12_INT1_CFG_DOUBLE_TAP },
{ SENSOR_EVENT_TYPE_FREE_FALL, 0, LIS2DS12_INT1_CFG_FF },
{ SENSOR_EVENT_TYPE_WAKEUP, 0, LIS2DS12_INT1_CFG_WU }
};
static struct hal_spi_settings spi_lis2ds12_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(lis2ds12_stat_section)
STATS_SECT_ENTRY(write_errors)
STATS_SECT_ENTRY(read_errors)
STATS_SECT_ENTRY(single_tap_notify)
STATS_SECT_ENTRY(double_tap_notify)
STATS_SECT_ENTRY(free_fall_notify)
STATS_SECT_ENTRY(sleep_notify)
STATS_SECT_ENTRY(wakeup_notify)
STATS_SECT_END
/* Define stat names for querying */
STATS_NAME_START(lis2ds12_stat_section)
STATS_NAME(lis2ds12_stat_section, write_errors)
STATS_NAME(lis2ds12_stat_section, read_errors)
STATS_NAME(lis2ds12_stat_section, single_tap_notify)
STATS_NAME(lis2ds12_stat_section, double_tap_notify)
STATS_NAME(lis2ds12_stat_section, free_fall_notify)
STATS_NAME(lis2ds12_stat_section, sleep_notify)
STATS_NAME(lis2ds12_stat_section, wakeup_notify)
STATS_NAME_END(lis2ds12_stat_section)
/* Global variable used to hold stats data */
STATS_SECT_DECL(lis2ds12_stat_section) g_lis2ds12stats;
#define LIS2DS12_LOG(lvl_, ...) \
MODLOG_ ## lvl_(MYNEWT_VAL(LIS2DS12_LOG_MODULE), __VA_ARGS__)
/* Exports for the sensor API */
static int lis2ds12_sensor_read(struct sensor *, sensor_type_t,
sensor_data_func_t, void *, uint32_t);
static int lis2ds12_sensor_get_config(struct sensor *, sensor_type_t,
struct sensor_cfg *);
static int lis2ds12_sensor_set_notification(struct sensor *,
sensor_event_type_t);
static int lis2ds12_sensor_unset_notification(struct sensor *,
sensor_event_type_t);
static int lis2ds12_sensor_handle_interrupt(struct sensor *);
static int lis2ds12_sensor_set_config(struct sensor *, void *);
static const struct sensor_driver g_lis2ds12_sensor_driver = {
.sd_read = lis2ds12_sensor_read,
.sd_set_config = lis2ds12_sensor_set_config,
.sd_get_config = lis2ds12_sensor_get_config,
.sd_set_notification = lis2ds12_sensor_set_notification,
.sd_unset_notification = lis2ds12_sensor_unset_notification,
.sd_handle_interrupt = lis2ds12_sensor_handle_interrupt
};
/**
* Write multiple length data to LIS2DS12 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
lis2ds12_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, OS_TICKS_PER_SEC / 10, 1,
MYNEWT_VAL(LIS2DS12_I2C_RETRIES));
if (rc) {
LIS2DS12_LOG(ERROR, "I2C access failed at address 0x%02X\n",
data_struct.address);
STATS_INC(g_lis2ds12stats, write_errors);
goto err;
}
return 0;
err:
return rc;
}
/**
* Write multiple length data to LIS2DS12 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
lis2ds12_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 |= LIS2DS12_SPI_READ_CMD_BIT;
}
/* 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;
LIS2DS12_LOG(ERROR, "SPI_%u register write failed addr:0x%02X\n",
itf->si_num, addr);
STATS_INC(g_lis2ds12stats, 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;
LIS2DS12_LOG(ERROR, "SPI_%u write failed addr:0x%02X:0x%02X\n",
itf->si_num, addr);
STATS_INC(g_lis2ds12stats, 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 LIS2DS12 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
lis2ds12_writelen(struct sensor_itf *itf, uint8_t addr, uint8_t *payload,
uint8_t len)
{
int rc;
if (itf->si_type == SENSOR_ITF_I2C) {
rc = lis2ds12_i2c_writelen(itf, addr, payload, len);
} else {
rc = lis2ds12_spi_writelen(itf, addr, payload, len);
}
return rc;
}
/**
* Read multiple bytes starting from specified register over i2c
*
* @param The sensor interface
* @param The register address start reading from
* @param Pointer to where the register value should be written
* @param Number of bytes to read
*
* @return 0 on success, non-zero error on failure.
*/
int
lis2ds12_i2c_readlen(struct sensor_itf *itf, uint8_t reg, uint8_t *buffer, uint8_t len)
{
int rc;
struct hal_i2c_master_data data_struct = {
.address = itf->si_addr,
.len = 1,
.buffer = &reg
};
/* Register write */
rc = i2cn_master_write(itf->si_num, &data_struct, OS_TICKS_PER_SEC / 10, 1,
MYNEWT_VAL(LIS2DS12_I2C_RETRIES));
if (rc) {
LIS2DS12_LOG(ERROR, "I2C access failed at address 0x%02X\n",
itf->si_addr);
STATS_INC(g_lis2ds12stats, write_errors);
return rc;
}
/* Read data */
data_struct.len = len;
data_struct.buffer = buffer;
rc = i2cn_master_read(itf->si_num, &data_struct, OS_TICKS_PER_SEC / 10, 1,
MYNEWT_VAL(LIS2DS12_I2C_RETRIES));
if (rc) {
LIS2DS12_LOG(ERROR, "Failed to read from 0x%02X:0x%02X\n",
itf->si_addr, reg);
STATS_INC(g_lis2ds12stats, read_errors);
}
return rc;
}
/**
* Read multiple bytes starting from specified register over SPI
*
* @param The sensor interface
* @param The register address start reading from
* @param Pointer to where the register value should be written
* @param Number of bytes to read
*
* @return 0 on success, non-zero on failure
*/
int
lis2ds12_spi_readlen(struct sensor_itf *itf, uint8_t reg, uint8_t *buffer,
uint8_t len)
{
int i;
uint16_t retval;
int rc = 0;
/* Select the device */
hal_gpio_write(itf->si_cs_pin, 0);
/* Send the address */
retval = hal_spi_tx_val(itf->si_num, reg | LIS2DS12_SPI_READ_CMD_BIT);
if (retval == 0xFFFF) {
rc = SYS_EINVAL;
LIS2DS12_LOG(ERROR, "SPI_%u register write failed addr:0x%02X\n",
itf->si_num, reg);
STATS_INC(g_lis2ds12stats, read_errors);
goto err;
}
for (i = 0; i < len; i++) {
/* Read data */
retval = hal_spi_tx_val(itf->si_num, 0);
if (retval == 0xFFFF) {
rc = SYS_EINVAL;
LIS2DS12_LOG(ERROR, "SPI_%u read failed addr:0x%02X\n",
itf->si_num, reg);
STATS_INC(g_lis2ds12stats, read_errors);
goto err;
}
buffer[i] = retval;
}
err:
/* De-select the device */
hal_gpio_write(itf->si_cs_pin, 1);
return rc;
}
/**
* Write byte to sensor over different interfaces
*
* @param The sensor interface
* @param The register address to write to
* @param The value to write
*
* @return 0 on success, non-zero on failure
*/
int
lis2ds12_write8(struct sensor_itf *itf, uint8_t reg, uint8_t value)
{
int rc;
rc = sensor_itf_lock(itf, MYNEWT_VAL(LIS2DS12_ITF_LOCK_TMO));
if (rc) {
return rc;
}
if (itf->si_type == SENSOR_ITF_I2C) {
rc = lis2ds12_i2c_writelen(itf, reg, &value, 1);
} else {
rc = lis2ds12_spi_writelen(itf, reg, &value, 1);
}
sensor_itf_unlock(itf);
return rc;
}
/**
* Read byte data from sensor over different interfaces
*
* @param The sensor interface
* @param The register address to read from
* @param Pointer to where the register value should be written
*
* @return 0 on success, non-zero on failure
*/
int
lis2ds12_read8(struct sensor_itf *itf, uint8_t reg, uint8_t *value)
{
int rc;
rc = sensor_itf_lock(itf, MYNEWT_VAL(LIS2DS12_ITF_LOCK_TMO));
if (rc) {
return rc;
}
if (itf->si_type == SENSOR_ITF_I2C) {
rc = lis2ds12_i2c_readlen(itf, reg, value, 1);
} else {
rc = lis2ds12_spi_readlen(itf, reg, value, 1);
}
sensor_itf_unlock(itf);
return rc;
}
/**
* Read multiple bytes starting from specified register over different interfaces
*
* @param The sensor interface
* @param The register address start reading from
* @param Pointer to where the register value should be written
* @param Number of bytes to read
*
* @return 0 on success, non-zero on failure
*/
int
lis2ds12_readlen(struct sensor_itf *itf, uint8_t reg, uint8_t *buffer,
uint8_t len)
{
int rc;
rc = sensor_itf_lock(itf, MYNEWT_VAL(LIS2DS12_ITF_LOCK_TMO));
if (rc) {
return rc;
}
if (itf->si_type == SENSOR_ITF_I2C) {
rc = lis2ds12_i2c_readlen(itf, reg, buffer, len);
} else {
rc = lis2ds12_spi_readlen(itf, reg, buffer, len);
}
sensor_itf_unlock(itf);
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
lis2ds12_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
lis2ds12_calc_acc_mg(float acc_ms2, int16_t *acc_mg)
{
*acc_mg = (acc_ms2 * 1000)/STANDARD_ACCEL_GRAVITY;
}
/**
* Reset lis2ds12
*
* @param The sensor interface
* @return 0 on success, non-zero on failure
*/
int
lis2ds12_reset(struct sensor_itf *itf)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG2, &reg);
if (rc) {
goto err;
}
reg |= LIS2DS12_CTRL_REG2_SOFT_RESET | LIS2DS12_CTRL_REG2_BOOT;
rc = lis2ds12_write8(itf, LIS2DS12_REG_CTRL_REG2, reg);
if (rc) {
goto err;
}
os_time_delay((OS_TICKS_PER_SEC * 6/1000) + 1);
err:
return rc;
}
/**
* Get chip ID
*
* @param sensor interface
* @param ptr to chip id to be filled up
*/
int
lis2ds12_get_chip_id(struct sensor_itf *itf, uint8_t *chip_id)
{
uint8_t reg;
int rc;
rc = lis2ds12_read8(itf, LIS2DS12_REG_WHO_AM_I, &reg);
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
lis2ds12_set_full_scale(struct sensor_itf *itf, uint8_t fs)
{
int rc;
uint8_t reg;
if (fs > LIS2DS12_FS_16G) {
LIS2DS12_LOG(ERROR, "Invalid full scale value\n");
rc = SYS_EINVAL;
goto err;
}
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG1, &reg);
if (rc) {
goto err;
}
reg &= ~LIS2DS12_CTRL_REG1_FS;
reg |= (fs & LIS2DS12_CTRL_REG1_FS);
rc = lis2ds12_write8(itf, LIS2DS12_REG_CTRL_REG1, reg);
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
lis2ds12_get_full_scale(struct sensor_itf *itf, uint8_t *fs)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG1, &reg);
if (rc) {
goto err;
}
*fs = reg & LIS2DS12_CTRL_REG1_FS;
return 0;
err:
return rc;
}
/**
* Sets the rate
*
* @param The sensor interface
* @param The rate
*
* @return 0 on success, non-zero on failure
*/
int
lis2ds12_set_rate(struct sensor_itf *itf, uint8_t rate)
{
int rc;
uint8_t reg;
// TODO probably not the best check for me
if (rate > LIS2DS12_DATA_RATE_LP_10BIT_400HZ) {
LIS2DS12_LOG(ERROR, "Invalid rate value\n");
rc = SYS_EINVAL;
goto err;
}
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG1, &reg);
if (rc) {
goto err;
}
// Setting power along with rate
reg &= ~(LIS2DS12_CTRL_REG1_ODR | LIS2DS12_CTRL_REG1_HF_ODR);
reg |= (rate & (LIS2DS12_CTRL_REG1_ODR | LIS2DS12_CTRL_REG1_HF_ODR));
rc = lis2ds12_write8(itf, LIS2DS12_REG_CTRL_REG1, reg);
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
lis2ds12_get_rate(struct sensor_itf *itf, uint8_t *rate)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG1, &reg);
if (rc) {
goto err;
}
*rate = reg & (LIS2DS12_CTRL_REG1_ODR | LIS2DS12_CTRL_REG1_HF_ODR);
return 0;
err:
return rc;
}
/**
* Sets the self test mode of the sensor
*
* @param The sensor interface
* @param self test mode
*
* @return 0 on success, non-zero on failure
*/
int
lis2ds12_set_self_test(struct sensor_itf *itf, uint8_t mode)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG3, &reg);
if (rc) {
goto err;
}
reg &= ~LIS2DS12_CTRL_REG3_ST_MODE;
reg |= (mode & LIS2DS12_CTRL_REG3_ST_MODE);
rc = lis2ds12_write8(itf, LIS2DS12_REG_CTRL_REG3, reg);
if (rc) {
goto err;
}
return 0;
err:
return rc;
}
/**
* Gets the self test mode of the sensor
*
* @param The sensor interface
* @param ptr to self test mode read from sensor
*
* @return 0 on success, non-zero on failure
*/
int
lis2ds12_get_self_test(struct sensor_itf *itf, uint8_t *mode)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG3, &reg);
if (rc) {
goto err;
}
*mode = reg & LIS2DS12_CTRL_REG3_ST_MODE;
return 0;
err:
return rc;
}
/**
* Sets the interrupt push-pull/open-drain selection
*
* @param The sensor interface
* @param interrupt setting (0 = push-pull, 1 = open-drain)
*
* @return 0 on success, non-zero on failure
*/
int
lis2ds12_set_int_pp_od(struct sensor_itf *itf, uint8_t mode)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG3, &reg);
if (rc) {
return rc;
}
reg &= ~LIS2DS12_CTRL_REG3_PP_OD;
reg |= mode ? LIS2DS12_CTRL_REG3_PP_OD : 0;
return lis2ds12_write8(itf, LIS2DS12_REG_CTRL_REG3, reg);
}
/**
* Gets the interrupt push-pull/open-drain selection
*
* @param The sensor interface
* @param ptr to store setting (0 = push-pull, 1 = open-drain)
*
* @return 0 on success, non-zero on failure
*/
int
lis2ds12_get_int_pp_od(struct sensor_itf *itf, uint8_t *mode)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG3, &reg);
if (rc) {
return rc;
}
*mode = (reg & LIS2DS12_CTRL_REG3_PP_OD) ? 1 : 0;
return 0;
}
/**
* Sets whether latched interrupts are enabled
*
* @param The sensor interface
* @param value to set (0 = not latched, 1 = latched)
*
* @return 0 on success, non-zero on failure
*/
int
lis2ds12_set_latched_int(struct sensor_itf *itf, uint8_t en)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG3, &reg);
if (rc) {
return rc;
}
reg &= ~LIS2DS12_CTRL_REG3_LIR;
reg |= en ? LIS2DS12_CTRL_REG3_LIR : 0;
return lis2ds12_write8(itf, LIS2DS12_REG_CTRL_REG3, reg);
}
/**
* Gets whether latched interrupts are enabled
*
* @param The sensor interface
* @param ptr to store value (0 = not latched, 1 = latched)
*
* @return 0 on success, non-zero on failure
*/
int
lis2ds12_get_latched_int(struct sensor_itf *itf, uint8_t *en)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG3, &reg);
if (rc) {
return rc;
}
*en = (reg & LIS2DS12_CTRL_REG3_LIR) ? 1 : 0;
return 0;
}
/**
* Sets whether interrupts are active high or low
*
* @param The sensor interface
* @param value to set (0 = active high, 1 = active low)
*
* @return 0 on success, non-zero on failure
*/
int
lis2ds12_set_int_active_low(struct sensor_itf *itf, uint8_t low)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG3, &reg);
if (rc) {
return rc;
}
reg &= ~LIS2DS12_CTRL_REG3_H_LACTIVE;
reg |= low ? LIS2DS12_CTRL_REG3_H_LACTIVE : 0;
return lis2ds12_write8(itf, LIS2DS12_REG_CTRL_REG3, reg);
}
/**
* Gets whether interrupts are active high or low
*
* @param The sensor interface
* @param ptr to store value (0 = active high, 1 = active low)
*
* @return 0 on success, non-zero on failure
*/
int
lis2ds12_get_int_active_low(struct sensor_itf *itf, uint8_t *low)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG3, &reg);
if (rc) {
return rc;
}
*low = (reg & LIS2DS12_CTRL_REG3_H_LACTIVE) ? 1 : 0;
return 0;
}
/**
* Set filter config
*
* @param the sensor interface
* @param filter type (1 = high pass, 0 = low pass)
* @return 0 on success, non-zero on failure
*/
int
lis2ds12_set_filter_cfg(struct sensor_itf *itf, uint8_t type)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG2, &reg);
if (rc) {
goto err;
}
reg &= ~LIS2DS12_CTRL_REG2_FDS_SLOPE;
if (type) {
reg |= LIS2DS12_CTRL_REG2_FDS_SLOPE;
}
rc = lis2ds12_write8(itf, LIS2DS12_REG_CTRL_REG2, reg);
if (rc) {
goto err;
}
return 0;
err:
return rc;
}
/**
* Get filter config
*
* @param the sensor interface
* @param ptr to filter type (1 = high pass, 0 = low pass)
* @return 0 on success, non-zero on failure
*/
int
lis2ds12_get_filter_cfg(struct sensor_itf *itf, uint8_t *type)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG2, &reg);
if (rc) {
goto err;
}
*type = (reg & LIS2DS12_CTRL_REG2_FDS_SLOPE) > 0;
return 0;
err:
return rc;
}
/**
* Set tap detection configuration
*
* @param the sensor interface
* @param the tap settings
* @return 0 on success, non-zero on failure
*/
int lis2ds12_set_tap_cfg(struct sensor_itf *itf, struct lis2ds12_tap_settings *cfg)
{
int rc;
uint8_t reg;
reg = cfg->en_4d ? LIS2DS12_TAP_6D_THS_4D_EN : 0;
reg |= (cfg->ths_6d & 0x3) << 5;
reg |= cfg->tap_ths & LIS2DS12_TAP_6D_THS_TAP_THS;
rc = lis2ds12_write8(itf, LIS2DS12_REG_TAP_6D_THS, reg);
if (rc) {
return rc;
}
reg = 0;
reg |= cfg->en_x ? LIS2DS12_CTRL_REG3_TAP_X_EN : 0;
reg |= cfg->en_y ? LIS2DS12_CTRL_REG3_TAP_Y_EN : 0;
reg |= cfg->en_z ? LIS2DS12_CTRL_REG3_TAP_Z_EN : 0;
rc = lis2ds12_write8(itf, LIS2DS12_REG_CTRL_REG3, reg);
if (rc) {
return rc;
}
reg = 0;
reg |= (cfg->latency & 0xf) << 4;
reg |= (cfg->quiet & 0x3) << 2;
reg |= cfg->shock & LIS2DS12_INT_DUR_SHOCK;
return lis2ds12_write8(itf, LIS2DS12_REG_INT_DUR, reg);
}
/**
* Get tap detection config
*
* @param the sensor interface
* @param ptr to the tap settings
* @return 0 on success, non-zero on failure
*/
int lis2ds12_get_tap_cfg(struct sensor_itf *itf, struct lis2ds12_tap_settings *cfg)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_TAP_6D_THS, &reg);
if (rc) {
return rc;
}
cfg->en_4d = (reg & LIS2DS12_TAP_6D_THS_4D_EN) > 0;
cfg->ths_6d = (reg & LIS2DS12_TAP_6D_THS_6D_THS) >> 5;
cfg->tap_ths = reg & LIS2DS12_TAP_6D_THS_TAP_THS;
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG3, &reg);
if (rc) {
return rc;
}
cfg->en_x = (reg & LIS2DS12_CTRL_REG3_TAP_X_EN) > 0;
cfg->en_y = (reg & LIS2DS12_CTRL_REG3_TAP_Y_EN) > 0;
cfg->en_z = (reg & LIS2DS12_CTRL_REG3_TAP_Z_EN) > 0;
rc = lis2ds12_read8(itf, LIS2DS12_REG_INT_DUR, &reg);
if (rc) {
return rc;
}
cfg->latency = (reg & LIS2DS12_INT_DUR_LATENCY) >> 4;
cfg->quiet = (reg & LIS2DS12_INT_DUR_QUIET) >> 2;
cfg->shock = reg & LIS2DS12_INT_DUR_SHOCK;
return 0;
}
/**
* Set freefall detection configuration
*
* @param the sensor interface
* @param freefall duration (6 bits LSB = 1/ODR)
* @param freefall threshold (3 bits)
* @return 0 on success, non-zero on failure
*/
int lis2ds12_set_freefall(struct sensor_itf *itf, uint8_t dur, uint8_t ths)
{
int rc;
uint8_t reg;
reg = 0;
reg |= (dur & 0x1F) << 3;
reg |= ths & LIS2DS12_FREEFALL_THS;
rc = lis2ds12_write8(itf, LIS2DS12_REG_FREEFALL, reg);
if (rc) {
return rc;
}
rc = lis2ds12_read8(itf, LIS2DS12_REG_WAKE_UP_DUR, &reg);
if (rc) {
return rc;
}
reg &= ~LIS2DS12_WAKE_DUR_FF_DUR;
reg |= dur & 0x20 ? LIS2DS12_WAKE_DUR_FF_DUR : 0;
return lis2ds12_write8(itf, LIS2DS12_REG_WAKE_UP_DUR, reg);
}
/**
* Get freefall detection config
*
* @param the sensor interface
* @param ptr to freefall duration
* @param ptr to freefall threshold
* @return 0 on success, non-zero on failure
*/
int lis2ds12_get_freefall(struct sensor_itf *itf, uint8_t *dur, uint8_t *ths)
{
int rc;
uint8_t ff_reg, wake_reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_FREEFALL, &ff_reg);
if (rc) {
return rc;
}
rc = lis2ds12_read8(itf, LIS2DS12_REG_WAKE_UP_DUR, &wake_reg);
if (rc) {
return rc;
}
*dur = (ff_reg & LIS2DS12_FREEFALL_DUR) >> 3;
*dur |= wake_reg & LIS2DS12_WAKE_DUR_FF_DUR ? (1 << 5) : 0;
*ths = ff_reg & LIS2DS12_FREEFALL_THS;
return 0;
}
/**
* Setup FIFO
*
* @param the sensor interface
* @param FIFO mode to setup
* @param Threshold to set for FIFO
* @return 0 on success, non-zero on failure
*/
int lis2ds12_set_fifo_cfg(struct sensor_itf *itf, enum lis2ds12_fifo_mode mode, uint8_t fifo_ths)
{
int rc;
rc = lis2ds12_write8(itf, LIS2DS12_REG_FIFO_THS, fifo_ths);
if (rc != 0) {
goto err;
}
rc = lis2ds12_write8(itf, LIS2DS12_REG_FIFO_CTRL, (mode & 0x7) << 5);
if (rc != 0) {
goto err;
}
return 0;
err:
return rc;
}
/**
* Get Number of Samples in FIFO
*
* @param the sensor interface
* @param Pointer to return number of samples in, 0 empty, 256 for full
* @return 0 on success, non-zero on failure
*/
int lis2ds12_get_fifo_samples(struct sensor_itf *itf, uint16_t *samples)
{
uint8_t low, high;
int rc;
rc = lis2ds12_read8(itf, LIS2DS12_REG_FIFO_SAMPLES, &low);
if (rc) {
return rc;
}
rc = lis2ds12_read8(itf, LIS2DS12_REG_FIFO_SRC, &high);
if (rc) {
return rc;
}
*samples = low;
*samples |= high & LIS2DS12_FIFO_SRC_DIFF8 ? (1 << 8) : 0;
return 0;
}
/**
* Clear interrupt pin configuration for interrupt 1
*
* @param the sensor interface
* @param config
* @return 0 on success, non-zero on failure
*/
int
lis2ds12_clear_int1_pin_cfg(struct sensor_itf *itf, uint8_t cfg)
{
int rc;
uint8_t reg;
reg = 0;
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG4, &reg);
if (rc) {
goto err;
}
reg &= ~cfg;
rc = lis2ds12_write8(itf, LIS2DS12_REG_CTRL_REG4, reg);
err:
return rc;
}
/**
* Clear interrupt pin configuration for interrupt 2
*
* @param the sensor interface
* @param config
* @return 0 on success, non-zero on failure
*/
int
lis2ds12_clear_int2_pin_cfg(struct sensor_itf *itf, uint8_t cfg)
{
int rc;
uint8_t reg;
reg = 0;
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG5, &reg);
if (rc) {
goto err;
}
reg &= ~cfg;
rc = lis2ds12_write8(itf, LIS2DS12_REG_CTRL_REG5, reg);
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
lis2ds12_set_int1_pin_cfg(struct sensor_itf *itf, uint8_t cfg)
{
int rc;
uint8_t reg;
reg = 0;
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG4, &reg);
if (rc) {
goto err;
}
reg |= cfg;
rc = lis2ds12_write8(itf, LIS2DS12_REG_CTRL_REG4, reg);
err:
return rc;
}
/**
* Set interrupt pin configuration for interrupt 2
*
* @param the sensor interface
* @param config
* @return 0 on success, non-zero on failure
*/
int
lis2ds12_set_int2_pin_cfg(struct sensor_itf *itf, uint8_t cfg)
{
int rc;
uint8_t reg;
reg = 0;
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG5, &reg);
if (rc) {
goto err;
}
reg |= cfg;
rc = lis2ds12_write8(itf, LIS2DS12_REG_CTRL_REG5, reg);
err:
return rc;
}
/**
* Set Wake Up Threshold configuration
*
* @param the sensor interface
* @param wake_up_ths value to set
* @return 0 on success, non-zero on failure
*/
int lis2ds12_set_wake_up_ths(struct sensor_itf *itf, uint8_t val)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_WAKE_UP_THS, &reg);
if (rc) {
return rc;
}
reg &= ~LIS2DS12_WAKE_THS_THS;
reg |= val & LIS2DS12_WAKE_THS_THS;
return lis2ds12_write8(itf, LIS2DS12_REG_WAKE_UP_THS, reg);
}
/**
* Get Wake Up Threshold config
*
* @param the sensor interface
* @param ptr to store wake_up_ths value
* @return 0 on success, non-zero on failure
*/
int lis2ds12_get_wake_up_ths(struct sensor_itf *itf, uint8_t *val)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_WAKE_UP_THS, &reg);
if (rc) {
return rc;
}
*val = reg & LIS2DS12_WAKE_THS_THS;
return 0;
}
/**
* Set whether sleep on inactivity is enabled
*
* @param the sensor interface
* @param value to set (0 = disabled, 1 = enabled)
* @return 0 on success, non-zero on failure
*/
int lis2ds12_set_inactivity_sleep_en(struct sensor_itf *itf, uint8_t en)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_WAKE_UP_THS, &reg);
if (rc) {
return rc;
}
reg &= ~LIS2DS12_WAKE_THS_SLEEP_ON;
reg |= en ? LIS2DS12_WAKE_THS_SLEEP_ON : 0;
return lis2ds12_write8(itf, LIS2DS12_REG_WAKE_UP_THS, reg);
}
/**
* Get whether sleep on inactivity is enabled
*
* @param the sensor interface
* @param ptr to store read state (0 = disabled, 1 = enabled)
* @return 0 on success, non-zero on failure
*/
int lis2ds12_get_inactivity_sleep_en(struct sensor_itf *itf, uint8_t *en)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_WAKE_UP_THS, &reg);
if (rc) {
return rc;
}
*en = (reg & LIS2DS12_WAKE_THS_SLEEP_ON) ? 1 : 0;
return 0;
}
/**
* Set whether double tap event is enabled
*
* @param the sensor interface
* @param value to set (0 = disabled, 1 = enabled)
* @return 0 on success, non-zero on failure
*/
int lis2ds12_set_double_tap_event_en(struct sensor_itf *itf, uint8_t en)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_WAKE_UP_THS, &reg);
if (rc) {
return rc;
}
reg &= ~LIS2DS12_WAKE_THS_SINGLE_DOUBLE_TAP;
reg |= en ? LIS2DS12_WAKE_THS_SINGLE_DOUBLE_TAP : en;
return lis2ds12_write8(itf, LIS2DS12_REG_WAKE_UP_THS, reg);
}
/**
* Get whether double tap event is enabled
*
* @param the sensor interface
* @param ptr to store read state (0 = disabled, 1 = enabled)
* @return 0 on success, non-zero on failure
*/
int lis2ds12_get_double_tap_event_en(struct sensor_itf *itf, uint8_t *en)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_WAKE_UP_THS, &reg);
if (rc) {
return rc;
}
*en = (reg & LIS2DS12_WAKE_THS_SINGLE_DOUBLE_TAP) ? 1 : 0;
return 0;
}
/**
* Set Wake Up Duration
*
* @param the sensor interface
* @param value to set
* @return 0 on success, non-zero on failure
*/
int lis2ds12_set_wake_up_dur(struct sensor_itf *itf, uint8_t val)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_WAKE_UP_DUR, &reg);
if (rc) {
return rc;
}
reg &= ~LIS2DS12_WAKE_DUR_DUR;
reg |= (val & LIS2DS12_WAKE_DUR_DUR) << 5;
return lis2ds12_write8(itf, LIS2DS12_REG_WAKE_UP_DUR, reg);
}
/**
* Get Wake Up Duration
*
* @param the sensor interface
* @param ptr to store wake_up_dur value
* @return 0 on success, non-zero on failure
*/
int lis2ds12_get_wake_up_dur(struct sensor_itf *itf, uint8_t *val)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_WAKE_UP_DUR, &reg);
if (rc) {
return rc;
}
*val = (reg & LIS2DS12_WAKE_DUR_DUR) >> 5;
return 0;
}
/**
* Set Sleep Duration
*
* @param the sensor interface
* @param value to set
* @return 0 on success, non-zero on failure
*/
int lis2ds12_set_sleep_dur(struct sensor_itf *itf, uint8_t val)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_WAKE_UP_DUR, &reg);
if (rc) {
return rc;
}
reg &= ~LIS2DS12_WAKE_DUR_SLEEP_DUR;
reg |= (val & LIS2DS12_WAKE_DUR_SLEEP_DUR);
return lis2ds12_write8(itf, LIS2DS12_REG_WAKE_UP_DUR, reg);
}
/**
* Get Sleep Duration
*
* @param the sensor interface
* @param ptr to store sleep_dur value
* @return 0 on success, non-zero on failure
*/
int lis2ds12_get_sleep_dur(struct sensor_itf *itf, uint8_t *val)
{
int rc;
uint8_t reg;
rc = lis2ds12_read8(itf, LIS2DS12_REG_WAKE_UP_DUR, &reg);
if (rc) {
return rc;
}
*val = reg & LIS2DS12_WAKE_DUR_SLEEP_DUR;
return 0;
}
/**
* Clear all interrupts by reading all four interrupt registers status
*
* @param itf The sensor interface
* @param src Ptr to return 4 interrupt sources in
* @return 0 on success, non-zero on failure
*/
int
lis2ds12_clear_int(struct sensor_itf *itf, uint8_t *int_src)
{
return lis2ds12_readlen(itf, LIS2DS12_REG_STATUS_DUP, int_src, 4);
}
/**
* Get Interrupt Status
*
* @param the sensor interface
* @param pointer to return interrupt status in
* @return 0 on success, non-zero on failure
*/
int lis2ds12_get_int_status(struct sensor_itf *itf, uint8_t *status)
{
return lis2ds12_read8(itf, LIS2DS12_REG_STATUS, status);
}
/**
* Get Wake Up Source
*
* @param the sensor interface
* @param pointer to return wake_up_src in
* @return 0 on success, non-zero on failure
*/
int lis2ds12_get_wake_up_src(struct sensor_itf *itf, uint8_t *status)
{
return lis2ds12_read8(itf, LIS2DS12_REG_WAKE_UP_SRC, status);
}
/**
* Get Tap Source
*
* @param the sensor interface
* @param pointer to return tap_src in
* @return 0 on success, non-zero on failure
*/
int lis2ds12_get_tap_src(struct sensor_itf *itf, uint8_t *status)
{
return lis2ds12_read8(itf, LIS2DS12_REG_TAP_SRC, status);
}
/**
* Get 6D Source
*
* @param the sensor interface
* @param pointer to return sixd_src in
* @return 0 on success, non-zero on failure
*/
int lis2ds12_get_sixd_src(struct sensor_itf *itf, uint8_t *status)
{
return lis2ds12_read8(itf, LIS2DS12_REG_6D_SRC, status);
}
/**
* Set whether interrupt 2 signals is mapped onto interrupt 1 pin
*
* @param the sensor interface
* @param value to set (false = disabled, true = enabled)
* @return 0 on success, non-zero on failure
*/
int lis2ds12_set_int2_on_int1_map(struct sensor_itf *itf, bool enable)
{
uint8_t reg;
int rc;
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG5, &reg);
if (rc) {
return rc;
}
if (enable) {
reg |= LIS2DS12_CTRL_REG5_INT2_ON_INT1;
} else {
reg &= ~LIS2DS12_CTRL_REG5_INT2_ON_INT1;
}
return lis2ds12_write8(itf, LIS2DS12_REG_CTRL_REG5, reg);
}
/**
* Get whether interrupt 2 signals is mapped onto interrupt 1 pin
*
* @param the sensor interface
* @param value to set (0 = disabled, 1 = enabled)
* @return 0 on success, non-zero on failure
*/
int lis2ds12_get_int2_on_int1_map(struct sensor_itf *itf, uint8_t *val)
{
uint8_t reg;
int rc;
rc = lis2ds12_read8(itf, LIS2DS12_REG_CTRL_REG5, &reg);
if (rc) {
return rc;
}
*val = (reg & LIS2DS12_CTRL_REG5_INT2_ON_INT1) >> 5;
return 0;
}
/**
* Run Self test on sensor
*
* @param the sensor interface
* @param pointer to return test result in (0 on pass, non-zero on failure)
*
* @return 0 on sucess, non-zero on failure
*/
int lis2ds12_run_self_test(struct sensor_itf *itf, int *result)
{
int rc;
int16_t no_st[3], st[3], data[3] = {0,0,0};
int32_t scratch[3] = {0,0,0};
int i;
uint8_t prev_config[6];
*result = 0;
uint8_t config[6] = { LIS2DS12_DATA_RATE_HR_14BIT_50HZ | LIS2DS12_FS_2G | LIS2DS12_CTRL_REG1_BDU, LIS2DS12_CTRL_REG2_IF_ADD_INC, 0, 0, 0, 0};
uint8_t fs;
rc = lis2ds12_readlen(itf, LIS2DS12_REG_CTRL_REG1, prev_config, 6);
if (rc) {
return rc;
}
rc = lis2ds12_writelen(itf, LIS2DS12_REG_CTRL_REG2, &config[1], 5);
if (rc) {
return rc;
}
rc = lis2ds12_write8(itf, LIS2DS12_REG_CTRL_REG1, config[0]);
if (rc) {
return rc;
}
/* wait 200ms */
os_time_delay(OS_TICKS_PER_SEC / 5 + 1);
rc = lis2ds12_get_fs(itf, &fs);
if (rc) {
return rc;
}
//discard
//TODO poll DRDY in STATUS (27h) instead?
rc = lis2ds12_get_data(itf, fs, &(data[0]), &(data[1]), &(data[2]));
if (rc) {
return rc;
}
/* take no st offset reading */
for(i=0; i<LIS2DS12_ST_NUM_READINGS; i++) {
// TODO poll DRDY in STATUS (27h) instead?
/* wait at least 20 ms */
os_time_delay(OS_TICKS_PER_SEC / 50 + 1);
rc = lis2ds12_get_data(itf, fs, &(data[0]), &(data[1]), &(data[2]));
if (rc) {
return rc;
}
scratch[0] += data[0];
scratch[1] += data[1];
scratch[2] += data[2];
}
//average
no_st[0] = scratch[0]/LIS2DS12_ST_NUM_READINGS;
no_st[1] = scratch[1]/LIS2DS12_ST_NUM_READINGS;
no_st[2] = scratch[2]/LIS2DS12_ST_NUM_READINGS;
//clean scratch
memset(&scratch, 0, sizeof scratch);
/* go into self test mode 1 */
rc = lis2ds12_set_self_test(itf, LIS2DS12_ST_MODE_MODE1);
if (rc) {
return rc;
}
/* wait 200ms */
os_time_delay(OS_TICKS_PER_SEC / 5 + 1);
//discard
//TODO poll DRDY in STATUS (27h) instead?
rc = lis2ds12_get_data(itf, fs, &(data[0]), &(data[1]), &(data[2]));
if (rc) {
return rc;
}
/* take positive offset reading */
for(i=0; i<LIS2DS12_ST_NUM_READINGS; i++) {
// TODO poll DRDY in STATUS (27h) instead?
/* wait at least 20 ms */
os_time_delay(OS_TICKS_PER_SEC / 50 + 1);
rc = lis2ds12_get_data(itf, fs, &(data[0]), &(data[1]), &(data[2]));
if (rc) {
return rc;
}
scratch[0] += data[0];
scratch[1] += data[1];
scratch[2] += data[2];
}
//average
st[0] = scratch[0]/LIS2DS12_ST_NUM_READINGS;
st[1] = scratch[1]/LIS2DS12_ST_NUM_READINGS;
st[2] = scratch[2]/LIS2DS12_ST_NUM_READINGS;
//clean scratch
memset(&scratch, 0, sizeof scratch);
// |Min(ST_X)| <=|OUTX_AVG_ST - OUTX_AVG_NO_ST| <= |Max(ST_X)|
/* compare values to thresholds */
for (i = 0; i < 3; i++) {
int16_t diff = abs(st[i] - no_st[i]);
if (diff < LIS2DS12_ST_MIN || diff > LIS2DS12_ST_MAX) {
*result -= 1;
}
}
/* go into self test mode 2 */
rc = lis2ds12_set_self_test(itf, LIS2DS12_ST_MODE_MODE2);
if (rc) {
return rc;
}
/* wait 200ms */
os_time_delay(OS_TICKS_PER_SEC / 5 + 1);
//discard
rc = lis2ds12_get_data(itf, fs, &(data[0]), &(data[1]), &(data[2]));
if (rc) {
return rc;
}
/* take negative offset reading */
for(i=0; i<LIS2DS12_ST_NUM_READINGS; i++) {
// TODO poll DRDY in STATUS (27h) instead?
/* wait at least 20 ms */
os_time_delay(OS_TICKS_PER_SEC / 50 + 1);
rc = lis2ds12_get_data(itf, fs, &(data[0]), &(data[1]), &(data[2]));
if (rc) {
return rc;
}
scratch[0] += data[0];
scratch[1] += data[1];
scratch[2] += data[2];
}
//average
st[0] = scratch[0]/LIS2DS12_ST_NUM_READINGS;
st[1] = scratch[1]/LIS2DS12_ST_NUM_READINGS;
st[2] = scratch[2]/LIS2DS12_ST_NUM_READINGS;
/* compare values to thresholds */
for (i = 0; i < 3; i++) {
int16_t diff = abs(st[i] - no_st[i]);
if (diff < LIS2DS12_ST_MIN || diff > LIS2DS12_ST_MAX) {
*result -= 1;
}
}
/* disable self test mode */
rc = lis2ds12_writelen(itf, LIS2DS12_REG_CTRL_REG1, prev_config, 6);
if (rc) {
return rc;
}
/* wait 200ms */
os_time_delay(OS_TICKS_PER_SEC / 5 + 1);
return 0;
}
static void
init_interrupt(struct lis2ds12_int *interrupt, struct sensor_int *ints)
{
os_error_t error;
error = os_sem_init(&interrupt->wait, 0);
assert(error == OS_OK);
interrupt->active = false;
interrupt->asleep = false;
interrupt->ints = ints;
}
static void
undo_interrupt(struct lis2ds12_int * interrupt)
{
OS_ENTER_CRITICAL(interrupt->lock);
interrupt->active = false;
interrupt->asleep = false;
OS_EXIT_CRITICAL(interrupt->lock);
}
static int
wait_interrupt(struct lis2ds12_int *interrupt, uint8_t int_num)
{
bool wait;
os_error_t error;
OS_ENTER_CRITICAL(interrupt->lock);
/* Check if we did not missed interrupt */
if (hal_gpio_read(interrupt->ints[int_num].host_pin) ==
interrupt->ints[int_num].active) {
OS_EXIT_CRITICAL(interrupt->lock);
return OS_OK;
}
if (interrupt->active) {
interrupt->active = false;
wait = false;
} else {
interrupt->asleep = true;
wait = true;
}
OS_EXIT_CRITICAL(interrupt->lock);
if (wait) {
error = os_sem_pend(&interrupt->wait, LIS2DS12_MAX_INT_WAIT);
if (error == OS_TIMEOUT) {
return error;
}
assert(error == OS_OK);
}
return OS_OK;
}
static void
wake_interrupt(struct lis2ds12_int *interrupt)
{
bool wake;
OS_ENTER_CRITICAL(interrupt->lock);
if (interrupt->asleep) {
interrupt->asleep = false;
wake = true;
} else {
interrupt->active = true;
wake = false;
}
OS_EXIT_CRITICAL(interrupt->lock);
if (wake) {
os_error_t error;
error = os_sem_release(&interrupt->wait);
assert(error == OS_OK);
}
}
static void
lis2ds12_int_irq_handler(void *arg)
{
struct sensor *sensor = arg;
struct lis2ds12 *lis2ds12;
lis2ds12 = (struct lis2ds12 *)SENSOR_GET_DEVICE(sensor);
if(lis2ds12->pdd.interrupt) {
wake_interrupt(lis2ds12->pdd.interrupt);
}
sensor_mgr_put_interrupt_evt(sensor);
}
static int
init_intpin(struct lis2ds12 *lis2ds12, hal_gpio_irq_handler_t handler,
void * arg)
{
hal_gpio_irq_trig_t trig;
int pin = -1;
int rc;
int i;
for (i = 0; i < MYNEWT_VAL(SENSOR_MAX_INTERRUPTS_PINS); i++){
pin = lis2ds12->sensor.s_itf.si_ints[i].host_pin;
if (pin >= 0) {
break;
}
}
if (pin < 0) {
LIS2DS12_LOG(ERROR, "Interrupt pin not configured\n");
return SYS_EINVAL;
}
if (lis2ds12->sensor.s_itf.si_ints[i].active) {
trig = HAL_GPIO_TRIG_RISING;
} else {
trig = HAL_GPIO_TRIG_FALLING;
}
rc = hal_gpio_irq_init(pin,
handler,
arg,
trig,
HAL_GPIO_PULL_NONE);
if (rc != 0) {
LIS2DS12_LOG(ERROR, "Failed to initialise interrupt pin %d\n", pin);
return rc;
}
return 0;
}
static int
disable_interrupt(struct sensor *sensor, uint8_t int_to_disable, uint8_t int_num)
{
struct lis2ds12 *lis2ds12;
struct lis2ds12_pdd *pdd;
struct sensor_itf *itf;
int rc;
if (int_to_disable == 0) {
return SYS_EINVAL;
}
lis2ds12 = (struct lis2ds12 *)SENSOR_GET_DEVICE(sensor);
itf = SENSOR_GET_ITF(sensor);
pdd = &lis2ds12->pdd;
pdd->int_enable &= ~(int_to_disable << (int_num * 8));
/* disable int pin */
if (!pdd->int_enable) {
hal_gpio_irq_disable(itf->si_ints[int_num].host_pin);
}
/* update interrupt setup in device */
if (int_num == 0) {
rc = lis2ds12_clear_int1_pin_cfg(itf, int_to_disable);
} else {
rc = lis2ds12_clear_int2_pin_cfg(itf, int_to_disable);
}
return rc;
}
static int
enable_interrupt(struct sensor *sensor, uint8_t int_to_enable, uint8_t int_num)
{
struct lis2ds12 *lis2ds12;
struct lis2ds12_pdd *pdd;
struct sensor_itf *itf;
uint8_t int_src[4];
int rc;
if (!int_to_enable) {
rc = SYS_EINVAL;
goto err;
}
lis2ds12 = (struct lis2ds12 *)SENSOR_GET_DEVICE(sensor);
itf = SENSOR_GET_ITF(sensor);
pdd = &lis2ds12->pdd;
rc = lis2ds12_clear_int(itf, int_src);
if (rc) {
goto err;
}
/* if no interrupts are currently in use enable int pin */
if (!pdd->int_enable) {
hal_gpio_irq_enable(itf->si_ints[int_num].host_pin);
}
pdd->int_enable |= (int_to_enable << (int_num * 8));
/* enable interrupt in device */
if (int_num == 0) {
rc = lis2ds12_set_int1_pin_cfg(itf, int_to_enable);
} else {
rc = lis2ds12_set_int2_pin_cfg(itf, int_to_enable);
}
if (rc) {
disable_interrupt(sensor, int_to_enable, int_num);
goto err;
}
return 0;
err:
return rc;
}
int
lis2ds12_get_fs(struct sensor_itf *itf, uint8_t *fs)
{
int rc;
rc = lis2ds12_get_full_scale(itf, fs);
if (rc) {
return rc;
}
if (*fs == LIS2DS12_FS_2G) {
*fs = 2;
} else if (*fs == LIS2DS12_FS_4G) {
*fs = 4;
} else if (*fs == LIS2DS12_FS_8G) {
*fs = 8;
} else if (*fs == LIS2DS12_FS_16G) {
*fs = 16;
} else {
return SYS_EINVAL;
}
return 0;
}
/**
* Gets a new data sample from the 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
lis2ds12_get_data(struct sensor_itf *itf, uint8_t fs, int16_t *x, int16_t *y, int16_t *z)
{
int rc;
uint8_t payload[6] = {0};
*x = *y = *z = 0;
rc = lis2ds12_readlen(itf, LIS2DS12_REG_OUT_X_L, payload, 6);
if (rc) {
goto err;
}
*x = payload[0] | (payload[1] << 8);
*y = payload[2] | (payload[3] << 8);
*z = payload[4] | (payload[5] << 8);
/*
* 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.
*/
*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;
}
static int lis2ds12_do_read(struct sensor *sensor, sensor_data_func_t data_func,
void * data_arg, uint8_t fs)
{
struct sensor_accel_data sad;
struct sensor_itf *itf;
int16_t x, y ,z;
float fx, fy ,fz;
int rc;
itf = SENSOR_GET_ITF(sensor);
x = y = z = 0;
rc = lis2ds12_get_data(itf, fs, &x, &y, &z);
if (rc) {
goto err;
}
/* converting values from mg to ms^2 */
lis2ds12_calc_acc_ms2(x, &fx);
lis2ds12_calc_acc_ms2(y, &fy);
lis2ds12_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;
}
/**
* Do accelerometer polling reads
*
* @param The sensor ptr
* @param The sensor type
* @param The function pointer to invoke for each accelerometer reading.
* @param The opaque pointer that will be passed in to the function.
* @param If non-zero, how long the stream should run in milliseconds.
*
* @return 0 on success, non-zero on failure.
*/
int
lis2ds12_poll_read(struct sensor *sensor, sensor_type_t sensor_type,
sensor_data_func_t data_func, void *data_arg,
uint32_t timeout)
{
struct lis2ds12 *lis2ds12;
struct lis2ds12_cfg *cfg;
struct sensor_itf *itf;
uint8_t fs;
int rc;
lis2ds12 = (struct lis2ds12 *)SENSOR_GET_DEVICE(sensor);
itf = SENSOR_GET_ITF(sensor);
cfg = &lis2ds12->cfg;
/* If the read isn't looking for accel data, don't do anything. */
if (!(sensor_type & SENSOR_TYPE_ACCELEROMETER)) {
rc = SYS_EINVAL;
goto err;
}
if (cfg->read_mode.mode != LIS2DS12_READ_M_POLL) {
rc = SYS_EINVAL;
goto err;
}
rc = lis2ds12_get_fs(itf, &fs);
if (rc) {
goto err;
}
rc = lis2ds12_do_read(sensor, data_func, data_arg, fs);
if (rc) {
goto err;
}
return 0;
err:
return rc;
}
int
lis2ds12_stream_read(struct sensor *sensor,
sensor_type_t sensor_type,
sensor_data_func_t read_func,
void *read_arg,
uint32_t time_ms)
{
struct lis2ds12_pdd *pdd;
struct lis2ds12 *lis2ds12;
struct sensor_itf *itf;
struct lis2ds12_cfg *cfg;
os_time_t time_ticks;
os_time_t stop_ticks = 0;
uint16_t fifo_samples;
uint8_t fs;
int rc, rc2;
/* If the read isn't looking for accel data, don't do anything. */
if (!(sensor_type & SENSOR_TYPE_ACCELEROMETER)) {
return SYS_EINVAL;
}
lis2ds12 = (struct lis2ds12 *)SENSOR_GET_DEVICE(sensor);
itf = SENSOR_GET_ITF(sensor);
pdd = &lis2ds12->pdd;
cfg = &lis2ds12->cfg;
if (cfg->read_mode.mode != LIS2DS12_READ_M_STREAM) {
return SYS_EINVAL;
}
undo_interrupt(&lis2ds12->intr);
if (pdd->interrupt) {
return SYS_EBUSY;
}
/* enable interrupt */
pdd->interrupt = &lis2ds12->intr;
rc = enable_interrupt(sensor, cfg->read_mode.int_cfg,
cfg->read_mode.int_num);
if (rc) {
return rc;
}
if (time_ms != 0) {
rc = os_time_ms_to_ticks(time_ms, &time_ticks);
if (rc) {
goto err;
}
stop_ticks = os_time_get() + time_ticks;
}
rc = lis2ds12_get_fs(itf, &fs);
if (rc) {
goto err;
}
for (;;) {
/* force at least one read for cases when fifo is disabled */
rc = wait_interrupt(&lis2ds12->intr, cfg->read_mode.int_num);
if (rc) {
goto err;
}
fifo_samples = 1;
while(fifo_samples > 0) {
/* read all data we beleive is currently in fifo */
while(fifo_samples > 0) {
rc = lis2ds12_do_read(sensor, read_func, read_arg, fs);
if (rc) {
goto err;
}
fifo_samples--;
}
/* check if any data is available in fifo */
rc = lis2ds12_get_fifo_samples(itf, &fifo_samples);
if (rc) {
goto err;
}
}
if (time_ms != 0 && OS_TIME_TICK_GT(os_time_get(), stop_ticks)) {
break;
}
}
err:
/* disable interrupt */
pdd->interrupt = NULL;
rc2 = disable_interrupt(sensor, cfg->read_mode.int_cfg,
cfg->read_mode.int_num);
if (rc) {
return rc;
} else {
return rc2;
}
}
static int
lis2ds12_sensor_read(struct sensor *sensor, sensor_type_t type,
sensor_data_func_t data_func, void *data_arg, uint32_t timeout)
{
int rc;
const struct lis2ds12_cfg *cfg;
struct lis2ds12 *lis2ds12;
struct sensor_itf *itf;
/* If the read isn't looking for accel data, don't do anything. */
if (!(type & SENSOR_TYPE_ACCELEROMETER)) {
rc = SYS_EINVAL;
goto err;
}
itf = SENSOR_GET_ITF(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_lis2ds12_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;
}
}
lis2ds12 = (struct lis2ds12 *)SENSOR_GET_DEVICE(sensor);
cfg = &lis2ds12->cfg;
if (cfg->read_mode.mode == LIS2DS12_READ_M_POLL) {
rc = lis2ds12_poll_read(sensor, type, data_func, data_arg, timeout);
} else {
rc = lis2ds12_stream_read(sensor, type, data_func, data_arg, timeout);
}
err:
if (rc) {
return SYS_EINVAL; /* XXX */
} else {
return SYS_EOK;
}
}
static int
lis2ds12_find_int_by_event(sensor_event_type_t event, uint8_t *int_cfg,
uint8_t *int_num, struct lis2ds12_cfg *cfg)
{
int i;
int rc;
rc = SYS_EINVAL;
*int_num = 0;
*int_cfg = 0;
if (!cfg) {
rc = SYS_EINVAL;
goto err;
}
for (i = 0; i < cfg->max_num_notif; i++) {
if (event == cfg->notif_cfg[i].event) {
*int_cfg = cfg->notif_cfg[i].int_cfg;
*int_num = cfg->notif_cfg[i].int_num;
break;
}
}
if (i == cfg->max_num_notif) {
/* here if type is set to a non valid event or more than one event
* we do not currently support registering for more than one event
* per notification
*/
rc = SYS_EINVAL;
goto err;
}
return 0;
err:
return rc;
}
static int
lis2ds12_sensor_set_notification(struct sensor *sensor, sensor_event_type_t event)
{
struct lis2ds12 *lis2ds12;
struct lis2ds12_pdd *pdd;
struct sensor_itf *itf;
uint8_t int_cfg;
uint8_t int_num;
int rc;
lis2ds12 = (struct lis2ds12 *)SENSOR_GET_DEVICE(sensor);
itf = SENSOR_GET_ITF(sensor);
pdd = &lis2ds12->pdd;
rc = lis2ds12_find_int_by_event(event, &int_cfg, &int_num, &lis2ds12->cfg);
if (rc) {
goto err;
}
rc = enable_interrupt(sensor, int_cfg, int_num);
if (rc) {
goto err;
}
/* enable double tap detection in wake_up_ths */
if(event == SENSOR_EVENT_TYPE_DOUBLE_TAP) {
rc = lis2ds12_set_double_tap_event_en(itf, 1);
if (rc) {
goto err;
}
}
pdd->notify_ctx.snec_evtype |= event;
return 0;
err:
return rc;
}
static int
lis2ds12_sensor_unset_notification(struct sensor *sensor, sensor_event_type_t event)
{
struct lis2ds12 *lis2ds12;
struct sensor_itf *itf;
uint8_t int_num;
uint8_t int_cfg;
int rc;
lis2ds12 = (struct lis2ds12 *)SENSOR_GET_DEVICE(sensor);
itf = SENSOR_GET_ITF(sensor);
lis2ds12->pdd.notify_ctx.snec_evtype &= ~event;
if(event == SENSOR_EVENT_TYPE_DOUBLE_TAP) {
rc = lis2ds12_set_double_tap_event_en(itf, 0);
if (rc) {
goto err;
}
}
rc = lis2ds12_find_int_by_event(event, &int_cfg, &int_num, &lis2ds12->cfg);
if (rc) {
goto err;
}
rc = disable_interrupt(sensor, int_cfg, int_num);
err:
return rc;
}
static int
lis2ds12_sensor_set_config(struct sensor *sensor, void *cfg)
{
struct lis2ds12 *lis2ds12;
lis2ds12 = (struct lis2ds12 *)SENSOR_GET_DEVICE(sensor);
return lis2ds12_config(lis2ds12, (struct lis2ds12_cfg*)cfg);
}
static int
lis2ds12_sensor_handle_interrupt(struct sensor *sensor)
{
struct lis2ds12 *lis2ds12;
struct sensor_itf *itf;
uint8_t int_src[4];
int rc;
lis2ds12 = (struct lis2ds12 *)SENSOR_GET_DEVICE(sensor);
itf = SENSOR_GET_ITF(sensor);
rc = lis2ds12_clear_int(itf, int_src);
if (rc) {
LIS2DS12_LOG(ERROR, "Could not read int src err=0x%02x\n", rc);
return rc;
}
if (int_src[0] & LIS2DS12_STATUS_STAP) {
/* Single tap is detected */
sensor_mgr_put_notify_evt(&lis2ds12->pdd.notify_ctx,
SENSOR_EVENT_TYPE_SINGLE_TAP);
STATS_INC(g_lis2ds12stats, single_tap_notify);
}
if (int_src[0] & LIS2DS12_STATUS_DTAP) {
/* Double tap is detected */
sensor_mgr_put_notify_evt(&lis2ds12->pdd.notify_ctx,
SENSOR_EVENT_TYPE_DOUBLE_TAP);
STATS_INC(g_lis2ds12stats, double_tap_notify);
}
if (int_src[0] & LIS2DS12_STATUS_FF_IA) {
/* Freefall is detected */
sensor_mgr_put_notify_evt(&lis2ds12->pdd.notify_ctx,
SENSOR_EVENT_TYPE_FREE_FALL);
STATS_INC(g_lis2ds12stats, free_fall_notify);
}
if (int_src[0] & LIS2DS12_STATUS_WU_IA) {
/* Wake up is detected */
sensor_mgr_put_notify_evt(&lis2ds12->pdd.notify_ctx,
SENSOR_EVENT_TYPE_WAKEUP);
STATS_INC(g_lis2ds12stats, wakeup_notify);
}
if (int_src[0] & LIS2DS12_STATUS_SLEEP_STATE) {
/* Sleep state detected */
sensor_mgr_put_notify_evt(&lis2ds12->pdd.notify_ctx,
SENSOR_EVENT_TYPE_SLEEP);
STATS_INC(g_lis2ds12stats, sleep_notify);
}
return 0;
}
static int
lis2ds12_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;
}
/**
* 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
lis2ds12_init(struct os_dev *dev, void *arg)
{
struct lis2ds12 *lis2ds12;
struct sensor *sensor;
int rc;
if (!arg || !dev) {
rc = SYS_ENODEV;
goto err;
}
lis2ds12 = (struct lis2ds12 *) dev;
lis2ds12->cfg.mask = SENSOR_TYPE_ALL;
sensor = &lis2ds12->sensor;
/* Initialise the stats entry */
rc = stats_init(
STATS_HDR(g_lis2ds12stats),
STATS_SIZE_INIT_PARMS(g_lis2ds12stats, STATS_SIZE_32),
STATS_NAME_INIT_PARMS(lis2ds12_stat_section));
SYSINIT_PANIC_ASSERT(rc == 0);
/* Register the entry with the stats registry */
rc = stats_register(dev->od_name, STATS_HDR(g_lis2ds12stats));
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_lis2ds12_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_lis2ds12_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;
}
}
init_interrupt(&lis2ds12->intr, lis2ds12->sensor.s_itf.si_ints);
lis2ds12->pdd.notify_ctx.snec_sensor = sensor;
lis2ds12->pdd.interrupt = NULL;
rc = init_intpin(lis2ds12, lis2ds12_int_irq_handler, sensor);
if (rc) {
return rc;
}
return 0;
err:
return rc;
}
/**
* Configure the sensor
*
* @param ptr to sensor driver
* @param ptr to sensor driver config
*/
int
lis2ds12_config(struct lis2ds12 *lis2ds12, struct lis2ds12_cfg *cfg)
{
int rc;
struct sensor_itf *itf;
uint8_t chip_id;
struct sensor *sensor;
itf = SENSOR_GET_ITF(&(lis2ds12->sensor));
sensor = &(lis2ds12->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_lis2ds12_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 = lis2ds12_get_chip_id(itf, &chip_id);
if (rc) {
goto err;
}
if (chip_id != LIS2DS12_ID) {
rc = SYS_EINVAL;
goto err;
}
rc = lis2ds12_reset(itf);
if (rc) {
goto err;
}
rc = lis2ds12_set_int_pp_od(itf, cfg->int_pp_od);
if (rc) {
goto err;
}
lis2ds12->cfg.int_pp_od = cfg->int_pp_od;
rc = lis2ds12_set_latched_int(itf, cfg->int_latched);
if (rc) {
goto err;
}
lis2ds12->cfg.int_latched = cfg->int_latched;
rc = lis2ds12_set_int_active_low(itf, cfg->int_active_low);
if (rc) {
goto err;
}
lis2ds12->cfg.int_active_low = cfg->int_active_low;
rc = lis2ds12_set_filter_cfg(itf, cfg->high_pass);
if (rc) {
goto err;
}
lis2ds12->cfg.high_pass = cfg->high_pass;
rc = lis2ds12_set_full_scale(itf, cfg->fs);
if (rc) {
goto err;
}
lis2ds12->cfg.fs = cfg->fs;
rc = lis2ds12_set_rate(itf, cfg->rate);
if (rc) {
goto err;
}
lis2ds12->cfg.rate = cfg->rate;
rc = lis2ds12_set_fifo_cfg(itf, cfg->fifo_mode, cfg->fifo_threshold);
if (rc) {
goto err;
}
lis2ds12->cfg.fifo_mode = cfg->fifo_mode;
lis2ds12->cfg.fifo_threshold = cfg->fifo_threshold;
rc = lis2ds12_set_wake_up_ths(itf, cfg->wake_up_ths);
if (rc) {
goto err;
}
lis2ds12->cfg.wake_up_ths = cfg->wake_up_ths;
rc = lis2ds12_set_wake_up_dur(itf, cfg->wake_up_dur);
if (rc) {
goto err;
}
lis2ds12->cfg.wake_up_dur = cfg->wake_up_dur;
rc = lis2ds12_set_sleep_dur(itf, cfg->sleep_duration);
if (rc) {
goto err;
}
lis2ds12->cfg.sleep_duration = cfg->sleep_duration;
rc = lis2ds12_set_inactivity_sleep_en(itf, cfg->inactivity_sleep_enable);
if (rc) {
goto err;
}
lis2ds12->cfg.inactivity_sleep_enable = cfg->inactivity_sleep_enable;
rc = lis2ds12_set_double_tap_event_en(itf, cfg->double_tap_event_enable);
if (rc) {
goto err;
}
lis2ds12->cfg.double_tap_event_enable = cfg->double_tap_event_enable;
rc = lis2ds12_set_freefall(itf, cfg->freefall_dur, cfg->freefall_ths);
if (rc) {
goto err;
}
lis2ds12->cfg.freefall_dur = cfg->freefall_dur;
lis2ds12->cfg.freefall_ths = cfg->freefall_ths;
rc = lis2ds12_set_int1_pin_cfg(itf, cfg->int1_pin_cfg);
if (rc) {
goto err;
}
lis2ds12->cfg.int1_pin_cfg = cfg->int1_pin_cfg;
rc = lis2ds12_set_int2_pin_cfg(itf, cfg->int2_pin_cfg);
if (rc) {
goto err;
}
lis2ds12->cfg.int2_pin_cfg = cfg->int2_pin_cfg;
rc = lis2ds12_set_tap_cfg(itf, &cfg->tap);
if (rc) {
goto err;
}
lis2ds12->cfg.tap = cfg->tap;
rc = lis2ds12_set_int2_on_int1_map(itf, cfg->map_int2_to_int1);
if(rc) {
goto err;
}
lis2ds12->cfg.map_int2_to_int1 = cfg->map_int2_to_int1;
rc = sensor_set_type_mask(&(lis2ds12->sensor), cfg->mask);
if (rc) {
goto err;
}
lis2ds12->cfg.read_mode.int_cfg = cfg->read_mode.int_cfg;
lis2ds12->cfg.read_mode.int_num = cfg->read_mode.int_num;
lis2ds12->cfg.read_mode.mode = cfg->read_mode.mode;
if (!cfg->notif_cfg) {
lis2ds12->cfg.notif_cfg = (struct lis2ds12_notif_cfg *)dflt_notif_cfg;
lis2ds12->cfg.max_num_notif = sizeof(dflt_notif_cfg)/sizeof(*dflt_notif_cfg);
} else {
lis2ds12->cfg.notif_cfg = cfg->notif_cfg;
lis2ds12->cfg.max_num_notif = cfg->max_num_notif;
}
lis2ds12->cfg.mask = cfg->mask;
return 0;
err:
return rc;
}