aboutsummaryrefslogtreecommitdiff
path: root/nuttx/configs/px4fmu/src/drv_hmc5833l.c
diff options
context:
space:
mode:
Diffstat (limited to 'nuttx/configs/px4fmu/src/drv_hmc5833l.c')
-rw-r--r--nuttx/configs/px4fmu/src/drv_hmc5833l.c352
1 files changed, 352 insertions, 0 deletions
diff --git a/nuttx/configs/px4fmu/src/drv_hmc5833l.c b/nuttx/configs/px4fmu/src/drv_hmc5833l.c
new file mode 100644
index 000000000..0e6dbe2ac
--- /dev/null
+++ b/nuttx/configs/px4fmu/src/drv_hmc5833l.c
@@ -0,0 +1,352 @@
+/*
+ * Copyright (C) 2012 Lorenz Meier. 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 of the author or the names of 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 Honeywell/ST HMC5883L MEMS magnetometer
+ */
+
+#include <nuttx/config.h>
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <debug.h>
+#include <errno.h>
+
+#include <nuttx/i2c.h>
+#include <arch/board/board.h>
+
+#include "chip.h"
+#include "px4fmu-internal.h"
+
+#include <arch/board/drv_hmc5883l.h>
+
+#define ADDR_CONF_A 0x00
+#define ADDR_CONF_B 0x01
+#define ADDR_MODE 0x02
+#define ADDR_DATA_OUT_X_MSB 0x03
+#define ADDR_DATA_OUT_X_LSB 0x04
+#define ADDR_DATA_OUT_Z_MSB 0x05
+#define ADDR_DATA_OUT_Z_LSB 0x06
+#define ADDR_DATA_OUT_Y_MSB 0x07
+#define ADDR_DATA_OUT_Y_LSB 0x08
+#define ADDR_STATUS 0x09
+#define ADDR_ID_A 0x10
+#define ADDR_ID_B 0x11
+#define ADDR_ID_C 0x12
+
+#define HMC5883L_ADDRESS 0x1E
+
+/* modes not changeable outside of driver */
+#define HMC5883L_MODE_NORMAL (0 << 0) /* default */
+#define HMC5883L_MODE_POSITIVE_BIAS (1 << 0) /* positive bias */
+#define HMC5883L_MODE_NEGATIVE_BIAS (1 << 1) /* negative bias */
+
+#define HMC5883L_AVERAGING_1 (0 << 5) /* conf a register */
+#define HMC5883L_AVERAGING_2 (1 << 5)
+#define HMC5883L_AVERAGING_4 (2 << 5)
+#define HMC5883L_AVERAGING_8 (3 << 5)
+
+#define MODE_REG_CONTINOUS_MODE (0 << 0)
+#define MODE_REG_SINGLE_MODE (1 << 0) /* default */
+
+#define STATUS_REG_DATA_OUT_LOCK (1 << 1) /* page 16: set if data is only partially read, read device to reset */
+#define STATUS_REG_DATA_READY (1 << 0) /* page 16: set if all axes have valid measurements */
+
+#define ID_A_WHO_AM_I 'H'
+#define ID_B_WHO_AM_I '4'
+#define ID_C_WHO_AM_I '3'
+
+static FAR struct hmc5883l_dev_s hmc5883l_dev;
+
+static ssize_t hmc5883l_read(struct file *filp, FAR char *buffer, size_t buflen);
+static int hmc5883l_ioctl(struct file *filp, int cmd, unsigned long arg);
+
+static const struct file_operations hmc5883l_fops = {
+ .open = 0,
+ .close = 0,
+ .read = hmc5883l_read,
+ .write = 0,
+ .seek = 0,
+ .ioctl = hmc5883l_ioctl,
+#ifndef CONFIG_DISABLE_POLL
+ .poll = 0
+#endif
+};
+
+struct hmc5883l_dev_s
+{
+ struct i2c_dev_s *i2c;
+ uint8_t rate;
+ struct hmc5883l_buffer *buffer;
+};
+
+static int hmc5883l_write_reg(uint8_t address, uint8_t data);
+static int hmc5883l_read_reg(uint8_t address);
+static int hmc5883l_reset(void);
+
+static int
+hmc5883l_write_reg(uint8_t address, uint8_t data)
+{
+ uint8_t cmd[] = {address, data};
+ return I2C_WRITE(hmc5883l_dev.i2c, cmd, 2);
+}
+
+static int
+hmc5883l_read_reg(uint8_t address)
+{
+ uint8_t cmd = address;
+ uint8_t data;
+
+ int ret = I2C_WRITEREAD(hmc5883l_dev.i2c, &cmd, 1, &data, 1);
+ /* return data on success, error code on failure */
+ if (ret == OK) {
+ ret = data;
+ }
+ return ret;
+}
+
+static int
+hmc5883l_set_range(uint8_t range)
+{
+ I2C_SETADDRESS(hmc5883l_dev.i2c, HMC5883L_ADDRESS, 7);
+
+
+ /* mask out illegal bit positions */
+ uint8_t write_range = range; //& REG4_RANGE_MASK;
+ /* immediately return if user supplied invalid value */
+ if (write_range != range) return EINVAL;
+ /* set remaining bits to a sane value */
+// write_range |= REG4_BDU;
+ /* write to device */
+ hmc5883l_write_reg(ADDR_CONF_B, write_range);
+ /* return 0 if register value is now written value, 1 if unchanged */
+ return !(hmc5883l_read_reg(ADDR_CONF_B) == write_range);
+}
+
+static int
+hmc5883l_set_rate(uint8_t rate)
+{
+ I2C_SETADDRESS(hmc5883l_dev.i2c, HMC5883L_ADDRESS, 7);
+ /* mask out illegal bit positions */
+ uint8_t write_rate = rate;// & REG1_RATE_LP_MASK;
+ /* immediately return if user supplied invalid value */
+ if (write_rate != rate) return EINVAL;
+ /* set remaining bits to a sane value */
+// write_rate |= REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE;
+ write_rate |= HMC5883L_AVERAGING_8;
+ /* write to device */
+ hmc5883l_write_reg(ADDR_CONF_A, write_rate);
+ /* return 0 if register value is now written value, 1 if unchanged */
+ return !(hmc5883l_read_reg(ADDR_CONF_A) == write_rate);
+}
+
+static bool
+read_values(int16_t *data)
+{
+ struct { /* status register and data as read back from the device */
+ int16_t x;
+ int16_t z;
+ int16_t y;
+ uint8_t status;
+ } __attribute__((packed)) hmc_report;
+ hmc_report.status = 0;
+
+ static int read_err_count = 0;
+
+ /* exchange the report structure with the device */
+
+ uint8_t cmd = ADDR_DATA_OUT_X_MSB;
+
+ int ret = 0;
+
+ I2C_SETADDRESS(hmc5883l_dev.i2c, HMC5883L_ADDRESS, 7);
+
+ /* set device into single mode, trigger next measurement */
+ ret = hmc5883l_write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE);
+
+ /* Only execute consecutive steps on success */
+ if (ret == OK)
+ {
+ cmd = ADDR_DATA_OUT_X_MSB;
+ ret = I2C_WRITEREAD(hmc5883l_dev.i2c, &cmd, 1, (uint8_t*)&hmc_report, 6);
+ if (ret == OK)
+ {
+ /* Six bytes to read, stop if timed out */
+ int hmc_status = hmc5883l_read_reg(ADDR_STATUS);
+ if (hmc_status < 0)
+ {
+ if (ret == ETIMEDOUT) hmc5883l_reset();
+ ret = hmc_status;
+ }
+ else
+ {
+ hmc_report.status = hmc_status;
+ ret = OK;
+ }
+ }
+ else
+ {
+ if (ret == ETIMEDOUT) hmc5883l_reset();
+ }
+ }
+ else
+ {
+ if (ret == ETIMEDOUT) hmc5883l_reset();
+ }
+
+ if (ret != OK)
+ {
+ read_err_count++;
+ /* If the last reads failed as well, reset the bus and chip */
+ if (read_err_count > 3) hmc5883l_reset();
+
+ *get_errno_ptr() = -ret;
+ } else {
+ read_err_count = 0;
+ /* write values, and exchange the two 8bit blocks (big endian to little endian) */
+ data[0] = ((hmc_report.x & 0x00FF) << 8) | ((hmc_report.x & 0xFF00) >> 8);
+ data[1] = ((hmc_report.y & 0x00FF) << 8) | ((hmc_report.y & 0xFF00) >> 8);
+ data[2] = ((hmc_report.z & 0x00FF) << 8) | ((hmc_report.z & 0xFF00) >> 8);
+ if ((hmc_report.status & STATUS_REG_DATA_READY) > 0)
+ {
+ ret = 6;
+ } else {
+ ret = -EAGAIN;
+ }
+ }
+
+ /* return len if new data is available, error else. hmc_report.status is 0 on errors */
+ return ret;
+}
+
+static ssize_t
+hmc5883l_read(struct file *filp, char *buffer, size_t buflen)
+{
+ /* if the buffer is large enough, and data are available, return success */
+ if (buflen >= 6) {
+ return read_values((int16_t *)buffer);
+ }
+
+ /* buffer too small */
+ *get_errno_ptr() = ENOSPC;
+ return -ERROR;
+}
+
+static int
+hmc5883l_ioctl(struct file *filp, int cmd, unsigned long arg)
+{
+ int result = ERROR;
+
+ switch (cmd) {
+ case HMC5883L_SETRATE:
+ result = hmc5883l_set_rate(arg);
+ break;
+
+ case HMC5883L_SETRANGE:
+ result = hmc5883l_set_range(arg);
+ break;
+//
+// case HMC5883L_SETBUFFER:
+// hmc5883l_dev.buffer = (struct hmc5883l_buffer *)arg;
+// result = 0;
+// break;
+
+ case HMC5883L_RESET:
+ result = hmc5883l_reset();
+ break;
+ }
+
+ if (result)
+ errno = EINVAL;
+ return result;
+}
+
+int hmc5883l_reset()
+{
+ int ret;
+ printf("[hmc5883l drv] Resettet I2C2 BUS\n");
+ up_i2cuninitialize(hmc5883l_dev.i2c);
+ hmc5883l_dev.i2c = up_i2cinitialize(2);
+ I2C_SETFREQUENCY(hmc5883l_dev.i2c, 400000);
+ // up_i2creset(hmc5883l_dev.i2c);
+ //I2C_SETADDRESS(hmc5883l_dev.i2c, HMC5883L_ADDRESS, 7);
+ //hmc5883l_set_range(HMC5883L_RANGE_0_88GA);
+ //hmc5883l_set_rate(HMC5883L_RATE_75HZ);
+ /* set device into single mode, start measurement */
+ //ret = hmc5883l_write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE);
+ return ret;
+}
+
+int
+hmc5883l_attach(struct i2c_dev_s *i2c)
+{
+ int result = ERROR;
+
+ hmc5883l_dev.i2c = i2c;
+
+// I2C_LOCK(hmc5883l_dev.i2c, true);
+ I2C_SETADDRESS(hmc5883l_dev.i2c, HMC5883L_ADDRESS, 7);
+
+ uint8_t cmd = ADDR_STATUS;
+ uint8_t status_id[4] = {0, 0, 0, 0};
+
+
+ int ret = I2C_WRITEREAD(i2c, &cmd, 1, status_id, 4);
+
+ /* verify that the device is attached and functioning */
+ if ((ret >= 0) && (status_id[1] == ID_A_WHO_AM_I) && (status_id[2] == ID_B_WHO_AM_I) && (status_id[3] == ID_C_WHO_AM_I)) {
+
+ /* set update rate to 75 Hz */
+ /* set 0.88 Ga range */
+ if ((ret != 0) || (hmc5883l_set_range(HMC5883L_RANGE_0_88GA) != 0) ||
+ (hmc5883l_set_rate(HMC5883L_RATE_75HZ) != 0))
+ {
+ errno = EIO;
+ } else {
+
+ /* set device into single mode, start measurement */
+ ret = hmc5883l_write_reg(ADDR_MODE, MODE_REG_SINGLE_MODE);
+
+ /* make ourselves available */
+ register_driver("/dev/hmc5883l", &hmc5883l_fops, 0666, NULL);
+
+ result = 0;
+ }
+
+ } else {
+ errno = EIO;
+ }
+
+
+
+ return result;
+}