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

 */