【正文】 } if (PitchAng =360) { PitchAng = PitchAng 360。7. LSM303DLHC加速度值計算角度公式函數(shù) X軸旋轉表示Pitch俯仰角 Y軸旋轉表示Yaw航向角 Z軸旋轉表示Roll 1. 計算角加速度的矢量模長 |A|=根號下(X*X+Y*Y+Z*Z) /* 加速度的矢量模長 |A|=根號下(X*X+Y*Y+Z*Z)*/ fNormAcc = sqrt((AccBuffer[0]*AccBuffer[0])+(AccBuffer[1]*AccBuffer[1])+(AccBuffer[2]*AccBuffer[2]))。0x40) { /* FIFO mode */ LSM_Acc_Sensitivity = 。 float LSM_Acc_Sensitivity = LSM_Acc_Sensitivity_2g。//數(shù)值355 break。 LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_L_M, buffer+5, 1)。 /* Wait until STOPF flag is set */ LSM303DLHC_Timeout = LSM303DLHC_LONG_TIMEOUT。 } /* Wait until STOPF flag is set */ LSM303DLHC_Timeout = LSM303DLHC_LONG_TIMEOUT。LSM303DLHCFilter_InitStructure)。 //配置值0x07 = LSM303DLHC_FULLSCALE_2G。 //配置值0x14 = LSM303DLHC_FS_8_1_GA。 /* Apply LSM303DLHC_I2C configuration after enabling it */ I2C_Init(LSM303DLHC_I2C, amp。 GPIO_Init(LSM303DLHC_DRDY_GPIO_PORT, amp。 GPIO_PinAFConfig(LSM303DLHC_I2C_SDA_GPIO_PORT, LSM303DLHC_I2C_SDA_SOURCE, LSM303DLHC_I2C_SDA_AF)。 /* Enable INT2 GPIO clock */ RCC_AHBPeriphClockCmd(LSM303DLHC_I2C_INT2_GPIO_CLK, ENABLE)。 = GPIO_PuPd_NOPULL。 = I2C_Ack_Enable。} 2. LSM303DLHC芯片初始化MAG(磁力)寄存器函數(shù) 給地址寄存器寫值及功能參見LSM303DLHC數(shù)據(jù)手冊void LSM303DLHC_MagInit(LSM303DLHCMag_InitTypeDef *LSM303DLHC_InitStruct){ uint8_t cra_regm = 0x00, crb_regm = 0x00, mr_regm = 0x00。 /* Configure the low level interface */ = LSM303DLHC_NORMAL_MODE。 //配置值0x10 = LSM303DLHC_HPF_AOI1_DISABLE。 } /* Read data from RXDR */ *pBuffer = I2C_ReceiveData(LSM303DLHC_I2C)。 /* Wait until TXIS flag is set */ LSM303DLHC_Timeout = LSM303DLHC_LONG_TIMEOUT。 LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_H_M, buffer+2, 1)。 //數(shù)值400 break。 uint8_t ctrlx[2]。 i++) pnRawData[i]=((int16_t)((uint16_t)buffer[2*i] 8) + buffer[2*i+1])/cDivider。 }} for(i=0。 } } if (RollAng =360) { RollAng = RollAng 360。 HeadingValue航向角（范圍：0~360度）fTiltedX = MagBuffer[0]*fCosPitch+MagBuffer[2]*fSinPitch。 /* 正弦 sinPitch=X/|A| */ fSinPitch = AccBuffer[0]/fNormAcc。 // break。 if(ctrlx[1]amp。//數(shù)值295 break。 //數(shù)值1100 Magn_Sensitivity_Z = LSM303DLHC_M_SENSITIVITY_Z_1_3Ga。 return LSM303DLHC_OK。 /* If all operations OK */ return LSM303DLHC_OK。 } /* Configure slave address, nbytes, reload, end mode and start or stop generation */ I2C_TransferHandling(LSM303DLHC_I2C, DeviceAddr, 1, I2C_SoftEnd_Mode, I2C_Generate_Start_Write)。 //配置值0x00 =LSM303DLHC_HR_ENABLE。 /* Configure MEMS: temp and Data rate */ cra_regm |= (uint8_t) (LSM303DLHC_InitStructTemperature_Sensor | LSM303DLHC_InitStructMagOutput_DataRate)。 /* Configure GPIO PINs to detect Interrupts */ = LSM303DLHC_I2C_INT1_PIN。 = LSM303DLHC_DRDY_EXTI_LINE。 = GPIO_PuPd_DOWN。 /* Enable the I2C periph */ RCC_APB1PeriphClockCmd(LSM303DLHC_I2C_CLK, ENABLE)。GPIO_InitStructure)。 = I2C_AnalogFilter_Enable。 = LSM303DLHC_I2C_INT2_PIN。 /* Write value to Mag MEMS MR_REG regsister */ LSM303DLHC_Write(MAG_I2C_ADDRESS, LSM303DLHC_MR_REG_M, amp。ct