1. 单片机端口配置初始化
以STM32F303程序为例
static void LSM303DLHC_LowLevel_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
EXTI_InitTypeDef EXTI_InitStructure;
I2C_InitTypeDef I2C_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数据手册
void LSM303DLHC_MagInit(LSM303DLHCMag_InitTypeDef *LSM303DLHC_InitStruct)
{
uint8_t cra_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(加速度)寄存器函数
void LSM303DLHC_AccInit(LSM303DLHCAcc_InitTypeDef *LSM303DLHC_InitStruct)
{
uint8_t ctrl1 = 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_t LSM303DLHC_Read(uint8_t DeviceAddr, uint8_t RegAddr, uint8_t* pBuffer, uint16_t NumByteToRead)
{
LSM303DLHC_Timeout = LSM303DLHC_LONG_TIMEOUT;
while(I2C_GetFlagStatus(LSM303DLHC_I2C, I2C_ISR_BUSY) != RESET)
{
if((LSM303DLHC_Timeout--) == 0) return LSM303DLHC_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) return LSM303DLHC_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) return LSM303DLHC_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) return LSM303DLHC_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) return LSM303DLHC_TIMEOUT_UserCallback();
}
I2C_ClearFlag(LSM303DLHC_I2C, I2C_ICR_STOPCF);
return LSM303DLHC_OK;
}
I2C写LSM303DLHC寄存器封装函数
uint16_t LSM303DLHC_Write(uint8_t DeviceAddr, uint8_t RegAddr, uint8_t* pBuffer)
{
LSM303DLHC_Timeout = LSM303DLHC_LONG_TIMEOUT;
while(I2C_GetFlagStatus(LSM303DLHC_I2C, I2C_ISR_BUSY) != RESET)
{
if((LSM303DLHC_Timeout--) == 0) return LSM303DLHC_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) return LSM303DLHC_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) return LSM303DLHC_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) return LSM303DLHC_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) return LSM303DLHC_TIMEOUT_UserCallback();
}
I2C_ClearFlag(LSM303DLHC_I2C, I2C_ICR_STOPCF);
return LSM303DLHC_OK;
}
5. 从LSM303DLHC取出3轴磁力数值(Mag),并进行单位处理。
void Demo_CompassReadMag (float* pfData)
{
static uint8_t buffer[6] = {0};
uint8_t CTRLB = 0;
uint16_t Magn_Sensitivity_XY = 0, Magn_Sensitivity_Z = 0;
uint8_t i =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)
{
case LSM303DLHC_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;
case LSM303DLHC_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;
case LSM303DLHC_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;
case LSM303DLHC_FS_4_0_GA:
Magn_Sensitivity_XY = LSM303DLHC_M_SENSITIVITY_XY_4Ga; //数值450
Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_4Ga; //数值400
break;
case LSM303DLHC_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;
case LSM303DLHC_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;
case LSM303DLHC_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轴加速度数值,并进行处理。
void Demo_CompassReadAcc(float* pfData)
{
int16_t pnRawData[3];
uint8_t ctrlx[2];
uint8_t buffer[6], cDivider;
uint8_t i = 0;
float LSM_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)
{
case LSM303DLHC_FULLSCALE_2G:
LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_2g; //1.0f
break;
case LSM303DLHC_FULLSCALE_4G:
LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_4g; //0.5f
break;
case LSM303DLHC_FULLSCALE_8G:
LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_8g; //0.25f
break;
case LSM303DLHC_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数据手册寄存器功能配置及地址请详细阅读,给寄存器地址写值时对应相应功能请仔细检查。