nuttx/drivers/sensors/lsm6dsl.c

/****************************************************************************
 * drivers/sensors/lsm6dsl.c
 *
 *   Copyright (C) 2018 Inc. All rights reserved.
 *   Author: Ben vd Veen <disruptivesolutionsnl@gmail.com> 
 *   Alias: DisruptiveNL
 *   
 * Based on:
 *
 *   Copyright (C) 2016 Omni Hoverboards Inc. All rights reserved.
 *   Author: Paul Alexander Patience <paul-a.patience@polymtl.ca>
 *
 *   Copyright (C) 2016 Gregory Nutt. All rights reserved.
 *   Author: Gregory Nutt <gnutt@nuttx.org>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 * 3. Neither the name NuttX nor the names of its contributors may be
 *    used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/

/****************************************************************************
 * Included Files
 ****************************************************************************/

#include <nuttx/config.h>

#include <errno.h>
#include <debug.h>
#include <stdlib.h>

#include <nuttx/kmalloc.h>
#include <nuttx/fs/fs.h>
#include <nuttx/i2c/i2c_master.h>
#include <nuttx/sensors/lsm6dsl.h>
#include <nuttx/random.h>

#if defined(CONFIG_I2C) && defined(CONFIG_SENSORS_LSM6DSL)

/****************************************************************************
 * Pre-processor Definitions
 ****************************************************************************/

#ifndef CONFIG_LSM6DSL_I2C_FREQUENCY
#  define CONFIG_LSM6DSL_I2C_FREQUENCY 400000
#endif

/****************************************************************************
 * Private Function Prototypes
 ****************************************************************************/

/* I2C Helpers */

static int lsm6dsl_readreg8(FAR struct lsm6dsl_dev_s *priv,
                            uint8_t regaddr, FAR uint8_t * regval);
static int lsm6dsl_writereg8(FAR struct lsm6dsl_dev_s *priv,
                             uint8_t regaddr, uint8_t regval);
static int lsm6dsl_modifyreg8(FAR struct lsm6dsl_dev_s *priv,
                              uint8_t regaddr,
                              uint8_t clearbits, uint8_t setbits);

/* Other Helpers */

static int lsm6dsl_find_minimum(int16_t a[], int n);
static int lsm6dsl_find_maximum(int16_t a[], int n);
static bool lsm6dsl_isbitset(int8_t b, int8_t n);

/* Accelerometer Operations */

static int lsm6dsl_sensor_config(FAR struct lsm6dsl_dev_s *priv);
static int lsm6dsl_sensor_start(FAR struct lsm6dsl_dev_s *priv);
static int lsm6dsl_sensor_stop(FAR struct lsm6dsl_dev_s *priv);
static int lsm6dsl_sensor_read(FAR struct lsm6dsl_dev_s *priv,
                               FAR struct lsm6dsl_sensor_data_s *sensor_data);
static int lsm6dsl_selftest(FAR struct lsm6dsl_dev_s *priv, uint32_t mode);

/* Character Driver Methods */

static int lsm6dsl_open(FAR struct file *filep);
static int lsm6dsl_close(FAR struct file *filep);
static ssize_t lsm6dsl_read(FAR struct file *filep,
                            FAR char *buffer, size_t buflen);
static ssize_t lsm6dsl_write(FAR struct file *filep,
                             FAR const char *buffer, size_t buflen);
static int lsm6dsl_ioctl(FAR struct file *filep, int cmd,
                         unsigned long arg);

/* Common Register Function */

static int lsm6dsl_register(FAR const char *devpath,
                            FAR struct i2c_master_s *i2c,
                            uint8_t addr,
                            FAR const struct lsm6dsl_ops_s *ops,
                            uint8_t datareg,
                            struct lsm6dsl_sensor_data_s sensor_data);

/****************************************************************************
 * Private Data
 ****************************************************************************/

static double g_accelerofactor = 0;
static double g_gyrofactor = 0;

static const struct file_operations g_fops =
{
  lsm6dsl_open,
  lsm6dsl_close,
  lsm6dsl_read,
  lsm6dsl_write,
  NULL,
  lsm6dsl_ioctl
#ifndef CONFIG_DISABLE_POLL
  , NULL
#endif
#ifndef CONFIG_DISABLE_PSEUDOFS_OPERATIONS
  , NULL
#  endif
};

static const struct lsm6dsl_ops_s g_LSM6DSLsensor_ops =
{
  lsm6dsl_sensor_config,
  lsm6dsl_sensor_start,
  lsm6dsl_sensor_stop,
  lsm6dsl_sensor_read,
  lsm6dsl_selftest
};

/****************************************************************************
 * Private Functions
 ****************************************************************************/

/****************************************************************************
 * Name: lsm6dsl_resetsensor
 *
 * Description:
 *   Reset sensor values
 *
 ****************************************************************************/

static void lsm6dsl_resetsensor(FAR struct lsm6dsl_dev_s *priv)
{
  priv->sensor_data.x_data      = 0;
  priv->sensor_data.y_data      = 0;
  priv->sensor_data.z_data      = 0;
  priv->sensor_data.temperature = 0;
  priv->sensor_data.g_x_data    = 0;
  priv->sensor_data.g_y_data    = 0;
  priv->sensor_data.g_z_data    = 0;
  priv->sensor_data.timestamp   = 0;
}

