int ReadGSensor(void)
{
unsigned char d1,d2,d3,d4,d5,d6;
if(i2c0master_StartN(G_SENSOR, I2C_WRITE, 1) == false)
{
printf("Gsensor : %s !!\n",roboio_GetErrMsg());
return false;
}
i2c0master_SetRestartN(I2C_READ, 6);
i2c0master_WriteN(0x32); //Read from X register (Address : 0x32)
/* write stop and start read */
d1 = i2c0master_ReadN(); //X MSB
d2 = i2c0master_ReadN(); //X LSB
d3 = i2c0master_ReadN(); //Y MSB
d4 = i2c0master_ReadN(); //Y LSB
d5 = i2c0master_ReadN(); //Z MSB
d6 = i2c0master_ReadN(); //Z LSB
G_AXIS_VALUE[0] = (d2 & 0x02) ? ~(0xFFFF ^ (d2*256+d1)) : d2*256+d1;
G_AXIS_VALUE[1] = (d4 & 0x02) ? ~(0xFFFF ^ (d4*256+d3)) : d4*256+d3;
G_AXIS_VALUE[2] = (d6 & 0x02) ? ~(0xFFFF ^ (d6*256+d5)) : d6*256+d5;
if(G_AXIS_VALUE[0] == 0 && G_AXIS_VALUE[1] == 0 && G_AXIS_VALUE[2] == 0)
return false;
return true;
}
void Compass_Sensor::read()
{
oldAzimuth=currentAzimuth;
oldTicks=currentTicks;
currentTicks=GetTickCount();
i2c0master_StartN(i2c_address, I2C_WRITE, 1);
i2c0master_SetRestartN(I2C_READ, 6);
i2c0master_WriteN(0x03); //Read from data register (Address : 0x03)
d1 = i2c0master_ReadN();//X MSB
d2 = i2c0master_ReadN();//X LSB
d3 = i2c0master_ReadN();//Y MSB
d4 = i2c0master_ReadN();//Y LSB
d5 = i2c0master_ReadN();//Z MSB
d6 = i2c0master_ReadN();//Z LSB
//std::cout<<d1<<", "<<d2<<", "<<d3<<", "<<d4<<", "<<d5<<", "<<d6<<"\n";
x=((d1 & 0x80) != 0) ? (((~0)>>16)<<16) | ((d1<<8)+d2): (d1<<8)+d2;
y=((d3 & 0x80) != 0) ? (((~0)>>16)<<16) | ((d3<<8)+d4): (d3<<8)+d4;
z=((d5 & 0x80) != 0) ? (((~0)>>16)<<16) | ((d5<<8)+d6): (d5<<8)+d6;
//std::cout<<x<<" "<<y<<": ";//" "<<z<<"\n";
result=atan((float)y/(float)x)*180.0/PI+90;
//result=(180*(atan((double)(-1*(y/x))/PI)))+180;
int quadrant;
if(x>=0 && y>=0)quadrant=1;
if(x<0 && y>=0)quadrant=2;
if(x<0 && y<0)quadrant=3;
if(x>=0 && y<0)quadrant=4;
if(quadrant==4 || quadrant==1)result+=180;
//don't need this if I'm using atan2
/*if(quadrant==1)result=90-result;
else result=450-result;
result=(int)result%360;
//bearing = (450-theta) mod 360*/
//std::cout<<(int)result<<"\n";//<<quadrant<<"\n";
currentAzimuth=result;
if(x==0 && y==0 && z==0)currentAzimuth= -999;
}
