uint8_t LIS3MDL_Init(void){
//Local Variables
uint8_t Device_Found = 0;
//Read data from register and check signature
I2C_Read_Reg(LIS3MDL_ADDRESS,LIS3MDL_WHO_AM_I);
//Check if device signature is correct
if (I2C1->RXDR == LIS3MDL_DEVICE_ID){
Device_Found = 1;
}
else{
Device_Found = 0;
}
if(Device_Found){
//Performance Vs. Power consumption XY (medium), and set data rate to 10Hz
I2C_Write_Reg(LIS3MDL_ADDRESS,LIS3MDL_CTRL_REG1,(LIS3MDL_CTRL_REG1_OM0 | LIS3MDL_CTRL_REG1_DO2));
//Full scale = +/- 4 gauss (Default value) - just in case
I2C_Write_Reg(LIS3MDL_ADDRESS,LIS3MDL_CTRL_REG2,0x0);
//Performance Vs. Power consumption Z (medium)
I2C_Write_Reg(LIS3MDL_ADDRESS,LTS3MDL_CTRL_REG4,LIS3MDL_CTRL_REG4_OMZ0);
//Enable BDU so you ensure MSB and LSB have been read
I2C_Write_Reg(LIS3MDL_ADDRESS,LTS3MDL_CTRL_REG5,LIS3MDL_CTRL_REG5_BDU);
}
return(Device_Found);
}
float LIS3MDL_Z_Read(void){
//Local Variables
uint8_t LIS3MDL_STATUS = 0;
uint8_t OUT_Z_L = 0;
uint8_t OUT_Z_H = 0;
float OUT_Z = 0;
int16_t Raw_Z = 0;
//Set device to continuous conversion mode
I2C_Write_Reg(LIS3MDL_ADDRESS,LTS3MDL_CTRL_REG3,LIS3MDL_CTRL_REG3_MD0);
//Wait for X coordinate data to be ready
do{
I2C_Read_Reg(LIS3MDL_ADDRESS,LIS3MDL_STATUS_REG);
LIS3MDL_STATUS = I2C1->RXDR;
}while((LIS3MDL_STATUS & LIS3MDL_STATUS_REG_ZDA) == 0);
//Read Z Axis magnetic field
I2C_Read_Reg(LIS3MDL_ADDRESS,LIS3MDL_OUT_Z_L);
OUT_Z_L = I2C1->RXDR;
I2C_Read_Reg(LIS3MDL_ADDRESS,LIS3MDL_OUT_Z_H);
OUT_Z_H = I2C1->RXDR;
Raw_Z = ((OUT_Z_H << 8) | OUT_Z_L);
/*
1/6842 = ~0.146 mG/LSB according to datasheet
*/
OUT_Z = (float)Raw_Z * 0.146f; // when using +/- 4 gauss
return(OUT_Z);
}
//==========================================
//初始化HMC5883
//==========================================
void HMC5883_Init(void)
{
I2C_init();
// I2C_Write_Reg(0x00,0x70);
// I2C_Write_Reg(0x01,0x00);
I2C_Write_Reg(0x02,0x00);
}