Diff: AHRS.cpp

Revision:

8:928673148b55

Parent:

7:3e74239e3778

--- a/AHRS.cpp	Wed Jan 20 02:36:32 2016 +0000
+++ b/AHRS.cpp	Wed Jan 20 02:42:22 2016 +0000
@@ -446,23 +446,23 @@
     delt_t = t.read_ms() - count;
     if (delt_t > 500) { // update LCD once per half-second independent of read rate
 
-        /*pc.printf("ax = %f", 1000*ax);
-        pc.printf(" ay = %f", 1000*ay);
-        pc.printf(" az = %f  mg\n\r", 1000*az);
+        /*pc2.printf("ax = %f", 1000*ax);
+        pc2.printf(" ay = %f", 1000*ay);
+        pc2.printf(" az = %f  mg\n\r", 1000*az);
 
-        pc.printf("gx = %f", gx);
-        pc.printf(" gy = %f", gy);
-        pc.printf(" gz = %f  deg/s\n\r", gz);
+        pc2.printf("gx = %f", gx);
+        pc2.printf(" gy = %f", gy);
+        pc2.printf(" gz = %f  deg/s\n\r", gz);
 
-        pc.printf("mx = %f", mx);
-        pc.printf(" my = %f", my);
-        pc.printf(" mz = %f  mG\n\r", mz);*/
+        pc2.printf("mx = %f", mx);
+        pc2.printf(" my = %f", my);
+        pc2.printf(" mz = %f  mG\n\r", mz);*/
 
 
-        //pc.printf("%f,%f,%f",mx,my,mz);
+        //pc2.printf("%f,%f,%f",mx,my,mz);
 
         whoami = readByte(AK8963_ADDRESS, AK8963_ST2);  // Read WHO_AM_I register for MPU-9250
-        // pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x10\n\r");
+        // pc2.printf("I AM 0x%x\n\r", whoami); pc2.printf("I SHOULD BE 0x10\n\r");
         if(whoami == 0x14) {
             printf("I AM 0x%x\n\r", whoami);
             while(1);
@@ -471,12 +471,12 @@
 
         readTempData();
         temperature = ((float) temperature) / 333.87f + 21.0f; // Temperature in degrees Centigrade
-        //pc.printf(" temperature = %f  C\n\r", temperature);
+        //pc2.printf(" temperature = %f  C\n\r", temperature);
 
-        // pc.printf("q0 = %f\n\r", q[0]);
-        // pc.printf("q1 = %f\n\r", q[1]);
-        // pc.printf("q2 = %f\n\r", q[2]);
-        // pc.printf("q3 = %f\n\r", q[3]);
+        // pc2.printf("q0 = %f\n\r", q[0]);
+        // pc2.printf("q1 = %f\n\r", q[1]);
+        // pc2.printf("q2 = %f\n\r", q[2]);
+        // pc2.printf("q3 = %f\n\r", q[3]);
 
         // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
         // In this coordinate system, the positive z-axis is down toward Earth.
@@ -495,9 +495,9 @@
         Yh = my*cos(roll)+mz*sin(roll);
 
         yawmag = atan2(Yh,Xh)+PI;
-        //pc.printf("Xh= %f Yh= %f ",Xh,Yh);
-        printf("Yaw[mag]= %f\n\r",yawmag*180.0f/PI);
-        printf(",%f",yawmag*180.0f/PI);
+        //pc2.printf("Xh= %f Yh= %f ",Xh,Yh);
+        //pc2.printf("Yaw[mag]= %f\n\r",yawmag*180.0f/PI);
+        //pc2.printf(",%f",yawmag*180.0f/PI);
 
 
 
@@ -506,16 +506,9 @@
         yaw   += 180.0f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
         roll  *= 180.0f / PI;
 
-        //pc.printf(",%f,%f,%f\n",roll,pitch,yaw);
-        printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
-        //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
-//    sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
-//    lcd.printString(buffer, 0, 4);
-//    sprintf(buffer, "rate = %f", (float) sumCount/sum);
-//    lcd.printString(buffer, 0, 5);
-
-
-
+        //pc2.printf(",%f,%f,%f\n",roll,pitch,yaw);
+        //pc2.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
+        //pc2.printf("average rate = %f\n\r", (float) sumCount/sum);
 
         myled= !myled;
         count = t.read_ms();
@@ -531,7 +524,7 @@
     }
 }
 
-void AHRS::printRollPitchYaw()
+void AHRS::PrintRollPitchYaw()
 {
-    //test.printf("roll : %f, pitch : %f, yaw : %f\n",roll,pitch,yaw);
+    pc2.printf("roll : %f, pitch : %f, yaw : %f\n",roll,pitch,yaw);
 }
\ No newline at end of file