accel & gyro offsets

MPU6050.cpp

@@ -252,6 +252,66 @@ Vector MPU6050::readNormalizeGyro()
     return ng;
 }
 
+int16_t MPU6050::getGyroOffsetX(void)
+{
+    return readRegister16(MPU6050_REG_GYRO_XOFFS_H);
+}
+
+int16_t MPU6050::getGyroOffsetY(void)
+{
+    return readRegister16(MPU6050_REG_GYRO_YOFFS_H);
+}
+
+int16_t MPU6050::getGyroOffsetZ(void)
+{
+    return readRegister16(MPU6050_REG_GYRO_ZOFFS_H);
+}
+
+void MPU6050::setGyroOffsetX(int16_t offset)
+{
+    writeRegister16(MPU6050_REG_GYRO_XOFFS_H, offset);
+}
+
+void MPU6050::setGyroOffsetY(int16_t offset)
+{
+    writeRegister16(MPU6050_REG_GYRO_YOFFS_H, offset);
+}
+
+void MPU6050::setGyroOffsetZ(int16_t offset)
+{
+    writeRegister16(MPU6050_REG_GYRO_ZOFFS_H, offset);
+}
+
+int16_t MPU6050::getAccelOffsetX(void)
+{
+    return readRegister16(MPU6050_REG_ACCEL_XOFFS_H);
+}
+
+int16_t MPU6050::getAccelOffsetY(void)
+{
+    return readRegister16(MPU6050_REG_ACCEL_YOFFS_H);
+}
+
+int16_t MPU6050::getAccelOffsetZ(void)
+{
+    return readRegister16(MPU6050_REG_ACCEL_ZOFFS_H);
+}
+
+void MPU6050::setAccelOffsetX(int16_t offset)
+{
+    writeRegister16(MPU6050_REG_ACCEL_XOFFS_H, offset);
+}
+
+void MPU6050::setAccelOffsetY(int16_t offset)
+{
+    writeRegister16(MPU6050_REG_ACCEL_YOFFS_H, offset);
+}
+
+void MPU6050::setAccelOffsetZ(int16_t offset)
+{
+    writeRegister16(MPU6050_REG_ACCEL_ZOFFS_H, offset);
+}
+
 // Fast read 8-bit from register
 uint8_t MPU6050::fastRegister8(uint8_t reg)
 {
@@ -318,6 +378,50 @@ void MPU6050::writeRegister8(uint8_t reg, uint8_t value)
     Wire.endTransmission();
 }
 
+int16_t MPU6050::readRegister16(uint8_t reg)
+{
+    int16_t value;
+    Wire.beginTransmission(MPU6050_ADDRESS);
+    #if ARDUINO >= 100
+        Wire.write(reg);
+    #else
+        Wire.send(reg);
+    #endif
+    Wire.endTransmission();
+
+    Wire.beginTransmission(MPU6050_ADDRESS);
+    Wire.requestFrom(MPU6050_ADDRESS, 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;
+    Wire.endTransmission();
+
+    value = vha << 8 | vla;
+
+    return value;
+}
+
+void MPU6050::writeRegister16(uint8_t reg, int16_t value)
+{
+    Wire.beginTransmission(MPU6050_ADDRESS);
+
+    #if ARDUINO >= 100
+	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);
+    #endif
+    Wire.endTransmission();
+}
+
 // Read register bit
 bool MPU6050::readRegisterBit(uint8_t reg, uint8_t pos)
 {

MPU6050.h

@@ -29,6 +29,18 @@ along with this program.  If not, see <http://www.gnu.org/licenses/>.
 
 #define MPU6050_ADDRESS           (0x68) // 0x69 when AD0 pin to Vcc
 
+#define MPU6050_REG_ACCEL_XOFFS_H (0x06)
+#define MPU6050_REG_ACCEL_XOFFS_L (0x07)
+#define MPU6050_REG_ACCEL_YOFFS_H (0x08)
+#define MPU6050_REG_ACCEL_YOFFS_L (0x09)
+#define MPU6050_REG_ACCEL_ZOFFS_H (0x0A)
+#define MPU6050_REG_ACCEL_ZOFFS_L (0x0B)
+#define MPU6050_REG_GYRO_XOFFS_H  (0x13)
+#define MPU6050_REG_GYRO_XOFFS_L  (0x14)
+#define MPU6050_REG_GYRO_YOFFS_H  (0x15)
+#define MPU6050_REG_GYRO_YOFFS_L  (0x16)
+#define MPU6050_REG_GYRO_ZOFFS_H  (0x17)
+#define MPU6050_REG_GYRO_ZOFFS_L  (0x18)
 #define MPU6050_REG_GYRO_CONFIG   (0x1B) // Gyroscope Configuration
 #define MPU6050_REG_ACCEL_CONFIG  (0x1C) // Accelerometer Configuration
 #define MPU6050_REG_ACCEL_XOUT_H  (0x3B)
@@ -98,6 +110,20 @@ class MPU6050
 	bool getSleepEnabled(void);
 	void setSleepEnabled(bool state);
 
+	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);
+
 	Vector readRawGyro(void);
 	Vector readNormalizeGyro(void);
 
