path: root/nuttx/configs/px4fmu/src/drv_hmc5833l.c

/*
 *   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;
}