LSM303DLHC應(yīng)用解析單片機程序-----STM32F303

以STM32F303程序為例
staticvoidLSM303DLHC_LowLevel_Init(void)
{
GPIO_InitTypeDefGPIO_InitStructure;
EXTI_InitTypeDefEXTI_InitStructure;
I2C_InitTypeDefI2C_InitStructure;

RCC_APB1PeriphClockCmd(LSM303DLHC_I2C_CLK,ENABLE);

RCC_AHBPeriphClockCmd(LSM303DLHC_I2C_SCK_GPIO_CLK|LSM303DLHC_I2C_SDA_GPIO_CLK,ENABLE);

RCC_AHBPeriphClockCmd(LSM303DLHC_I2C_INT1_GPIO_CLK,ENABLE);

RCC_AHBPeriphClockCmd(LSM303DLHC_I2C_INT2_GPIO_CLK,ENABLE);

RCC_AHBPeriphClockCmd(LSM303DLHC_DRDY_GPIO_CLK,ENABLE);
GPIO_PinAFConfig(LSM303DLHC_I2C_SCK_GPIO_PORT,LSM303DLHC_I2C_SCK_SOURCE,LSM303DLHC_I2C_SCK_AF);
GPIO_PinAFConfig(LSM303DLHC_I2C_SDA_GPIO_PORT,LSM303DLHC_I2C_SDA_SOURCE,LSM303DLHC_I2C_SDA_AF);
GPIO_InitStructure.GPIO_Mode=GPIO_Mode_AF;
GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_DOWN;
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;

GPIO_InitStructure.GPIO_Pin=LSM303DLHC_I2C_SCK_PIN;
GPIO_Init(LSM303DLHC_I2C_SCK_GPIO_PORT,&GPIO_InitStructure);

GPIO_InitStructure.GPIO_Pin=LSM303DLHC_I2C_SDA_PIN;
GPIO_Init(LSM303DLHC_I2C_SDA_GPIO_PORT,&GPIO_InitStructure);

GPIO_InitStructure.GPIO_Mode=GPIO_Mode_IN;
GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;
GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_NOPULL;
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;

GPIO_InitStructure.GPIO_Pin=LSM303DLHC_DRDY_PIN;
GPIO_Init(LSM303DLHC_DRDY_GPIO_PORT,&GPIO_InitStructure);

SYSCFG_EXTILineConfig(LSM303DLHC_DRDY_EXTI_PORT_SOURCE,LSM303DLHC_DRDY_EXTI_PIN_SOURCE);

EXTI_InitStructure.EXTI_Line=LSM303DLHC_DRDY_EXTI_LINE;
EXTI_InitStructure.EXTI_Mode=EXTI_Mode_Interrupt;
EXTI_InitStructure.EXTI_Trigger=EXTI_Trigger_Rising;
EXTI_InitStructure.EXTI_LineCmd=ENABLE;

EXTI_Init(&EXTI_InitStructure);

I2C_InitStructure.I2C_Mode=I2C_Mode_I2C;
I2C_InitStructure.I2C_AnalogFilter=I2C_AnalogFilter_Enable;
I2C_InitStructure.I2C_DigitalFilter=0x00;
I2C_InitStructure.I2C_OwnAddress1=0x00;
I2C_InitStructure.I2C_Ack=I2C_Ack_Enable;
I2C_InitStructure.I2C_AcknowledgedAddress=I2C_AcknowledgedAddress_7bit;
I2C_InitStructure.I2C_Timing=0x00902025;

I2C_Init(LSM303DLHC_I2C,&I2C_InitStructure);

I2C_Cmd(LSM303DLHC_I2C,ENABLE);

GPIO_InitStructure.GPIO_Pin=LSM303DLHC_I2C_INT1_PIN;
GPIO_InitStructure.GPIO_Mode=GPIO_Mode_IN;
GPIO_InitStructure.GPIO_OType=GPIO_OType_PP;
GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
GPIO_InitStructure.GPIO_PuPd=GPIO_PuPd_NOPULL;
GPIO_Init(LSM303DLHC_I2C_INT1_GPIO_PORT,&GPIO_InitStructure);

GPIO_InitStructure.GPIO_Pin=LSM303DLHC_I2C_INT2_PIN;
GPIO_Init(LSM303DLHC_I2C_INT2_GPIO_PORT,&GPIO_InitStructure);
}
2.LSM303DLHC芯片初始化MAG(磁力)寄存器函數(shù)
給地址寄存器寫值及功能參見LSM303DLHC數(shù)據(jù)手冊
voidLSM303DLHC_MagInit(LSM303DLHCMag_InitTypeDef*LSM303DLHC_InitStruct)
{
uint8_tcra_regm=0x00,crb_regm=0x00,mr_regm=0x00;

LSM303DLHC_InitStructure.Temperature_Sensor=LSM303DLHC_TEMPSENSOR_DISABLE;//配置值0x00
LSM303DLHC_InitStructure.MagOutput_DataRate=LSM303DLHC_ODR_30_HZ; //配置值0x14
LSM303DLHC_InitStructure.MagFull_Scale=LSM303DLHC_FS_8_1_GA; //配置值0xE0
LSM303DLHC_InitStructure.Working_Mode=LSM303DLHC_CONTINUOS_CONVERSION; //配置值0x00
LSM303DLHC_LowLevel_Init();

cra_regm|=(uint8_t)(LSM303DLHC_InitStruct->Temperature_Sensor|LSM303DLHC_InitStruct->MagOutput_DataRate);

crb_regm|=(uint8_t)(LSM303DLHC_InitStruct->MagFull_Scale);

mr_regm|=(uint8_t)(LSM303DLHC_InitStruct->Working_Mode);

LSM303DLHC_Write(MAG_I2C_ADDRESS,LSM303DLHC_CRA_REG_M,&cra_regm);

LSM303DLHC_Write(MAG_I2C_ADDRESS,LSM303DLHC_CRB_REG_M,&crb_regm);

LSM303DLHC_Write(MAG_I2C_ADDRESS,LSM303DLHC_MR_REG_M,&mr_regm);
}
3.LSM303DLHC芯片初始化ACC(加速度)寄存器函數(shù)
voidLSM303DLHC_AccInit(LSM303DLHCAcc_InitTypeDef*LSM303DLHC_InitStruct)
{
uint8_tctrl1=0x00,ctrl4=0x00;

LSM303DLHCAcc_InitStructure.Power_Mode=LSM303DLHC_NORMAL_MODE; //配置值0x00
LSM303DLHCAcc_InitStructure.AccOutput_DataRate=LSM303DLHC_ODR_50_HZ;//配置值0x40
LSM303DLHCAcc_InitStructure.Axes_Enable=LSM303DLHC_AXES_ENABLE; //配置值0x07
LSM303DLHCAcc_InitStructure.AccFull_Scale=LSM303DLHC_FULLSCALE_2G; //配置值0x00
LSM303DLHCAcc_InitStructure.BlockData_Update=LSM303DLHC_BlockUpdate_Continous;//配置值0x00
LSM303DLHCAcc_InitStructure.Endianness=LSM303DLHC_BLE_LSB; //配置值0x00
LSM303DLHCAcc_InitStructure.High_Resolution=LSM303DLHC_HR_ENABLE; //配置值0x08
LSM303DLHC_LowLevel_Init();

ctrl1|=(uint8_t)(LSM303DLHC_InitStruct->Power_Mode|LSM303DLHC_InitStruct->AccOutput_DataRate|LSM303DLHC_InitStruct->Axes_Enable);
ctrl4|=(uint8_t)(LSM303DLHC_InitStruct->BlockData_Update|LSM303DLHC_InitStruct->Endianness|
LSM303DLHC_InitStruct->AccFull_Scale|LSM303DLHC_InitStruct->High_Resolution);

LSM303DLHC_Write(ACC_I2C_ADDRESS,LSM303DLHC_CTRL_REG1_A,&ctrl1);

LSM303DLHC_Write(ACC_I2C_ADDRESS,LSM303DLHC_CTRL_REG4_A,&ctrl4);
}

