gyro.ino

// gist.github.com/doojinkang  gyro.ino

#include "MPU6050.h"

MPU6050 accelgyro;
int16_t ax, ay, az;
int16_t gx, gy, gz;

#define GYRO_FACTOR  131.0 * M_PI / 180

float roll, pitch, yaw;
unsigned long last_read_time = 0;

void setup() {
  Serial.begin(115200);

  // I2C initialize
  Wire.begin();
  Wire.setClock(400000);

  // MPU6050 sensor initialize
  accelgyro.initialize();
  Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");

  // MPU6050 sensor calibrate
  accelgyro.CalibrateGyro();
  accelgyro.CalibrateAccel();
  // accelgyro.setXGyroOffset(220);
  // accelgyro.setYGyroOffset(76);
  // accelgyro.setZGyroOffset(-85);

  while (!Serial.available());  // wait until something in
  // last_read_time = millis();
}

void loop() {

  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);

  // 1. gyro ==> angle

  float fgx = (float)gx / GYRO_FACTOR;  // 16bitVal / 131.0 * M_PI / 180 ==> rad/s
  float fgy = (float)gy / GYRO_FACTOR;
  float fgz = (float)gz / GYRO_FACTOR;

  // current time (ms)
  unsigned long t_now = millis();
  float dt =(t_now - last_read_time)/1000.0;
  last_read_time = t_now;

  roll  = roll  + dt * fgx;
  pitch = pitch + dt * fgy;
  yaw   = yaw   + dt * fgz;

  // 2. accelerometer ==> angle
  float fax = ax;
  float fay = ay;
  float faz = az;

  float a_roll  = atan2(fay, sqrt(fax*fax + faz*faz));
  float a_pitch = -atan2(fax, sqrt(fay*fay + faz*faz));
  float a_yaw   = 0.0f;

  // 3. gyro * 0.95 + accel * 0.05
  roll  = 0.95 * roll  + 0.05 * a_roll;
  pitch = 0.95 * pitch + 0.05 * a_pitch;


  // Show Result
  Serial.print(roll);
  Serial.print(",");
  Serial.print(pitch);
  Serial.print(",");
  Serial.println(yaw);
  delay(100);

}