Diff: main.cpp

Revision:

1:29713f02de29

Parent:

0:80d32420bc63

Child:

2:3ffce3e97527

--- a/main.cpp	Fri Nov 25 05:26:06 2011 +0000
+++ b/main.cpp	Sat Nov 26 15:01:52 2011 +0000
@@ -1,22 +1,26 @@
 #include "ADXL345_I2C.h"
 #include "ITG3200.h"
+#include "HMC5843.h"
 
 ADXL345_I2C accelerometer(p9, p10);
 ITG3200 gyro(p9, p10);
+HMC5843 compass(p9, p10);
 Serial pc(USBTX, USBRX);
 
 int main(){ 
     float dt = 0.1;
     float t  = 0.0;    
     pc.baud(115200);
-    int    bitAcc[3] = {0, 0, 0};
-    double Acc   [3] = {0, 0, 0};
-    double Gyro0 [3] = {0, 0, 0};
-    double Gyro1 [3] = {0, 0, 0};
-    double Gyro2 [3] = {0, 0, 0};
-    double Ang0  [3] = {0, 0, 0};
-    double Ang1  [3] = {0, 0, 0};
-    double Ang2  [3] = {0, 0, 0};
+    int    bitAcc[3]    =   {0, 0, 0};
+    int    getMag[3];
+    double Acc   [3]    =   {0, 0, 0};
+    double Gyro0 [3]    =   {0, 0, 0};
+    double Gyro1 [3]    =   {0, 0, 0};
+    double Gyro2 [3]    =   {0, 0, 0};
+    double Ang0  [3]    =   {0, 0, 0};
+    double Ang1  [3]    =   {0, 0, 0};
+    double Ang2  [3]    =   {0, 0, 0};
+    int    Mag1  [3]    =   {0, 0, 0};
     
     // *** Part of the accelerometer ***
     // These are here to test whether any of the initialization fails. It will print the failure
@@ -36,9 +40,7 @@
         pc.printf("didn't set data rate\n");
         return 0;    }
     wait(.001);
-     
-    // Measurement mode.
-     
+      
     if(accelerometer.setPowerControl(MeasurementMode)) {
         pc.printf("didn't set the power control to measurement\n"); 
         return 0;   } 
@@ -46,33 +48,51 @@
     
     gyro.setLpBandwidth(LPFBW_42HZ);
     
