Diff: Compass.cpp

Revision:

0:96cf16c88314

diff -r 000000000000 -r 96cf16c88314 Compass.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Compass.cpp	Wed Jan 20 01:01:04 2016 +0000
@@ -0,0 +1,44 @@
+#include "Compass.h"
+extern I2C acc;
+char rawBytes[6];
+short data[6];
+short magComponents[3];
+double heading;
+const int addr1 = (0x1E << 1);
+char config[2];
+char init[2];
+double Compass() 
+{
+    config[0]= 0x02;
+    config[1] = 0x00;
+    acc.write(addr1, config,2);
+    init[0]=0x03;
+    while(1)
+    {
+       acc.write(addr1,init,1);
+       acc.read(addr1,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;    
+        }
+       
+        return heading;
+    }
+}
+
+
+