QMC5883P 地磁传感器&PT32L007 源代码分享

/**
  ******************************************************************************
  * @file    qmc5883p.c
  * @author  Yangzhiqiang@qst
  * @version V1.0
  * @date    2020-5-27
  * @brief    qmc5883p
  ******************************************************************************
  * @attention
  *
  *
  ******************************************************************************
  */ 

#include "QMC5883P.h"
#include "PT32x007x.h"
#include "string.h"

#define QMC5883_ADDR               (0x2c<<1)//0x0D //0x1a
#define fabs(x)                    (x < 0 ? -x : x)

enum{
    AXIS_X = 0,
    AXIS_Y = 1,
    AXIS_Z = 2,

    AXIS_TOTAL
};

typedef struct{
    signed char sign[3];
    unsigned char map[3];
} QMC5883P_map;

static uint8_t chipid = 0;

static QMC5883P_map c_map;

void delay_us(uint32_t delay)
{
    uint32_t i,j,k;
    for(i=0;i<delay;i++)
    {
        for(j=0;j<10;j++)
        {
            k++;
        }
    }
}
//---------------------------------------

/**
* @brief IIC写函数
* @param pBuffer：需要写入的数据
* @param WriteAddr：从机地址
* @param NumByteToWrite：需要写入的数据长度
* @retval 无
*/
void Acce_Write_Byte(unsigned int WriteAddr, u8 data)
{
//    int i;
/******************等待从机ready***************/        
    I2C_GenerateEvent(I2C,I2C_Event_Start,DISABLE);
    I2C->CCR |= I2C_CCR_SI | I2C_CCR_ACK;
    I2C_Cmd(I2C,DISABLE);
    I2C_Cmd(I2C,ENABLE);
//    printf("%s %d\r\n", __func__, __LINE__) ;
    I2C_GenerateEvent(I2C,I2C_Event_Start,ENABLE);    
    while(I2C_GetFlagStatus(I2C,I2C_FLAG_StartOk)!= SET);
    
    I2C_SendAddr(I2C, QMC5883_ADDR);//器件地址，写
    while(I2C_GetFlagStatus(I2C,I2C_FLAG_MASGetAckW)!=SET);
//    printf("%s %d\r\n", __func__, __LINE__) ;
    I2C_SendData(I2C,WriteAddr);//发送要写的字地址
    while(I2C_GetFlagStatus(I2C,I2C_FLAG_MDSGetAck)!=SET);

    I2C_SendData(I2C, data);
    while(I2C_GetFlagStatus(I2C,I2C_FLAG_MDSGetAck) != SET);

        /******************发送停止位***************/
    I2C_GenerateEvent(I2C,I2C_Event_Stop,ENABLE);    
}

//uint8_t I2C_EE_Read(u8* pBuffer,u16 ReadAddr, u16 DeviceAddr, u16 data_size)
uint8_t Acce_Read_Byte(u16 ReadAddr)
{
//    int i;
    uint8_t date = 0 ;
/******************等待从机ready***************/        
//printf("---%s  %d---\n", __func__, __LINE__) ;
    I2C_GenerateEvent(I2C,I2C_Event_Start,ENABLE);
    while(I2C_GetFlagStatus(I2C,I2C_FLAG_StartOk)!= SET);

    I2C_SendAddr(I2C, QMC5883_ADDR);//器件地址，写
    while(I2C_GetFlagStatus(I2C,I2C_FLAG_MASGetAckW)!=SET); 

    I2C_SendData(I2C,ReadAddr);//发送要读的页地址
    while(I2C_GetFlagStatus(I2C,I2C_FLAG_MDSGetAck)!=SET); 

    I2C_GenerateEvent(I2C,I2C_Event_Stop,ENABLE);
    
/******************接收数据***************/        
    I2C_GenerateEvent(I2C,I2C_Event_Start,ENABLE);
    while(I2C_GetFlagStatus(I2C,I2C_FLAG_StartOk)!= SET); 

    I2C_SendAddr(I2C, QMC5883_ADDR + 1);//器件地址，读
    while(I2C_GetFlagStatus(I2C,I2C_FLAG_MASGetAckR)!=SET); 

    I2C->CCR=I2C_CCR_ACK|I2C_CCR_SI;//主机发送NACK
    while(I2C_GetFlagStatus(I2C,I2C_FLAG_MDGSendNack) != SET); 

    date = I2C_ReceiveData(I2C);

    /******************发送停止位***************/
    I2C_GenerateEvent(I2C,I2C_Event_Stop,ENABLE);

    return date ;
}
//---------------------------------------

//uint8_t qmc5883p_read_block(uint8_t addr)
//{
//    uint8_t data = 0;

//    data = Acce_Read_Byte(addr);

//    return data;
//}

int qmc5883p_write_reg(uint8_t addr, uint8_t data)
{
    int ret = 0;
    int retry = 0;

    while((!ret) && (retry++ < 5))
    {
        ret = mx_i2c1_write(mag_addr<<1, addr, data);
    }

    return ret;
}

