325 lines
6.8 KiB
C
325 lines
6.8 KiB
C
/**
|
|
******************************************************************************
|
|
* @file qmcX983.c
|
|
* @author STMicroelectronics
|
|
* @version V1.0
|
|
* @date 2013-xx-xx
|
|
* @brief qmcX983驱动
|
|
******************************************************************************
|
|
* @attention
|
|
*
|
|
* 实验平台:秉火 指南者 开发板
|
|
* 论坛 :http://www.firebbs.cn
|
|
* 淘宝 :https://fire-stm32.taobao.com
|
|
*
|
|
******************************************************************************
|
|
*/
|
|
|
|
#include "qmc6310.h"
|
|
#include "qst_sw_i2c.h"
|
|
#include <math.h>
|
|
|
|
|
|
|
|
|
|
#define printf(a,fmt...)
|
|
|
|
|
|
|
|
uint8_t chipid = 0;
|
|
uint8_t mag_addr;
|
|
static unsigned char sensor_mask[4] = {
|
|
0x80,
|
|
0xA0,
|
|
0xB0,
|
|
0xC0
|
|
};
|
|
|
|
static QMC6310_map c_map;
|
|
|
|
int qmc6310_read_block(uint8_t addr, uint8_t *data, uint8_t len)
|
|
{
|
|
return qst_sw_readreg(mag_addr, addr, data, len);
|
|
}
|
|
|
|
/*
|
|
static int QMC6310_write_block(uint8_t addr, uint8_t *data, uint8_t len)
|
|
{
|
|
int ret = 0;
|
|
|
|
mutex_lock(&g_QMC6310->op_lock);
|
|
ret = i2c_smbus_write_i2c_block_data(g_QMC6310->client, addr, len, data);
|
|
mutex_unlock(&g_QMC6310->op_lock);
|
|
if(ret < 0)
|
|
return ret;
|
|
return 0;
|
|
}
|
|
*/
|
|
|
|
int qmc6310_write_reg(uint8_t addr, uint8_t data)
|
|
{
|
|
return qst_sw_writereg(mag_addr, addr, data);
|
|
|
|
}
|
|
|
|
static void qmc6310_set_layout(int layout)
|
|
{
|
|
if(layout == 0)
|
|
{
|
|
c_map.sign[AXIS_X] = 1;
|
|
c_map.sign[AXIS_Y] = 1;
|
|
c_map.sign[AXIS_Z] = 1;
|
|
c_map.map[AXIS_X] = AXIS_X;
|
|
c_map.map[AXIS_Y] = AXIS_Y;
|
|
c_map.map[AXIS_Z] = AXIS_Z;
|
|
}
|
|
else if(layout == 1)
|
|
{
|
|
c_map.sign[AXIS_X] = -1;
|
|
c_map.sign[AXIS_Y] = 1;
|
|
c_map.sign[AXIS_Z] = 1;
|
|
c_map.map[AXIS_X] = AXIS_Y;
|
|
c_map.map[AXIS_Y] = AXIS_X;
|
|
c_map.map[AXIS_Z] = AXIS_Z;
|
|
}
|
|
else if(layout == 2)
|
|
{
|
|
c_map.sign[AXIS_X] = -1;
|
|
c_map.sign[AXIS_Y] = -1;
|
|
c_map.sign[AXIS_Z] = 1;
|
|
c_map.map[AXIS_X] = AXIS_X;
|
|
c_map.map[AXIS_Y] = AXIS_Y;
|
|
c_map.map[AXIS_Z] = AXIS_Z;
|
|
}
|
|
else if(layout == 3)
|
|
{
|
|
c_map.sign[AXIS_X] = 1;
|
|
c_map.sign[AXIS_Y] = -1;
|
|
c_map.sign[AXIS_Z] = 1;
|
|
c_map.map[AXIS_X] = AXIS_Y;
|
|
c_map.map[AXIS_Y] = AXIS_X;
|
|
c_map.map[AXIS_Z] = AXIS_Z;
|
|
}
|
|
else if(layout == 4)
|
|
{
|
|
c_map.sign[AXIS_X] = -1;
|
|
c_map.sign[AXIS_Y] = 1;
|
|
c_map.sign[AXIS_Z] = -1;
|
|
c_map.map[AXIS_X] = AXIS_X;
|
|
c_map.map[AXIS_Y] = AXIS_Y;
|
|
c_map.map[AXIS_Z] = AXIS_Z;
|
|
}
|
|
else if(layout == 5)
|
|
{
|
|
c_map.sign[AXIS_X] = 1;
|
|
c_map.sign[AXIS_Y] = 1;
|
|
c_map.sign[AXIS_Z] = -1;
|
|
c_map.map[AXIS_X] = AXIS_Y;
|
|
c_map.map[AXIS_Y] = AXIS_X;
|
|
c_map.map[AXIS_Z] = AXIS_Z;
|
|
}
|
|
else if(layout == 6)
|
|
{
|
|
c_map.sign[AXIS_X] = 1;
|
|
c_map.sign[AXIS_Y] = -1;
|
|
c_map.sign[AXIS_Z] = -1;
|
|
c_map.map[AXIS_X] = AXIS_X;
|
|
c_map.map[AXIS_Y] = AXIS_Y;
|
|
c_map.map[AXIS_Z] = AXIS_Z;
|
|
}
|
|
else if(layout == 7)
|
|
{
|
|
c_map.sign[AXIS_X] = -1;
|
|
c_map.sign[AXIS_Y] = -1;
|
|
c_map.sign[AXIS_Z] = -1;
|
|
c_map.map[AXIS_X] = AXIS_Y;
|
|
c_map.map[AXIS_Y] = AXIS_X;
|
|
c_map.map[AXIS_Z] = AXIS_Z;
|
|
}
|
|
else
|
|
{
|
|
c_map.sign[AXIS_X] = 1;
|
|
c_map.sign[AXIS_Y] = 1;
|
|
c_map.sign[AXIS_Z] = 1;
|
|
c_map.map[AXIS_X] = AXIS_X;
|
|
c_map.map[AXIS_Y] = AXIS_Y;
|
|
c_map.map[AXIS_Z] = AXIS_Z;
|
|
}
|
|
}
|
|
|
|
static int qmc6310_get_chipid(void)
|
|
{
|
|
int ret = 0;
|
|
ret = qmc6310_read_block(QMC6310_CHIP_ID_REG, &chipid , 1);
|
|
if(ret == 0){
|
|
printf("change_iic_addr = 0x3c\n");
|
|
mag_addr = QMC6310N_IIC_ADDR;
|
|
}
|
|
else
|
|
{
|
|
printf("QMC6308_get_chipid chipid = 0x%x,i2c_addr = 0x2c\n", chipid);
|
|
return 1;
|
|
}
|
|
|
|
ret = qmc6310_read_block(QMC6310_CHIP_ID_REG, &chipid , 1);
|
|
if(ret == 0){
|
|
printf("change_iic_addr = 0x1c\n");
|
|
mag_addr = QMC6310U_IIC_ADDR;
|
|
}
|
|
else
|
|
{
|
|
printf("QMC6310N_get_chipid chipid = 0x%x,i2c_addr = 0x3c\n", chipid);
|
|
return 1;
|
|
}
|
|
|
|
ret = qmc6310_read_block(QMC6310_CHIP_ID_REG, &chipid , 1);
|
|
if(ret == 0){
|
|
printf("Get_Chip_ID_Failed!\n");
|
|
return 0;
|
|
}
|
|
else
|
|
{
|
|
printf("QMC6310U_get_chipid chipid = 0x%x,i2c_addr = 0x1c\n", chipid);
|
|
return 1;
|
|
}
|
|
}
|
|
|
|
|
|
uint8_t qmc6310_read_mag_xyz(float *data)
|
|
{
|
|
int res;
|
|
unsigned char mag_data[6];
|
|
short hw_d[3] = {0};
|
|
short raw_c[3];
|
|
int t1 = 0;
|
|
unsigned char rdy = 0;
|
|
|
|
/* Check status register for data availability */
|
|
while(!(rdy & 0x01) && (t1 < 5))
|
|
{
|
|
rdy = QMC6310_STATUS_REG;
|
|
res = qmc6310_read_block(QMC6310_STATUS_REG, &rdy, 1);
|
|
t1++;
|
|
}
|
|
|
|
mag_data[0] = QMC6310_DATA_OUT_X_LSB_REG;
|
|
|
|
res = qmc6310_read_block(QMC6310_DATA_OUT_X_LSB_REG, mag_data, 6);
|
|
if(res == 0)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
hw_d[0] = (short)(((mag_data[1]) << 8) | mag_data[0]);
|
|
hw_d[1] = (short)(((mag_data[3]) << 8) | mag_data[2]);
|
|
hw_d[2] = (short)(((mag_data[5]) << 8) | mag_data[4]);
|
|
|
|
//Unit:mG 1G = 100uT = 1000mG
|
|
//printf("Hx=%d, Hy=%d, Hz=%d\n",hw_d[0],hw_d[1],hw_d[2]);
|
|
raw_c[AXIS_X] = (int)(c_map.sign[AXIS_X]*hw_d[c_map.map[AXIS_X]]);
|
|
raw_c[AXIS_Y] = (int)(c_map.sign[AXIS_Y]*hw_d[c_map.map[AXIS_Y]]);
|
|
raw_c[AXIS_Z] = (int)(c_map.sign[AXIS_Z]*hw_d[c_map.map[AXIS_Z]]);
|
|
|
|
data[0] = (float)raw_c[0] / 10.0f;
|
|
data[1] = (float)raw_c[1] / 10.0f;
|
|
data[2] = (float)raw_c[2] / 10.0f;
|
|
|
|
return res;
|
|
}
|
|
|
|
|
|
/* Set the sensor mode */
|
|
int qmc6310_set_mode(unsigned char mode)
|
|
{
|
|
int err = 0;
|
|
unsigned char ctrl1_value = 0;
|
|
|
|
err = qmc6310_read_block(QMC6310_CTL_REG_ONE, &ctrl1_value, 1);
|
|
ctrl1_value = (ctrl1_value&(~0x03))|mode;
|
|
printf("QMC6310_set_mode, 0x0A = [%02x]->[%02x] \r\n", QMC6310_CTL_REG_ONE,ctrl1_value);
|
|
err = qmc6310_write_reg(QMC6310_CTL_REG_ONE, ctrl1_value);
|
|
|
|
return err;
|
|
}
|
|
|
|
int qmc6310_set_output_data_rate(unsigned char rate){
|
|
|
|
int err = 0;
|
|
unsigned char ctrl1_value = 0;
|
|
|
|
err = qmc6310_read_block(QMC6310_CTL_REG_ONE, &ctrl1_value, 1);
|
|
ctrl1_value = (ctrl1_value& (~0xE8)) | (rate << 5);
|
|
printf("QMC6310_set_output_data_rate, 0x0A = [%02x]->[%02x] \r\n", QMC6310_CTL_REG_ONE,ctrl1_value);
|
|
err = qmc6310_write_reg(QMC6310_CTL_REG_ONE, ctrl1_value);
|
|
|
|
return err;
|
|
}
|
|
|
|
static int qmc6308_enable(void)
|
|
{
|
|
int ret = 0;
|
|
if(chipid == 0x80)
|
|
{
|
|
qmc6310_write_reg(0x0a, 0xc3);
|
|
qmc6310_write_reg(0x0b, 0x00);
|
|
qmc6310_write_reg(0x0d, 0x40);
|
|
}
|
|
else
|
|
{
|
|
qmc6310_write_reg(0x0a, 0x63);
|
|
qmc6310_write_reg(0x0b, 0x08);
|
|
qmc6310_write_reg(0x0d, 0x40);
|
|
|
|
}
|
|
return ret;
|
|
}
|
|
static int qmc6310_enable(void)
|
|
{
|
|
int ret = 0;
|
|
|
|
qmc6310_write_reg(0x0d, 0x40);
|
|
qmc6310_write_reg(0x29, 0x06);
|
|
qmc6310_write_reg(0x0a, 0x0F);//0XA9=ODR =100HZ 0XA5 = 50HZ
|
|
qmc6310_write_reg(0x0b, 0x00); //30 GS
|
|
|
|
return 1;
|
|
}
|
|
|
|
int qmc6310_init(void)
|
|
{
|
|
|
|
//初始化iic引脚
|
|
i2c_sw_gpio_config();
|
|
|
|
|
|
int ret = 0;
|
|
mag_addr = QMC6308_IIC_ADDR;
|
|
ret = qmc6310_get_chipid();
|
|
if(ret==0)
|
|
{
|
|
return 0;
|
|
}
|
|
qmc6310_set_layout(0);
|
|
//qmc6310_set_layout(0);
|
|
if(mag_addr == QMC6308_IIC_ADDR)
|
|
{
|
|
qmc6308_enable();
|
|
}
|
|
else
|
|
{
|
|
qmc6310_enable();
|
|
}
|
|
|
|
|
|
{
|
|
unsigned char ctrl_value;
|
|
qmc6310_read_block(QMC6310_CTL_REG_ONE, &ctrl_value, 1);
|
|
printf("QMC6310 0x%x=0x%x \r\n", QMC6310_CTL_REG_ONE, ctrl_value);
|
|
qmc6310_read_block(QMC6310_CTL_REG_TWO, &ctrl_value, 1);
|
|
printf("QMC6310 0x%x=0x%x \r\n", QMC6310_CTL_REG_TWO, ctrl_value);
|
|
qmc6310_read_block(0x0d, &ctrl_value, 1);
|
|
printf("QMC6310 0x%x=0x%x \r\n", 0x0d, ctrl_value);
|
|
}
|
|
return 1;
|
|
}
|