bool Gyro_init(void)
{
Wire.begin();
mpu.initialize();
if (mpu.testConnection() == false) {
return false;
}
devStatus = mpu.dmpInitialize();
if (devStatus == 0) {
mpu.setXGyroOffset(X_GYRO_OFFSET);
mpu.setYGyroOffset(Y_GYRO_OFFSET);
mpu.setZGyroOffset(Z_GYRO_OFFSET);
mpu.setXAccelOffset(X_ACCEL_OFFSET);
mpu.setYAccelOffset(Y_ACCEL_OFFSET);
mpu.setZAccelOffset(Z_ACCEL_OFFSET);
mpu.setDMPEnabled(true);
dmpReady = true;
attachInterrupt(0, dmp_data_ready, RISING);
mpuIntStatus = mpu.getIntStatus();
packetSize = mpu.dmpGetFIFOPacketSize();
return true;
}
else {
return false;
}
}
void calibrate_accel(){
xoff = accelgyro.getXAccelOffset();
yoff = accelgyro.getYAccelOffset();
zoff = accelgyro.getZAccelOffset();
byte i=0;
while (i < ITERATIONS){ //hope that offsets converge in 6 iterations
accelgyro.getAcceleration(&ax, &ay, &az);
if (count == SAMPLE_COUNT){
xoff += int(axm/-6);
yoff += int(aym/-6);
zoff += int((azm+16384)/-6);
accelgyro.setXAccelOffset(xoff);
accelgyro.setYAccelOffset(yoff);
accelgyro.setZAccelOffset(zoff);
#ifdef CAL_DEBUG
Serial.print(axm); Serial.print(" ");
Serial.print(aym); Serial.print(" ");
Serial.println(azm);
Serial.print(xoff); Serial.print(" ");
Serial.print(yoff); Serial.print(" ");
Serial.println(zoff);
Serial.println("*********************");
#endif
count = 0;
i++; //iteration++
#ifdef CAL_DEBUG
Serial.print(".");
#endif
}
else{
axm = (axm*count + ax)/(count+1.0);
aym = (aym*count + ay)/(count+1.0);
azm = (azm*count + az)/(count+1.0);
count++;
}
}
#ifdef CAL_DEBUG
Serial.println(" Done.");
#endif
}