static void qmc5883p_delay(unsigned int ms)
{
    delay_us(ms * 1000);
}

static void qmc5883p_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 qmc5883p_get_chipid(void)
{
    int ret = 0;
    int i;    
  unsigned char chipid = 0x00;

    for(i = 0; i<10; i++)
    {
        chipid = Acce_Read_Byte(QMC5883P_CHIP_ID_REG);
        if(chipid == 0x80)
        {    
            //ret = qmc5883p_write_reg(0x0a, 0x00);    // suspend mode
            printf("qmc5883p read chip id OK  id:0x%x\r\n", chipid);
            return 1;
        }
    }

    printf("qmc5883p read chip id fail\r\n");

    return 0;
}

uint8_t qmc5883p_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 = QMC5883P_STATUS_REG;
        rdy = Acce_Read_Byte(QMC5883P_STATUS_REG);
        t1++;
//        printf("while\r\n") ;
    }
//    printf("Check status register for data availability success\r\n ") ;
    

    mag_data[0] = Acce_Read_Byte(QMC5883P_DATA_OUT_X_LSB_REG) ;   qmc5883p_delay(1);
    
    mag_data[1] = Acce_Read_Byte(QMC5883P_DATA_OUT_X_MSB_REG) ;   qmc5883p_delay(1);
    
    mag_data[2] = Acce_Read_Byte(QMC5883P_DATA_OUT_Y_LSB_REG) ; ; qmc5883p_delay(1);
    mag_data[3] = Acce_Read_Byte(QMC5883P_DATA_OUT_Y_MSB_REG) ;   qmc5883p_delay(1);
    
    mag_data[4] = Acce_Read_Byte(QMC5883P_DATA_OUT_Z_LSB_REG) ;   qmc5883p_delay(1);
    mag_data[5] = Acce_Read_Byte(QMC5883P_DATA_OUT_Z_MSB_REG) ;   qmc5883p_delay(1);
//    mag_data[0] = QMC5883P_DATA_OUT_X_LSB_REG;    
//    res = qmc5883p_read_block(QMC5883P_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;
    
    printf("%f %f %f\n",data[0],data[1],data[2]);

    return res;
}

/* Set the sensor mode */
int qmc5883p_set_mode(unsigned char mode)
{
    int err = 0;
    unsigned char ctrl1_value = 0;     
    
    ctrl1_value = Acce_Read_Byte(QMC5883P_CTL_REG_ONE);
    ctrl1_value = (ctrl1_value&(~0x03))|mode;
    printf("QMC5883P_set_mode, 0x0A = [%02x]->[%02x] \r\n", QMC5883P_CTL_REG_ONE,ctrl1_value);
    Acce_Write_Byte(QMC5883P_CTL_REG_ONE, ctrl1_value);  

    return 1;
}

int qmc5883p_set_output_data_rate(unsigned char rate){
    
    unsigned char ctrl1_value = 0;
    
    ctrl1_value = Acce_Read_Byte(QMC5883P_CTL_REG_ONE);
    ctrl1_value = (ctrl1_value& (~0xE8)) | (rate << 5);
    printf("QMC5883P_set_output_data_rate, 0x0A = [%02x]->[%02x] \r\n", QMC5883P_CTL_REG_ONE,ctrl1_value);
    Acce_Write_Byte(QMC5883P_CTL_REG_ONE, ctrl1_value);

    return 1;    
}

static int qmc5883p_enable(void)
{
    int ret = 0;

    Acce_Write_Byte(0x0a, 0x00);  delay_us(10) ;
    qmc5883p_delay(1);
    Acce_Write_Byte(0x0d, 0x40);    delay_us(10) ;
    Acce_Write_Byte(0x29, 0x06);    delay_us(10) ;
    //qmc5883p_write_reg(0x0a, 0x0F);//0XA9=ODR =100HZ 0XA5 = 50HZ
    Acce_Write_Byte(0x0b, 0x00); //30 GS
    delay_us(10) ;
    Acce_Write_Byte(0x0a, 0xc3);    delay_us(10) ;
    qmc5883p_delay(1);
    
    return 1;
}

int qmc5883p_init(void)
{
    int ret = 0;

    qmc5883p_set_layout(0);
    qmc5883p_enable();

    qmc5883p_get_chipid();
    {
        unsigned char ctrl_value; 
        ctrl_value = Acce_Read_Byte(QMC5883P_CTL_REG_ONE);
        printf("QMC5883P  0x%x=0x%x \r\n", QMC5883P_CTL_REG_ONE, ctrl_value);
        
        ctrl_value = Acce_Read_Byte(QMC5883P_CTL_REG_TWO);
        printf("QMC5883P  0x%x=0x%x \r\n", QMC5883P_CTL_REG_TWO, ctrl_value);
        
        ctrl_value = Acce_Read_Byte(0x0d);
        printf("QMC5883P  0x%x=0x%x \r\n", 0x0d, ctrl_value);
    }
    return 1;
}