Page 161 - 만들면서 배우는 아두이노 드론

}

void readAccelGyro() {
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B);
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr, 14, true);
AcX = Wire.read() << 8 | Wire.read();
AcY = Wire.read() << 8 | Wire.read();
AcZ = Wire.read() << 8 | Wire.read();
Tmp = Wire.read() << 8 | Wire.read();
GyX = Wire.read() << 8 | Wire.read();
GyY = Wire.read() << 8 | Wire.read();
GyZ = Wire.read() << 8 | Wire.read();

}

void calibAccelGyro() {
float sumAcX = 0, sumAcY = 0, sumAcZ = 0;
float sumGyX = 0, sumGyY = 0, sumGyZ = 0;

readAccelGyro();

for(int i = 0; i < 10; i++) {
readAccelGyro();
sumAcX += AcX; sumAcY += AcY; sumAcZ += AcZ;
sumGyX += GyX; sumGyY += GyY; sumGyZ += GyZ;
delay(100);

}

baseAcX = sumAcX / 10;
baseAcY = sumAcY / 10;
baseAcZ = sumAcZ / 10;

baseGyX = sumGyX / 10;
baseGyY = sumGyY / 10;
baseGyZ = sumGyZ / 10;
}

void calcAccelYPR() {

160