Class of MPU9250
Dependencies: AHRS_fillter mbed
Fork of MPU9250AHRS by
Diff: AHRS.cpp
- Revision:
- 8:928673148b55
- Parent:
- 7:3e74239e3778
diff -r 3e74239e3778 -r 928673148b55 AHRS.cpp --- 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