#include <p33FJ64MC802.h>


#include "drvI2C.h"
#include "drv6DOF.h"


void Init6DOF()
{
	InitI2C1();
}	

unsigned char GetWhoAmIGyro(void)
{
	unsigned char varlocal;
	varlocal=ReadByteI2C1(ADDR_ITG3200,ITG3200_WHO_AM_I);
	return (varlocal);
}

unsigned char GetWhoAmIAcc(void)
{
	unsigned char varlocal;
	varlocal=ReadByteI2C1(ADDR_ADXL345,ADXL345_DEVID);
	return (varlocal);
}
 


/*
int a_offx,a_offy,a_offz;
int g_offx,g_offy,g_offz;
float RwAcc[3];
*/




/*
void InitAcc(void)
{		StartI2C1();
		IdleI2C1();
		MasterWriteI2C1(AddrAccW);
		IdleI2C1();
		MasterWriteI2C1(ADXL345_POWER_CTL);
		IdleI2C1();
		MasterWriteI2C1( 0x8);
		IdleI2C1();
		IdleI2C1();
		StopI2C1();IdleI2C1();
}



void InitGyro(void)
{
		StartI2C1();
		IdleI2C1();
		MasterWriteI2C1(AddrGyrW);
		IdleI2C1();
		MasterWriteI2C1(SMPLRT_DIV); 
		IdleI2C1();
		MasterWriteI2C1(0x07);
		IdleI2C1();
		MasterWriteI2C1(0x1E);
		IdleI2C1();
		MasterWriteI2C1(0x00);
		IdleI2C1();
		StopI2C1();IdleI2C1();
}
*/



/*
void getAccelerometerData(int * result) {
	unsigned char buff[A_TO_READ];
	
	ReadI2C1(AddrAccW, ADXL345_DATAX0, A_TO_READ, buff);
//each axis reading coms in 10 bit resolution, ie 2 bytes. Least Significant Byte first!!
//thus we are converting both bytes in to one int
result[0] = (((int)buff[1]) << 8) | buff[0] - a_offx;
result[1] = (((int)buff[3]) << 8) | buff[2] - a_offx;
result[2] = (((int)buff[5]) << 8) | buff[4] - a_offx;
}



void rawAccToToG(int * raw, float * RwAcc) {
	RwAcc[0] = ((float) raw[0]) / 256.0;
	RwAcc[1] = ((float) raw[1]) / 256.0;
	RwAcc[2] = ((float) raw[2]) / 256.0;
}



void getGyroscopeData(int * result)
{
	//int temp, x, y, z;
	unsigned char buff[G_TO_READ];
	ReadI2C1(AddrGyrW, GYRO_DATA, G_TO_READ, buff); //read the gyro data from the ITG3200

	result[0] = (((int)buff[0] << 8) | buff[1]) - g_offx;
	result[1] = (((int)buff[2] << 8) | buff[3]) - g_offx;
	result[2] = (((int)buff[4] << 8) | buff[5]) - g_offx;
	result[4] = ((int)buff[6] << 8) | buff[7];  //temperature
}


*/
