this is the code for compass for HMC6352 i got it from some where on the net cam you explain me what does it do
float angleNextPoint;
int various angles;
int directionOfAngle() //i can’t explain it clearly. I can’t even in Greek:P. I only can tell you that this is a routine that calculate the angle the car s
//ould turn in order to “see” the next waypoint”. It return the angle and the direction.
{
getAngleInDeg();
// First quadrant
if((nextLatitude-latitude)>=0 && (nextLongitude-longitude)>0)
{
float sintAtan1=fabs(nextLongitude-longitude); //Differencebetween current and stored GEOGRAFICALLongitude
float sintAtan2=fabs(nextLatitude-latitude); //Difference between current and stored geographical latitude
angleNextPoint=atan2(sintAtan1,sintAtan2); //ESTIMATE THEAngleWhosformed byTHE CURRENTANDThe saved geographical Latitude and Longitude
angleNextPoint=((angleNextPoint*180)/M_PI); //Convert the angle of BY ray in degrees
variousangles=fabs(angleNextPoint-reading); //MISCELLANEOUS angle to IS THAT WHAT YOU CALCULATE AP MAKES OUR COMPASS
//CONTROLHOW TO GO Quadrant
if(reading>angleNextPoint&&variousangles<180)
{
servoDir='L';
return(reading-angleNextPoint);
}
else if(reading>angleNextPoint&&variousangles>=180)
{
servoDir='R';
return(360-reading+angleNextPoint);
}
else if(reading<angleNextPoint)
{
servoDir='R';
return(angleNextPoint-reading);
}
else
{
servoDir='S';
return(0);
}
}
//Second Quarter
else if((nextLatitude-latitude)<0 && (nextLongitude-longitude)>=0)
{
float sintAtan1=fabs(nextLongitude-longitude);
float sintAtan2=fabs(nextLatitude-latitude);
angleNextPoint=atan2(sintAtan2,sintAtan1);
angleNextPoint=((angleNextPoint*180)/M_PI)+90;
variousangles=fabs(angleNextPoint-reading);
if(reading>angleNextPoint&&variousangles<180)
{
servoDir='L';
return(reading-angleNextPoint);
}
else if(reading>angleNextPoint&&variousangles>=180)
{
servoDir='R';
return(360-reading+angleNextPoint);
}
else if(reading<angleNextPoint)
{
servoDir='R';
return(angleNextPoint-reading);
}
else
{
servoDir='S';
return(0);
}
}
//Third Quarter
else if((nextLatitude-latitude)<=0 && (nextLongitude-longitude)<0)
{
float sintAtan1=fabs(nextLongitude-longitude);
float sintAtan2=fabs(nextLatitude-latitude);
angleNextPoint=atan2(sintAtan1,sintAtan2);
angleNextPoint=((angleNextPoint*180)/M_PI)+180;
variousangles=fabs(angleNextPoint-reading);
if(reading>angleNextPoint)
{
servoDir='L';
return(reading-angleNextPoint);
}
else if(reading<angleNextPoint&&variousangles>=180)
{
servoDir='L';
return(360-angleNextPoint+reading);
}
else if(reading<angleNextPoint&&variousangles<180)
{
servoDir='R';
return(angleNextPoint-reading);
}
else
{
servoDir='S';
return(0);
}
}
//fourth quadrant
else if((nextLatitude-latitude)>0 && (nextLongitude-longitude)<=0)
{
float sintAtan1=fabs(nextLongitude-longitude);
float sintAtan2=fabs(nextLatitude-latitude);
angleNextPoint=atan2(sintAtan2,sintAtan1);
angleNextPoint=((angleNextPoint*180)/M_PI)+270;
variousangles=fabs(angleNextPoint-reading);
if(reading>angleNextPoint)
{
servoDir='L';
return(reading-angleNextPoint);
}
else if(reading<angleNextPoint&&variousangles>=180)
{
servoDir='L';
return(360-angleNextPoint+reading);
}
else if(reading<angleNextPoint&&variousanglesn<180)
{
servoDir='R';
return(angleNextPoint-reading);
}
else
{
servoDir='S';
return(0);
}
}
}
//====================================
//Routine that reads the COMPASS
//===================================
intgetAngleInDeg()
{
Wire.beginTransmission(63);
Wire.send(0x41);
Wire.endTransmission();
delayViaGps(10);
Wire.requestFrom(63, 2);
byte MSB = Wire.receive();
byte LSB = Wire.receive();
reading = ((MSB<< 8) + LSB)/10;
Serial.print(" reading ");
Serial.println(reading);
return(reading);
}