diff options
Diffstat (limited to 'nuttx/configs/px4fmu/src/drv_hmc5833l.c')
-rw-r--r-- | nuttx/configs/px4fmu/src/drv_hmc5833l.c | 386 |
1 files changed, 0 insertions, 386 deletions
diff --git a/nuttx/configs/px4fmu/src/drv_hmc5833l.c b/nuttx/configs/px4fmu/src/drv_hmc5833l.c deleted file mode 100644 index d51d654f7..000000000 --- a/nuttx/configs/px4fmu/src/drv_hmc5833l.c +++ /dev/null @@ -1,386 +0,0 @@ -/* - * 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. - */ - -/** - * @file drv_hmc5883l.c - * 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 bool hmc5883l_calibration_enabled = false; - -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 int -hmc5883l_set_mode(uint8_t mode) -{ - // I2C_SETADDRESS(hmc5883l_dev.i2c, HMC5883L_ADDRESS, 7); - // /* mask out illegal bit positions */ - // uint8_t write_mode = mode & 0x03; - // /* immediately return if user supplied invalid value */ - // if (write_mode != mode) return EINVAL; - // /* set mode */ - // write_mode |= hmc5883l_read_reg(ADDR_CONF_A); - // /* set remaining bits to a sane value */ - // write_mode |= HMC5883L_AVERAGING_8; - // /* write to device */ - // hmc5883l_write_reg(ADDR_CONF_A, write_mode); - // /* return 0 if register value is now written value, 1 if unchanged */ - // return !(hmc5883l_read_reg(ADDR_CONF_A) == write_mode); -} - -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 (hmc_status == ETIMEDOUT) - hmc5883l_reset(); - ret = hmc_status; - } - else - { - hmc_report.status = hmc_status; - ret = OK; - } - } - else - { - if (ret == ETIMEDOUT || ret == -ETIMEDOUT) hmc5883l_reset(); - } - } - else - { - if (ret == ETIMEDOUT || 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); - // XXX TODO - // write mode, range and lp-frequency enum values into data[3]-[6] - if ((hmc_report.status & STATUS_REG_DATA_READY) > 0) - { - ret = 14; - } 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 >= 14) { - 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_CALIBRATION_ON: - hmc5883l_calibration_enabled = true; - result = OK; - break; - - case HMC5883L_CALIBRATION_OFF: - hmc5883l_calibration_enabled = false; - result = OK; - 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; -} - -extern int up_i2creset(FAR struct i2c_dev_s * dev); - -int hmc5883l_reset() -{ - int ret; -#if 1 - ret = up_i2creset(hmc5883l_dev.i2c); - printf("HMC5883: BUS RESET %s\n", ret ? "FAIL" : "OK"); -#else - printf("[hmc5883l drv] Resettet I2C2 BUS\n"); - up_i2cuninitialize(hmc5883l_dev.i2c); - hmc5883l_dev.i2c = up_i2cinitialize(2); - I2C_SETFREQUENCY(hmc5883l_dev.i2c, 400000); -#endif - 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; -} |