/****************************************************************************
 * Name: lsm6dsl_readreg8
 *
 * Description:
 *   Read from an 8-bit register.
 *
 ****************************************************************************/

static int lsm6dsl_readreg8(FAR struct lsm6dsl_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_LSM6DSL_I2C_FREQUENCY;
  config.address = priv->addr;
  config.addrlen = 7;

  /* Write the register address */

  ret = i2c_write(priv->i2c, &config, &regaddr, 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: lsm6dsl_writereg8
 *
 * Description:
 *   Write to an 8-bit register.
 *
 ****************************************************************************/

static int lsm6dsl_writereg8(FAR struct lsm6dsl_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_LSM6DSL_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: lsm6dsl_modifyreg8
 *
 * Description:
 *   Modify an 8-bit register.
 *
 ****************************************************************************/

static int lsm6dsl_modifyreg8(FAR struct lsm6dsl_dev_s *priv,
                              uint8_t regaddr,
                              uint8_t clearbits, uint8_t setbits)
{
  int ret;
  uint8_t regval;

  /* Sanity check */

  DEBUGASSERT(priv != NULL);

  ret = lsm6dsl_readreg8(priv, regaddr, &regval);
  if (ret < 0)
    {
      snerr("ERROR: lsm6dsl_readreg8 failed: %d\n", ret);
      return ret;
    }

  regval &= ~clearbits;
  regval |= setbits;

  ret = lsm6dsl_writereg8(priv, regaddr, regval);
  if (ret < 0)
    {
      snerr("ERROR: lsm6dsl_writereg8 failed: %d\n", ret);
      return ret;
    }

  return OK;
}

/****************************************************************************
 * Name: lsm6dsl_find_minimum
 *
 * Description:
 *   Find the minimum value in an array of numbers.
 *
 ****************************************************************************/

static int lsm6dsl_find_minimum(int16_t a[], int n)
{
  int c;
  int min = a[0];
  int index = 0;

  for (c = 1; c < n; c++)
    {
      if (a[c] < min)
        {
          index = c;
          min = a[c];
        }
    }

  return index;
}

/****************************************************************************
 * Name: lsm6dsl_find_maximum
 *
 * Description:
 *   Find the maximum value in an array of numbers.
 *
 ****************************************************************************/

static int lsm6dsl_find_maximum(int16_t am[], int n)
{
  int c;
  int max = am[0];
  int index = 0;

  for (c = 1; c < n; c++)
    {
      if (am[c] > max)
        {
          index = c;
          max = am[c];
        }
    }

  return index;
}

/****************************************************************************
 * Name: lsm6dsl_sensor_config
 *
 * Description:
 *   Configure the accelerometer and gyroscope.
 *
 ****************************************************************************/

static int lsm6dsl_sensor_config(FAR struct lsm6dsl_dev_s *priv)
{
  int ret;
  uint8_t regval;

  /* Sanity check */

  DEBUGASSERT(priv != NULL);

  /* Get the device identification */

  ret = lsm6dsl_readreg8(priv, LSM6DSL_WHO_AM_I, &regval);
  if (ret < 0)
    {
      snerr("ERROR: lsm6dsl_readreg8 failed: %d\n", ret);
      return ret;
    }

  if (regval != LSM6DSL_WHO_AM_I_VALUE)
    {
      snerr("ERROR: Invalid device identification %02x\n", regval);
      return -ENODEV;
    }

  return OK;
}

/****************************************************************************
 * Name: lsm6dsl_isbitset
 *
 * Description:
 *   Check if bit it set from mask, not bit number.
 *
 ****************************************************************************/

static bool lsm6dsl_isbitset(int8_t b, int8_t m)
{
  if ((b & m) != 0)
    {
      return true;
    }

  return false;
}

/****************************************************************************
 * Name: lsm6dsl_sensor_start
 *
 * Description:
 *   Start the accelerometer.
 *
 ****************************************************************************/

static int lsm6dsl_sensor_start(FAR struct lsm6dsl_dev_s *priv)
{
  uint8_t value;

  /* Enable the accelerometer */

  /* Reset values */

  lsm6dsl_resetsensor(priv);

  /* Sanity check */

  DEBUGASSERT(priv != NULL);

  sninfo("Starting....");

  /* Accelerometer config registers Turn on the accelerometer: 833Hz, +- 16g */

  lsm6dsl_writereg8(priv, LSM6DSL_CTRL1_XL, 0x74);
  g_accelerofactor = 0.488;

  /* Gyro config registers Turn on the gyro: FS=2000dps, ODR=833Hz Not using
   * modifyreg with empty value!!!! Then read value first!!!
   */

  lsm6dsl_writereg8(priv, LSM6DSL_CTRL2_G, 0x7C);
  g_gyrofactor = 70;

  lsm6dsl_writereg8(priv, LSM6DSL_CTRL6_C, 0x00);

  /* Timestamp registers */

  lsm6dsl_writereg8(priv, LSM6DSL_CTRL10_C, 0x20);

  return OK;
}

/****************************************************************************
 * Name: lsm6dsl_sensor_stop
 *
 * Description:
 *   Stop the accelerometer.
 *
 ****************************************************************************/

static int lsm6dsl_sensor_stop(FAR struct lsm6dsl_dev_s *priv)
{
  /* Sanity check */

  DEBUGASSERT(priv != NULL);

  /* Stop accelerometer */

  lsm6dsl_modifyreg8(priv,
                     LSM6DSL_CTRL1_XL_ODR_XL_SHIFT,
                     LSM6DSL_CTRL1_XL_ODR_XL_MASK,
                     LSM6DSL_CTRL1_XL_ODR_XL_POWER_DOWN);

  /* Stop gyro */

  lsm6dsl_modifyreg8(priv,
                     LSM6DSL_CTRL2_G_ODR_G_SHIFT,
                     LSM6DSL_CTRL2_G_ODR_G_MASK,
                     LSM6DSL_CTRL2_G_ODR_G_POWER_DOWN);

  return OK;
}

/****************************************************************************
 * Name: lsm6dsl_selftest
 *
 * Description:
 *   Selftesting the sensor.
 *   Mode 0 = selftest accelerometer and mode 1 = selftest gyro
 *
 ****************************************************************************/

static int lsm6dsl_selftest(FAR struct lsm6dsl_dev_s *priv, uint32_t mode)
{

  int samples = 5;
  int i;
  int i2;
  int i3;

  uint8_t value = 0;
  int8_t lox    = 0;
  int8_t loxst  = 0;
  int8_t hix    = 0;
  int8_t hixst  = 0;
  int8_t loy    = 0;
  int8_t loyst  = 0;
  int8_t hiy    = 0;
  int8_t hiyst  = 0;
  int8_t loz    = 0;
  int8_t lozst  = 0;
  int8_t hiz    = 0;
  int8_t hizst  = 0;

  int16_t OUTX_NOST[samples];
  int16_t OUTY_NOST[samples];
  int16_t OUTZ_NOST[samples];
  int16_t OUTX_ST[samples];
  int16_t OUTY_ST[samples];
  int16_t OUTZ_ST[samples];

  int16_t AVR_OUTX_NOST[samples];
  int16_t AVR_OUTY_NOST[samples];
  int16_t AVR_OUTZ_NOST[samples];
  int16_t AVR_OUTX_ST[samples];
  int16_t AVR_OUTY_ST[samples];
  int16_t AVR_OUTZ_ST[samples];

  int16_t avr_x   = 0;
  int16_t avr_y   = 0;
  int16_t avr_z   = 0;
  int16_t avr_xst = 0;
  int16_t avr_yst = 0;
  int16_t avr_zst = 0;

  int16_t min_x = 0;
  int16_t min_y = 0;
  int16_t min_z = 0;
  int16_t max_x = 0;
  int16_t max_y = 0;
  int16_t max_z = 0;

  int16_t min_xst = 0;
  int16_t min_yst = 0;
  int16_t min_zst = 0;
  int16_t max_xst = 0;
  int16_t max_yst = 0;
  int16_t max_zst = 0;

  int16_t ltemp = 0;
  int16_t htemp = 0;
  int16_t tempi = 0;
  int16_t temp  = 0;

  int16_t raw_x = 0;
  int16_t raw_y = 0;
  int16_t raw_z = 0;

  int16_t raw_xst = 0;
  int16_t raw_yst = 0;
  int16_t raw_zst = 0;

  /* mode = 0 then add hex 0x06 to OUT registers */

  int8_t registershift;

  /* Keep the device still during the self-test procedure. Setting registers
   * Power up, wait for 100ms for stable output.
   */

  if (mode == 0)
    {
      registershift = 0x06;

      /* Accelero ; power down gyro CTRL2_G 4g factor: 1000 for mg -> g value
       * is in mg/LSB FS=4g,52Hz 4000mg=65535
       */

      lsm6dsl_writereg8(priv, LSM6DSL_CTRL1_XL, 0x38);
      lsm6dsl_writereg8(priv, LSM6DSL_CTRL2_G, 0x00);
      lsm6dsl_writereg8(priv, LSM6DSL_CTRL3_C, 0x44);
      g_accelerofactor = (0.122 / 1000);
    }
  else
    {
      registershift = 0x00;

      /* Gyro; power down accelero CTRL1_XL FS=2000dps,208Hz 2000dps=65535 */

      lsm6dsl_writereg8(priv, LSM6DSL_CTRL1_XL, 0x00);
      lsm6dsl_writereg8(priv, LSM6DSL_CTRL2_G, 0x5C);
      lsm6dsl_writereg8(priv, LSM6DSL_CTRL3_C, 0x44);
      g_gyrofactor = (70 / 1000); /* 2000dps */
    }

  lsm6dsl_writereg8(priv, LSM6DSL_CTRL4_C, 0x00);
  lsm6dsl_writereg8(priv, LSM6DSL_CTRL5_C, 0x00);
  lsm6dsl_writereg8(priv, LSM6DSL_CTRL6_C, 0x00);
  lsm6dsl_writereg8(priv, LSM6DSL_CTRL7_G, 0x00);
  lsm6dsl_writereg8(priv, LSM6DSL_CTRL8_XL, 0x00);
  lsm6dsl_writereg8(priv, LSM6DSL_CTRL9_XL, 0x00);
  lsm6dsl_writereg8(priv, LSM6DSL_CTRL10_C, 0x00);

  nxsig_usleep(100000);         /* 100ms */

  /* Read the output registers after checking XLDA bit 5 times */

  bool checkbit = false;

  /* Wait until first sample and data is available */

  while (checkbit)
    {
      lsm6dsl_readreg8(priv, LSM6DSL_STATUS_REG, &value);
      if (mode == 0)
        {
          checkbit = lsm6dsl_isbitset(value, LSM6DSL_STATUS_REG_XLDA);
        }
      else
        {
          checkbit = lsm6dsl_isbitset(value, LSM6DSL_STATUS_REG_GDA);
        }
    }

  nxsig_usleep(100000);    /* 100ms */

  /* Read OUT registers Gyro is starting at 22h and Accelero at 28h */

  lsm6dsl_readreg8(priv, LSM6DSL_OUTX_L_G + registershift, &lox);
  lsm6dsl_readreg8(priv, LSM6DSL_OUTX_H_G + registershift, &hix);

  lsm6dsl_readreg8(priv, LSM6DSL_OUTY_L_G + registershift, &loy);
  lsm6dsl_readreg8(priv, LSM6DSL_OUTY_H_G + registershift, &hiy);

  lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_L_G + registershift, &loz);
  lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_H_G + registershift, &hiz);

  /* check XLDA 5 times */

  for (i = 0; i < samples; i++)
    {
      lsm6dsl_readreg8(priv, LSM6DSL_STATUS_REG, &value);
      if (mode == 0)
        {
          checkbit = lsm6dsl_isbitset(value, LSM6DSL_STATUS_REG_XLDA);
        }
      else
        {
          checkbit = lsm6dsl_isbitset(value, LSM6DSL_STATUS_REG_GDA);
        }

      /* Average the stored data on each axis
       * http://ozzmaker.com/accelerometer-to-g/
       */

      lsm6dsl_readreg8(priv, LSM6DSL_OUTX_L_G + registershift, &lox);
      lsm6dsl_readreg8(priv, LSM6DSL_OUTX_H_G + registershift, &hix);
      raw_x = (int16_t) (((uint16_t) hix << 8U) | (uint16_t) lox);

      lsm6dsl_readreg8(priv, LSM6DSL_OUTY_L_G + registershift, &loy);
      lsm6dsl_readreg8(priv, LSM6DSL_OUTY_H_G + registershift, &hiy);
      raw_y = (int16_t) (((uint16_t) hiy << 8U) | (uint16_t) loy);

      lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_L_G + registershift, &loz);
      lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_H_G + registershift, &hiz);
      raw_z = (int16_t) (((uint16_t) hiz << 8U) | (uint16_t) loz);

      /* Selftest only uses raw values */

      OUTX_NOST[i] = raw_x;
      OUTY_NOST[i] = raw_y;
      OUTZ_NOST[i] = raw_z;
    }

  /* Enable Selftest */

  if (mode == 0)
    {
      lsm6dsl_writereg8(priv, LSM6DSL_CTRL5_C, 0x01);
    }
  else
    {
      lsm6dsl_writereg8(priv, LSM6DSL_CTRL5_C, 0x04);
    }

  nxsig_usleep(100000);         /* 100ms */

  checkbit = false;
  while (checkbit)              /* wait until first sample and data is
                                 * available */
    {
      lsm6dsl_readreg8(priv, LSM6DSL_STATUS_REG, &value);
      if (mode == 0)
        {
          checkbit = lsm6dsl_isbitset(value, LSM6DSL_STATUS_REG_XLDA);
        }
      else
        {
          checkbit = lsm6dsl_isbitset(value, LSM6DSL_STATUS_REG_GDA);
        }
    }

  nxsig_usleep(100000);         /* 100ms */

  /* Now do all the ST values */

  lsm6dsl_readreg8(priv, LSM6DSL_OUTX_L_G + registershift, &loxst);
  lsm6dsl_readreg8(priv, LSM6DSL_OUTX_H_G + registershift, &hixst);

  lsm6dsl_readreg8(priv, LSM6DSL_OUTY_L_G + registershift, &loyst);
  lsm6dsl_readreg8(priv, LSM6DSL_OUTY_H_G + registershift, &hiyst);

  lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_L_G + registershift, &lozst);
  lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_H_G + registershift, &hizst);

  for (i2 = 0; i2 < samples; i2++)
    {
      lsm6dsl_readreg8(priv, LSM6DSL_STATUS_REG, &value);
      if (mode == 0)
        {
          checkbit = lsm6dsl_isbitset(value, LSM6DSL_STATUS_REG_XLDA);
        }
      else
        {
          checkbit = lsm6dsl_isbitset(value, LSM6DSL_STATUS_REG_GDA);
        }

      nxsig_usleep(100000);     /* 100ms */

      lsm6dsl_readreg8(priv, LSM6DSL_OUTX_L_G + registershift, &loxst);
      lsm6dsl_readreg8(priv, LSM6DSL_OUTX_H_G + registershift, &hixst);
      raw_xst = (int16_t) (((uint16_t) hixst << 8U) | (uint16_t) loxst);

      lsm6dsl_readreg8(priv, LSM6DSL_OUTY_L_G + registershift, &loyst);
      lsm6dsl_readreg8(priv, LSM6DSL_OUTY_H_G + registershift, &hiyst);
      raw_yst = (int16_t) (((uint16_t) hiyst << 8U) | (uint16_t) loyst);

      lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_L_G + registershift, &lozst);
      lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_H_G + registershift, &hizst);
      raw_zst = (int16_t) (((uint16_t) hizst << 8U) | (uint16_t) lozst);

      /* Selftest only uses raw values */

      OUTX_ST[i2] = raw_xst;
      OUTY_ST[i2] = raw_yst;
      OUTZ_ST[i2] = raw_zst;
    }

  /* Average stored data on each axis */

  for (i3 = 0; i3 < samples; i3++)
    {
      avr_x = avr_x + (int16_t) OUTX_NOST[i3];
      avr_y = avr_y + (int16_t) OUTY_NOST[i3];
      avr_z = avr_z + (int16_t) OUTZ_NOST[i3];

      avr_xst = avr_xst + (int16_t) OUTX_ST[i3];
      avr_yst = avr_yst + (int16_t) OUTY_ST[i3];
      avr_zst = avr_zst + (int16_t) OUTZ_ST[i3];
    }

  avr_x = (int16_t) avr_x / samples;
  avr_y = (int16_t) avr_y / samples;
  avr_z = (int16_t) avr_z / samples;

  avr_xst = (int16_t) avr_xst / samples;
  avr_yst = (int16_t) avr_yst / samples;
  avr_zst = (int16_t) avr_zst / samples;

  min_x = OUTX_NOST[lsm6dsl_find_minimum(OUTX_NOST, samples)];
  min_y = OUTY_NOST[lsm6dsl_find_minimum(OUTY_NOST, samples)];
  min_z = OUTZ_NOST[lsm6dsl_find_minimum(OUTZ_NOST, samples)];

  max_x = OUTX_NOST[lsm6dsl_find_maximum(OUTX_NOST, samples)];
  max_y = OUTY_NOST[lsm6dsl_find_maximum(OUTY_NOST, samples)];
  max_z = OUTZ_NOST[lsm6dsl_find_maximum(OUTZ_NOST, samples)];

  min_xst = OUTX_ST[lsm6dsl_find_minimum(OUTX_ST, samples)];
  min_yst = OUTY_ST[lsm6dsl_find_minimum(OUTY_ST, samples)];
  min_zst = OUTZ_ST[lsm6dsl_find_minimum(OUTZ_ST, samples)];

  max_xst = OUTX_ST[lsm6dsl_find_maximum(OUTX_ST, samples)];
  max_yst = OUTY_ST[lsm6dsl_find_maximum(OUTY_ST, samples)];
  max_zst = OUTZ_ST[lsm6dsl_find_maximum(OUTZ_ST, samples)];

  sninfo("stdev_x: -%d %d +%d\n", avr_x - min_x, avr_x, max_x - avr_x);
  sninfo("stdev_y: -%d %d +%d\n", avr_y - min_y, avr_y, max_y - avr_y);
  sninfo("stdev_z: -%d %d +%d\n", avr_z - min_z, avr_z, max_z - avr_z);

  sninfo("stdev_xst: -%d %d +%d\n", avr_xst - min_xst, avr_xst,
         max_xst - avr_xst);
  sninfo("stdev_yst: -%d %d +%d\n", avr_yst - min_yst, avr_yst,
         max_yst - avr_yst);
  sninfo("stdev_zst: -%d %d +%d\n", avr_zst - min_zst, avr_zst,
         max_zst - avr_zst);

  sninfo("avr_x: %d\n", avr_x);
  sninfo("avr_y: %d\n", avr_y);
  sninfo("avr_z: %d\n", avr_z);
  sninfo("min_x: %d\n", min_x);
  sninfo("max_x: %d\n", max_x);
  sninfo("min_xst: %d\n", min_xst);
  sninfo("max_xst: %d\n", max_xst);

  /* Validation Question is placed at ST FAE because the equation in the
   * datasheet is doubtful.
   */

  if ((avr_x >= min_x && avr_x <= max_x) &&
      (avr_xst >= min_xst && avr_xst <= max_xst))
    {
      sninfo("PASSED NOST AND ST FOR X!\n");
    }
  else
    {
      sninfo("FAILED NOST AND ST FOR X!\n");
      sninfo("[ %d - %d ]", min_x, min_xst);
      sninfo(" <=\n ");
      sninfo("[ %d - %d ]", avr_x, avr_xst);
      sninfo(" <=\n ");
      sninfo("[ %d - %d ]", max_x, max_xst);
      sninfo("\n");
    }

  if ((avr_y >= min_y && avr_y <= max_y) &&
      (avr_yst >= min_yst && avr_yst <= max_yst))
    {
      sninfo("PASSED NOST AND ST FOR Y!\n");
    }
  else
    {
      sninfo("FAILED NOST AND ST FOR Y!\n");
      sninfo("[ %d - %d ]", min_y, min_yst);
      sninfo(" <=\n ");
      sninfo("[ %d - %d ]", avr_y, avr_yst);
      sninfo(" <=\n ");
      sninfo("[ %d - %d ]", max_y, max_yst);
      sninfo("\n");
    }

  if ((avr_z >= min_z && avr_z <= max_z) &&
      (avr_zst >= min_zst && avr_zst <= max_zst))
    {
      sninfo("PASSED NOST AND ST FOR Z!\n");
    }
  else
    {
      sninfo("FAILED NOST AND ST FOR Z!\n");
      sninfo("[ %d - %d ]", min_z, min_zst);
      sninfo(" <=\n ");
      sninfo("[ %d - %d ]", avr_z, avr_zst);
      sninfo(" <=\n ");
      sninfo("[ %d - %d ]", max_z, max_zst);
      sninfo("\n");
    }

  sleep(2);

  /* Disable test */

  switch (mode)
    {
    case 0:
      {
        sninfo("SELFTEST ACCELERO DISABLED\n");
        lsm6dsl_writereg8(priv, LSM6DSL_CTRL1_XL, 0x00);
        lsm6dsl_writereg8(priv, LSM6DSL_CTRL5_C, 0x00);
      }
      break;
    case 1:
      {
        sninfo("SELFTEST GYRO DISABLED\n");
        lsm6dsl_writereg8(priv, LSM6DSL_CTRL2_G, 0x00);
        lsm6dsl_writereg8(priv, LSM6DSL_CTRL5_C, 0x00);
      }
      break;

    default:
      break;
    }

  return OK;
}

