equency=LSM303DLHC_HPFCF_16; //配置值0x10
LSM303DLHCFilter_InitStructure.HighPassFilter_AOI1=LSM303DLHC_HPF_AOI1_DISABLE; //配置值0x00
LSM303DLHCFilter_InitStructure.HighPassFilter_AOI2=LSM303DLHC_HPF_AOI2_DISABLE; //配置值0x00
LSM303DLHC_AccFilterConfig(&LSM303DLHCFilter_InitStructure);
4.I2C通信读、写LSM303DLHC芯片寄存器封装函数
I2C读取LSM303DLHC寄存器封装函数
uint16_tLSM303DLHC_Read(uint8_tDeviceAddr,uint8_tRegAddr,uint8_t*pBuffer,uint16_tNumByteToRead)
{
LSM303DLHC_Timeout=LSM303DLHC_LONG_TIMEOUT;
while(I2C_GetFlagStatus(LSM303DLHC_I2C,I2C_ISR_BUSY)!=RESET)
{
if((LSM303DLHC_Timeout--)==0)returnLSM303DLHC_TIMEOUT_UserCallback();
}
I2C_TransferHandling(LSM303DLHC_I2C,DeviceAddr,1,I2C_SoftEnd_Mode,I2C_Generate_Start_Write);
LSM303DLHC_Timeout=LSM303DLHC_LONG_TIMEOUT;
while(I2C_GetFlagStatus(LSM303DLHC_I2C,I2C_ISR_TXIS)==RESET)
{
if((LSM303DLHC_Timeout--)==0)returnLSM303DLHC_TIMEOUT_UserCallback();
}
if(NumByteToRead>1)
RegAddr|=0x80;
I2C_SendData(LSM303DLHC_I2C,(uint8_t)RegAddr);
LSM303DLHC_Timeout=LSM303DLHC_LONG_TIMEOUT;
while(I2C_GetFlagStatus(LSM303DLHC_I2C,I2C_ISR_TC)==RESET)
{
if((LSM303DLHC_Timeout--)==0)returnLSM303DLHC_TIMEOUT_UserCallback();
}
I2C_TransferHandling(LSM303DLHC_I2C,DeviceAddr,NumByteToRead,I2C_AutoEnd_Mode,I2C_Generate_Start_Read);
while(NumByteToRead)
{
LSM303DLHC_Timeout=LSM303DLHC_LONG_TIMEOUT;
while(I2C_GetFlagStatus(LSM303DLHC_I2C,I2C_ISR_RXNE)==RESET)
{
if((LSM303DLHC_Timeout--)==0)returnLSM303DLHC_TIMEOUT_UserCallback();
}
*pBuffer=I2C_ReceiveData(LSM303DLHC_I2C);
pBuffer++;
NumByteToRead--;
}
LSM303DLHC_Timeout=LSM303DLHC_LONG_TIMEOUT;
while(I2C_GetFlagStatus(LSM303DLHC_I2C,I2C_ISR_STOPF)==RESET)
{
if((LSM303DLHC_Timeout--)==0)returnLSM303DLHC_TIMEOUT_UserCallback();
}
I2C_ClearFlag(LSM303DLHC_I2C,I2C_ICR_STOPCF);
returnLSM303DLHC_OK;
}
I2C写LSM303DLHC寄存器封装函数
uint16_tLSM303DLHC_Write(uint8_tDeviceAddr,uint8_tRegAddr,uint8_t*pBuffer)
{
LSM303DLHC_Timeout=LSM303DLHC_LONG_TIMEOUT;
while(I2C_GetFlagStatus(LSM303DLHC_I2C,I2C_ISR_BUSY)!=RESET)
{
if((LSM303DLHC_Timeout--)==0)returnLSM303DLHC_TIMEOUT_UserCallback();
}
I2C_TransferHandling(LSM303DLHC_I2C,DeviceAddr,1,I2C_Reload_Mode,I2C_Generate_Start_Write);
LSM303DLHC_Timeout=LSM303DLHC_LONG_TIMEOUT;
while(I2C_GetFlagStatus(LSM303DLHC_I2C,I2C_ISR_TXIS)==RESET)
{
if((LSM303DLHC_Timeout--)==0)returnLSM303DLHC_TIMEOUT_UserCallback();
}
I2C_SendData(LSM303DLHC_I2C,(uint8_t)RegAddr);
LSM303DLHC_Timeout=LSM303DLHC_LONG_TIMEOUT;
while(I2C_GetFlagStatus(LSM303DLHC_I2C,I2C_ISR_TCR)==RESET)
{
if((LSM303DLHC_Timeout--)==0)returnLSM303DLHC_TIMEOUT_UserCallback();
}
I2C_TransferHandling(LSM303DLHC_I2C,DeviceAddr,1,I2C_AutoEnd_Mode,I2C_No_StartStop);
LSM303DLHC_Timeout=LSM303DLHC_LONG_TIMEOUT;
while(I2C_GetFlagStatus(LSM303DLHC_I2C,I2C_ISR_TXIS)==RESET)
{
if((LSM303DLHC_Timeout--)==0)returnLSM303DLHC_TIMEOUT_UserCallback();
}
I2C_SendData(LSM303DLHC_I2C,*pBuffer);
LSM303DLHC_Timeout=LSM303DLHC_LONG_TIMEOUT;
while(I2C_GetFlagStatus(LSM303DLHC_I2C,I2C_ISR_STOPF)==RESET)
{
if((LSM303DLHC_Timeout--)==0)returnLSM303DLHC_TIMEOUT_UserCallback();
}
I2C_ClearFlag(LSM303DLHC_I2C,I2C_ICR_STOPCF);
returnLSM303DLHC_OK;
}
5.从LSM303DLHC取出3轴磁力数值（Mag），并进行单位处理。
voidDemo_CompassReadMag(float*pfData)
{
staticuint8_tbuffer[6]={0};
uint8_tCTRLB=0;
uint16_tMagn_Sensitivity_XY=0,Magn_Sensitivity_Z=0;
uint8_ti=0;
LSM303DLHC_Read(MAG_I2C_ADDRESS,LSM303DLHC_CRB_REG_M,&CTRLB,1);
LSM303DLHC_Read(MAG_I2C_ADDRESS,LSM303DLHC_OUT_X_H_M,buffer,1);
LSM303DLHC_Read(MAG_I2C_ADDRESS,LSM303DLHC_OUT_X_L_M,buffer+1,1);
LSM303DLHC_Read(MAG_I2C_ADDRESS,LSM303DLHC_OUT_Y_H_M,buffer+2,1);
LSM303DLHC_Read(MAG_I2C_ADDRESS,LSM303DLHC_OUT_Y_L_M,buffer+3,1);
LSM303DLHC_Read(MAG_I2C_ADDRESS,LSM303DLHC_OUT_Z_H_M,buffer+4,1);
LSM303DLHC_Read(MAG_I2C_ADDRESS,LSM303DLHC_OUT_Z_L_M,buffer+5,1);
switch(CTRLB&0xE0)
{
caseLSM303DLHC_FS_1_3_GA:
Magn_Sensitivity_XY=LSM303DLHC_M_SENSITIVITY_XY_1_3Ga;//数值1100
Magn_Sensitivity_Z=LSM303DLHC_M_SENSITIVITY_Z_1_3Ga;//数值980
break;
caseLSM303DLHC_FS_1_9_GA:
Magn_Sensitivity_XY=LSM303DLHC_M_SENSITIVITY_XY_1_9Ga;//数值855
Magn_Sensitivity_Z=LSM303DLHC_M_SENSITIVITY_Z_1_9Ga;//数值760
break;
caseLSM303DLHC_FS_2_5_GA:
Magn_Sensitivity_XY=LSM303DLHC_M_SENSITIVITY_XY_2_5Ga;//数值670
Magn_Sensitivity_Z=LSM303DLHC_M_SENSITIVITY_Z_2_5Ga;//数值600
break;
caseLSM303DLHC_FS_4_0_GA:
Magn_Sensitivity_XY=LSM303DLHC_M_SENSITIVITY_XY_4Ga;//数值450
Magn_Sensitivity_Z=LSM303DLHC_M_SENSITIVITY_Z_4Ga;//数值400
break;
caseLSM303DLHC_FS_4_7_GA:
Magn_Sensitivity_XY=LSM303DLHC_M_SENSITIVITY_XY_4_7Ga;//数值400
Magn_Sensitivity_Z=LSM303DLHC_M_SENSITIVITY_Z_4_7Ga;//数值355
break;
caseLSM303DLHC_FS_5_6_GA:
Magn_Sensitivity_XY=LSM303DLHC_M_SENSITIVITY_XY_5_6Ga;//数值330
Magn_Sensitivity_Z=LSM303DLHC_M_SENSITIVITY_Z_5_6Ga;//数值295
break;
caseLSM303DLHC_FS_8_1_GA:
Magn_Sensitivity_XY=LSM303DLHC_M_SENSITIVITY_XY_8_1Ga;//数值230
Magn_Sensitivity_Z=LSM303DLHC_M_SENSITIVITY_Z_8_1Ga;//数值205
break;
}
for(i=0;i<2;i++)
{
pfData[i]=(float)((int16_t)(((uint16_t)buffer[2*i]8)+buffer[2*i+1])*1000)/Magn_Sensitivity_XY;
}
pfData[2]=(float)((int16_t)(((uint16_t)buffer[4]8)+buffer[5])*1000)/Magn_Sensitivity_Z;
}
6.从LSM303DLHC取出3轴加速度数值，并进行处理。
voidDemo_CompassReadAcc(float*pfData)
{
int16_tpnRawData[3];
uint8_tctrlx[2];
uint8_tbuffer[6],cDivider;
uint8_ti=0;
floatLSM_Acc_Sensitivity=LSM_Acc_Sensitivity_2g;
LSM303DLHC_Read(ACC_I2C_ADDRESS,LSM303DLHC_CTRL_REG4_A,ctrlx,2);
LSM303DLHC_Read(ACC_I2C_ADDRESS,LSM303DLHC_OUT_X_L_A,buffer,6);
if(ctrlx[1]&0x40)
cDivider=64;//大端
else
cDivider=16;//小端
if(!(ctrlx[0]&0x40)||(ctrlx[1]&0x40))
{
for(i=0;i<3;i++)
{
pnRawData[i]=((int16_t)((uint16_t)buffer[2*i+1]8)+buffer[2*i])/cDivider;
}
}
else
{
for(i=0;i<3;i++)
pnRawData[i]=((int16_t)((uint16_t)buffer[2*i]8)+buffer[2*i+1])/cDivider;
}
LSM303DLHC_Read(ACC_I2C_ADDRESS,LSM303DLHC_CTRL_REG4_A,ctrlx,2);
if(ctrlx[1]&0x40)
{
LSM_Acc_Sensitivity=0.25;
}
else
{
switch(ctrlx[0]&0x30)
{
caseLSM303DLHC_FULLSCALE_2G:
LSM_Acc_Sensitivity=LSM_Acc_Sensitivity_2g;//1.0f
break;
caseLSM303DLHC_FULLSCALE_4G:
LSM_Acc_Sensitivity=LSM_Acc_Sensitivity_4g;//0.5f
break;
caseLSM303DLHC_FULLSCALE_8G:
LSM_Acc_Sensitivity=LSM_Acc_Sensitivity_8g;//0.25f
break;
caseLSM303DLHC_FULLSCALE_16G:
LSM_Acc_Sensitivity=LSM_Acc_Sensitivity_16g;//0.0834f
break;
}
}
for(i=0;i<3;i++)
{
pfData[i]=(float)pnRawData[i]/LSM_Acc_Sensitivity;
}
}
for(i=0;i<3;i++)
AccBuffer[i]/=100.0f;
7.LSM303DLHC加速度值计算角度公式函数
X轴旋转表示Pitch----俯仰角
Y轴旋转表示Yaw----航向角
Z轴旋转表示Roll
1.计算角加速度的矢量模长|A|=根号下(X*X+Y*Y+Z*Z)
fNormAcc=sqrt((AccBuffer[0]*AccBuffer[0])+(AccBuffer[1]*AccBuffer[1])+(AccBuffer[2]*AccBuffer[2]));
2.计算Roll（Z轴旋转）横滚角Pitch(X轴)正弦、
