void HMC5883L()
{
m_Scale = 1;
MPU6050_setI2CMasterModeEnabled(FALSE);
MPU6050_setI2CBypassEnabled(TRUE) ;
MPU6050_setSleepEnabled(FALSE);
}
/**************************实现函数********************************************
*函数原型: void MPU6050_initialize(void)
*功 能: 初始化 MPU6050 以进入可用状态。
*******************************************************************************/
void MPU6050_initialize(void) {
MPU6050_setClockSource(MPU6050_CLOCK_PLL_YGYRO); //设置时钟
MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_2000);//陀螺仪最大量程 +-1000度每秒
MPU6050_setFullScaleAccelRange(MPU6050_ACCEL_FS_2); //加速度度最大量程 +-2G
MPU6050_setSleepEnabled(0); //进入工作状态
MPU6050_setI2CMasterModeEnabled(0); //不让MPU6050 控制AUXI2C
MPU6050_setI2CBypassEnabled(0); //主控制器的I2C与 MPU6050的AUXI2C 直通。控制器可以直接访问HMC5883L
}
/****SCL PC12 SDA PC11*****/
void MPU6050_Init(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
/**************大尺寸开发板姿态传感器代码****************/
#ifdef USE_BIG_SIZE_BOARD
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOC, ENABLE);
//PC12 SCL
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);
GPIO_SetBits(GPIOC,GPIO_Pin_12);
//PC11 SDA
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);
GPIO_SetBits(GPIOC,GPIO_Pin_11);
#endif
/**************小尺寸开发板姿态传感器代码****************/
#ifdef USE_LITTLE_SIZE_BOARD
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
//PB15 SCL
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_SetBits(GPIOB,GPIO_Pin_15);
//PB13 SDA
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_13;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_SetBits(GPIOB,GPIO_Pin_13);
#endif
/*************姿态模块初始化*************/ //加速度计和陀螺仪都是使用1000Hz采样
MPU6050_WriteByte(MPU6050_Addr, PWR_MGMT_1, 0x00); //解除休眠状态
MPU6050_WriteByte(MPU6050_Addr, SMPLRT_DIV, 0x00); //SMPLRT_DIV -- SMPLRT_DIV = 3， Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
MPU6050_WriteByte(MPU6050_Addr, MPU_CONFIG, 0x03); //片内寄存器设置的低通滤波，使用带宽42Hz
MPU6050_WriteByte(MPU6050_Addr, GYRO_CONFIG, 0x18); // 量程2000/s
MPU6050_WriteByte(MPU6050_Addr, ACCEL_CONFIG, 0x00); // 量程2g，0x08对应4g
MPU6050_setI2CMasterModeEnabled();
MPU6050_setI2CBypassEnabled();
MPU6050_WriteByte(MPU6050_Addr,INT_PIN_CFG, 0x42); //使能旁路I2C
MPU6050_WriteByte(MPU6050_Addr,USER_CTRL, 0x40); //使能旁路I2C
}
//初始化DMP引擎
uint8_t MPU6050_DMP_Initialize(void)
{
uint8_t dmpUpdate[16], j;
uint16_t pos = 0;
uint8_t fifoCount;
uint8_t fifoBuffer[128];
int8_t xgOffset , ygOffset , zgOffset;
// reset device
//printf(("\r\n=======配置DMP引擎=========\r\n"));
//printf(("复位MPU6050...\r\n"));
MPU6050_reset();
delay_ms(50); // wait after reset 50ms
//printf(("禁止休眠模式...\r\n"));
MPU6050_setSleepEnabled(0);
//printf(("\r\n读取MPU6050硬件版本...\r\n"));
//printf(("正在选择用户内存块...\r\n"));
MPU6050_setMemoryBank(0x10, 1, 1);
//printf(("正在选择用户内存字节...\r\n"));
MPU6050_setMemoryStartAddress(0x06);
//printf(("正在检查硬件版本...\r\n"));
MPU6050_readMemoryByte();
////printf(("Revision @ user[16][6] = \r\n"));
//printf(("复位内存块...\r\n"));
MPU6050_setMemoryBank(0, 0, 0);
// check OTP bank valid
//printf(("读取OTP块有效标志...\r\n"));
MPU6050_getOTPBankValid();
// //printf("OTP bank is ");
// //printf(otpValid ? ("valid!") : ("invalid!"));
// get X/Y/Z gyro offsets
//printf(("读取加速度偏移值...\r\n"));
xgOffset = MPU6050_getXGyroOffsetTC();
ygOffset = MPU6050_getYGyroOffsetTC();
zgOffset = MPU6050_getZGyroOffsetTC();
// setup weird slave stuff (?)
//printf(("设置从器件地址为0x7F...\r\n"));
MPU6050_setSlaveAddress(0, 0x7F);
//printf(("禁止IIC主器件模式...\r\n"));
MPU6050_setI2CMasterModeEnabled(0);
//主控制器的I2C与 MPU6050的AUXI2C 直通。控制器可以直接访问HMC5883L
MPU6050_setI2CBypassEnabled(1);
//printf(("设置从器件地址为0x68...\r\n"));
MPU6050_setSlaveAddress(0, 0x68);
//printf(("复位IIC主器件控制权...\r\n"));
MPU6050_resetI2CMaster();
delay_ms(20);
// load DMP code into memory banks
//printf(("正在写入DMP代码段到MPU6050 \r\n"));
if (MPU6050_writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, 1)) {
//printf(("DMP代码写入校验成功...\r\n"));
//printf(("配置DMP和有关设置...\r\n"));
// write DMP configuration
//printf(("正在写入DMP配置代码到MPU6050内存...\r\n"));
if (MPU6050_writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
//printf(("DMP配置代码写入校验成功\r\n"));
//printf(("设置Z轴角速度时钟源...\r\n"));
MPU6050_setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
//printf(("使能DMP引擎和FIFO中断...\r\n"));
MPU6050_setIntEnabled(0x12);
//printf(("设置DMP采样率为200Hz...\r\n"));
MPU6050_setRate(4); // 1khz / (1 + 4) = 200 Hz
//printf(("设置外部同步帧到TEMP_OUT_L[0]...\r\n"));
MPU6050_setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
//printf(("设置DLPF带宽为42Hz...\r\n"));
MPU6050_setDLPFMode(MPU6050_DLPF_BW_42);
//printf(("设置角速度精度为 +/- 2000 deg/sec...\r\n"));
MPU6050_setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
//printf(("设置DMP配置字节...\r\n"));
MPU6050_setDMPConfig1(0x03);
MPU6050_setDMPConfig2(0x00);
//printf(("清楚OTP块标志...\r\n"));
MPU6050_setOTPBankValid(0);
//printf(("设置X/Y/Z轴角速度为先前值...\r\n"));
MPU6050_setXGyroOffsetTC(xgOffset);
MPU6050_setYGyroOffsetTC(ygOffset);
MPU6050_setZGyroOffsetTC(zgOffset);
// DMPCalibrate(); //tobe tested
//printf(("写入最后内存跟新到 1/7 ...\r\n"));
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos];
MPU6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0);
//printf(("写入最后内存跟新到 2/7 ...\r\n"));
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos];
MPU6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 0, 0);
//printf(("正在复位FIFO...\r\n"));
MPU6050_resetFIFO();
//printf(("正在读取FIFO计数...\r\n"));
fifoCount = MPU6050_getFIFOCount();
MPU6050_getFIFOBytes(fifoBuffer, fifoCount);
//printf(("正在设置运动阈值为2...\r\n"));
MPU6050_setMotionDetectionThreshold(2);
//printf(("正在设置0运动检测阈值为156...\r\n"));
MPU6050_setZeroMotionDetectionThreshold(156);
//printf(("正在设置运动检测持续时间为80...\r\n"));
MPU6050_setMotionDetectionDuration(80);
//printf(("正在设置0运动检测持续时间为0...\r\n"));
MPU6050_setZeroMotionDetectionDuration(0);
//printf(("复位FIFO...\r\n"));
MPU6050_resetFIFO();
//printf(("正在使能FIFO...\r\n"));
MPU6050_setFIFOEnabled(1);
//printf(("正在使能DMP...\r\n"));
MPU6050_setDMPEnabled(1);
//printf(("复位DMPDMP...\r\n"));
MPU6050_resetDMP();
//printf(("写入最后内存跟新到 3/7 ......\r\n"));
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos];
MPU6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 0, 0);
//printf(("写入最后内存跟新到 4/7 ......\r\n"));
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos];
MPU6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 0, 0);
//printf(("写入最后内存跟新到 5/7 ......\r\n"));
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos];
MPU6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 0, 0);
//printf(("等待FIFO计数>=2...\r\n"));
while ((fifoCount = MPU6050_getFIFOCount()) < 3);
//printf(("复位 FIFO...\r\n"));
MPU6050_getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
//printf(("读取中断状态...\r\n"));
MPU6050_getIntStatus();
//printf(("写入最后内存跟新到 6/7 ......\r\n"));
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos];
MPU6050_readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
//printf(("等待FIFO计数>=2...\r\n"));
while ((fifoCount = MPU6050_getFIFOCount()) < 3);
//printf(("正在读取FIFO...\r\n"));
MPU6050_getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
//printf(("正在读取中断状态...\r\n"));
MPU6050_getIntStatus();
//printf(("写入最后内存跟新到 7/7 ......\r\n"));
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos];
MPU6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 0, 0);
//printf(("DMP设置一切正常...\r\n"));
//printf(("关闭DMP引擎...\r\n"));
MPU6050_setDMPEnabled(0);
//printf(("设置内部42字节缓冲包...\r\n"));
dmpPacketSize = 42;
//printf(("最后一次复位FIFO和中断状态...\r\n"));
MPU6050_resetFIFO();
MPU6050_getIntStatus();
//printf(("打开DMP引擎...\r\n"));
MPU6050_setDMPEnabled(1);
//printf(("DMP引擎准备就绪,等待第一次数据中断...\r\n"));
MPU6050_getIntStatus();
} else {
//printf(("DMP引擎配置校验出错...\r\n"));
return 2; // configuration block loading failed
}
} else {
//printf(("DMP代码校验出错.\r\n"));
return 1; // main binary block loading failed
}
//printf(("======DMP引擎初始化完成========\r\n"));
return 0; // success
}