/****************************************************************************
 * Name: lsm6dsl_sensor_read
 *
 * Description:
 *   Read the sensor.
 * A sensor in a steady state on a horizontal surface will 
 *  measure 0 g on both the X-axis and Y-axis, whereas the Z-axis will
 *  measure 1 g. (page 30 datasheet). The X- and Y-axis have an offset
 *  of 40 mg/LSB
 *
 ****************************************************************************/

static int lsm6dsl_sensor_read(FAR struct lsm6dsl_dev_s *priv,
                               FAR struct lsm6dsl_sensor_data_s *sensor_data)
{

  int16_t lo    = 0;
  int16_t lox   = 0;
  int16_t loxg  = 0;
  int16_t hi    = 0;
  int16_t hix   = 0;
  int16_t hixg  = 0;
  int16_t loy   = 0;
  int16_t loyg  = 0;
  int16_t hiy   = 0;
  int16_t hiyg  = 0;
  int16_t loz   = 0;
  int16_t lozg  = 0;
  int16_t hiz   = 0;
  int16_t hizg  = 0;

  int16_t templ = 0;
  int16_t temph = 0;

  uint8_t status1 = 0;
  uint8_t status2 = 0;
  uint8_t status3 = 0;
  uint8_t status4 = 0;
  uint8_t value   = 0;

  uint8_t tstamp0 = 0;
  uint8_t tstamp1 = 0;
  uint8_t tstamp2 = 0;
  uint8_t tstamp3 = 0;
  uint32_t ts     = 0;

  int16_t x_val = 0;
  int16_t y_val = 0;
  int16_t z_val = 0;
  int16_t tempi = 0;
  int16_t temp_val = 0;

  int16_t x_valg = 0;
  int16_t y_valg = 0;
  int16_t z_valg = 0;

  int16_t xf_val = 0;
  int16_t yf_val = 0;
  int16_t zf_val = 0;

  /* Accelerometer */

  lsm6dsl_readreg8(priv, LSM6DSL_OUTX_L_XL, &lox);
  lsm6dsl_readreg8(priv, LSM6DSL_OUTX_H_XL, &hix);

  lsm6dsl_readreg8(priv, LSM6DSL_OUTY_L_XL, &loy);
  lsm6dsl_readreg8(priv, LSM6DSL_OUTY_H_XL, &hiy);

  lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_L_XL, &loz);
  lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_H_XL, &hiz);

  /* Gyro */

  lsm6dsl_readreg8(priv, LSM6DSL_OUTX_L_G, &loxg);
  lsm6dsl_readreg8(priv, LSM6DSL_OUTX_H_G, &hixg);

  lsm6dsl_readreg8(priv, LSM6DSL_OUTY_L_G, &loyg);
  lsm6dsl_readreg8(priv, LSM6DSL_OUTY_H_G, &hiyg);

  lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_L_G, &lozg);
  lsm6dsl_readreg8(priv, LSM6DSL_OUTZ_H_G, &hizg);

  /* Timestamp */

  lsm6dsl_readreg8(priv, LSM6DSL_TIMESTAMP0_REG, &tstamp0);
  lsm6dsl_readreg8(priv, LSM6DSL_TIMESTAMP1_REG, &tstamp1);
  lsm6dsl_readreg8(priv, LSM6DSL_TIMESTAMP2_REG, &tstamp2);

  ts = (tstamp2 << 16) | (tstamp1 << 8) | tstamp0;

  /* Temperature */

  lsm6dsl_readreg8(priv, LSM6DSL_OUT_TEMP_L, &templ);
  lsm6dsl_readreg8(priv, LSM6DSL_OUT_TEMP_H, &temph);

  xf_val = (int16_t) ((hix << 8) | lox);
  yf_val = (int16_t) ((hiy << 8) | loy);
  zf_val = (int16_t) ((hiz << 8) | loz);

  tempi = (int16_t) ((((int16_t) temph << 8) | (int16_t) templ));

  temp_val = (tempi / 256) + 25;

  sninfo("Data 16-bit XL_X--->: %d mg\n", (short)(xf_val * g_accelerofactor));
  sninfo("Data 16-bit XL_Y--->: %d mg\n", (short)(yf_val * g_accelerofactor));
  sninfo("Data 16-bit XL_Z--->: %d mg\n", (short)(zf_val * g_accelerofactor));
  sninfo("Data 16-bit TEMP--->: %d Celsius\n", temp_val);

  sensor_data->x_data = xf_val * g_accelerofactor;
  sensor_data->y_data = yf_val * g_accelerofactor;
  sensor_data->z_data = zf_val * g_accelerofactor;
  sensor_data->temperature = temp_val;
  sensor_data->timestamp = ts;

  x_valg = (int16_t) (((hixg) << 8) | loxg);
  y_valg = (int16_t) (((hiyg) << 8) | loyg);
  z_valg = (int16_t) (((hizg) << 8) | lozg);

  sninfo("Data 16-bit G_X--->: %d mdps\n", (short)(x_valg * g_gyrofactor));
  sninfo("Data 16-bit G_Y--->: %d mdps\n", (short)(y_valg * g_gyrofactor));
  sninfo("Data 16-bit G_Z--->: %d mdps\n", (short)(z_valg * g_gyrofactor));

  sensor_data->g_x_data = x_valg * g_gyrofactor;
  sensor_data->g_y_data = y_valg * g_gyrofactor;
  sensor_data->g_z_data = z_valg * g_gyrofactor;

  return OK;
}

