diff --git a/MPU6050.cpp b/MPU6050.cpp
index d9cc131..9a0f2d3 100644
--- a/MPU6050.cpp
+++ b/MPU6050.cpp
@@ -16,12 +16,14 @@ GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see .
+
+https://www.digikey.com/en/pdf/i/invensense/mpu-hardware-offset-registers
*/
#if ARDUINO >= 100
-#include "Arduino.h"
+ #include "Arduino.h"
#else
-#include "WProgram.h"
+ #include "WProgram.h"
#endif
#include
@@ -29,37 +31,32 @@ along with this program. If not, see .
#include
-bool MPU6050::begin(mpu6050_dps_t scale, mpu6050_range_t range, int mpua)
+#define GRAVITY_VALUE 9.80665 // https://en.wikipedia.org/wiki/Gravity_of_Earth
+
+
+
+bool MPU6050::begin(mpu6050_dps_t gyroScale, mpu6050_range_t accelRange, int mpuAddress)
{
// Set Address
- mpuAddress = mpua;
+ mMpuAddress = mpuAddress;
Wire.begin();
- // Reset calibrate values
- dg.XAxis = 0;
- dg.YAxis = 0;
- dg.ZAxis = 0;
- useCalibrate = false;
-
// Reset threshold values
- tg.XAxis = 0;
- tg.YAxis = 0;
- tg.ZAxis = 0;
- actualThreshold = 0;
+ mGyroscopeThreshold = 0;
// Check MPU6050 Who Am I Register
- if (fastRegister8(MPU6050_REG_WHO_AM_I) != 0x68)
+ if (readRegister8(MPU6050_REG_WHO_AM_I) != 0x68 && readRegister8(MPU6050_REG_WHO_AM_I) != 0x72) // For some reason my device returns 0x72
{
- return false;
+ return false;
}
// Set Clock Source
setClockSource(MPU6050_CLOCK_PLL_XGYRO);
// Set Scale & Range
- setScale(scale);
- setRange(range);
+ setGyroscopeScale(gyroScale);
+ setAccelerometerRange(accelRange);
// Disable Sleep Mode
setSleepEnabled(false);
@@ -67,26 +64,63 @@ bool MPU6050::begin(mpu6050_dps_t scale, mpu6050_range_t range, int mpua)
return true;
}
-void MPU6050::setScale(mpu6050_dps_t scale)
+void MPU6050::setAccelerometerRange(mpu6050_range_t range)
+{
+ uint8_t value;
+
+ switch (range)
+ {
+ case MPU6050_RANGE_2G:
+ mRangePerDigit = .000061f;
+ break;
+ case MPU6050_RANGE_4G:
+ mRangePerDigit = .000122f;
+ break;
+ case MPU6050_RANGE_8G:
+ mRangePerDigit = .000244f;
+ break;
+ case MPU6050_RANGE_16G:
+ mRangePerDigit = .0004882f;
+ break;
+ default:
+ break;
+ }
+
+ value = readRegister8(MPU6050_REG_ACCEL_CONFIG);
+ value &= 0b11100111;
+ value |= (range << 3);
+ writeRegister8(MPU6050_REG_ACCEL_CONFIG, value);
+}
+
+mpu6050_range_t MPU6050::getAccelerometerRange(void)
+{
+ uint8_t value;
+ value = readRegister8(MPU6050_REG_ACCEL_CONFIG);
+ value &= 0b00011000;
+ value >>= 3;
+ return (mpu6050_range_t)value;
+}
+
+void MPU6050::setGyroscopeScale(mpu6050_dps_t scale)
{
uint8_t value;
switch (scale)
{
- case MPU6050_SCALE_250DPS:
- dpsPerDigit = .007633f;
- break;
- case MPU6050_SCALE_500DPS:
- dpsPerDigit = .015267f;
- break;
- case MPU6050_SCALE_1000DPS:
- dpsPerDigit = .030487f;
- break;
- case MPU6050_SCALE_2000DPS:
- dpsPerDigit = .060975f;
- break;
- default:
- break;
+ case MPU6050_SCALE_250DPS:
+ mDegreesPerDigit = .007633f;
+ break;
+ case MPU6050_SCALE_500DPS:
+ mDegreesPerDigit = .015267f;
+ break;
+ case MPU6050_SCALE_1000DPS:
+ mDegreesPerDigit = .030487f;
+ break;
+ case MPU6050_SCALE_2000DPS:
+ mDegreesPerDigit = .060975f;
+ break;
+ default:
+ break;
}
value = readRegister8(MPU6050_REG_GYRO_CONFIG);
@@ -95,7 +129,7 @@ void MPU6050::setScale(mpu6050_dps_t scale)
writeRegister8(MPU6050_REG_GYRO_CONFIG, value);
}
-mpu6050_dps_t MPU6050::getScale(void)
+mpu6050_dps_t MPU6050::getGyroscopeScale(void)
{
uint8_t value;
value = readRegister8(MPU6050_REG_GYRO_CONFIG);
@@ -104,41 +138,213 @@ mpu6050_dps_t MPU6050::getScale(void)
return (mpu6050_dps_t)value;
}
-void MPU6050::setRange(mpu6050_range_t range)
+void MPU6050::getPitchAndRoll(Vector normAccel, float *pitch, float *roll)
{
- uint8_t value;
+ *pitch = -(atan2(normAccel.XAxis, sqrt(normAccel.YAxis * normAccel.YAxis + normAccel.ZAxis * normAccel.ZAxis)) * 180.0) / M_PI;
+ *roll = (atan2(normAccel.YAxis, normAccel.ZAxis) * 180.0) / M_PI;
+}
- switch (range)
+// By Henrique Bruno Fantauzzi de Almeida (SrBrahma) - Minerva Rockets, UFRJ, Rio de Janeiro - Brazil
+// To calibrate, put your hardware in a stable and straight surface until the end of the process.
+// https://forum.arduino.cc/index.php?action=dlattach;topic=418665.0;attach=229569
+void MPU6050::calibrateAccelerometer(uint16_t samples)
+{
+ mpu6050_range_t previousAccelerometerRange = getAccelerometerRange();
+
+ int32_t accelVectorSum[3] = {0, 0, 0}; // Make sure it initializes zeroed!
+ uint16_t counter;
+ Vector rawAccel;
+
+ int16_t newOffset, previousOffset;
+
+ setAccelerometerRange(MPU6050_RANGE_16G); // The versions below 1.2 of the MPU HW Offset registers say the test should happen
+ // on +-8g Range. However, after hours getting results that didn't match the theory, found out the 1.2 version, which as I thought
+ // (as I was getting the double of the expected results), corrected to the +-16g range.
+
+ for (counter = 0; counter < samples; counter++)
{
- case MPU6050_RANGE_2G:
- rangePerDigit = .000061f;
- break;
- case MPU6050_RANGE_4G:
- rangePerDigit = .000122f;
- break;
- case MPU6050_RANGE_8G:
- rangePerDigit = .000244f;
- break;
- case MPU6050_RANGE_16G:
- rangePerDigit = .0004882f;
- break;
- default:
- break;
+ rawAccel = readRawAccelerometer();
+
+ accelVectorSum[0] += rawAccel.XAxis;
+ accelVectorSum[1] += rawAccel.YAxis;
+ accelVectorSum[2] += rawAccel.ZAxis;
}
- value = readRegister8(MPU6050_REG_ACCEL_CONFIG);
- value &= 0b11100111;
- value |= (range << 3);
- writeRegister8(MPU6050_REG_ACCEL_CONFIG, value);
+ // As this https://forum.arduino.cc/index.php?action=dlattach;topic=418665.0;attach=229569 says,
+ // we must preserve the bit 0 of the default offset value, for each vector.
+
+ previousOffset = getAccelerometerOffsetX();
+ newOffset = previousOffset - (int16_t) (accelVectorSum[0] / counter); // The X vector must be 0 when on the normal position
+ newOffset = (newOffset & 0xFFFE) | (previousOffset & 0x0001); // To keep the previous value of the bit 0, as said in manual.
+ setAccelerometerOffsetX(newOffset);
+
+ previousOffset = getAccelerometerOffsetY();
+ newOffset = previousOffset - (int16_t) (accelVectorSum[1] / counter); // The Y vector must be 0 when on the normal position
+ newOffset = (newOffset & 0xFFFE) | (previousOffset & 0x0001); // To keep the previous value of the bit 0, as said in manual.
+ setAccelerometerOffsetY(newOffset);
+
+ previousOffset = getAccelerometerOffsetZ();
+ newOffset = previousOffset - (int16_t) ((accelVectorSum[2] / counter) - 2048); // The Z vector must be 1 when on the normal position
+ newOffset = (newOffset & 0xFFFE) | (previousOffset & 0x0001); // To keep the previous value of the bit 0, as said in manual.
+ setAccelerometerOffsetZ(newOffset);
+
+ setAccelerometerRange(previousAccelerometerRange); // Restore previous range
}
-mpu6050_range_t MPU6050::getRange(void)
+// https://forum.arduino.cc/index.php?action=dlattach;topic=418665.0;attach=229569
+void MPU6050::calibrateGyroscope(uint16_t samples)
{
- uint8_t value;
- value = readRegister8(MPU6050_REG_ACCEL_CONFIG);
- value &= 0b00011000;
- value >>= 3;
- return (mpu6050_range_t)value;
+ mpu6050_dps_t previousGyroscopeScale = getGyroscopeScale();
+
+ int32_t gyroVectorSum[3] = {0, 0, 0}; // Make sure it initializes zeroed!
+ uint16_t counter;
+ Vector rawGyro;
+
+ int16_t previousOffset;
+
+ setGyroscopeScale(MPU6050_SCALE_1000DPS); //As seen in the link above, we must set the scale to +-1000dps
+
+ for (counter = 0; counter < samples; counter++)
+ {
+ rawGyro = readRawGyroscope();
+
+ gyroVectorSum[0] += rawGyro.XAxis;
+ gyroVectorSum[1] += rawGyro.YAxis;
+ gyroVectorSum[2] += rawGyro.ZAxis;
+ }
+
+ previousOffset = getGyroscopeOffsetX();
+ setGyroscopeOffsetX(previousOffset - (int16_t) (gyroVectorSum[0] / counter));
+
+ previousOffset = getGyroscopeOffsetY();
+ setGyroscopeOffsetY(previousOffset - (int16_t) (gyroVectorSum[1] / counter));
+
+ previousOffset = getGyroscopeOffsetZ();
+ setGyroscopeOffsetZ(previousOffset - (int16_t) (gyroVectorSum[2] / counter));
+
+ setGyroscopeScale(previousGyroscopeScale); // Restore previous range
+}
+
+Vector MPU6050::readRawAccelerometer(void)
+{
+ Wire.beginTransmission(mMpuAddress);
+ #if ARDUINO >= 100
+ Wire.write(MPU6050_REG_ACCEL_XOUT_H);
+ #else
+ Wire.send(MPU6050_REG_ACCEL_XOUT_H);
+ #endif
+ Wire.endTransmission();
+
+ Wire.beginTransmission(mMpuAddress);
+ Wire.requestFrom(mMpuAddress, 6);
+
+ while (Wire.available() < 6);
+
+ #if ARDUINO >= 100
+ uint8_t xha = Wire.read();
+ uint8_t xla = Wire.read();
+ uint8_t yha = Wire.read();
+ uint8_t yla = Wire.read();
+ uint8_t zha = Wire.read();
+ uint8_t zla = Wire.read();
+ #else
+ uint8_t xha = Wire.receive();
+ uint8_t xla = Wire.receive();
+ uint8_t yha = Wire.receive();
+ uint8_t yla = Wire.receive();
+ uint8_t zha = Wire.receive();
+ uint8_t zla = Wire.receive();
+ #endif
+
+ mRawAccelerometer.XAxis = (int16_t) (xha << 8 | xla);
+ mRawAccelerometer.YAxis = (int16_t) (yha << 8 | yla);
+ mRawAccelerometer.ZAxis = (int16_t) (zha << 8 | zla);
+
+ return mRawAccelerometer;
+}
+
+Vector MPU6050::readNormalizedAccelerometer(void)
+{
+ readRawAccelerometer();
+
+ mNormalizedAccelerometer.XAxis = mRawAccelerometer.XAxis * mRangePerDigit * GRAVITY_VALUE;
+ mNormalizedAccelerometer.YAxis = mRawAccelerometer.YAxis * mRangePerDigit * GRAVITY_VALUE;
+ mNormalizedAccelerometer.ZAxis = mRawAccelerometer.ZAxis * mRangePerDigit * GRAVITY_VALUE;
+
+ return mNormalizedAccelerometer;
+}
+
+Vector MPU6050::readScaledAccelerometer(void)
+{
+ readRawAccelerometer();
+
+ mNormalizedAccelerometer.XAxis = mRawAccelerometer.XAxis * mRangePerDigit;
+ mNormalizedAccelerometer.YAxis = mRawAccelerometer.YAxis * mRangePerDigit;
+ mNormalizedAccelerometer.ZAxis = mRawAccelerometer.ZAxis * mRangePerDigit;
+
+ return mNormalizedAccelerometer;
+}
+
+
+Vector MPU6050::readRawGyroscope(void)
+{
+ Wire.beginTransmission(mMpuAddress);
+ #if ARDUINO >= 100
+ Wire.write(MPU6050_REG_GYRO_XOUT_H);
+ #else
+ Wire.send(MPU6050_REG_GYRO_XOUT_H);
+ #endif
+ Wire.endTransmission();
+
+ Wire.beginTransmission(mMpuAddress);
+ Wire.requestFrom(mMpuAddress, 6);
+
+ while (Wire.available() < 6);
+
+ #if ARDUINO >= 100
+ uint8_t xha = Wire.read();
+ uint8_t xla = Wire.read();
+ uint8_t yha = Wire.read();
+ uint8_t yla = Wire.read();
+ uint8_t zha = Wire.read();
+ uint8_t zla = Wire.read();
+ #else
+ uint8_t xha = Wire.receive();
+ uint8_t xla = Wire.receive();
+ uint8_t yha = Wire.receive();
+ uint8_t yla = Wire.receive();
+ uint8_t zha = Wire.receive();
+ uint8_t zla = Wire.receive();
+ #endif
+
+ mRawGyroscope.XAxis = (int16_t) (xha << 8 | xla);
+ mRawGyroscope.YAxis = (int16_t) (yha << 8 | yla);
+ mRawGyroscope.ZAxis = (int16_t) (zha << 8 | zla);
+
+ return mRawGyroscope;
+}
+
+Vector MPU6050::readNormalizedGyroscope(void)
+{
+ readRawGyroscope();
+
+ if (mGyroscopeThreshold)
+ {
+ if (abs(mRawGyroscope.XAxis) < mGyroscopeThreshold)
+ mRawGyroscope.XAxis = 0;
+ if (abs(mRawGyroscope.YAxis) < mGyroscopeThreshold)
+ mRawGyroscope.YAxis = 0;
+ if (abs(mRawGyroscope.ZAxis) < mGyroscopeThreshold)
+ mRawGyroscope.ZAxis = 0;
+ }
+
+ mNormalizedGyroscope.XAxis = mRawGyroscope.XAxis * mDegreesPerDigit;
+ mNormalizedGyroscope.YAxis = mRawGyroscope.YAxis * mDegreesPerDigit;
+ mNormalizedGyroscope.ZAxis = mRawGyroscope.ZAxis * mDegreesPerDigit;
+
+
+
+ return mNormalizedGyroscope;
}
void MPU6050::setDHPFMode(mpu6050_dhpf_t dhpf)
@@ -296,7 +502,7 @@ bool MPU6050::getI2CBypassEnabled(void)
return readRegisterBit(MPU6050_REG_INT_PIN_CFG, 1);
}
-void MPU6050::setAccelPowerOnDelay(mpu6050_onDelay_t delay)
+void MPU6050::setAccelerometerPowerOnDelay(mpu6050_onDelay_t delay)
{
uint8_t value;
value = readRegister8(MPU6050_REG_MOT_DETECT_CTRL);
@@ -305,7 +511,7 @@ void MPU6050::setAccelPowerOnDelay(mpu6050_onDelay_t delay)
writeRegister8(MPU6050_REG_MOT_DETECT_CTRL, value);
}
-mpu6050_onDelay_t MPU6050::getAccelPowerOnDelay(void)
+mpu6050_onDelay_t MPU6050::getAccelerometerPowerOnDelay(void)
{
uint8_t value;
value = readRegister8(MPU6050_REG_MOT_DETECT_CTRL);
@@ -318,154 +524,31 @@ uint8_t MPU6050::getIntStatus(void)
return readRegister8(MPU6050_REG_INT_STATUS);
}
-Activites MPU6050::readActivites(void)
+Activities MPU6050::readActivites(void)
{
uint8_t data = readRegister8(MPU6050_REG_INT_STATUS);
- a.isOverflow = ((data >> 4) & 1);
- a.isFreeFall = ((data >> 7) & 1);
- a.isInactivity = ((data >> 5) & 1);
- a.isActivity = ((data >> 6) & 1);
- a.isDataReady = ((data >> 0) & 1);
+ mActivities.isOverflow = ((data >> 4) & 1);
+ mActivities.isFreeFall = ((data >> 7) & 1);
+ mActivities.isInactivity = ((data >> 5) & 1);
+ mActivities.isActivity = ((data >> 6) & 1);
+ mActivities.isDataReady = ((data >> 0) & 1);
data = readRegister8(MPU6050_REG_MOT_DETECT_STATUS);
- a.isNegActivityOnX = ((data >> 7) & 1);
- a.isPosActivityOnX = ((data >> 6) & 1);
-
- a.isNegActivityOnY = ((data >> 5) & 1);
- a.isPosActivityOnY = ((data >> 4) & 1);
-
- a.isNegActivityOnZ = ((data >> 3) & 1);
- a.isPosActivityOnZ = ((data >> 2) & 1);
-
- return a;
-}
-
-Vector MPU6050::readRawAccel(void)
-{
- Wire.beginTransmission(mpuAddress);
- #if ARDUINO >= 100
- Wire.write(MPU6050_REG_ACCEL_XOUT_H);
- #else
- Wire.send(MPU6050_REG_ACCEL_XOUT_H);
- #endif
- Wire.endTransmission();
-
- Wire.beginTransmission(mpuAddress);
- Wire.requestFrom(mpuAddress, 6);
-
- while (Wire.available() < 6);
-
- #if ARDUINO >= 100
- uint8_t xha = Wire.read();
- uint8_t xla = Wire.read();
- uint8_t yha = Wire.read();
- uint8_t yla = Wire.read();
- uint8_t zha = Wire.read();
- uint8_t zla = Wire.read();
- #else
- uint8_t xha = Wire.receive();
- uint8_t xla = Wire.receive();
- uint8_t yha = Wire.receive();
- uint8_t yla = Wire.receive();
- uint8_t zha = Wire.receive();
- uint8_t zla = Wire.receive();
- #endif
-
- ra.XAxis = xha << 8 | xla;
- ra.YAxis = yha << 8 | yla;
- ra.ZAxis = zha << 8 | zla;
-
- return ra;
-}
-
-Vector MPU6050::readNormalizeAccel(void)
-{
- readRawAccel();
-
- na.XAxis = ra.XAxis * rangePerDigit * 9.80665f;
- na.YAxis = ra.YAxis * rangePerDigit * 9.80665f;
- na.ZAxis = ra.ZAxis * rangePerDigit * 9.80665f;
+ mActivities.isNegActivityOnX = ((data >> 7) & 1);
+ mActivities.isPosActivityOnX = ((data >> 6) & 1);
- return na;
-}
-
-Vector MPU6050::readScaledAccel(void)
-{
- readRawAccel();
+ mActivities.isNegActivityOnY = ((data >> 5) & 1);
+ mActivities.isPosActivityOnY = ((data >> 4) & 1);
- na.XAxis = ra.XAxis * rangePerDigit;
- na.YAxis = ra.YAxis * rangePerDigit;
- na.ZAxis = ra.ZAxis * rangePerDigit;
+ mActivities.isNegActivityOnZ = ((data >> 3) & 1);
+ mActivities.isPosActivityOnZ = ((data >> 2) & 1);
- return na;
+ return mActivities;
}
-Vector MPU6050::readRawGyro(void)
-{
- Wire.beginTransmission(mpuAddress);
- #if ARDUINO >= 100
- Wire.write(MPU6050_REG_GYRO_XOUT_H);
- #else
- Wire.send(MPU6050_REG_GYRO_XOUT_H);
- #endif
- Wire.endTransmission();
-
- Wire.beginTransmission(mpuAddress);
- Wire.requestFrom(mpuAddress, 6);
-
- while (Wire.available() < 6);
-
- #if ARDUINO >= 100
- uint8_t xha = Wire.read();
- uint8_t xla = Wire.read();
- uint8_t yha = Wire.read();
- uint8_t yla = Wire.read();
- uint8_t zha = Wire.read();
- uint8_t zla = Wire.read();
- #else
- uint8_t xha = Wire.receive();
- uint8_t xla = Wire.receive();
- uint8_t yha = Wire.receive();
- uint8_t yla = Wire.receive();
- uint8_t zha = Wire.receive();
- uint8_t zla = Wire.receive();
- #endif
-
- rg.XAxis = xha << 8 | xla;
- rg.YAxis = yha << 8 | yla;
- rg.ZAxis = zha << 8 | zla;
-
- return rg;
-}
-
-Vector MPU6050::readNormalizeGyro(void)
-{
- readRawGyro();
-
- if (useCalibrate)
- {
- ng.XAxis = (rg.XAxis - dg.XAxis) * dpsPerDigit;
- ng.YAxis = (rg.YAxis - dg.YAxis) * dpsPerDigit;
- ng.ZAxis = (rg.ZAxis - dg.ZAxis) * dpsPerDigit;
- } else
- {
- ng.XAxis = rg.XAxis * dpsPerDigit;
- ng.YAxis = rg.YAxis * dpsPerDigit;
- ng.ZAxis = rg.ZAxis * dpsPerDigit;
- }
-
- if (actualThreshold)
- {
- if (abs(ng.XAxis) < tg.XAxis) ng.XAxis = 0;
- if (abs(ng.YAxis) < tg.YAxis) ng.YAxis = 0;
- if (abs(ng.ZAxis) < tg.ZAxis) ng.ZAxis = 0;
- }
-
- return ng;
-}
float MPU6050::readTemperature(void)
{
@@ -474,143 +557,79 @@ float MPU6050::readTemperature(void)
return (float)T/340 + 36.53;
}
-int16_t MPU6050::getGyroOffsetX(void)
+int16_t MPU6050::getGyroscopeOffsetX(void)
{
return readRegister16(MPU6050_REG_GYRO_XOFFS_H);
}
-int16_t MPU6050::getGyroOffsetY(void)
+int16_t MPU6050::getGyroscopeOffsetY(void)
{
return readRegister16(MPU6050_REG_GYRO_YOFFS_H);
}
-int16_t MPU6050::getGyroOffsetZ(void)
+int16_t MPU6050::getGyroscopeOffsetZ(void)
{
return readRegister16(MPU6050_REG_GYRO_ZOFFS_H);
}
-void MPU6050::setGyroOffsetX(int16_t offset)
+void MPU6050::setGyroscopeOffsetX(int16_t offset)
{
writeRegister16(MPU6050_REG_GYRO_XOFFS_H, offset);
}
-void MPU6050::setGyroOffsetY(int16_t offset)
+void MPU6050::setGyroscopeOffsetY(int16_t offset)
{
writeRegister16(MPU6050_REG_GYRO_YOFFS_H, offset);
}
-void MPU6050::setGyroOffsetZ(int16_t offset)
+void MPU6050::setGyroscopeOffsetZ(int16_t offset)
{
writeRegister16(MPU6050_REG_GYRO_ZOFFS_H, offset);
}
-int16_t MPU6050::getAccelOffsetX(void)
+int16_t MPU6050::getAccelerometerOffsetX(void)
{
return readRegister16(MPU6050_REG_ACCEL_XOFFS_H);
}
-int16_t MPU6050::getAccelOffsetY(void)
+int16_t MPU6050::getAccelerometerOffsetY(void)
{
return readRegister16(MPU6050_REG_ACCEL_YOFFS_H);
}
-int16_t MPU6050::getAccelOffsetZ(void)
+int16_t MPU6050::getAccelerometerOffsetZ(void)
{
return readRegister16(MPU6050_REG_ACCEL_ZOFFS_H);
}
-void MPU6050::setAccelOffsetX(int16_t offset)
+void MPU6050::setAccelerometerOffsetX(int16_t offset)
{
writeRegister16(MPU6050_REG_ACCEL_XOFFS_H, offset);
}
-void MPU6050::setAccelOffsetY(int16_t offset)
+void MPU6050::setAccelerometerOffsetY(int16_t offset)
{
writeRegister16(MPU6050_REG_ACCEL_YOFFS_H, offset);
}
-void MPU6050::setAccelOffsetZ(int16_t offset)
+void MPU6050::setAccelerometerOffsetZ(int16_t offset)
{
writeRegister16(MPU6050_REG_ACCEL_ZOFFS_H, offset);
}
-// Calibrate algorithm
-void MPU6050::calibrateGyro(uint8_t samples)
-{
- // Set calibrate
- useCalibrate = true;
-
- // Reset values
- float sumX = 0;
- float sumY = 0;
- float sumZ = 0;
- float sigmaX = 0;
- float sigmaY = 0;
- float sigmaZ = 0;
-
- // Read n-samples
- for (uint8_t i = 0; i < samples; ++i)
- {
- readRawGyro();
- sumX += rg.XAxis;
- sumY += rg.YAxis;
- sumZ += rg.ZAxis;
-
- sigmaX += rg.XAxis * rg.XAxis;
- sigmaY += rg.YAxis * rg.YAxis;
- sigmaZ += rg.ZAxis * rg.ZAxis;
-
- delay(5);
- }
-
- // Calculate delta vectors
- dg.XAxis = sumX / samples;
- dg.YAxis = sumY / samples;
- dg.ZAxis = sumZ / samples;
-
- // Calculate threshold vectors
- th.XAxis = sqrt((sigmaX / 50) - (dg.XAxis * dg.XAxis));
- th.YAxis = sqrt((sigmaY / 50) - (dg.YAxis * dg.YAxis));
- th.ZAxis = sqrt((sigmaZ / 50) - (dg.ZAxis * dg.ZAxis));
-
- // If already set threshold, recalculate threshold vectors
- if (actualThreshold > 0)
- {
- setThreshold(actualThreshold);
- }
-}
-
// Get current threshold value
-uint8_t MPU6050::getThreshold(void)
+float MPU6050::getGyroscopeThreshold(void)
{
- return actualThreshold;
+ return mGyroscopeThreshold;
}
// Set treshold value
-void MPU6050::setThreshold(uint8_t multiple)
+void MPU6050::setGyroscopeThreshold(float percentOfMaximumValue) // Argument is in %. So, 1.5 = 1.5%.
{
- if (multiple > 0)
- {
- // If not calibrated, need calibrate
- if (!useCalibrate)
- {
- calibrateGyro();
- }
-
- // Calculate threshold vectors
- tg.XAxis = th.XAxis * multiple;
- tg.YAxis = th.YAxis * multiple;
- tg.ZAxis = th.ZAxis * multiple;
- } else
- {
- // No threshold
- tg.XAxis = 0;
- tg.YAxis = 0;
- tg.ZAxis = 0;
- }
-
- // Remember old threshold value
- actualThreshold = multiple;
+ if (percentOfMaximumValue > 0)
+ mGyroscopeThreshold = (32767 / 100.0) * percentOfMaximumValue;
+ else
+ mGyroscopeThreshold = 0;
}
// Fast read 8-bit from register
@@ -618,21 +637,24 @@ uint8_t MPU6050::fastRegister8(uint8_t reg)
{
uint8_t value;
- Wire.beginTransmission(mpuAddress);
+ Wire.beginTransmission(mMpuAddress);
#if ARDUINO >= 100
- Wire.write(reg);
+ Wire.write(reg);
#else
- Wire.send(reg);
+ Wire.send(reg);
#endif
+
Wire.endTransmission();
- Wire.beginTransmission(mpuAddress);
- Wire.requestFrom(mpuAddress, 1);
+ Wire.beginTransmission(mMpuAddress);
+ Wire.requestFrom(mMpuAddress, 1);
+
#if ARDUINO >= 100
- value = Wire.read();
+ value = Wire.read();
#else
- value = Wire.receive();
- #endif;
+ value = Wire.receive();
+ #endif
+
Wire.endTransmission();
return value;
@@ -643,22 +665,25 @@ uint8_t MPU6050::readRegister8(uint8_t reg)
{
uint8_t value;
- Wire.beginTransmission(mpuAddress);
+ Wire.beginTransmission(mMpuAddress);
#if ARDUINO >= 100
- Wire.write(reg);
+ Wire.write(reg);
#else
- Wire.send(reg);
+ Wire.send(reg);
#endif
Wire.endTransmission();
- Wire.beginTransmission(mpuAddress);
- Wire.requestFrom(mpuAddress, 1);
+ Wire.beginTransmission(mMpuAddress);
+ Wire.requestFrom(mMpuAddress, 1);
+
while(!Wire.available()) {};
+
#if ARDUINO >= 100
- value = Wire.read();
+ value = Wire.read();
#else
- value = Wire.receive();
- #endif;
+ value = Wire.receive();
+ #endif
+
Wire.endTransmission();
return value;
@@ -667,39 +692,45 @@ uint8_t MPU6050::readRegister8(uint8_t reg)
// Write 8-bit to register
void MPU6050::writeRegister8(uint8_t reg, uint8_t value)
{
- Wire.beginTransmission(mpuAddress);
+ Wire.beginTransmission(mMpuAddress);
#if ARDUINO >= 100
- Wire.write(reg);
- Wire.write(value);
+ Wire.write(reg);
+ Wire.write(value);
#else
- Wire.send(reg);
- Wire.send(value);
+ Wire.send(reg);
+ Wire.send(value);
#endif
+
Wire.endTransmission();
}
int16_t MPU6050::readRegister16(uint8_t reg)
{
int16_t value;
- Wire.beginTransmission(mpuAddress);
+ Wire.beginTransmission(mMpuAddress);
+
#if ARDUINO >= 100
Wire.write(reg);
#else
Wire.send(reg);
#endif
+
Wire.endTransmission();
- Wire.beginTransmission(mpuAddress);
- Wire.requestFrom(mpuAddress, 2);
+ Wire.beginTransmission(mMpuAddress);
+ Wire.requestFrom(mMpuAddress, 2);
+
while(!Wire.available()) {};
+
#if ARDUINO >= 100
uint8_t vha = Wire.read();
uint8_t vla = Wire.read();
#else
uint8_t vha = Wire.receive();
uint8_t vla = Wire.receive();
- #endif;
+ #endif
+
Wire.endTransmission();
value = vha << 8 | vla;
@@ -709,17 +740,18 @@ int16_t MPU6050::readRegister16(uint8_t reg)
void MPU6050::writeRegister16(uint8_t reg, int16_t value)
{
- Wire.beginTransmission(mpuAddress);
+ Wire.beginTransmission(mMpuAddress);
#if ARDUINO >= 100
- Wire.write(reg);
- Wire.write((uint8_t)(value >> 8));
- Wire.write((uint8_t)value);
+ Wire.write(reg);
+ Wire.write((uint8_t)(value >> 8));
+ Wire.write((uint8_t)value);
#else
- Wire.send(reg);
- Wire.send((uint8_t)(value >> 8));
- Wire.send((uint8_t)value);
+ Wire.send(reg);
+ Wire.send((uint8_t)(value >> 8));
+ Wire.send((uint8_t)value);
#endif
+
Wire.endTransmission();
}
@@ -738,12 +770,26 @@ void MPU6050::writeRegisterBit(uint8_t reg, uint8_t pos, bool state)
value = readRegister8(reg);
if (state)
- {
value |= (1 << pos);
- } else
- {
+ else
value &= ~(1 << pos);
- }
writeRegister8(reg, value);
}
+
+void MPU6050::resetDevice(mpu6050_dps_t gyroScale, mpu6050_range_t accelRange, int mpuAddress)
+{
+ writeRegisterBit(MPU6050_REG_PWR_MGMT_1, 7, 1); // as seen in the register map
+ delay(100);
+
+ // Reset threshold values
+ mGyroscopeThreshold = 0;
+
+ setClockSource(MPU6050_CLOCK_PLL_XGYRO);
+
+ setGyroscopeScale(gyroScale);
+ setAccelerometerRange(accelRange);
+
+ // Disable Sleep Mode
+ setSleepEnabled(false);
+}
\ No newline at end of file
diff --git a/MPU6050.h b/MPU6050.h
index fbf077e..ffcd23b 100644
--- a/MPU6050.h
+++ b/MPU6050.h
@@ -16,6 +16,8 @@ GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see .
+
+Edited by Henrique Bruno Fantauzzi de Almeida (SrBrahma) - Minerva Rockets, UFRJ, Rio de Janeiro - Brazil
*/
#ifndef MPU6050_h
@@ -27,7 +29,7 @@ along with this program. If not, see .
#include "WProgram.h"
#endif
-#define MPU6050_ADDRESS (0x68) // 0x69 when AD0 pin to Vcc
+#define MPU6050_ADDRESS (0x68) // 0x69 when AD0 pin to Vcc
#define MPU6050_REG_ACCEL_XOFFS_H (0x06)
#define MPU6050_REG_ACCEL_XOFFS_L (0x07)
@@ -75,15 +77,17 @@ along with this program. If not, see .
#ifndef VECTOR_STRUCT_H
#define VECTOR_STRUCT_H
+
struct Vector
{
float XAxis;
float YAxis;
float ZAxis;
};
+
#endif
-struct Activites
+struct Activities
{
bool isOverflow;
bool isFreeFall;
@@ -156,102 +160,124 @@ typedef enum
class MPU6050
{
- public:
-
- bool begin(mpu6050_dps_t scale = MPU6050_SCALE_2000DPS, mpu6050_range_t range = MPU6050_RANGE_2G, int mpua = MPU6050_ADDRESS);
-
- void setClockSource(mpu6050_clockSource_t source);
- void setScale(mpu6050_dps_t scale);
- void setRange(mpu6050_range_t range);
- mpu6050_clockSource_t getClockSource(void);
- mpu6050_dps_t getScale(void);
- mpu6050_range_t getRange(void);
- void setDHPFMode(mpu6050_dhpf_t dhpf);
- void setDLPFMode(mpu6050_dlpf_t dlpf);
- mpu6050_onDelay_t getAccelPowerOnDelay();
- void setAccelPowerOnDelay(mpu6050_onDelay_t delay);
-
- uint8_t getIntStatus(void);
-
- bool getIntZeroMotionEnabled(void);
- void setIntZeroMotionEnabled(bool state);
- bool getIntMotionEnabled(void);
- void setIntMotionEnabled(bool state);
- bool getIntFreeFallEnabled(void);
- void setIntFreeFallEnabled(bool state);
-
- uint8_t getMotionDetectionThreshold(void);
- void setMotionDetectionThreshold(uint8_t threshold);
- uint8_t getMotionDetectionDuration(void);
- void setMotionDetectionDuration(uint8_t duration);
-
- uint8_t getZeroMotionDetectionThreshold(void);
- void setZeroMotionDetectionThreshold(uint8_t threshold);
- uint8_t getZeroMotionDetectionDuration(void);
- void setZeroMotionDetectionDuration(uint8_t duration);
-
- uint8_t getFreeFallDetectionThreshold(void);
- void setFreeFallDetectionThreshold(uint8_t threshold);
- uint8_t getFreeFallDetectionDuration(void);
- void setFreeFallDetectionDuration(uint8_t duration);
-
- bool getSleepEnabled(void);
- void setSleepEnabled(bool state);
- bool getI2CMasterModeEnabled(void);
- void setI2CMasterModeEnabled(bool state);
- bool getI2CBypassEnabled(void);
- void setI2CBypassEnabled(bool state);
-
- float readTemperature(void);
- Activites readActivites(void);
-
- int16_t getGyroOffsetX(void);
- void setGyroOffsetX(int16_t offset);
- int16_t getGyroOffsetY(void);
- void setGyroOffsetY(int16_t offset);
- int16_t getGyroOffsetZ(void);
- void setGyroOffsetZ(int16_t offset);
-
- int16_t getAccelOffsetX(void);
- void setAccelOffsetX(int16_t offset);
- int16_t getAccelOffsetY(void);
- void setAccelOffsetY(int16_t offset);
- int16_t getAccelOffsetZ(void);
- void setAccelOffsetZ(int16_t offset);
-
- void calibrateGyro(uint8_t samples = 50);
- void setThreshold(uint8_t multiple = 1);
- uint8_t getThreshold(void);
-
- Vector readRawGyro(void);
- Vector readNormalizeGyro(void);
-
- Vector readRawAccel(void);
- Vector readNormalizeAccel(void);
- Vector readScaledAccel(void);
-
- private:
- Vector ra, rg; // Raw vectors
- Vector na, ng; // Normalized vectors
- Vector tg, dg; // Threshold and Delta for Gyro
- Vector th; // Threshold
- Activites a; // Activities
-
- float dpsPerDigit, rangePerDigit;
- float actualThreshold;
- bool useCalibrate;
- int mpuAddress;
-
- uint8_t fastRegister8(uint8_t reg);
-
- uint8_t readRegister8(uint8_t reg);
- void writeRegister8(uint8_t reg, uint8_t value);
-
- int16_t readRegister16(uint8_t reg);
- void writeRegister16(uint8_t reg, int16_t value);
-
- bool readRegisterBit(uint8_t reg, uint8_t pos);
- void writeRegisterBit(uint8_t reg, uint8_t pos, bool state);
+
+public:
+
+ bool begin(mpu6050_dps_t gyroScale = MPU6050_SCALE_2000DPS, mpu6050_range_t accelRange = MPU6050_RANGE_2G, int mpuAddress = MPU6050_ADDRESS);
+
+ void resetDevice(mpu6050_dps_t gyroScale = MPU6050_SCALE_2000DPS, mpu6050_range_t accelRange = MPU6050_RANGE_2G, int mpuAddress = MPU6050_ADDRESS);
+
+ void calibrateGyroscope(uint16_t samples = 1000);
+ void calibrateAccelerometer(uint16_t samples = 1000);
+
+ Vector readRawGyroscope(void);
+ Vector readNormalizedGyroscope(void);
+
+ Vector readRawAccelerometer(void);
+ Vector readNormalizedAccelerometer(void);
+ Vector readScaledAccelerometer(void);
+
+ void getPitchAndRoll(Vector normAccel, float *pitch, float *roll);
+
+ void setClockSource(mpu6050_clockSource_t source);
+
+ mpu6050_dps_t getGyroscopeScale(void);
+ void setGyroscopeScale(mpu6050_dps_t scale);
+
+ mpu6050_range_t getAccelerometerRange(void);
+ void setAccelerometerRange(mpu6050_range_t range);
+
+ mpu6050_clockSource_t getClockSource(void);
+
+ void setDHPFMode(mpu6050_dhpf_t dhpf);
+ void setDLPFMode(mpu6050_dlpf_t dlpf);
+
+ mpu6050_onDelay_t getAccelerometerPowerOnDelay();
+ void setAccelerometerPowerOnDelay(mpu6050_onDelay_t delay);
+
+ uint8_t getIntStatus(void);
+
+ bool getIntZeroMotionEnabled(void);
+ void setIntZeroMotionEnabled(bool state);
+
+ bool getIntMotionEnabled(void);
+ void setIntMotionEnabled(bool state);
+
+ bool getIntFreeFallEnabled(void);
+ void setIntFreeFallEnabled(bool state);
+
+ uint8_t getMotionDetectionThreshold(void);
+ void setMotionDetectionThreshold(uint8_t threshold);
+
+ uint8_t getMotionDetectionDuration(void);
+ void setMotionDetectionDuration(uint8_t duration);
+
+ uint8_t getZeroMotionDetectionThreshold(void);
+ void setZeroMotionDetectionThreshold(uint8_t threshold);
+
+ uint8_t getZeroMotionDetectionDuration(void);
+ void setZeroMotionDetectionDuration(uint8_t duration);
+
+ uint8_t getFreeFallDetectionThreshold(void);
+ void setFreeFallDetectionThreshold(uint8_t threshold);
+
+ uint8_t getFreeFallDetectionDuration(void);
+ void setFreeFallDetectionDuration(uint8_t duration);
+
+ bool getSleepEnabled(void);
+ void setSleepEnabled(bool state);
+
+ bool getI2CMasterModeEnabled(void);
+ void setI2CMasterModeEnabled(bool state);
+
+ bool getI2CBypassEnabled(void);
+ void setI2CBypassEnabled(bool state);
+
+ float readTemperature(void);
+
+ Activities readActivites(void);
+
+ int16_t getGyroscopeOffsetX(void);
+ void setGyroscopeOffsetX(int16_t offset);
+
+ int16_t getGyroscopeOffsetY(void);
+ void setGyroscopeOffsetY(int16_t offset);
+
+ int16_t getGyroscopeOffsetZ(void);
+ void setGyroscopeOffsetZ(int16_t offset);
+
+ int16_t getAccelerometerOffsetX(void);
+ void setAccelerometerOffsetX(int16_t offset);
+
+ int16_t getAccelerometerOffsetY(void);
+ void setAccelerometerOffsetY(int16_t offset);
+
+ int16_t getAccelerometerOffsetZ(void);
+ void setAccelerometerOffsetZ(int16_t offset);
+
+ void setGyroscopeThreshold(float percentOfMaximumValue);
+ float getGyroscopeThreshold(void);
+
+private:
+
+ int mMpuAddress;
+ Activities mActivities; // Activities
+
+ Vector mRawAccelerometer, mRawGyroscope; // Raw vectors
+ float mDegreesPerDigit, mRangePerDigit;
+ Vector mNormalizedAccelerometer, mNormalizedGyroscope; // Normalized vectors
+
+ float mGyroscopeThreshold; // Threshold and Delta for Gyro
+
+ void writeRegisterBit(uint8_t reg, uint8_t pos, bool state);
+ bool readRegisterBit (uint8_t reg, uint8_t pos);
+
+ uint8_t fastRegister8 (uint8_t reg);
+ uint8_t readRegister8 (uint8_t reg);
+ int16_t readRegister16(uint8_t reg);
+
+ void writeRegister8 (uint8_t reg, uint8_t value);
+ void writeRegister16(uint8_t reg, int16_t value);
};
diff --git a/MPU6050_better_test/MPU6050_better_test.ino b/MPU6050_better_test/MPU6050_better_test.ino
new file mode 100644
index 0000000..97e41b2
--- /dev/null
+++ b/MPU6050_better_test/MPU6050_better_test.ino
@@ -0,0 +1,178 @@
+/*
+ MPU6050 Triple Axis Gyroscope & Accelerometer. Simple Gyroscope Example.
+ Read more: http://www.jarzebski.pl/arduino/czujniki-i-sensory/3-osiowy-zyroskop-i-akcelerometr-mpu6050.html
+ GIT: https://github.com/jarzebski/Arduino-MPU6050
+ Web: http://www.jarzebski.pl
+ (c) 2014 by Korneliusz Jarzebski
+
+ This version was made by Henrique Bruno Fantauzzi de Almeida (SrBrahma) - Minerva Rockets, UFRJ, Rio de Janeiro - Brazil
+
+ Will first calibrate the accelerometer and the gyroscope, than will get its normalized values and the angles.
+ It's much better now.
+*/
+
+#include
+#include
+
+MPU6050 mpu;
+
+#define BASIC_FILTER_SAMPLES 10
+
+unsigned long timer;
+
+// Pitch, Roll and Yaw values
+float pitchByGyroscope = 0, rollByGyroscope = 0, yawByGyroscope = 0;
+float pitchByAccelerometer, rollByAccelerometer;
+
+int counterBasicFilter = 0, counterPrint = 0;
+
+Vector normAccel, newAccel;
+Vector normGyro, newGyro;
+
+void setup()
+{
+ Serial.begin(115200);
+
+ while (!Serial); // Wait the Serial Monitor
+
+ Serial.println("Initialize MPU6050");
+
+ while(!mpu.begin(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G))
+ {
+ Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
+ delay(500);
+ }
+
+ mpu.resetDevice(MPU6050_SCALE_2000DPS, MPU6050_RANGE_2G); // remove any previous settings on the register from a previous session
+
+ // Calibrate Accelerometer
+ Serial.print("Offsets of the accelerometer previously were (x, y, z): ");
+ Serial.print(mpu.getAccelerometerOffsetX());
+ Serial.print(", ");
+ Serial.print(mpu.getAccelerometerOffsetY());
+ Serial.print(", ");
+ Serial.print(mpu.getAccelerometerOffsetZ());
+ Serial.print("\n\n");
+
+ mpu.calibrateAccelerometer();
+
+ Serial.print("Offsets of the accelerometer now are (x, y, z): ");
+ Serial.print(mpu.getAccelerometerOffsetX());
+ Serial.print(", ");
+ Serial.print(mpu.getAccelerometerOffsetY());
+ Serial.print(", ");
+ Serial.print(mpu.getAccelerometerOffsetZ());
+ Serial.print("\n\n");
+
+
+
+ // Calibrate Gyroscope
+ Serial.print("Offsets of the gyroscope previously were (x, y, z): ");
+ Serial.print(mpu.getGyroscopeOffsetX());
+ Serial.print(", ");
+ Serial.print(mpu.getGyroscopeOffsetY());
+ Serial.print(", ");
+ Serial.print(mpu.getGyroscopeOffsetZ());
+ Serial.print("\n\n");
+
+ mpu.calibrateGyroscope();
+
+ Serial.print("Offsets of the gyroscope now are (x, y, z): ");
+ Serial.print(mpu.getGyroscopeOffsetX());
+ Serial.print(", ");
+ Serial.print(mpu.getGyroscopeOffsetY());
+ Serial.print(", ");
+ Serial.print(mpu.getGyroscopeOffsetZ());
+ Serial.print("\n\n");
+
+ mpu.setGyroscopeThreshold(0.5);
+
+ Serial.println("Setup ended. Press any key to continue to the main loop.");
+ while(!Serial.available());
+
+ // Set threshold sensivty. Default 3.
+ // If you don't want use threshold, comment this line or set 0.
+ //mpu.setGyroscopeThreshold(0);
+
+ timer = millis();
+}
+
+void loop()
+{
+
+ normAccel.XAxis = 0;
+ normAccel.YAxis = 0;
+ normAccel.ZAxis = 0;
+
+ normGyro.XAxis = 0;
+ normGyro.YAxis = 0;
+ normGyro.ZAxis = 0;
+
+ // A simple filter.
+ for(counterBasicFilter = 0; counterBasicFilter < BASIC_FILTER_SAMPLES; counterBasicFilter++)
+ {
+ newAccel = mpu.readNormalizedAccelerometer();
+ normAccel.XAxis += newAccel.XAxis;
+ normAccel.YAxis += newAccel.YAxis;
+ normAccel.ZAxis += newAccel.ZAxis;
+
+ newGyro = mpu.readNormalizedGyroscope();
+ normGyro.XAxis += newGyro.XAxis;
+ normGyro.YAxis += newGyro.YAxis;
+ normGyro.ZAxis += newGyro.ZAxis;
+ }
+
+ normAccel.XAxis /= BASIC_FILTER_SAMPLES;
+ normAccel.YAxis /= BASIC_FILTER_SAMPLES;
+ normAccel.ZAxis /= BASIC_FILTER_SAMPLES;
+
+ normGyro.XAxis /= BASIC_FILTER_SAMPLES;
+ normGyro.YAxis /= BASIC_FILTER_SAMPLES;
+ normGyro.ZAxis /= BASIC_FILTER_SAMPLES;
+
+ // Output
+ mpu.getPitchAndRoll(normAccel, &pitchByAccelerometer, &rollByAccelerometer);
+
+ // Read normalized values
+ normGyro = mpu.readNormalizedGyroscope();
+
+
+
+ // Calculate Pitch, Roll and Yaw
+ pitchByGyroscope = pitchByGyroscope + (normGyro.YAxis * (millis() - timer)) / 1000.0;
+ rollByGyroscope = rollByGyroscope + (normGyro.XAxis * (millis() - timer)) / 1000.0;
+ yawByGyroscope = yawByGyroscope + (normGyro.ZAxis * (millis() - timer)) / 1000.0;
+ timer = millis();
+
+ counterPrint++;
+
+ if (counterPrint % 3 == 0) // Less time spent writing to the Serial, also, makes the reading easier. Change it to fit your needs.
+ {
+ Serial.print("AccelX = ");
+ Serial.print(normAccel.XAxis);
+ Serial.print(" | AccelY = ");
+ Serial.print(normAccel.YAxis);
+ Serial.print(" | AccelZ = ");
+ Serial.println(normAccel.ZAxis);
+ Serial.print(" | AccelPitch = ");
+ Serial.print(pitchByAccelerometer);
+ Serial.print(" | AccelRoll = ");
+ Serial.println(rollByAccelerometer);
+
+ Serial.print("GyrosX = ");
+ Serial.print(normGyro.XAxis);
+ Serial.print(" | GyrosY = ");
+ Serial.print(normGyro.YAxis);
+ Serial.print(" | GyrosZ = ");
+ Serial.print(normGyro.ZAxis);
+ Serial.print(" | GyrosPitch = ");
+ Serial.print(pitchByGyroscope);
+ Serial.print(" | GyrosRoll = ");
+ Serial.print(rollByGyroscope);
+ Serial.print(" | GyrosYaw = ");
+ Serial.println(yawByGyroscope);
+ Serial.println();
+ counterPrint = 0;
+ }
+
+}
\ No newline at end of file
diff --git a/README.md b/README.md
index a2fedf0..88aafc0 100644
--- a/README.md
+++ b/README.md
@@ -1,6 +1,11 @@
Arduino-MPU6050
===============
+This version was edited by Henrique Bruno Fantauzzi de Almeida (SrBrahma) - Minerva Rockets, UFRJ, Rio de Janeiro - Brazil.
+Some vital stuff on the MPU6050.cpp were fixed (like wrong or missing typecasts), some nice stuff were added (like calibration based on offsets, which are written on the sensor registers), and general code improvement.
+
+--- Text below is from the original repository. ---
+
MPU6050 Triple Axis Gyroscope & Accelerometer Arduino Library.
