int CurieIMUClass::getGyroOffset(int axis)
{
if (axis == X_AXIS) {
return getXGyroOffset();
} else if (axis == Y_AXIS) {
return getYGyroOffset();
} else if (axis == Z_AXIS) {
return getZGyroOffset();
}
return -1;
}
float CurieIMUClass::getGyroOffset(int axis)
{
int bmiOffset;
if (axis == X_AXIS) {
bmiOffset = getXGyroOffset();
} else if (axis == Y_AXIS) {
bmiOffset = getYGyroOffset();
} else if (axis == Z_AXIS) {
bmiOffset = getZGyroOffset();
} else {
return -1;
}
return (bmiOffset * 0.061);
}
unsigned char dmpInitialize()
{
// reset device
mpu_reset();
DELAY_US(30*1000);
// disable sleep mode
setSleepEnabled(false);
// get MPU hardware revision
setMemoryBank(0x10, true, true);
setMemoryStartAddress(0x06);
unsigned char hwRevision = readMemoryByte();
setMemoryBank(0, false, false);
// check OTP bank valid
unsigned char otpValid = getOTPBankValid();
// get X/Y/Z gyro offsets
char xgOffsetTC = getXGyroOffset();
char ygOffsetTC = getYGyroOffset();
char zgOffsetTC = getZGyroOffset();
// setup weird slave stuff (?)
// setSlaveAddress(0, 0x7F);
// setI2CMasterModeEnabled(false);
// setSlaveAddress(0, 0x68);
// resetI2CMaster();
DELAY_US(20*1000);
// load DMP code into memory banks
if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE,0,0,false))
{
if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
setIntEnabled(0x12);
setRate(4); // 1khz / (1 + 4) = 200 Hz
setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
setDLPFMode(MPU6050_DLPF_BW_42);
// setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
// setFullScaleAccelRange(MPU6050_ACCEL_FS_16);
setDMPConfig1(0x03);
setDMPConfig2(0x00);
setOTPBankValid(false);
// setXGyroOffset(xgOffsetTC);
// setYGyroOffset(ygOffsetTC);
// setZGyroOffset(zgOffsetTC);
unsigned char dmpUpdate[16], j;
unsigned int pos = 0;
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(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]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true, false);
resetFIFO();
unsigned int fifoCount = getFIFOCount();
if (fifoCount > 128)
{
fifoCount = 128;
}
getFIFOBytes(fifoBuffer, fifoCount);
setMotionDetectionThreshold(2);
setZeroMotionDetectionThreshold(156);
setMotionDetectionDuration(80);
setZeroMotionDetectionDuration(0);
resetFIFO();
setFIFOEnabled(true);
setDMPEnabled(true);
resetDMP();
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(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]);
writeMemoryBlock(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]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true, false);
// while ((getFIFOCount()) < 3) {DELAY_US(500);}
//
// fifoCount = getFIFOCount();
//
// getFIFOBytes(fifoBuffer, fifoCount);
unsigned char mpuIntStatus = getIntStatus();
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
// while ((getFIFOCount()) < 3) {DELAY_US(500);}
//
// fifoCount = getFIFOCount();
// getFIFOBytes(fifoBuffer, fifoCount);
mpuIntStatus = getIntStatus();
for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true, false);
setDMPEnabled(true);
resetFIFO();
getIntStatus();
} else {
return 2; // configuration block loading failed
}
} else {
return 1; // main binary block loading failed
}
return 0; // success
}