/****************************************************************************
 * Name: lsm6dsl_open
 *
 * Description:
 *   This method is called when the device is opened.
 *
 ****************************************************************************/

static int lsm6dsl_open(FAR struct file *filep)
{
  sninfo("Device LSM6DSL opened!!\r\n");
  return OK;
}

/****************************************************************************
 * Name: lsm6dsl_close
 *
 * Description:
 *   This method is called when the device is closed.
 *
 ****************************************************************************/

static int lsm6dsl_close(FAR struct file *filep)
{
  return OK;
}

/****************************************************************************
 * Name: lsm6dsl_read
 *
 * Description:
 *   The standard read method.
 *
 ****************************************************************************/

static ssize_t lsm6dsl_read(FAR struct file *filep,
                            FAR char *buffer, size_t buflen)
{
  FAR struct inode *inode;
  FAR struct lsm6dsl_dev_s *priv;
  int ret;
  size_t i;
  size_t j;
  size_t samplesize;
  size_t nsamples;
  uint16_t data;
  FAR int16_t *ptr;
  uint8_t regaddr;
  uint8_t lo;
  uint8_t hi;
  uint32_t merge = 0;

  /* Sanity check */

  DEBUGASSERT(filep != NULL);
  inode = filep->f_inode;

  DEBUGASSERT(inode != NULL);
  priv = (FAR struct lsm6dsl_dev_s *)inode->i_private;

  DEBUGASSERT(priv != NULL);
  DEBUGASSERT(priv->datareg == LSM6DSL_OUTX_L_G_SHIFT ||
              priv->datareg == LSM6DSL_OUTX_L_XL_SHIFT);
  DEBUGASSERT(buffer != NULL);

  samplesize = 3 * sizeof(*ptr);
  nsamples = buflen / samplesize;
  ptr = (FAR int16_t *) buffer;

  /* Get the requested number of samples */

  for (i = 0; i < nsamples; i++)
    {
      /* Reset the register address to the X low byte register */

      regaddr = priv->datareg;

      /* Read the X, Y and Z data */

      for (j = 0; j < 3; j++)
        {
          /* Read the low byte */

          ret = lsm6dsl_readreg8(priv, regaddr, &lo);
          if (ret < 0)
            {
              snerr("ERROR: lsm6dsl_readreg8 failed: %d\n", ret);
              return (ssize_t) ret;
            }

          regaddr++;

          /* Read the high byte */

          ret = lsm6dsl_readreg8(priv, regaddr, &hi);
          if (ret < 0)
            {
              snerr("ERROR: lsm6dsl_readreg8 failed: %d\n", ret);
              return (ssize_t) ret;
            }

          regaddr++;

          /* The data is 16 bits in two's complement representation */

          data = ((uint16_t) hi << 8) | (uint16_t) lo;

          /* Collect entropy */

          merge += data ^ (merge >> 16);

          /* The value is positive */

          if (data < 0x8000)
            {
              ptr[j] = (int16_t) data;
            }

          /* The value is negative, so find its absolute value by taking the
           * two's complement 
           */

          else if (data > 0x8000)
            {
              data = ~data + 1;
              ptr[j] = -(int16_t) data;
            }

          /* The value is negative and can't be represented as a positive
           * int16_t value
           */

          else
            {
              ptr[j] = (int16_t) (-32768);
            }
        }
    }

  /* Feed sensor data to entropy pool */

  add_sensor_randomness(merge);

  return nsamples * samplesize;
}

