Diff: gyroReader.cpp

Revision:

4:adc885f4ab75

Parent:

3:a223b0bf8256

Child:

7:3b2cf7efe5d1

--- a/gyroReader.cpp	Sun Nov 17 04:15:18 2013 +0000
+++ b/gyroReader.cpp	Sun Nov 17 21:04:47 2013 +0000
@@ -1,9 +1,11 @@
  #include "gyroReader.h"
+ //#include "math.h"
  
  void gyroReader::gyroUpkeep(){
     //testPin=1;
     int xdiff,ydiff,zdiff;
     checkGyro(xdiff,ydiff,zdiff);
+    checkCompass();
     gyroZcount+=zdiff+zcal;
     gyroXcount+=xdiff+xcal;
     polls++;
@@ -12,24 +14,40 @@
     
 int gyroReader::startAccel(){
     char cmd[4];
-    char addr=0x30;//0b0011000;
+    char addr=0x32;//0b0011000;
     char gyroaddr=0xD6;
     char magaddr=0x3C;
     cmd[0]=0x20;
     cmd[1]=0x2f;//0b00101111;
     cmd[2]=0;
     cmd[3]=0;
-    printf("%d\r\n",accel.write(addr,cmd,2));
+    //printf("%d\r\n",accel.write(addr,cmd,2));
     cmd[1]=0x0f;
     printf("%d\r\n",accel.write(gyroaddr,cmd,2));
     printf("%X,%X,%X,%X\r\n",cmd[0],cmd[1],cmd[2],cmd[3]);
-    cmd[0]=0x02;
-    cmd[1]=0x00;//0b00000000
+    cmd[0]=0x00;
+    cmd[1]=0x04;//75 samples/sec
+    cmd[2]=0xE0;//largest possible range (8.1 gauss)
+    cmd[3]=0x00;//continuous mode
     //pc.printf("%d\r\n",accel.write(magaddr,cmd,2));
-    DBGPRINT("%d\r\n",accel.write(magaddr,cmd,2));
+    DBGPRINT("%d\r\n",accel.write(magaddr,cmd,4));
     return 1;
 }
 
+float gyroReader::checkCompass(){
+    char com[1];
+    char cmd[6]={0,0,0,0,0,0};
+    char addr=0x3C;//0b0011110;
+    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]);
+    //DBGPRINT("%d\t\t%d\t\t%d\r\n",xmag,ymag,zmag);
+    return 0;//atan2((double)xacc,(double)yacc);
+}
+
 float gyroReader::checkAccel(){
     char com[1];
     char cmd[6]={0,0,0,0,0,0};
@@ -113,6 +131,7 @@
     xcal=120;
     polls=0;
     startAccel();
+    xmag=ymag=zmag=0;
     //testPin=0;
     //upkeepTimer.start(10);
     gyroUpkeepTicker.attach_us(this,&gyroReader::gyroUpkeep,10000);
@@ -139,4 +158,16 @@
     zcal=-(gyroZcount/polls)+zcal;
     xcal=-(gyroXcount/polls)+xcal;
     DBGPRINT("cal: %d, %d (%d)\r\n",zcal,xcal,polls);
-}
\ No newline at end of file
+}
+
+int gyroReader::compZ(int compval){
+    int midval= (compval-gyroZcount)%4050000;
+    if(midval > 2025000){
+        midval-=4050000;
+    }
+    return midval;
+}
+
+double gyroReader::getZDegrees(){
+    return gyroZcount*360.0/4050000.0;
+}