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