/****************************************************************************
 * Name: lsm6dsl_write
 *
 * Description:
 *   A dummy write method.
 *
 ****************************************************************************/

static ssize_t lsm6dsl_write(FAR struct file *filep,
                             FAR const char *buffer, size_t buflen)
{
  return -ENOSYS;
}

/****************************************************************************
 * Name: lsm6dsl_ioctl
 *
 * Description:
 *   The standard ioctl method.
 *
 ****************************************************************************/

static int lsm6dsl_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
{
  FAR struct inode *inode;
  FAR struct lsm6dsl_dev_s *priv;
  int ret;

  /* Sanity check */

  DEBUGASSERT(filep != NULL);
  inode = filep->f_inode;

  DEBUGASSERT(inode != NULL);
  priv = (FAR struct lsm6dsl_dev_s *)inode->i_private;

  DEBUGASSERT(priv != NULL);

  /* Handle ioctl commands */

  switch (cmd)
    {
      /* Start converting. Arg: None. */

    case SNIOC_START:
      ret = priv->ops->start(priv);
      break;

      /* Stop converting. Arg: None. */

    case SNIOC_STOP:
      ret = priv->ops->stop(priv);
      break;

    case SNIOC_LSM6DSLSENSORREAD:
      ret = priv->ops->sensor_read(priv, (FAR struct lsm6dsl_sensor_data_s *) arg);
      break;

    case SNIOC_START_SELFTEST:
      ret = priv->ops->selftest(priv, (uint32_t) arg);
      break;

      /* Unrecognized commands */

    default:
      {
        snerr("ERROR: Unrecognized cmd: %d arg: %lu\n", cmd, arg);
        ret = -ENOTTY;
      }
      break;
    }

  return ret;
}

