void calibrate_gyros(){
byte i=0;
while (i < ITERATIONS){ //hope that offsets converge in 6 iterations
accelgyro.getRotation(&gx, &gy, &gz);
if (count == SAMPLE_COUNT){
xoff += int(gxm/-3);
yoff += int(gym/-3);
zoff += int(gzm/-3);
accelgyro.setXGyroOffset(xoff);
accelgyro.setYGyroOffset(yoff);
accelgyro.setZGyroOffset(zoff);
#ifdef CAL_DEBUG
Serial.print(gxm); Serial.print(" ");
Serial.print(gym); Serial.print(" ");
Serial.println(gzm);
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{
gxm = (gxm*count + gx)/(count+1.0);
gym = (gym*count + gy)/(count+1.0);
gzm = (gzm*count + gz)/(count+1.0);
count++;
}
}
#ifdef CAL_DEBUG
Serial.println(" Done.");
#endif
}
// PUBLIC METHOD TO PROVIDE DATA ACQUIRED FROM MPU TO DEPENDENT SYSTEMS
uint8_t mpuAcquire(int16_t *accelOffsetX, int16_t *accelOffsetY,int16_t *accelOffsetZ, int16_t *currAccelX, int16_t *currAccelY, int16_t *currAccelZ, int16_t *currYaw, int16_t *currPitch, int16_t *currRoll){ /*Changed to int16*/
int16_t rot[3]; //Equivalent to uint8_t yaw; uint8_t pitch; uint8_t roll;
int16_t accel[3];//Equivalent to uint8_t accelX; uint8_t accelY; uint8_t accelZ;
//Error flag
uint8_t errorCode;
//DEFAULT SEND 0's
for(uint8_t j=0; j<4;j++){
accel[j]=0;
}
for(uint8_t k=0; k<4;k++){
rot[k]=0;
}
//Monitor MPU
errorCode=mpuMonitor(currAccelX,currAccelY,currAccelZ);
//IF PROBLEM, SEND PREVIOUS VALUES
if(errorCode!=0){
accel[0]=*currAccelX;
accel[1]=*currAccelY;
accel[2]=*currAccelZ;
rot[0]=*currYaw;
rot[1]=*currPitch;
rot[2]=*currRoll;
}
//IF GOT VALUES, SUPPLY NEW VALUES
else if(errorCode==0){
// display raw acceleration values
// Chosen getAcceleration method because only one showing approximately independent dimensions
// ALL others having problems; linearAccelInWorld in particular only giving zeroes, and linearAccel giving all ~same
#ifdef MPU_RAW_ACCEL
mpu.getAcceleration(accel,accel+1,accel+2);
accel[0]=(accel[0] - *accelOffsetX); // Components with offset applied
accel[1]=(accel[1] - *accelOffsetY); // No scaling factor 2048, to maximize resolution in integer rep.
accel[2]=(accel[2] - *accelOffsetZ);
#endif
// TO DO: WILL DETERMINE AXIS OF MAX DIFFERENCE BETWEEN accel AND currAccel VALUES...
// AND DIVIDE OTHER AXES' VALUES BY DERIVATIVE (USING TIMER COUNT)
// -OR- TAKE TIME AVERAGE OF BUFFERED VALUES AND DIVIDE OTHERS BY AXIS OF MAX RATE OF CHANGE
// -OR- MAX/MIN HOLD VALUES ON AXIS AND FIND RATE OF CHANGE OF PEAKS. DIVIDE OTHER AXES BY MAX RATE OF CHANGE
// display raw gyroscope values
//Took raw values because all others not independent (and dmpGetGyro in particular not sensitive)
#ifdef MPU_RAW_GYRO
mpu.getRotation(rot,rot+1,rot+2);
/* rot[0]=(rot[0]/16.4); // No scaling of rotation, so as maximize resolution with integer
rot[1]=(rot[1]/16.4);
rot[2]=(rot[2]/16.4); */
//Return absolute angle values, if desired
/* #ifdef ABSOLUTE_ANGLE
//Apply complementary filter to get
Complementary_Filter(accel[0],accel[1],accel[2],rot[0],rot[1],rot[2]);
#endif */
#endif
}
//Print out and update "previous" values to current ones
#ifdef MPU_RAW_ACCEL
/* FOR DEBUG, SERIAL PRINT
#ifdef accelAsComponents
Serial.print(accel[0] + ", " + accel[1] + ", " + accel[2]));
#endif
#ifdef accelAsMag
Serial.print(sqrt((abs(accel[0]))^2+(abs(accel[1]))^2+(abs(accel[2]))^2));
#endif
*/
*currAccelX=accel[0];
*currAccelY=accel[1];
*currAccelZ=accel[2];
#endif
#ifdef MPU_RAW_GYRO
*currYaw=rot[0];
*currPitch=rot[1];
*currRoll=rot[2];
#endif
/* #ifdef GET_TAPS
if(&(devAddr+TAPXYZ)){ //If tap received register goes high, send 1 tap indication
Serial.print(", 1");
}
#ifdef MANUAL_TAP
else if(gotTaps(rot[0],rot[1],rot[2],sFIFO_Y,sFIFO_P,sFIFO_R)){ //Same from gotTap() function, if returns true
Serial.print(", 1");
}
#endif
#endif */
//Return error code, to indicate type of problem if any
return errorCode;
}