LSM303DLHCFilter_InitStructure.HighPassFilter_Mode_Selection=LSM303DLHC_HPM_NORMAL_MODE;//配置值0x80
LSM303DLHCFilter_InitStructure.HighPassFilter_CutOff_Frequency=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芯片寄存器封裝函數(shù)
I2C讀取LSM303DLHC寄存器封裝函數(shù)
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寄存器封裝函數(shù)
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軸磁力數(shù)值（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;//數(shù)值1100
Magn_Sensitivity_Z=LSM303DLHC_M_SENSITIVITY_Z_1_3Ga;//數(shù)值980
break;
caseLSM303DLHC_FS_1_9_GA:
Magn_Sensitivity_XY=LSM303DLHC_M_SENSITIVITY_XY_1_9Ga;//數(shù)值855
Magn_Sensitivity_Z=LSM303DLHC_M_SENSITIVITY_Z_1_9Ga;//數(shù)值760
break;
caseLSM303DLHC_FS_2_5_GA:
Magn_Sensitivity_XY=LSM303DLHC_M_SENSITIVITY_XY_2_5Ga;//數(shù)值670
Magn_Sensitivity_Z=LSM303DLHC_M_SENSITIVITY_Z_2_5Ga;//數(shù)值600
break;
caseLSM303DLHC_FS_4_0_GA:
Magn_Sensitivity_XY=LSM303DLHC_M_SENSITIVITY_XY_4Ga;//數(shù)值450
Magn_Sensitivity_Z=LSM303DLHC_M_SENSITIVITY_Z_4Ga;//數(shù)值400
break;
caseLSM303DLHC_FS_4_7_GA:
Magn_Sensitivity_XY=LSM303DLHC_M_SENSITIVITY_XY_4_7Ga;//數(shù)值400
Magn_Sensitivity_Z=LSM303DLHC_M_SENSITIVITY_Z_4_7Ga;//數(shù)值355
break;
caseLSM303DLHC_FS_5_6_GA:
Magn_Sensitivity_XY=LSM303DLHC_M_SENSITIVITY_XY_5_6Ga;//數(shù)值330
Magn_Sensitivity_Z=LSM303DLHC_M_SENSITIVITY_Z_5_6Ga;//數(shù)值295
break;
caseLSM303DLHC_FS_8_1_GA:
Magn_Sensitivity_XY=LSM303DLHC_M_SENSITIVITY_XY_8_1Ga;//數(shù)值230
Magn_Sensitivity_Z=LSM303DLHC_M_SENSITIVITY_Z_8_1Ga;//數(shù)值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軸加速度數(shù)值，并進行處理。
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加速度值計算角度公式函數(shù)
X軸旋轉(zhuǎn)表示Pitch----俯仰角
Y軸旋轉(zhuǎn)表示Yaw----航向角
Z軸旋轉(zhuǎn)表示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軸旋轉(zhuǎn)）橫滾角Pitch(X軸)正弦、余弦值俯仰角

