Compass.cpp

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