// version INCOMPLETE
#include <p33FJ64MC802.h>

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

void Init6DOF()
{
	InitI2C1();
	InitAcc();
	InitGyro();
	Delay(Delay_1S_Cnt); 
}	



////////////////////////////////////////////////////////
/// ACCELERO
void InitAcc(void)
{	// ADXL345 is in stand by measurement at reset(p14 an p16)
	// set Measure Bit to 1 (p16)	
	WriteByteI2C1( ADDR_ADXL345,ADXL345_POWER_CTL,0x08);
}

unsigned char GetWhoAmIAcc(void)
{
	unsigned char varlocal;
	varlocal=ReadByteI2C1(ADDR_ADXL345,ADXL345_DEVID);
	return (varlocal);
}
 
void getRawAccelerometerData(Dof6Accelero * ptrAccel) 
{
unsigned char buff[6];
	
ReadMultipleI2C1(ADDR_ADXL345, ADXL345_DATAX0, 6, 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
ptrAccel->rawData[0] = (((int)buff[1]) << 8) | buff[0] ;
ptrAccel->rawData[1]  = (((int)buff[3]) << 8) | buff[2] ;
ptrAccel->rawData[2]  = (((int)buff[5]) << 8) | buff[4] ;
}


void getOffsetAccelerometer(Dof6Accelero * ptrAccel)
{
unsigned char buff[3];
	
ReadMultipleI2C1(ADDR_ADXL345, OFSX, 3, 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
ptrAccel->offset[0] = (int)buff[0] ;
ptrAccel->offset[1]  = (int) buff[1] ;
ptrAccel->offset[2]  = (int) buff[2] ;
}

void CalcAverageOffsetAccelerometer(Dof6Accelero * ptrAccel)
{
int X=0,Y=0,Z=0,i;
unsigned char buff[6];
// average loop
for (i=0;i<128;i++)
	{
	// retrieve raw data
	ReadMultipleI2C1(ADDR_ADXL345, ADXL345_DATAX0, 6, buff);
	X += (((int)buff[1]) << 8) | buff[0] ;
	Y += (((int)buff[3]) << 8) | buff[2] ;
	Z  += ( ( ((int)buff[5]) << 8) | buff[4] )-256 ;// -256 is because we have +1g on Z axe
	}
ptrAccel->offset[0]=(X>>7); // >>7 equivalent /128
ptrAccel->offset[1]=(Y>>7);
ptrAccel->offset[2]=(Z>>7);
}	

void getAccelerometerData(Dof6Accelero * ptrAccel) 
{
unsigned char buff[6];
	
ReadMultipleI2C1(ADDR_ADXL345, ADXL345_DATAX0, 6, 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
ptrAccel->data[0] = ( (((int)buff[1]) << 8) | buff[0] ) - ptrAccel->offset[0] ;
ptrAccel->data[1]  = ((((int)buff[3]) << 8) | buff[2]) - ptrAccel->offset[1] ;
ptrAccel->data[2]  = ((((int)buff[5]) << 8) | buff[4]) - ptrAccel->offset[2] ;
}


/// END ACCELERO
////////////////////////////////////////////////////////


////////////////////////////////////////////////////////
/// GYRO

void InitGyro(void)
{		WriteByteI2C1( ADDR_ITG3200,ITG3200_SMPLRT_DIV,0x07);// 
		WriteByteI2C1( ADDR_ITG3200,ITG3200_DLPF_FS,0x1E);//Low pass filter, kHz
		WriteByteI2C1( ADDR_ITG3200,ITG3200_INT_CFG,0x00);
}

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


char getGyroData(Dof6Gyro * ptrGyro) 
{
unsigned char buff[8];
char result;	
result=ReadMultipleI2C1(ADDR_ITG3200, ITG3200_GYRO_XOUT, 8, buff);
ptrGyro->data[0] = ( (((int)buff[1]) << 8) | buff[0]) -ptrGyro->offset[0] ;
ptrGyro->data[1]  = ((((int)buff[3]) << 8) | buff[2])- ptrGyro->offset[1] ;
ptrGyro->data[2]  = ((((int)buff[5]) << 8) | buff[4]) - ptrGyro->offset[2] ;
return(result);
}

char getRawGyroData(Dof6Gyro * ptrGyro) 
{
unsigned char buff[8];
	
char result;	
result=ReadMultipleI2C1(ADDR_ITG3200, ITG3200_GYRO_XOUT, 8, buff);
ptrGyro->data[0] = ( (((int)buff[1]) << 8) | buff[0])  ;
ptrGyro->data[1]  = ((((int)buff[3]) << 8) | buff[2]) ;
ptrGyro->data[2]  = ((((int)buff[5]) << 8) | buff[4])  ;
return(result);
}

void CalcAverageOffsetGyro(Dof6Gyro * ptrGyro) 
{
long int X=0,Y=0,Z=0,i;
unsigned char buff[6];
// average loop
for (i=0;i<128;i++)
	{
	// retrieve raw data
	ReadMultipleI2C1(ADDR_ITG3200, ITG3200_GYRO_XOUT, 6, buff);
	X += (((int)buff[1]) << 8) | buff[0] ;
	Y += (((int)buff[3]) << 8) | buff[2] ;
	Z  += ( ( ((int)buff[5]) << 8) | buff[4] );
	}
ptrGyro->offset[0]=(X>>7); // >>7 (/128) + >>16(long int to int)
ptrGyro->offset[1]=(Y>>7);
ptrGyro->offset[2]=(Z>>7);
}

/// GYRO
////////////////////////////////////////////////////////






/*
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;
}

*/



