#include "qmi8658.h"
#include "AI8051U_I2C.h"
#include "global.h"

void qmi8658_init(void)
{
	i2c_init();
}

void qmi8658_get_accelerometer(float* acc)
{
	uint8_t i;
	uint8_t val[6];
	
	for(i = 0; i < 6; i++)
	{
		i2c_read_data(QMI8658_ADDR << 1, ACCEL_XOUT_H + i, &val[i], 1);
	}
	
	acc[0] = (float)((int16_t)BYTE_TO_HW(val[0], val[1])) / 100.0f;
	acc[1] = (float)((int16_t)BYTE_TO_HW(val[2], val[3])) / 100.0f;
	acc[2] = (float)((int16_t)BYTE_TO_HW(val[4], val[5])) / 100.0f;
}

void qmi8658_get_gyro(float* gyro)
{
	uint8_t i;
	uint8_t val[6];
	
	for(i = 0; i < 6; i++)
	{
		i2c_read_data(QMI8658_ADDR << 1, GYRO_XOUT_H + i, &val[i], 1);
	}
	gyro[0] = (float)((int16_t)BYTE_TO_HW(val[0], val[1])) / 100.0f;
	gyro[1] = (float)((int16_t)BYTE_TO_HW(val[2], val[3])) / 100.0f;
	gyro[2] = (float)((int16_t)BYTE_TO_HW(val[4], val[5])) / 100.0f;	
}

void qmi8658_get_euler(float* euler)
{
	uint8_t i;
	uint8_t val[6];
	
	for(i = 0; i < 6; i++)
	{
		i2c_read_data(QMI8658_ADDR << 1, EULER_XOUT_H + i, &val[i], 1);
	}
	euler[0] = (float)((int16_t)BYTE_TO_HW(val[0], val[1])) / 100.0f;
	euler[1] = (float)((int16_t)BYTE_TO_HW(val[2], val[3])) / 100.0f;
	euler[2] = (float)((int16_t)BYTE_TO_HW(val[4], val[5])) / 100.0f;
}
