void mpu6050Task(void *pdata) { uint8 mpuIntStatus; uint8 fifoBuffer[48]; int16 q[4]; (void)pdata; while (mpu6050DMPInitFromTask()); while (1) { mpu6050DMPSemPend(); mpuIntStatus = MPU6050getIntStatus(); if (mpuIntStatus & 0x02) { MPU6050getFIFOBytes(fifoBuffer, 48); MPU6050dmpGetMag(q, fifoBuffer); /* usart0Hex16(q[0]); usart0Print(" "); usart0Hex16(q[1]); usart0Print(" "); usart0Hex16(q[2]); //usart0Print(" "); //usart0Hex16(q[3]); usart0Print("\r\n"); */ usart0PrintLen((char *)q, 6); } else if (mpuIntStatus & 0x10) { MPU6050resetFIFO(); } else { MPU6050resetFIFO(); } } }
uint8 MPU6050dmpInitialize(void) { int8 xgOffset, ygOffset, zgOffset; uint8 dmpUpdate[16]; uint8 j; uint16 pos = 0; uint8 buf; uint8 fifoBuffer[128]; uint8 fifoCount; MPU6050reset(); OSTimeDly(OS_TICKS_PER_SEC * 3 / 100 + 1); MPU6050setSleepEnabled(FALSE); xgOffset = MPU6050getXGyroOffset(); ygOffset = MPU6050getYGyroOffset(); zgOffset = MPU6050getZGyroOffset(); readByte(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, &buf); writeByte(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x32); writeByte(MPU6050_ADDRESS_COMPASS, 0x0A, 0x00); writeByte(MPU6050_ADDRESS_COMPASS, 0x0A, 0x0F); writeByte(MPU6050_ADDRESS_COMPASS, 0x0A, 0x00); if (MPU6050writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, TRUE)) { if (MPU6050writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { MPU6050setIntEnabled(0x12); MPU6050setRate(4); MPU6050setClockSource(MPU6050_CLOCK_PLL_ZGYRO); MPU6050setDLPFMode(MPU6050_DLPF_BW_42); MPU6050setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); MPU6050setFullScaleGyroRange(MPU6050_GYRO_FS_2000); MPU6050setDMPConfig1(0x03); MPU6050setDMPConfig2(0x00); MPU6050setOTPBankValid(FALSE); MPU6050setXGyroOffset(xgOffset); MPU6050setYGyroOffset(ygOffset); MPU6050setZGyroOffset(zgOffset); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) { dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); } MPU6050writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], TRUE, FALSE); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) { dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); } MPU6050writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], TRUE, FALSE); MPU6050resetFIFO(); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) { dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); } MPU6050writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], TRUE, FALSE); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) { dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); } MPU6050writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], TRUE, FALSE); writeByte(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00); writeByte(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0x00); MPU6050setMotionDetectionThreshold(2); MPU6050setZeroMotionDetectionThreshold(156); MPU6050setMotionDetectionDuration(80); MPU6050setZeroMotionDetectionDuration(0); writeByte(MPU6050_ADDRESS_COMPASS, 0x0A, 0x01); //remember to change address writeByte(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x8C); //remember to change address writeByte(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x01); writeByte(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0xDA); writeByte(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x0C); //remember to change address writeByte(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x0A); writeByte(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x81); writeByte(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x01); writeByte(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x18); writeByte(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x05); writeByte(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00); writeByte(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x20); writeByte(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x24); writeByte(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x20); writeByte(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0xE8); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) { dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); } MPU6050writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], TRUE, FALSE); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) { dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); } MPU6050writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], TRUE, FALSE); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) { dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); } MPU6050writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], TRUE, FALSE); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) { dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); } MPU6050writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], TRUE, FALSE); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) { dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); } MPU6050writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], TRUE, FALSE); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) { dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); } MPU6050writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], TRUE, FALSE); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) { dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); } MPU6050writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], TRUE, FALSE); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) { dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); } MPU6050readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) { dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); } MPU6050writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], TRUE, FALSE); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) { dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); } MPU6050writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], TRUE, FALSE); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) { dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); } MPU6050writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], TRUE, FALSE); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) { dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); } MPU6050writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], TRUE, FALSE); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) { dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); } MPU6050writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], TRUE, FALSE); // while ((fifoCount = MPU6050getFIFOCount()) < 46); if (fifoCount > 128) { MPU6050getFIFOBytes(fifoBuffer, 128); } else { MPU6050getFIFOBytes(fifoBuffer, fifoCount); } MPU6050getIntStatus(); // for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) { dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); } MPU6050writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], TRUE, FALSE); // while ((fifoCount = MPU6050getFIFOCount()) < 48); if (fifoCount > 128) { MPU6050getFIFOBytes(fifoBuffer, 128); } else { MPU6050getFIFOBytes(fifoBuffer, fifoCount); } MPU6050getIntStatus(); // // while ((fifoCount = MPU6050getFIFOCount()) < 48); if (fifoCount > 128) { MPU6050getFIFOBytes(fifoBuffer, 128); } else { MPU6050getFIFOBytes(fifoBuffer, fifoCount); } MPU6050getIntStatus(); // for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) { dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); } MPU6050writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], TRUE, FALSE); MPU6050setDMPEnabled(FALSE); MPU6050resetFIFO(); MPU6050getIntStatus(); } else { return 2; } } else { return 1; } return 0; }