Diff: Compass.cpp

Revision:

0:1acd78600f46

Child:

2:ef6778c2438e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Compass.cpp	Wed Jan 13 21:31:15 2016 +0000
@@ -0,0 +1,45 @@
+#include "Compass.h"
+
+I2C i2c_port(p9,p10);
+char rawBytes[6];
+short data[6];
+short magComponents[3];
+double heading;
+const int addr = (0x1E << 1);
+char config[2];
+char init[2];
+double Compass() 
+{
+    config[0]= 0x02;
+    config[1] = 0x00;
+    i2c_port.write(addr, config,2);
+    init[0]=0x03;
+    while(1)
+    {
+       i2c_port.write(addr,init,1);
+       i2c_port.read(addr,rawBytes,6);
+        
+        
+        for (int i = 0; i<6; i++) 
+        {
+            data[i] = rawBytes[i];
+        }
+        
+        magComponents[0] = data[0]<<8 | data[1];
+        magComponents[1] = data[4]<<8 | data[5];
+        magComponents[2] = data[2]<<8 | data[3];
+        
+        heading = atan2((double)magComponents[1],(double)magComponents[0]); // Angle in Radians
+        heading = (heading*180)/3.14159;   // Convert to Degrees
+        heading = heading + 14.85;  // Find True North
+        if(heading > 180)
+        {
+            heading -= 360;    
+        }
+        wait(.05);
+        
+        return heading;
+    }
+}
+
+