@@ -110,9 +136,14 @@ class MPU6050
 
 	float dpsPerDigit, rangePerDigit;
 
-	void writeRegister8(uint8_t reg, uint8_t value);
-	uint8_t readRegister8(uint8_t reg);
 	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);
 

MPU6050_accel_simple/MPU6050_accel_simple.ino

@@ -22,6 +22,11 @@ void setup()
     Serial.println("Could not find a valid MPU6050 sensor, check wiring!");
     delay(500);
   }
+
+  // If you want, you can set accelerometer offsets
+  // mpu.setAccelOffsetX();
+  // mpu.setAccelOffsetY();
+  // mpu.setAccelOffsetZ();
   
   checkSettings();
 }
@@ -30,10 +35,10 @@ void checkSettings()
 {
   Serial.println();
   
-  Serial.print(" * Sleep Mode:    ");
+  Serial.print(" * Sleep Mode:            ");
   Serial.println(mpu.getSleepEnabled() ? "Enabled" : "Disabled");
   
-  Serial.print(" * Clock Source:  ");
+  Serial.print(" * Clock Source:          ");
   switch(mpu.getClockSource())
   {
     case MPU6050_CLOCK_KEEP_RESET:     Serial.println("Stops the clock and keeps the timing generator in reset"); break;
@@ -45,16 +50,7 @@ void checkSettings()
     case MPU6050_CLOCK_INTERNAL_8MHZ:  Serial.println("Internal 8MHz oscillator"); break;
   }
   
-  Serial.print(" * Gyroscope:     ");
-  switch(mpu.getScale())
-  {
-    case MPU6050_SCALE_2000DPS:        Serial.println("2000 dps"); break;
-    case MPU6050_SCALE_1000DPS:        Serial.println("1000 dps"); break;
-    case MPU6050_SCALE_500DPS:         Serial.println("500 dps"); break;
-    case MPU6050_SCALE_250DPS:         Serial.println("250 dps"); break;
-  } 
-  
-  Serial.print(" * Accelerometer: ");
+  Serial.print(" * Accelerometer:         ");
   switch(mpu.getRange())
   {
     case MPU6050_RANGE_16G:            Serial.println("+/- 16 g"); break;
@@ -62,6 +58,13 @@ void checkSettings()
     case MPU6050_RANGE_4G:             Serial.println("+/- 4 g"); break;
     case MPU6050_RANGE_2G:             Serial.println("+/- 2 g"); break;
   }  
+
+  Serial.print(" * Accelerometer offsets: ");
+  Serial.print(mpu.getAccelOffsetX());
+  Serial.print(" / ");
+  Serial.print(mpu.getAccelOffsetY());
+  Serial.print(" / ");
+  Serial.println(mpu.getAccelOffsetZ());
   
   Serial.println();
 }

MPU6050_gyro_simple/MPU6050_gyro_simple.ino

@@ -23,6 +23,11 @@ void setup()
     delay(500);
   }
   
+  // If you want, you can set gyroscope offsets
+  // mpu.setGyroOffsetX(155);
+  // mpu.setGyroOffsetY(15);
+  // mpu.setGyroOffsetZ(15);
+  
   checkSettings();
 }
 
@@ -30,10 +35,10 @@ void checkSettings()
 {
   Serial.println();
   
-  Serial.print(" * Sleep Mode:    ");
+  Serial.print(" * Sleep Mode:        ");
   Serial.println(mpu.getSleepEnabled() ? "Enabled" : "Disabled");
   
-  Serial.print(" * Clock Source:  ");
+  Serial.print(" * Clock Source:      ");
   switch(mpu.getClockSource())
   {
     case MPU6050_CLOCK_KEEP_RESET:     Serial.println("Stops the clock and keeps the timing generator in reset"); break;
@@ -45,7 +50,7 @@ void checkSettings()
     case MPU6050_CLOCK_INTERNAL_8MHZ:  Serial.println("Internal 8MHz oscillator"); break;
   }
   
-  Serial.print(" * Gyroscope:     ");
+  Serial.print(" * Gyroscope:         ");
   switch(mpu.getScale())
   {
     case MPU6050_SCALE_2000DPS:        Serial.println("2000 dps"); break;
@@ -54,14 +59,12 @@ void checkSettings()
     case MPU6050_SCALE_250DPS:         Serial.println("250 dps"); break;
   } 
   
-  Serial.print(" * Accelerometer: ");
-  switch(mpu.getRange())
-  {
-    case MPU6050_RANGE_16G:            Serial.println("+/- 16 g"); break;
-    case MPU6050_RANGE_8G:             Serial.println("+/- 8 g"); break;
-    case MPU6050_RANGE_4G:             Serial.println("+/- 4 g"); break;
-    case MPU6050_RANGE_2G:             Serial.println("+/- 2 g"); break;
-  }  
+  Serial.print(" * Gyroscope offsets: ");
+  Serial.print(mpu.getGyroOffsetX());
+  Serial.print(" / ");
+  Serial.print(mpu.getGyroOffsetY());
+  Serial.print(" / ");
+  Serial.println(mpu.getGyroOffsetZ());
   
   Serial.println();
 }