电子罗盘,LSM303DLHC

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 =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 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;    

      }

 在平面坐标系中载体的方向定义与XYZ三轴是对应关系如下:

  X----E(),Y----N()Z-----

  LSM303DLHC数据手册寄存器功能配置及地址请详细阅读,给寄存器地址写值时对应相应功能请仔细检查。

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值