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 }