/****************************************************************************
 * Name: lsm6dsl_register
 *
 * Description:
 *   Register the LSM6DSL accelerometer, gyroscope device as 'devpath'.
 *
 * Input Parameters:
 *   devpath - The full path to the driver to register, e.g., "/dev/lsm6dsl0",
 *             "/dev/gyro0" or "/dev/mag0".
 *   i2c     - An I2C driver instance.
 *   addr    - The I2C address of the LSM6DSL accelerometer, gyroscope or
 *             magnetometer.
 *   ops     - The device operations structure.
 *   datareg - The register address of the low byte of the X-coordinate data.
 *
 * Returned Value:
 *   Zero (OK) on success; a negated errno value on failure.
 *
 ****************************************************************************/

static int lsm6dsl_register(FAR const char *devpath,
                            FAR struct i2c_master_s *i2c,
                            uint8_t addr,
                            FAR const struct lsm6dsl_ops_s *ops,
                            uint8_t datareg,
                            struct lsm6dsl_sensor_data_s sensor_data)
{
  FAR struct lsm6dsl_dev_s *priv;
  int ret;

  /* Sanity check */

  DEBUGASSERT(devpath != NULL);
  DEBUGASSERT(i2c != NULL);
  DEBUGASSERT(datareg == LSM6DSL_OUTX_L_XL_SHIFT ||
              datareg == LSM6DSL_OUTX_L_G_SHIFT);

