I recently purchased a SparkFun (InvenSense) MPU-6050, six degrees of freedom Gyroscope & Accelerometer from Robosavvy. It’s a great bit of kit, which combines a 3-axis gyroscope and a 3-axis accelerometer on the same board. It hooks up easily to an Arduino using the I2C bus. So far, so good…

I then begin searching the Internet for example code with these two devices working together. My search always led me to a huge, unwieldy library, which seemed very bloated, considering all I wanted to do was read some values from the board.

I began to dig deeper and experiment, which enabled me to create the code sample below. It relies on using default values, which are fine for my application – and it’s light! The accelerometer channels are very twitchy, so the general advice is to incorporate a low pass filter, which I’ve done.

Hopefully, you will also find it useful for your projects.

Main Program

#include "Wire.h"
#include "MPU6050.h"
 
MPU6050 mpu;  
int ax, ay, az, gx, gy, gz;
float smooth_ax, smooth_ay, smooth_az;
 
void setup()
{
  Wire.begin();
  Serial.begin(38400);
  mpu.wakeup();  
}
 
void loop()
{
  /* Read device to extract current
   accelerometer and gyroscope values. */
  mpu.read6dof(&ax, &ay, &az, &gx, &gy, &gz);
 
  /* Apply low pass filter to smooth
   accelerometer values. */
  smooth_ax = 0.95 * smooth_ax + 0.05 * ax;
  smooth_ay = 0.95 * smooth_ay + 0.05 * ay;
  smooth_az = 0.95 * smooth_az + 0.05 * az;
 
  /* Output to serial monitor. */
  Serial.print(smooth_ax);
  Serial.print("t");
  Serial.print(smooth_ay);
  Serial.print("t");
  Serial.print(smooth_az);
  Serial.print("t");
  Serial.print(gx);
  Serial.print("t");
  Serial.print(gy);
  Serial.print("t");
  Serial.println(gz);
}

MPU6050.h

#include "Arduino.h"
#include "Wire.h"
 
#define MPU6050_DEVICE_ADDRESS   0x68
#define MPU6050_RA_ACCEL_XOUT_H  0x3B
#define MPU6050_RA_PWR_MGMT_1    0x6B
#define MPU6050_PWR1_SLEEP_BIT   6
 
class MPU6050 {
public:
  MPU6050();
  void wakeup();
  void read6dof(int* ax, int* ay, int* az, int* gx, int* gy, int* gz);
 
private:
  byte id;
  byte buffer[14];
  void readByte(byte reg, byte *data);
  void readBytes(byte reg, byte len, byte *data);
  void writeBit(byte reg, byte num, byte data);
  void writeByte(byte reg, byte data);
};

MPU6050.cpp

#include "MPU6050.h"
 
MPU6050::MPU6050() {
  id = MPU6050_DEVICE_ADDRESS;
}
 
/* Wake up device and use default values for
 accelerometer (±2g) and gyroscope (±250°/sec). */
void MPU6050::wakeup() {
  writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 0);
}
 
/* Read device memory to extract current
 accelerometer and gyroscope values. */
void MPU6050::read6dof(int* ax, int* ay, int* az, int* gx, int* gy, int* gz) {
  readBytes(MPU6050_RA_ACCEL_XOUT_H, 14, buffer);
  *ax = (((int)buffer[0]) << 8) | buffer[1];
  *ay = (((int)buffer[2]) << 8) | buffer[3];
  *az = (((int)buffer[4]) << 8) | buffer[5];
  *gx = (((int)buffer[8]) << 8) | buffer[9];
  *gy = (((int)buffer[10]) << 8) | buffer[11];
  *gz = (((int)buffer[12]) << 8) | buffer[13];
}
 
/* Read a single byte from specified register. */
void MPU6050::readByte(byte reg, byte *data) {
  readBytes(reg, 1, data);
}
 
/* Read multiple bytes starting at specified register. */
void MPU6050::readBytes(byte reg, byte len, byte *data) {
  byte count = 0;
  Wire.beginTransmission(id);
  Wire.write(reg);
  Wire.requestFrom(id, len);
  while (Wire.available()) data[count++] = Wire.read();
  Wire.endTransmission();
}
 
/* Write bit to specified register and location. */
void MPU6050::writeBit(byte reg, byte num, byte data) {
  byte b;
  readByte(reg, &b);
  b = (data != 0) ? (b | (1 << num)) : (b & ~(1 << num));
  writeByte(reg, b);
}
 
/* Write byte to specified register. */
void MPU6050::writeByte(byte reg, byte data) {
  Wire.beginTransmission(id);
  Wire.write(reg); 
  Wire.write(data);
  Wire.endTransmission(); 
}