1 files changed, 272 insertions, 0 deletions
diff --git a/nuttx/configs/px4fmu/src/drv_lis331.c b/nuttx/configs/px4fmu/src/drv_lis331.c
new file mode 100644
index 000000000..fd477b46f
--- /dev/null
+++ b/nuttx/configs/px4fmu/src/drv_lis331.c
@@ -0,0 +1,272 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * 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 PX4 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.
+ *
+ ****************************************************************************/
+
+/*
+ * Driver for the ST LIS331 MEMS accelerometer
+ */
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/spi.h>
+#include <arch/board/board.h>
+
+#include "up_arch.h"
+#include "chip.h"
+#include "stm32_internal.h"
+#include "px4fmu-internal.h"
+
+#include <arch/board/drv_lis331.h>
+
+/*
+ * LIS331 registers
+ */
+
+#define DIR_READ		(1<<7)
+#define DIR_WRITE		(0<<7)
+#define ADDR_INCREMENT		(1<<6)
+
+#define ADDR_WHO_AM_I		0x0f
+#define WHO_I_AM		0x32
+
+#define ADDR_CTRL_REG1		0x20		/* sample rate constants are in the public header */
+#define REG1_POWER_NORMAL		(1<<5)
+#define REG1_RATE_MASK			(3<<3)
+#define REG1_Z_ENABLE			(1<<2)
+#define REG1_Y_ENABLE			(1<<1)
+#define REG1_X_ENABLE			(1<<0)
+
+#define ADDR_CTRL_REG2		0x21
+
+#define ADDR_CTRL_REG3		0x22
+
+#define ADDR_CTRL_REG4		0x23
+#define REG4_BDU			(1<<7)
+#define REG4_BIG_ENDIAN			(1<<6)
+#define REG4_RANGE_MASK			(3<<4)
+#define REG4_SPI_3WIRE			(1<<0)
+
+#define ADDR_CTRL_REG5		0x24
+
+#define ADDR_HP_FILTER_RESET	0x25
+#define ADDR_REFERENCE		0x26
+#define ADDR_STATUS_REG		0x27
+#define STATUS_ZYXOR			(1<<7)
+#define STATUS_ZOR				(1<<6)
+#define STATUS_YOR				(1<<5)
+#define STATUS_XOR				(1<<4)
+#define STATUS_ZYXDA			(1<<3)
+#define STATUS_ZDA				(1<<2)
+#define STATUS_YDA				(1<<1)
+#define STATUS_XDA				(1<<0)
+
+#define ADDR_OUT_X		0x28	/* 16 bits */
+#define ADDR_OUT_Y		0x2A	/* 16 bits */
+#define ADDR_OUT_Z		0x2C	/* 16 bits */
+
+
+static ssize_t	lis331_read(struct file *filp, FAR char *buffer, size_t buflen);
+static int	lis331_ioctl(struct file *filp, int cmd, unsigned long arg);
+
+static const struct file_operations lis331_fops = {
+	.read  = lis331_read,
+	.ioctl = lis331_ioctl,
+};
+
+struct lis331_dev_s
+{
+	struct spi_dev_s	*spi;
+	int			spi_id;
+
+	uint8_t			rate;
+	struct lis331_buffer	*buffer;
+};
+
+static struct lis331_dev_s	lis331_dev;
+
+static void	write_reg(uint8_t address, uint8_t data);
+static uint8_t	read_reg(uint8_t address);
+static bool	read_fifo(uint16_t *data);
+static void	set_range(uint8_t range);
+static void	set_rate(uint8_t rate);
+
+static void
+write_reg(uint8_t address, uint8_t data)
+{
+	uint8_t cmd[2] = { address | DIR_WRITE, data };
+
+	SPI_SELECT(lis331_dev.spi, lis331_dev.spi_id, true);
+      	SPI_SNDBLOCK(lis331_dev.spi, &cmd, sizeof(cmd));
+	SPI_SELECT(lis331_dev.spi, lis331_dev.spi_id, false);
+}
+
+static uint8_t
+read_reg(uint8_t address)
+{
+	uint8_t	cmd[2] = {address | DIR_READ, 0};
+	uint8_t data[2];
+
+	SPI_SELECT(lis331_dev.spi, lis331_dev.spi_id, true);
+	SPI_EXCHANGE(lis331_dev.spi, cmd, data, sizeof(cmd));
+	SPI_SELECT(lis331_dev.spi, lis331_dev.spi_id, false);
+
+	return data[1];	
+}
+
+static bool
+read_fifo(uint16_t *data)
+{
+	struct {					/* status register and data as read back from the device */
+		uint8_t		cmd;
+		uint8_t		status;
+		int16_t		x;
+		int16_t		y;
+		int16_t		z;
+	} __attribute__((packed))	report;
+
+	report.cmd = ADDR_STATUS_REG | DIR_READ | ADDR_INCREMENT;
+
+	/* exchange the report structure with the device */
+	SPI_LOCK(lis331_dev.spi, true);
+	SPI_SELECT(lis331_dev.spi, lis331_dev.spi_id, true);
+	SPI_EXCHANGE(lis331_dev.spi, &report, &report, sizeof(report));
+	SPI_SELECT(lis331_dev.spi, lis331_dev.spi_id, false);
+	SPI_LOCK(lis331_dev.spi, false);
+
+	data[0] = report.x;
+	data[1] = report.y;
+	data[2] = report.z;
+
+	return report.status & STATUS_ZYXDA;
+}
+
+static void
+set_range(uint8_t range)
+{
+	range &= REG4_RANGE_MASK;
+	write_reg(ADDR_CTRL_REG4, range | REG4_BDU);
+}
+
+static void
+set_rate(uint8_t rate)
+{
+	rate &= REG1_RATE_MASK;
+	write_reg(ADDR_CTRL_REG1, rate | REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE);
+}
+
+static ssize_t
+lis331_read(struct file *filp, char *buffer, size_t buflen)
+{
+	/* if the buffer is large enough, and data are available, return success */
+	if (buflen >= 12) {
+		if (read_fifo((uint16_t *)buffer))
+			return 12;
+
+		/* no data */
+		return 0;
+	}
+
+	/* buffer too small */
+	errno = ENOSPC;
+	return ERROR;
+}
+
+static int
+lis331_ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+	int	result = ERROR;
+
+	switch (cmd) {
+	case LIS331_SETRATE:
+		if ((arg & REG1_RATE_MASK) == arg) {
+			set_rate(arg);
+			result = 0;
+			lis331_dev.rate = arg;
+		}
+		break;
+
+	case LIS331_SETRANGE:
+		if ((arg & REG4_RANGE_MASK) == arg) {
+			set_range(arg);
+			result = 0;
+		}
+		break;
+
+	case LIS331_SETBUFFER:
+		lis331_dev.buffer = (struct lis331_buffer *)arg;
+		result = 0;
+		break;
+	}
+
+	if (result)
+		errno = EINVAL;
+	return result;
+}
+
+int
+lis331_attach(struct spi_dev_s *spi, int spi_id)
+{
+	int	result = ERROR;
+	
+	lis331_dev.spi = spi;
+
+	SPI_LOCK(lis331_dev.spi, true);
+
+	/* verify that the device is attached and functioning */
+	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
+
+		/* set default configuration */
+		write_reg(ADDR_CTRL_REG2, 0);	/* disable interrupt-generating high-pass filters */
+		write_reg(ADDR_CTRL_REG3, 0);	/* no interrupts - we don't use them */
+		write_reg(ADDR_CTRL_REG5, 0);	/* disable wake-on-interrupt */
+
+		set_range(LIS331_RANGE_4G);
+		set_rate(LIS331_RATE_400Hz);	/* takes device out of low-power mode */
+
+		/* make ourselves available */
+		register_driver("/dev/lis331", &lis331_fops, 0666, NULL);
+
+		result = 0;
+	} else {
+		errno = EIO;
+	}
+
+	SPI_LOCK(lis331_dev.spi, false);
+
+	return result;
+}
+