Diff: gyroReader.cpp

Revision:

28:c7e8d2db03f5

Parent:

12:925f52da3ba9

Child:

29:1132155bc7da

diff -r 688409727452 -r c7e8d2db03f5 gyroReader.cpp
--- a/gyroReader.cpp	Fri Mar 14 02:13:10 2014 +0000
+++ b/gyroReader.cpp	Fri Mar 21 00:48:29 2014 +0000
@@ -4,7 +4,7 @@
  void gyroReader::gyroUpkeep(){
     int xdiff,ydiff,zdiff;
     checkGyro(xdiff,ydiff,zdiff);
-    //checkCompass();
+    checkCompass();
     gyroZcount+=zdiff+zcal;
     gyroXcount+=xdiff+xcal;
     polls++;
@@ -26,7 +26,7 @@
     printf("%X,%X,%X,%X\r\n",cmd[0],cmd[1],cmd[2],cmd[3]);
     cmd[0]=0x00;
     cmd[1]=0x04;//75 samples/sec
-    cmd[2]=0xE0;//largest possible range (8.1 gauss)
+    cmd[2]=0x80;//second to smallest possible range (+-1.9 gauss)
     cmd[3]=0x00;//continuous mode
     //pc.printf("%d\r\n",accel.write(magaddr,cmd,2));
     DBGPRINT("%d\r\n",accel.write(magaddr,cmd,4));
@@ -40,10 +40,11 @@
     com[0]=0x03|0x80;
     accel.write(addr, com, 1,true);
     accel.read(addr, cmd, 6);
-    xmag=(signed short)((((unsigned char)cmd[1])<<8)+(unsigned char)cmd[0]);
-    ymag=(signed short)((((unsigned char)cmd[3])<<8)+(unsigned char)cmd[2]);
-    zmag=(signed short)((((unsigned char)cmd[5])<<8)+(unsigned char)cmd[4]);
+    xmag=(signed short)((((unsigned char)cmd[0])<<8)+(unsigned char)cmd[1]);
+    ymag=(signed short)((((unsigned char)cmd[2])<<8)+(unsigned char)cmd[3]);
+    zmag=(signed short)((((unsigned char)cmd[4])<<8)+(unsigned char)cmd[5]);
     //DBGPRINT("%d\t\t%d\t\t%d\r\n",xmag,ymag,zmag);
+    compDir = atan2(((double)xmag-xoffs)*xamp,((double)zmag-zoffs)*zamp);
     return 0;//atan2((double)xacc,(double)yacc);
 }
 
@@ -114,8 +115,13 @@
     zcal=-54;
     xcal=120;
     polls=0;
+    xoffs = -19.88;
+    zoffs = -56.58;
+    xamp = 1.0/318.0;
+    zamp = 1.0/357.0;
     startAccel();
     xmag=ymag=zmag=0;
+    compDir=0;
     gyroUpkeepTicker.attach_us(this,&gyroReader::gyroUpkeep,10000);
 }
 void gyroReader::setLevel(){