summaryrefslogtreecommitdiff
path: root/nuttx/drivers
diff options
context:
space:
mode:
authorGregory Nutt <gnutt@nuttx.org>2014-12-16 08:16:53 -0600
committerGregory Nutt <gnutt@nuttx.org>2014-12-16 08:16:53 -0600
commit08be24e16d857b79c0e3ac7e3f6beb6c114d0662 (patch)
treec150c40575cf68ac8795019a9cf04dd7bbed2d45 /nuttx/drivers
parent8db1b5542eade3ec3f0e20fd8ab1fd748cb2886c (diff)
downloadnuttx-08be24e16d857b79c0e3ac7e3f6beb6c114d0662.tar.gz
nuttx-08be24e16d857b79c0e3ac7e3f6beb6c114d0662.tar.bz2
nuttx-08be24e16d857b79c0e3ac7e3f6beb6c114d0662.zip
Add ADXL345 accelerometer driver. From Alan Carvalho de Assis
Diffstat (limited to 'nuttx/drivers')
-rw-r--r--nuttx/drivers/sensors/Kconfig44
-rw-r--r--nuttx/drivers/sensors/Make.defs17
-rw-r--r--nuttx/drivers/sensors/README.txt10
-rw-r--r--nuttx/drivers/sensors/adxl345.h198
-rw-r--r--nuttx/drivers/sensors/adxl345_base.c484
-rw-r--r--nuttx/drivers/sensors/adxl345_i2c.c212
-rw-r--r--nuttx/drivers/sensors/adxl345_spi.c213
7 files changed, 1178 insertions, 0 deletions
diff --git a/nuttx/drivers/sensors/Kconfig b/nuttx/drivers/sensors/Kconfig
index 20b3ea028..fa50e9c8e 100644
--- a/nuttx/drivers/sensors/Kconfig
+++ b/nuttx/drivers/sensors/Kconfig
@@ -8,6 +8,49 @@ config LIS331DL
default n
select I2C
+config SENSORS_ADXL345
+ bool "AnalogDevices ADXL345 Driver"
+ default n
+ ---help---
+ Enables support for the ADXL345 driver
+
+if SENSORS_ADXL345
+
+choice
+ prompt "ADXL345 Interface"
+ default ADXL345_SPI
+
+config ADXL345_SPI
+ bool "ADXL345 SPI Interface"
+ select SPI
+ ---help---
+ Enables support for the SPI interface.
+
+config ADXL345_I2C
+ bool "ADXL345 I2C Interface"
+ select I2C
+ ---help---
+ Enables support for the I2C interface
+
+endchoice
+
+config ADXL345_ACTIVELOW
+ bool "Active Low Interrupt"
+ default n
+ ---help---
+ The ADXL345 interrupt will be inverted. Instead starting low and going
+ high, it will start high and will go low when an interrupt is fired.
+ Default: Active high/rising edge.
+
+config ADXL345_REGDEBUG
+ bool "Enable Register-Level ADXL345 Debug"
+ default n
+ depends on DEBUG
+ ---help---
+ Enable very low register-level debug output.
+
+endif # SENSORS_ADXL345
+
config I2C_LM75
bool
default y if LM75
@@ -31,3 +74,4 @@ config DEBUG_QENCODER
bool "Enable Qencoder Debug"
default n
depends on QENCODER
+
diff --git a/nuttx/drivers/sensors/Make.defs b/nuttx/drivers/sensors/Make.defs
index f6e2f94b6..a7069ef4e 100644
--- a/nuttx/drivers/sensors/Make.defs
+++ b/nuttx/drivers/sensors/Make.defs
@@ -34,6 +34,11 @@
############################################################################
# Include sensor drivers
+
+ifeq ($(CONFIG_SENSORS_ADXL345),y)
+ CSRCS += adxl345_base.c
+endif
+
# These drivers depend on I2C support
ifeq ($(CONFIG_I2C),y)
@@ -42,10 +47,22 @@ ifeq ($(CONFIG_LIS331DL),y)
CSRCS += lis331dl.c
endif
+ifeq ($(CONFIG_ADXL345_I2C),y)
+ CSRCS += adxl345_i2c.c
+endif
+
ifeq ($(CONFIG_I2C_LM75),y)
CSRCS += lm75.c
endif
+endif # CONFIG_I2C
+
+# These drivers depend on SPI support
+
+ifeq ($(CONFIG_SPI),y)
+ifeq ($(CONFIG_ADXL345_SPI),y)
+ CSRCS += adxl345_spi.c
endif
+endif # CONFIG_SPI
# Quadrature encoder upper half
diff --git a/nuttx/drivers/sensors/README.txt b/nuttx/drivers/sensors/README.txt
new file mode 100644
index 000000000..4f6c0072c
--- /dev/null
+++ b/nuttx/drivers/sensors/README.txt
@@ -0,0 +1,10 @@
+ADXL345
+=======
+
+The ADXL345 accelerometer can operate in I2C or SPI mode. To operate in I2C
+mode just connect the CS pin to Vddi/o.
+
+In order to operate in SPI mode CS need to use connected to microcontroller,
+it cannot leave unconnected.
+
+In SPI mode it works with clock polarity (CPOL) = 1 and clock phase (CPHA) = 1.
diff --git a/nuttx/drivers/sensors/adxl345.h b/nuttx/drivers/sensors/adxl345.h
new file mode 100644
index 000000000..e2a06cea9
--- /dev/null
+++ b/nuttx/drivers/sensors/adxl345.h
@@ -0,0 +1,198 @@
+/********************************************************************************************
+ * drivers/sensors/adxl345.h
+ *
+ * Copyright (C) 2014 Alan Carvalho de Assis
+ * Author: Alan Carvalho de Assis <acassis@gmail.com>
+ * using ADXL345 driver as template reference
+ * 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.
+ *
+ ********************************************************************************************/
+
+#ifndef __DRIVERS_SENSORS_ADXL345_H
+#define __DRIVERS_SENSORS_ADXL345_H
+
+/********************************************************************************************
+ * Included Files
+ ********************************************************************************************/
+
+#include <nuttx/config.h>
+
+#include <semaphore.h>
+
+#include <nuttx/wdog.h>
+#include <nuttx/clock.h>
+#include <nuttx/wqueue.h>
+#include <nuttx/sensors/adxl345.h>
+
+#if defined(CONFIG_SENSORS_ADXL345)
+
+/********************************************************************************************
+ * Pre-Processor Definitions
+ ********************************************************************************************/
+
+#ifdef CONFIG_ADXL345_I2C
+# error "Only the ADXL345 SPI interface is supported by this driver"
+#endif
+
+/* Driver support ***************************************************************************/
+/* This format is used to construct the /dev/accel[n] device driver path. It defined here
+ * so that it will be used consistently in all places.
+ */
+
+#define DEV_FORMAT "/dev/accel%d"
+#define DEV_NAMELEN 16
+
+/* Driver flags */
+
+#define ADXL345_STAT_INITIALIZED 1 /* Device has been initialized */
+
+/********************************************************************************************
+ * Public Types
+ ********************************************************************************************/
+/* This defines type of events */
+
+enum adxl345_event
+{
+ DATA_READY = 0, /* New data available */
+ SINGLE_TAP, /* A tap event detected */
+ DOUBLE_TAP, /* A double tap event detected */
+ ACTIVITY, /* Activity detected */
+ INACTIVITY, /* Inactivity detected */
+ FREE_FAL, /* Free fall event */
+ WATERMARK, /* Number samples in FIFO is equal to sample bits */
+ OVERRUN, /* New data replaced unread data */
+};
+
+/* This defines operating mode */
+
+enum adxl345_mode
+{
+ BYPASS_MODE = 0, /* Bypass FIFO, then it remain empty */
+ FIFO_MODE, /* Sampled data are put in FIFO, up to 32 samples */
+ STREAM_MODE, /* Sampled data are put in FIFO, when full remove old samples */
+ TRIGGER_MODE /* Similar to Stream Mode, but when Trigger event happen FIFO freeze */
+};
+
+/* This structure describes the results of one ADXL345 sample */
+
+struct adxl345_sample_s
+{
+ uint16_t data_x; /* Measured X-axis acceleration */
+ uint16_t data_y; /* Measured Y-axis acceleration */
+ uint8_t data_z; /* Measured Z-axis acceleration */
+};
+
+/* This structure represents the state of the ADXL345 driver */
+
+struct adxl345_dev_s
+{
+ /* Common fields */
+
+ FAR struct adxl345_config_s *config; /* Board configuration data */
+ sem_t exclsem; /* Manages exclusive access to this structure */
+#ifdef CONFIG_ADXL345_SPI
+ FAR struct spi_dev_s *spi; /* Saved SPI driver instance */
+#else
+ FAR struct i2c_dev_s *i2c; /* Saved I2C driver instance */
+#endif
+
+ uint8_t status; /* See ADXL345_STAT_* definitions */
+ struct work_s work; /* Supports the interrupt handling "bottom half" */
+
+#ifdef CONFIG_ADXL345_REFCNT
+ uint8_t crefs; /* Number of times the device has been opened */
+#endif
+ uint8_t nwaiters; /* Number of threads waiting for ADXL345 data */
+
+ uint16_t ofsx; /* Offset X value */
+ uint16_t ofsy; /* Offset Y value */
+ uint16_t ofsz; /* Offset Z value */
+ sem_t waitsem; /* Used to wait for the availability of data */
+
+ struct work_s timeout; /* Supports timeout work */
+ struct adxl345_sample_s sample; /* Last sampled accelerometer data */
+
+ /* The following is a list if poll structures of threads waiting for
+ * driver events. The 'struct pollfd' reference for each open is also
+ * retained in the f_priv field of the 'struct file'.
+ */
+
+#ifndef CONFIG_DISABLE_POLL
+ struct pollfd *fds[CONFIG_ADXL345_NPOLLWAITERS];
+#endif
+};
+
+/********************************************************************************************
+ * Public Function Prototypes
+ ********************************************************************************************/
+
+/********************************************************************************************
+ * Name: adxl345_getreg8
+ *
+ * Description:
+ * Read from an 8-bit ADXL345 register
+ *
+ ********************************************************************************************/
+
+uint8_t adxl345_getreg8(FAR struct adxl345_dev_s *priv, uint8_t regaddr);
+
+/********************************************************************************************
+ * Name: adxl345_putreg8
+ *
+ * Description:
+ * Write a value to an 8-bit ADXL345 register
+ *
+ ********************************************************************************************/
+
+void adxl345_putreg8(FAR struct adxl345_dev_s *priv, uint8_t regaddr, uint8_t regval);
+
+/********************************************************************************************
+ * Name: adxl345_getreg16
+ *
+ * Description:
+ * Read 16-bits of data from an ADXL345 register
+ *
+ ********************************************************************************************/
+
+uint16_t adxl345_getreg16(FAR struct adxl345_dev_s *priv, uint8_t regaddr);
+
+/********************************************************************************************
+ * Name: adxl345_accworker
+ *
+ * Description:
+ * Handle accelerometer interrupt events (this function actually executes in the context of
+ * the worker thread).
+ *
+ ********************************************************************************************/
+
+void adxl345_accworker(FAR struct adxl345_dev_s *priv, uint8_t intsta) weak_function;
+
+#endif /* CONFIG_SENSORS_ADXL345 */
+#endif /* __DRIVERS_SENSORS_ADXL345_H */
diff --git a/nuttx/drivers/sensors/adxl345_base.c b/nuttx/drivers/sensors/adxl345_base.c
new file mode 100644
index 000000000..7455d7089
--- /dev/null
+++ b/nuttx/drivers/sensors/adxl345_base.c
@@ -0,0 +1,484 @@
+/****************************************************************************
+ * drivers/sensors/adxl345.c
+ *
+ * Copyright (C) 2014 Alan Carvalho de Assis. All rights reserved.
+ * Author: Alan Carvalho de Assis <acassis@gmail.com>
+ * Based on STME811 driver
+ * 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 <unistd.h>
+#include <errno.h>
+#include <debug.h>
+#include <stdio.h>
+
+#include <nuttx/kmalloc.h>
+#include <nuttx/sensors/adxl345.h>
+
+#include "adxl345.h"
+
+#if defined(CONFIG_SENSORS_ADXL345)
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/* Character driver methods */
+
+static int adxl345_open(FAR struct file *filep);
+static int adxl345_close(FAR struct file *filep);
+static ssize_t adxl345_read(FAR struct file *filep, FAR char *buffer,
+ size_t len);
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+static struct adxl345_dev_s g_adxl345;
+
+/* This the vtable that supports the character driver interface */
+
+static const struct file_operations g_adxl345fops =
+{
+ adxl345_open, /* open */
+ adxl345_close, /* close */
+ adxl345_read, /* read */
+ 0, /* write */
+ 0, /* seek */
+ 0, /* ioctl */
+};
+
+/****************************************************************************
+ * Name: adxl345_open
+ *
+ * Description:
+ * Standard character driver open method.
+ *
+ ****************************************************************************/
+
+static int adxl345_open(FAR struct file *filep)
+{
+ return OK;
+}
+
+/****************************************************************************
+ * Name: adxl345_close
+ *
+ * Description:
+ * Standard character driver close method.
+ *
+ ****************************************************************************/
+
+static int adxl345_close(FAR struct file *filep)
+{
+ return OK;
+}
+
+/****************************************************************************
+ * Name: adxl345_read
+ *
+ * Description:
+ * Standard character driver read method.
+ *
+ ****************************************************************************/
+
+static ssize_t adxl345_read(FAR struct file *filep, FAR char *buffer, size_t len)
+{
+ FAR struct inode *inode;
+ FAR struct adxl345_dev_s *priv;
+ struct adxl345_sample_s sample;
+ int ret;
+
+ ivdbg("len=%d\n", len);
+ DEBUGASSERT(filep);
+ inode = filep->f_inode;
+
+ DEBUGASSERT(inode && inode->i_private);
+ priv = (FAR struct adxl345_dev_s *)inode->i_private;
+
+ /* Verify that the caller has provided a buffer large enough to receive
+ * the accelerometer data.
+ */
+
+ if (len < sizeof(struct adxl345_sample_s))
+ {
+ /* We could provide logic to break up a touch report into segments and
+ * handle smaller reads... but why?
+ */
+
+ return -ENOSYS;
+ }
+
+ /* Get exclusive access to the driver data structure */
+
+ ret = sem_wait(&priv->exclsem);
+ if (ret < 0)
+ {
+ /* This should only happen if the wait was canceled by an signal */
+
+ DEBUGASSERT(errno == EINTR);
+ return -EINTR;
+ }
+
+ /* Read accelerometer X Y Z axes */
+
+ sample.data_x = adxl345_getreg8(priv, ADXL345_DATAX1);
+ sample.data_x = (sample.data_x << 8) | adxl345_getreg8(priv, ADXL345_DATAX0);
+ sample.data_y = adxl345_getreg8(priv, ADXL345_DATAY1);
+ sample.data_y = (sample.data_y << 8) | adxl345_getreg8(priv, ADXL345_DATAY0);
+ sample.data_z = adxl345_getreg8(priv, ADXL345_DATAZ1);
+ sample.data_z = (sample.data_z << 8) | adxl345_getreg8(priv, ADXL345_DATAZ0);
+
+ /* Return read sample */
+
+ buffer = (FAR char *) &sample;
+
+ sem_post(&priv->exclsem);
+ return sizeof(struct adxl345_sample_s);
+}
+
+/****************************************************************************
+ * Name: adxl345_register
+ *
+ * Description:
+ * This function will register the touchscreen driver as /dev/accelN where N
+ * is the minor device number
+ *
+ * Input Parameters:
+ * handle - The handle previously returned by adxl345_register
+ * minor - The input device minor number
+ *
+ * Returned Value:
+ * Zero is returned on success. Otherwise, a negated errno value is
+ * returned to indicate the nature of the failure.
+ *
+ ****************************************************************************/
+
+int adxl345_register(ADXL345_HANDLE handle, int minor)
+{
+ FAR struct adxl345_dev_s *priv = (FAR struct adxl345_dev_s *)handle;
+ char devname[DEV_NAMELEN];
+ int ret;
+
+ ivdbg("handle=%p minor=%d\n", handle, minor);
+ DEBUGASSERT(priv);
+
+ /* Get exclusive access to the device structure */
+
+ ret = sem_wait(&priv->exclsem);
+ if (ret < 0)
+ {
+ int errval = errno;
+ idbg("ERROR: sem_wait failed: %d\n", errval);
+ return -errval;
+ }
+
+ /* Initialize the structure fields to their default values */
+
+ priv->ofsx = 0;
+ priv->ofsy = 0;
+ priv->ofsz = 0;
+
+ /* Register the character driver */
+
+ snprintf(devname, DEV_NAMELEN, DEV_FORMAT, minor);
+ ret = register_driver(devname, &g_adxl345fops, 0666, priv);
+ if (ret < 0)
+ {
+ idbg("ERROR: Failed to register driver %s: %d\n", devname, ret);
+ sem_post(&priv->exclsem);
+ return ret;
+ }
+
+ /* Indicate that the accelerometer was successfully initialized */
+
+ priv->status |= ADXL345_STAT_INITIALIZED; /* Accelerometer is initialized */
+ sem_post(&priv->exclsem);
+ return ret;
+}
+
+/****************************************************************************
+ * Name: adxl345_worker
+ *
+ * Description:
+ * This is the "bottom half" of the ADXL345 interrupt handler
+ *
+ ****************************************************************************/
+
+static void adxl345_worker(FAR void *arg)
+{
+ FAR struct adxl345_dev_s *priv = (FAR struct adxl345_dev_s *)arg;
+ uint8_t regval;
+
+ DEBUGASSERT(priv && priv->config);
+
+ /* Get the global interrupt status */
+
+ regval = adxl345_getreg8(priv, ADXL345_INT_SOURCE);
+
+ /* Check for a data ready interrupt */
+
+ if ((regval & INT_DATA_READY) != 0)
+ {
+ /* Read accelerometer data to sample */
+
+ priv->sample.data_x = adxl345_getreg8(priv, ADXL345_DATAX1);
+ priv->sample.data_x = (priv->sample.data_x << 8) | adxl345_getreg8(priv, ADXL345_DATAX0);
+ priv->sample.data_y = adxl345_getreg8(priv, ADXL345_DATAY1);
+ priv->sample.data_y = (priv->sample.data_y << 8) | adxl345_getreg8(priv, ADXL345_DATAY0);
+ priv->sample.data_z = adxl345_getreg8(priv, ADXL345_DATAZ1);
+ priv->sample.data_z = (priv->sample.data_z << 8) | adxl345_getreg8(priv, ADXL345_DATAZ0);
+ }
+
+ /* Re-enable the ADXL345 GPIO interrupt */
+
+ priv->config->enable(priv->config, true);
+}
+
+/****************************************************************************
+ * Name: adxl345_interrupt
+ *
+ * Description:
+ * The ADXL345 interrupt handler
+ *
+ ****************************************************************************/
+
+static int adxl345_interrupt(int irq, FAR void *context)
+{
+ FAR struct adxl345_dev_s *priv;
+ FAR struct adxl345_config_s *config;
+ int ret;
+
+ /* Get a pointer the callbacks for convenience (and so the code is not so
+ * ugly).
+ */
+
+ config = priv->config;
+ DEBUGASSERT(config != NULL);
+
+ /* Disable further interrupts */
+
+ config->enable(config, false);
+
+ /* Check if interrupt work is already queue. If it is already busy, then
+ * we already have interrupt processing in the pipeline and we need to do
+ * nothing more.
+ */
+
+ if (work_available(&priv->work))
+ {
+ /* Yes.. Transfer processing to the worker thread. Since ADXL345
+ * interrupts are disabled while the work is pending, no special
+ * action should be required to protect the work queue.
+ */
+
+ ret = work_queue(HPWORK, &priv->work, adxl345_worker, priv, 0);
+ if (ret != 0)
+ {
+ illdbg("Failed to queue work: %d\n", ret);
+ }
+ }
+
+ /* Clear any pending interrupts and return success */
+
+ config->clear(config);
+ return OK;
+}
+
+/****************************************************************************
+ * Name: adxl345_checkid
+ *
+ * Description:
+ * Read and verify the ADXL345 chip ID
+ *
+ ****************************************************************************/
+
+static int adxl345_checkid(FAR struct adxl345_dev_s *priv)
+{
+ uint8_t devid = 0;
+
+ /* Read device ID */
+
+ devid = adxl345_getreg8(priv, ADXL345_DEVID);
+ ivdbg("devid: %04x\n", devid);
+
+ if (devid != (uint16_t) DEVID)
+ {
+ /* ID is not Correct */
+
+ return -ENODEV;
+ }
+
+ return OK;
+}
+
+/****************************************************************************
+ * Name: adxl345_reset
+ *
+ * Description:
+ * Reset the ADXL345
+ *
+ ****************************************************************************/
+
+static void adxl345_reset(FAR struct adxl345_dev_s *priv)
+{
+ /* ADXL345 doesn't have software reset */
+
+ /* Wait a bit to make the GOD of TIME happy */
+
+ usleep(20*1000);
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: adxl345_instantiate
+ *
+ * Description:
+ * Instantiate and configure the ADXL345 device driver to use the provided
+ * I2C or SPIdevice instance.
+ *
+ * Input Parameters:
+ * dev - An I2C or SPI driver instance
+ * config - Persistent board configuration data
+ *
+ * Returned Value:
+ * A non-zero handle is returned on success. This handle may then be used
+ * to configure the ADXL345 driver as necessary. A NULL handle value is
+ * returned on failure.
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_ADXL345_SPI
+ADXL345_HANDLE adxl345_instantiate(FAR struct spi_dev_s *dev,
+ FAR struct adxl345_config_s *config)
+#else
+ADXL345_HANDLE adxl345_instantiate(FAR struct i2c_dev_s *dev,
+ FAR struct adxl345_config_s *config)
+#endif
+{
+ FAR struct adxl345_dev_s *priv;
+ uint8_t regval;
+ int ret;
+
+ /* Use the one-and-only ADXL345 driver instance */
+
+ priv = &g_adxl345;
+
+ /* Initialize the device state structure */
+
+ sem_init(&priv->exclsem, 0, 1);
+ priv->config = config;
+
+#ifdef CONFIG_ADXL345_SPI
+ priv->spi = dev;
+
+ /* If this SPI bus is not shared, then we can config it now.
+ * If it is shared, then other device could change our config,
+ * then just configure before sending data.
+ */
+
+ #ifdef CONFIG_SPI_OWNBUS
+ /* Configure SPI for the ADXL345 */
+
+ SPI_SELECT(priv->spi, SPIDEV_GSENSOR, true);
+ SPI_SETMODE(priv->spi, SPIDEV_MODE3);
+ SPI_SETBITS(priv->spi, 8);
+ SPI_SETFREQUENCY(priv->spi, ADXL345_SPI_MAXFREQUENCY);
+ SPI_SELECT(priv->spi, SPIDEV_GSENSOR, false);
+ #endif
+#else
+ priv->i2c = dev;
+
+ /* Set the I2C address and frequency. REVISIT: This logic would be
+ * insufficient if we share the I2C bus with any other devices that also
+ * modify the address and frequency.
+ */
+
+ I2C_SETADDRESS(dev, config->address, 7);
+ I2C_SETFREQUENCY(dev, config->frequency);
+#endif
+
+ /* Read and verify the ADXL345 device ID */
+
+ ret = adxl345_checkid(priv);
+ if (ret < 0)
+ {
+ illdbg("Wrong Device ID!\n");
+ return NULL;
+ }
+
+ /* Generate ADXL345 Software reset */
+
+ adxl345_reset(priv);
+
+ /* Configure the interrupt output pin to generate interrupts on high or low level. */
+
+ regval = adxl345_getreg8(priv, ADXL345_DATA_FORMAT);
+#ifdef CONFIG_ADXL345_ACTIVELOW
+ regval |= DATA_FMT_INT_INVERT; /* Pin polarity: Active low / falling edge */
+#else
+ regval &= ~DATA_FMT_INT_INVERT; /* Pin polarity: Active high / rising edge */
+#endif
+ adxl345_putreg8(priv, ADXL345_DATA_FORMAT, regval);
+
+ /* Attach the ADXL345 interrupt handler. */
+
+ config->attach(config, adxl345_interrupt);
+
+ /* Leave standby mode */
+
+ adxl345_putreg8(priv, ADXL345_POWER_CTL, POWER_CTL_MEASURE);
+
+ config->clear(config);
+ config->enable(config, true);
+
+ /* Enable interrupts */
+
+ adxl345_putreg8(priv, ADXL345_INT_ENABLE, INT_DATA_READY);
+
+ /* Return our private data structure as an opaque handle */
+ return (ADXL345_HANDLE)priv;
+}
+
+#endif /* CONFIG_SENSORS_ADXL345 */
diff --git a/nuttx/drivers/sensors/adxl345_i2c.c b/nuttx/drivers/sensors/adxl345_i2c.c
new file mode 100644
index 000000000..8db213a3c
--- /dev/null
+++ b/nuttx/drivers/sensors/adxl345_i2c.c
@@ -0,0 +1,212 @@
+/****************************************************************************
+ * drivers/sensors/adxl345_i2c.c
+ *
+ * Copyright (C) 2014 Alan Carvalho de Assis. All rights reserved.
+ * Author: Alan Carvalho de Assis <acassis@gmail.com>
+ * Based on STME811 driver
+ * 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 <unistd.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/kmalloc.h>
+#include <nuttx/sensors/adxl345.h>
+
+#include "adxl345.h"
+
+#if defined(CONFIG_SENSORS_ADXL345)
+
+/****************************************************************************
+ * Name: adxl345_getreg8
+ *
+ * Description:
+ * Read from an 8-bit ADXL345 register
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_ADXL345_I2C
+uint8_t adxl345_getreg8(FAR struct adxl345_dev_s *priv, uint8_t regaddr)
+{
+ /* 8-bit data read sequence:
+ *
+ * Start - I2C_Write_Address - ADXL345_Reg_Address -
+ * Repeated_Start - I2C_Read_Address - ADXL345_Read_Data - STOP
+ */
+
+ struct i2c_msg_s msg[2];
+ uint8_t regval;
+ int ret;
+
+ /* Setup 8-bit ADXL345 address write message */
+
+ msg[0].addr = priv->config->address; /* 7-bit address */
+ msg[0].flags = 0; /* Write transaction, beginning with START */
+ msg[0].buffer = &regaddr; /* Transfer from this address */
+ msg[0].length = 1; /* Send one byte following the address
+ * (no STOP) */
+
+ /* Set up the 8-bit ADXL345 data read message */
+
+ msg[1].addr = priv->config->address; /* 7-bit address */
+ msg[1].flags = I2C_M_READ; /* Read transaction, beginning with Re-START */
+ msg[1].buffer = &regval; /* Transfer to this address */
+ msg[1].length = 1; /* Receive one byte following the address
+ * (then STOP) */
+
+ /* Perform the transfer */
+
+ ret = I2C_TRANSFER(priv->i2c, msg, 2);
+ if (ret < 0)
+ {
+ idbg("I2C_TRANSFER failed: %d\n", ret);
+ return 0;
+ }
+
+#ifdef CONFIG_ADXL345_REGDEBUG
+ dbg("%02x->%02x\n", regaddr, regval);
+#endif
+ return regval;
+}
+#endif
+
+/****************************************************************************
+ * Name: adxl345_putreg8
+ *
+ * Description:
+ * Write a value to an 8-bit ADXL345 register
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_ADXL345_I2C
+void adxl345_putreg8(FAR struct adxl345_dev_s *priv,
+ uint8_t regaddr, uint8_t regval)
+{
+ /* 8-bit data read sequence:
+ *
+ * Start - I2C_Write_Address - ADXL345_Reg_Address - ADXL345_Write_Data - STOP
+ */
+
+ struct i2c_msg_s msg;
+ uint8_t txbuffer[2];
+ int ret;
+
+#ifdef CONFIG_ADXL345_REGDEBUG
+ dbg("%02x<-%02x\n", regaddr, regval);
+#endif
+
+ /* Setup to the data to be transferred. Two bytes: The ADXL345 register
+ * address followed by one byte of data.
+ */
+
+ txbuffer[0] = regaddr;
+ txbuffer[1] = regval;
+
+ /* Setup 8-bit ADXL345 address write message */
+
+ msg.addr = priv->config->address; /* 7-bit address */
+ msg.flags = 0; /* Write transaction, beginning with START */
+ msg.buffer = txbuffer; /* Transfer from this address */
+ msg.length = 2; /* Send two byte following the address
+ * (then STOP) */
+
+ /* Perform the transfer */
+
+ ret = I2C_TRANSFER(priv->i2c, &msg, 1);
+ if (ret < 0)
+ {
+ idbg("I2C_TRANSFER failed: %d\n", ret);
+ }
+}
+#endif
+
+/****************************************************************************
+ * Name: adxl345_getreg16
+ *
+ * Description:
+ * Read 16-bits of data from an STMPE-11 register
+ *
+ ****************************************************************************/
+
+#ifdef CONFIG_ADXL345_I2C
+uint16_t adxl345_getreg16(FAR struct adxl345_dev_s *priv, uint8_t regaddr)
+{
+ /* 16-bit data read sequence:
+ *
+ * Start - I2C_Write_Address - ADXL345_Reg_Address -
+ * Repeated_Start - I2C_Read_Address - ADXL345_Read_Data_1 -
+ * ADXL345_Read_Data_2 - STOP
+ */
+
+ struct i2c_msg_s msg[2];
+ uint8_t rxbuffer[2];
+ int ret;
+
+ /* Setup 8-bit ADXL345 address write message */
+
+ msg[0].addr = priv->config->address; /* 7-bit address */
+ msg[0].flags = 0; /* Write transaction, beginning with START */
+ msg[0].buffer = &regaddr; /* Transfer from this address */
+ msg[0].length = 1; /* Send one byte following the address
+ * (no STOP) */
+
+ /* Set up the 8-bit ADXL345 data read message */
+
+ msg[1].addr = priv->config->address; /* 7-bit address */
+ msg[1].flags = I2C_M_READ; /* Read transaction, beginning with Re-START */
+ msg[1].buffer = rxbuffer; /* Transfer to this address */
+ msg[1].length = 2; /* Receive two bytes following the address
+ * (then STOP) */
+
+ /* Perform the transfer */
+
+ ret = I2C_TRANSFER(priv->i2c, msg, 2);
+ if (ret < 0)
+ {
+ idbg("I2C_TRANSFER failed: %d\n", ret);
+ return 0;
+ }
+
+#ifdef CONFIG_ADXL345_REGDEBUG
+ dbg("%02x->%02x%02x\n", regaddr, rxbuffer[0], rxbuffer[1]);
+#endif
+ return (uint16_t)rxbuffer[0] << 8 | (uint16_t)rxbuffer[1];
+}
+#endif
+
+#endif /* CONFIG_SENSORS_ADXL345 */
diff --git a/nuttx/drivers/sensors/adxl345_spi.c b/nuttx/drivers/sensors/adxl345_spi.c
new file mode 100644
index 000000000..1e42273c5
--- /dev/null
+++ b/nuttx/drivers/sensors/adxl345_spi.c
@@ -0,0 +1,213 @@
+/****************************************************************************
+ * drivers/sensors/adxl345_spi.c
+ *
+ * Copyright (C) 2014 Alan Carvalho de Assis. All rights reserved.
+ * Author: Alan Carvalho de Assis <acassis@gmail.com>
+ * Based on STME811 driver
+ * 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 <unistd.h>
+#include <errno.h>
+#include <debug.h>
+
+#include <nuttx/kmalloc.h>
+#include <nuttx/sensors/adxl345.h>
+
+#include "adxl345.h"
+
+#if defined(CONFIG_SENSORS_ADXL345) && defined(CONFIG_ADXL345_SPI)
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: adxl345_configspi
+ *
+ * Description:
+ *
+ ****************************************************************************/
+
+#ifndef CONFIG_SPI_OWNBUS
+static inline void adxl345_configspi(FAR struct spi_dev_s *spi)
+{
+ /* Configure SPI for the ADXL345 */
+
+ SPI_SELECT(spi, SPIDEV_GSENSOR, true);
+ SPI_SETMODE(spi, SPIDEV_MODE3);
+ SPI_SETBITS(spi, 8);
+ SPI_SETFREQUENCY(spi, ADXL345_SPI_MAXFREQUENCY);
+ SPI_SELECT(spi, SPIDEV_GSENSOR, false);
+}
+#endif
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: adxl345_getreg8
+ *
+ * Description:
+ * Read from an 8-bit ADXL345 register
+ *
+ ****************************************************************************/
+
+uint8_t adxl345_getreg8(FAR struct adxl345_dev_s *priv, uint8_t regaddr)
+{
+ uint8_t regval;
+
+ /* If SPI bus is shared then lock and configure it */
+
+#ifndef CONFIG_SPI_OWNBUS
+ (void)SPI_LOCK(priv->spi, true);
+ adxl345_configspi(priv->spi);
+#endif
+
+ /* Select the ADXL345 */
+
+ SPI_SELECT(priv->spi, SPIDEV_GSENSOR, true);
+
+ /* Send register to read and get the next byte */
+
+ (void)SPI_SEND(priv->spi, regaddr);
+ SPI_RECVBLOCK(priv->spi, &regval, 1);
+
+ /* Deselect the ADXL345 */
+
+ SPI_SELECT(priv->spi, SPIDEV_GSENSOR, false);
+
+ /* Unlock bus */
+
+#ifndef CONFIG_SPI_OWNBUS
+ (void)SPI_LOCK(priv->spi, false);
+#endif
+
+#ifdef CONFIG_ADXL345_REGDEBUG
+ dbg("%02x->%02x\n", regaddr, regval);
+#endif
+ return regval;
+}
+
+/****************************************************************************
+ * Name: adxl345_putreg8
+ *
+ * Description:
+ * Write a value to an 8-bit ADXL345 register
+ *
+ ****************************************************************************/
+
+void adxl345_putreg8(FAR struct adxl345_dev_s *priv, uint8_t regaddr,
+ uint8_t regval)
+{
+#ifdef CONFIG_ADXL345_REGDEBUG
+ dbg("%02x<-%02x\n", regaddr, regval);
+#endif
+
+ /* If SPI bus is shared then lock and configure it */
+
+#ifndef CONFIG_SPI_OWNBUS
+ (void)SPI_LOCK(priv->spi, true);
+ adxl345_configspi(priv->spi);
+#endif
+
+ /* Select the ADXL345 */
+
+ SPI_SELECT(priv->spi, SPIDEV_GSENSOR, true);
+
+ /* Send register address and set the value */
+
+ (void)SPI_SEND(priv->spi, regaddr);
+ (void)SPI_SEND(priv->spi, regval);
+
+ /* Deselect the ADXL345 */
+
+ SPI_SELECT(priv->spi, SPIDEV_GSENSOR, false);
+
+ /* Unlock bus */
+#ifndef CONFIG_SPI_OWNBUS
+ (void)SPI_LOCK(priv->spi, false);
+#endif
+}
+
+/****************************************************************************
+ * Name: adxl345_getreg16
+ *
+ * Description:
+ * Read 16-bits of data from an ADXL345 register
+ *
+ ****************************************************************************/
+
+uint16_t adxl345_getreg16(FAR struct adxl345_dev_s *priv, uint8_t regaddr)
+{
+ uint16_t regval;
+
+ /* If SPI bus is shared then lock and configure it */
+
+#ifndef CONFIG_SPI_OWNBUS
+ (void)SPI_LOCK(priv->spi, true);
+ adxl345_configspi(priv->spi);
+#endif
+
+ /* Select the ADXL345 */
+
+ SPI_SELECT(priv->spi, SPIDEV_GSENSOR, true);
+
+ /* Send register to read and get the next 2 bytes */
+
+ (void)SPI_SEND(priv->spi, regaddr);
+ SPI_RECVBLOCK(priv->spi, &regval, 2);
+
+ /* Deselect the ADXL345 */
+
+ SPI_SELECT(priv->spi, SPIDEV_GSENSOR, false);
+
+ /* Unlock bus */
+
+#ifndef CONFIG_SPI_OWNBUS
+ (void)SPI_LOCK(priv->spi, false);
+#endif
+
+#ifdef CONFIG_ADXL345_REGDEBUG
+ dbg("%02x->%04x\n", regaddr, regval);
+#endif
+
+ return regval;
+}
+
+#endif /* CONFIG_SENSORS_ADXL345 && CONFIG_ADXL345_SPI*/