fSinRoll=-AccBuffer[1]/fNormAcc;

fCosRoll=sqrt(1.0-(fSinRoll*fSinRoll));

fSinPitch=AccBuffer[0]/fNormAcc;

fCosPitch=sqrt(1.0-(fSinPitch*fSinPitch));
8.根據(jù)加速度正弦余弦值來判斷角度
RollAng-----橫滾角（繞Z軸轉(zhuǎn)）
PitchAng-----俯仰角（繞X軸轉(zhuǎn)）
if(fSinRoll>0)
{
if(fCosRoll>0)
{
RollAng=acos(fCosRoll)*180/PI;
}
else
{
RollAng=acos(fCosRoll)*180/PI+180;
}
}
else
{
if(fCosRoll>0)
{
RollAng=acos(fCosRoll)*180/PI+360;
}
else
{
RollAng=acos(fCosRoll)*180/PI+180;
}
}

if(fSinPitch>0)
{
if(fCosPitch>0)
{
PitchAng=acos(fCosPitch)*180/PI;
}
else
{
PitchAng=acos(fCosPitch)*180/PI+180;
}
}
else
{
if(fCosPitch>0)
{
PitchAng=acos(fCosPitch)*180/PI+360;
}
else
{
PitchAng=acos(fCosPitch)*180/PI+180;
}
}

if(RollAng>=360)
{
RollAng=RollAng-360;
}
if(PitchAng>=360)
{
PitchAng=PitchAng-360;
}
if(RollAng>=360)
{
RollAng=RollAng-360;
}
if(PitchAng>=360)
{
PitchAng=PitchAng-360;
}
9.航向角計算
航向角是指移動物體前進方向和正北方向之間的夾角。以順時針方向為正角度。
HeadingValue---航向角（范圍：0~360度）
fTiltedX=MagBuffer[0]*fCosPitch+MagBuffer[2]*fSinPitch;
fTiltedY=MagBuffer[0]*fSinRoll*fSinPitch+MagBuffer[1]*fCosRoll-MagBuffer[1]*fSinRoll*fCosPitch;
航向角：Y軸與X軸的正切（atan2f）
HeadingValue=(float)((atan2f((float)fTiltedY,(float)fTiltedX))*180)/PI;
if(HeadingValue<0)
{
HeadingValue=HeadingValue+360;
}
在平面坐標系中載體的方向定義與X、Y、Z三軸是對應(yīng)關(guān)系如下：
X軸----E(東),Y軸----N(北)，Z軸-----天
LSM303DLHC數(shù)據(jù)手冊寄存器功能配置及地址請詳細閱讀，給寄存器地址寫值時對應(yīng)相應(yīng)功能請仔細檢查。