LSM303DLHC使用解析单片机程序—–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(磁力)寄存器函数
给地址寄存器写值及功用拜见LSM303DLHC数据手册
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(加速度)寄存器函数
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芯片寄存器封装函数
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轴)正弦、余弦值俯仰角

fSinRoll=-AccBuffer[1]/fNormAcc;

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

fSinPitch=AccBuffer[0]/fNormAcc;

fCosPitch=sqrt(1.0-(fSinPitch*fSinPitch));
8.依据加速度正弦余弦值来判别视点
RollAng—–横滚角（绕Z轴转）
PitchAng—–俯仰角（绕X轴转）
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三轴是对应联系如下：
X轴—-E(东),Y轴—-N(北)，Z轴—–天
LSM303DLHC数据手册寄存器功用装备及地址请具体阅览，给寄存器地址写值时对应相应功用请仔细检查。