-    pc.printf("Starting ADXL345 and ITG3200 test...\n");
-    //pc.printf("  Time   AccX   AccY   AccZ   GyroX   GyroY   GyroZ\n");
-    pc.printf("  Time    GyroX    GyroY    GyroZ   AngleX   AngleY   AngleZ\n");
+    //Continuous mode, , 10Hz measurement rate.
+    // HMC5843_CONTINUOUS, HMC5843_10HZ_NORMAL HMC5843_1_0GA
+    compass.setDefault();
+    
+    //Wait some time(Need at least 5ms)
+    wait(0.1);
+    
+    pc.printf("Starting ADXL345, ITG3200 and HMC5843 test...\n");
+    pc.printf("  Time     AccX     AccY     AccZ    GyroX    GyroY    GyroZ   MagX   MagY   MagZ\n");
+    //pc.printf("  Time    GyroX    GyroY    GyroZ   AngleX   AngleY   AngleZ\n");
                 
     while(1){
         accelerometer.getOutput(bitAcc);
-        // Transfering units (Acc[g], Gyro[deg/s])
-        Acc[0] = ((int16_t)bitAcc[0])*0.004;         Acc[1] = ((int16_t)bitAcc[1])*0.004;         Acc[2] = ((int16_t)bitAcc[2]+50)*0.004;
-        Gyro2[0] = (gyro.getGyroX()+30)/14.375;      Gyro2[1] = (gyro.getGyroY()-28)/14.375;      Gyro2[2] = (gyro.getGyroZ()-17)/14.375;
+        compass.readData(getMag);
+        // Transfering units (Acc[g], Gyro[deg/s], Compass[?])
+        // Calibration YAL 9DOF Acc:X+2, Y-12, Z+44
+        Acc[0]   =  ((int16_t)bitAcc[0]+2)*0.004;
+        Acc[1]   =  ((int16_t)bitAcc[1]-12)*0.004;
+        Acc[2]   =  ((int16_t)bitAcc[2]+44)*0.004;
+        // Calibration YAL 9DOF Gyro:X+26, Y-45, Z+34 
+        Gyro2[0] =  (gyro.getGyroX()+26)/14.375;
+        Gyro2[1] =  (gyro.getGyroY()-45)/14.375;
+        Gyro2[2] =  (gyro.getGyroZ()+34)/14.375;
+        // Calibration YAL 9DOF Compass:X, Y, Z
+        Mag1[0]  =  (int16_t)getMag[0];
+        Mag1[1]  =  (int16_t)getMag[1];
+        Mag1[2]  =  (int16_t)getMag[2];
         
         // Low pass filter for gyro
-        if( -1.0<Gyro2[0] && Gyro2[0]<1.0 ){    Gyro2[0]=0;  }
-        if( -1.0<Gyro2[1] && Gyro2[1]<1.0 ){    Gyro2[1]=0;  }
-        if( -1.0<Gyro2[2] && Gyro2[2]<1.0 ){    Gyro2[2]=0;  }
+        //if( -1.0<Gyro2[0] && Gyro2[0]<1.0 ){    Gyro2[0]=0;  }
+        //if( -1.0<Gyro2[1] && Gyro2[1]<1.0 ){    Gyro2[1]=0;  }
+        //if( -1.0<Gyro2[2] && Gyro2[2]<1.0 ){    Gyro2[2]=0;  }
         
         // Trapezoidal integration
-        Ang1[0] = Ang0[0]+(Gyro0[0]+Gyro1[0])*dt/2;
-        Ang1[1] = Ang0[1]+(Gyro0[1]+Gyro1[1])*dt/2;
-        Ang1[2] = Ang0[2]+(Gyro0[2]+Gyro1[2])*dt/2;
+        //Ang1[0] = Ang0[0]+(Gyro0[0]+Gyro1[0])*dt/2;
+        //Ang1[1] = Ang0[1]+(Gyro0[1]+Gyro1[1])*dt/2;
+        //Ang1[2] = Ang0[2]+(Gyro0[2]+Gyro1[2])*dt/2;
         
         // Simpson integration
         // Ang1[0] = Ang0[0]+(Gyro0[0]+4*Gyro1[0]+Gyro2[0])*dt/6;
         // Ang1[1] = Ang0[1]+(Gyro0[1]+4*Gyro1[1]+Gyro2[1])*dt/6;
         // Ang1[2] = Ang0[2]+(Gyro0[2]+4*Gyro1[2]+Gyro2[2])*dt/6;
         
-        //pc.printf("%6.1f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n\r", t, Acc[0], Acc[1], Acc[2], Gyro1[0], Gyro1[1], Gyro1[2]);
-        pc.printf("%6.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, Gyro1[0], Gyro1[1], Gyro1[2], Ang1[0], Ang1[1], Ang1[2]);
+        pc.printf("%6.1f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %5d, %5d, %5d\n\r", t, Acc[0], Acc[1], Acc[2], Gyro1[0], Gyro1[1], Gyro1[2], Mag1[0], Mag1[1], Mag1[2]);
+        //pc.printf("%6.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, Gyro1[0], Gyro1[1], Gyro1[2], Ang1[0], Ang1[1], Ang1[2]);
         //pc.printf("%6.1f, %6i, %6i, %6i, %7.2f, %7.2f, %7.2f\n\r", t, gyro.getGyroX()+32, gyro.getGyroY()-27, gyro.getGyroZ()-17, Ang1[0], Ang1[1], Ang1[2]);
         //pc.printf("%6.1f, %6i, %6i, %6i, %7.3f, %7.3f, %7.3f\n\r", t, (int16_t)bitAcc[0], (int16_t)bitAcc[1], (int16_t)bitAcc[2], Acc[0], Acc[1], Acc[2]);
         Gyro1[0] = Gyro2[0];  Gyro1[1] = Gyro2[1];  Gyro1[2] = Gyro2[2];