  /* Initialize the device's structure */

  priv = (FAR struct lsm6dsl_dev_s *)kmm_malloc(sizeof(*priv));
  if (priv == NULL)
    {
      snerr("ERROR: Failed to allocate instance\n");
      return -ENOMEM;
    }

  priv->i2c         = i2c;
  priv->addr        = addr;
  priv->ops         = ops;
  priv->datareg     = datareg;
  priv->sensor_data = sensor_data;

  /* Configure the device */

  ret = priv->ops->config(priv);
  if (ret < 0)
    {
      snerr("ERROR: Failed to configure device: %d\n", ret);
      kmm_free(priv);
      return ret;
    }

  /* Register the character driver */

  ret = register_driver(devpath, &g_fops, 0666, priv);
  if (ret < 0)
    {
      snerr("ERROR: Failed to register driver: %d\n", ret);
      kmm_free(priv);
      return ret;
    }

  return OK;
}

/****************************************************************************
 * Public Functions
 ****************************************************************************/

/****************************************************************************
 * Name: lsm6dsl_sensor_register
 *
 * Description:
 *   Register the LSM6DSL accelerometer character device as 'devpath'.
 *
 * Input Parameters:
 *   devpath - The full path to the driver to register, e.g., "/dev/lsm6dsl0".
 *   i2c     - An I2C driver instance.
 *   addr    - The I2C address of the LSM6DSL accelerometer.
 *
 * Returned Value:
 *   Zero (OK) on success; a negated errno value on failure.
 *
 ****************************************************************************/

int lsm6dsl_sensor_register(FAR const char *devpath,
                            FAR struct i2c_master_s *i2c, uint8_t addr)
{
  struct lsm6dsl_sensor_data_s sensor_data;

  DEBUGASSERT(addr == LSM6DSLACCEL_ADDR0 || addr == LSM6DSLACCEL_ADDR1);

  sninfo("Trying to register accel\n");

  return lsm6dsl_register(devpath, i2c, addr, &g_LSM6DSLsensor_ops,
                          LSM6DSL_OUTX_L_XL_SHIFT, sensor_data);
}

#endif /* CONFIG_I2C && CONFIG_SENSORS_LSM6DSL */