#include "i2c.h"
#include "mpu6050.h"
#include "qmc5883l.h"

double x, y, z;
uint8_t v;
double t;

char buf[100];

void setup() {
	Serial.begin(115200);
  i2c_enable(100000);
  qmc5883l_init();
  mpu6050_init();
  mpu6050_enable(mpu6050PLLGyroscopeX);
  mpu6050_set_accelerometer_config(mpu6050AccelerometerRange4G);
  mpu6050_set_gyroscope_config(mpu6050GyroscopeRange2000Deg);
  i2c_read_register(0x68, 0x75, &v);
  Serial.println(v);
}

void loop() {
  //mpu6050_read_temperature(&t);
  //Serial.println(t);
  //sprintf(&buf[0], "%d %d %d", x * 10000, y * 10000, z * 10000);
  mpu6050_read_acceleration(&x, &y, &z);
  Serial.println("A");
  Serial.println(x);
  Serial.println(y);
  Serial.println(z);
  mpu6050_read_angular_velocity(&x, &y, &z);
  Serial.println("G");
  Serial.println(x);
  Serial.println(y);
  Serial.println(z);

	delay(1000);
}