129 lines
2.9 KiB
C
129 lines
2.9 KiB
C
/*
|
|
* qmc5883l.c
|
|
*
|
|
* Created on: Mar 4, 2024
|
|
* Author: gxms0
|
|
*/
|
|
|
|
#include "qmc5883l.h"
|
|
|
|
void qmcxxx_init(QMC_ptr ptr,I2C_ptr i2c,uint16_t address)
|
|
{
|
|
ptr->address = address;
|
|
|
|
ptr->i2c = i2c;
|
|
|
|
ptr->read = &i2c_read;
|
|
ptr->write = &i2c_write;
|
|
|
|
|
|
uint8_t cmd = QMC5883_CMD_SOFT_RST_ENABLE;
|
|
|
|
//使能软件复位
|
|
cmd = QMC5883_CMD_SOFT_RST_ENABLE;
|
|
ptr->write(ptr->i2c, ptr->address, QMC5883_REG_CTRL2, &cmd , 1);
|
|
|
|
//连续模式、满量程为8G、过采样率
|
|
cmd = QMC5883_CMD_MODE_CON|QMC5883_CMD_ODR_10HZ|QMC5883_CMD_RNG_8G|QMC5883_CMD_OSR_512;
|
|
ptr->write(ptr->i2c, ptr->address, QMC5883_REG_CTRL1, &cmd , 1);
|
|
|
|
//不使用中断，数据指针自动移动到下一个地址（在寄存器00H~05H之间）
|
|
cmd = QMC5883_CMD_INT_DISABLE|QMC5883_CMD_ROL_PNT_ENABLE ;
|
|
ptr->write(ptr->i2c, ptr->address, QMC5883_REG_CTRL2, &cmd , 1);
|
|
|
|
}
|
|
|
|
uint8_t qmcxxx_checkchip(QMC_ptr ptr)
|
|
{
|
|
uint8_t ChipID=0;
|
|
|
|
ptr->read(ptr->i2c, ptr->address, QMC5883_REG_PRODUCTID ,&ChipID, 1);
|
|
|
|
if(ChipID==0xFF)
|
|
{
|
|
return 0x01;
|
|
}
|
|
else
|
|
{
|
|
return 0x00;
|
|
}
|
|
}
|
|
|
|
void qmcxxx_getdata(QMC_ptr ptr)
|
|
{
|
|
int16_t mxyz[10][3] = {0};
|
|
int32_t avermxyz[3]={0};
|
|
uint8_t buffer[6]={0};
|
|
static float magGaussData[3] = {0.0};
|
|
static uint8_t cnt=0;
|
|
|
|
ptr->read(ptr->i2c, ptr->address, QMC5883_REG_DATA ,buffer, 6);
|
|
|
|
//printf("Xl=%d,Xh=%d,Yl=%d,Yh=%d,Zl=%d,Zh=%d\r\n",buffer[0],buffer[1],buffer[2],buffer[3],buffer[4],buffer[5]);
|
|
mxyz[cnt][0] = (int16_t)(buffer[1] << 8 | buffer[0]);
|
|
mxyz[cnt][1] = (int16_t)(buffer[3] << 8 | buffer[2]);
|
|
mxyz[cnt][2] = (int16_t)(buffer[5] << 8 | buffer[4]);
|
|
cnt++;
|
|
if(cnt>=10)
|
|
{
|
|
cnt=0;
|
|
for(uint8_t i=0,j=0;i<3;i++)
|
|
{
|
|
for(j=0;j<10;j++)
|
|
{
|
|
avermxyz[i]+=mxyz[j][i];
|
|
}
|
|
avermxyz[i]=avermxyz[i]/10;
|
|
}
|
|
//printf("average original data :x=%d,y=%d,z=%d\r\n",avermxyz[0],avermxyz[1],avermxyz[2]);
|
|
|
|
for(uint8_t k=0;k<3;k++)
|
|
{
|
|
magGaussData[k]=magGaussData[k]-32768; //32768:Null Field Output
|
|
magGaussData[k]=avermxyz[k]/3000; //3000:sensitivity, 3000 counts/G
|
|
}
|
|
|
|
//printf("convert the raw data :x=%f,y=%f,z=%f\r\n",magGaussData[0],magGaussData[1],magGaussData[2]);
|
|
//printf("\r\n");
|
|
}
|
|
//DelayMs(3000);
|
|
}
|
|
|
|
|
|
/*
|
|
3、空间校准：把椭球修到正球
|
|
|
|
a）、空间旋转芯片，找出xyz的最大最小值xmax、xmin、ymax 、ymin、zmax 、zmin。
|
|
|
|
b）、于是，x的偏移： Xoffset= (xmax + xmin) / 2;
|
|
|
|
c）、x的比例取：x_gain=1
|
|
|
|
d）、y的偏移：Yoffset =(ymax + ymin) / 2;
|
|
|
|
e）、y的比例：y_gain=(xmax - xmin) / (y_max -y_min);
|
|
|
|
f）、 z的偏移：Zoffset =(zmax + zmin) / 2;
|
|
|
|
g）、z的比例：z_gain=(xmax - xmin) / (z_max -z_min);
|
|
|
|
|
|
|
|
X_QMC = QMC_Xgain *(X_QMC - QMC_Xoffset);
|
|
|
|
Y_QMC = QMC_Ygain * (Y_QMC - QMC_Yoffset);
|
|
|
|
Z_QMC = QMC_Zgain * (Z_QMC- QMC_Zoffset);
|
|
|
|
*/
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|