Compass.cpp

00001 #include "Compass.h"
00002 extern I2C acc;
00003 char rawBytes[6];
00004 short data[6];
00005 short magComponents[3];
00006 double heading;
00007 const int addr1 = (0x1E << 1);
00008 char config[2];
00009 char init[2];
00010 double Compass() 
00011 {
00012     config[0]= 0x02;
00013     config[1] = 0x00;
00014     acc.write(addr1, config,2);
00015     init[0]=0x03;
00016     while(1)
00017     {
00018        acc.write(addr1,init,1);
00019        acc.read(addr1,rawBytes,6);
00020         
00021         
00022         for (int i = 0; i<6; i++) 
00023         {
00024             data[i] = rawBytes[i];
00025         }
00026         
00027         magComponents[0] = data[0]<<8 | data[1];
00028         magComponents[1] = data[4]<<8 | data[5];
00029         magComponents[2] = data[2]<<8 | data[3];
00030         
00031         heading = atan2((double)magComponents[1],(double)magComponents[0]); // Angle in Radians
00032         heading = (heading*180)/3.14159;   // Convert to Degrees
00033         heading = heading + 14.85;  // Find True North
00034         if(heading > 180)
00035         {
00036             heading -= 360;    
00037         }
00038        
00039         return heading;
00040     }
00041 }
00042 
00043 
00044