#include "stm32f0xx.h"
#include "i2c.h"
#include "utils.h"
#include "mpu6050.h"

void mpu6050_init(void)
{
  mpu6050_write_byte(MPU_PWR_MGMT_1, 0x80); // device reset
  delay_ms(100);
  mpu6050_write_byte(MPU_SIGNAL_PATH_RESET, 0x07); // gyro, accelerometer & temperature signal path reset
  delay_ms(100);
  mpu6050_write_byte(MPU_PWR_MGMT_1, 0x01); // power on, gyro axis x clock reference
  mpu6050_write_byte(MPU_ACCEL_CONFIG, 0x08); // accelerometer scale +- 4g
  mpu6050_write_byte(MPU_GYRO_CONFIG, 0x08); // gyro scale +- 500 °/s
  mpu6050_write_byte(MPU_CONFIG, 0x02); // digital low pass filter, bandwidth ~100 Hz
}

void mpu6050_read_data(void)
{
	i2c_read(MPU6050_ADDR << 1, MPU_ACCEL_XOUT_H, accel.data, sizeof(accel.data));
	i2c_read(MPU6050_ADDR << 1, MPU_GYRO_XOUT_H, gyro.data, sizeof(gyro.data));
	mpu6050_convert_data(&accel);
	mpu6050_convert_data(&gyro);
}

void mpu6050_convert_data(ACCEL_GYRO_STRUCT *ag)
{
	ag->x = (ag->data[0] << 8) | ag->data[1];
	ag->y = (ag->data[2] << 8) | ag->data[3];
	ag->z = (ag->data[4] << 8) | ag->data[5];
}

void mpu6050_write_byte(uint8_t address, uint8_t data)
{
	i2c_write(MPU6050_ADDR << 1, address, &data, 1);
}

uint8_t mpu6050_read_byte(uint8_t address)
{
  uint8_t data;

  i2c_read(MPU6050_ADDR << 1, address, &data, 1);

  return data;
}
