Diff: main.cpp

Revision:

1:ca8638ade4ab

Parent:

0:706ce7540c8f

--- a/main.cpp	Sun Mar 29 08:50:27 2015 +0000
+++ b/main.cpp	Fri Oct 26 05:08:02 2018 +0000
@@ -38,11 +38,11 @@
 #define RAD_TO_DEG(x) ( x * 57.29578 )
 
 
-RawSerial pc(USBTX, USBRX);
+Serial pc(USBTX, USBRX);
 
 
 MPU6050 mpu(PB_9, PB_8);     // sda, scl pin
-InterruptIn INT0(PA_8);     // INT0 pin
+InterruptIn INT0(PA_9);     // INT0 pin
 
 const int FIFO_BUFFER_SIZE = 128;
 uint8_t fifoBuffer[FIFO_BUFFER_SIZE];
@@ -62,6 +62,8 @@
 struct MPU6050_DmpData {
     Quaternion q;
     VectorFloat gravity;    // g
+    VectorInt16 acc;        // g
+    VectorInt16 gryo;    // g
     float roll, pitch, yaw;     // rad
 }dmpData;
 
@@ -86,15 +88,15 @@
     
     mpu.initialize();
     if (mpu.testConnection()) {
-        pc.puts("MPU6050 test connection passed.\n");
+        pc.printf("MPU6050 test connection passed.\n");
     } else {
-        pc.puts("MPU6050 test connection failed.\n");
+        pc.printf("MPU6050 test connection failed.\n");
         return false;
     }
     if (mpu.dmpInitialize() == 0) {
-        pc.puts("succeed in MPU6050 DMP Initializing.\n");
+        pc.printf("succeed in MPU6050 DMP Initializing.\n");
     } else {
-        pc.puts("failed in MPU6050 DMP Initializing.\n");
+        pc.printf("failed in MPU6050 DMP Initializing.\n");
         return false;
     }
     mpu.setXAccelOffset(offset.ax);
@@ -109,7 +111,7 @@
     packetSize = mpu.dmpGetFIFOPacketSize();
     dmpReady = true;    // Enable interrupt.
     
-    pc.puts("Init finish!\n");
+    pc.printf("Init finish!\n");
     
     return true;
 }
@@ -125,7 +127,7 @@
     // Check that this interrupt is a FIFO buffer overflow interrupt.
     if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
         mpu.resetFIFO();
-        pc.puts("FIFO overflow!\n");
+        pc.printf("FIFO overflow!\n");
         return;
         
     // Check that this interrupt is a Data Ready interrupt.
@@ -137,7 +139,7 @@
     #ifdef OUTPUT_QUATERNION
         mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
         if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Quaternion : w=%f, x=%f, y=%f, z=%f\n", dmpData.q.w, dmpData.q.x, dmpData.q.y, dmpData.q.z ) < 0 ) return;
-        pc.puts(snprintf_buffer);
+        pc.printf(snprintf_buffer);
     #endif
         
     #ifdef OUTPUT_EULER
@@ -145,20 +147,21 @@
         mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
         mpu.dmpGetEuler(euler, &dmpData.q);
         if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Euler : psi=%fdeg, theta=%fdeg, phi=%fdeg\n", RAD_TO_DEG(euler[0]), RAD_TO_DEG(euler[1]), RAD_TO_DEG(euler[2]) ) < 0 ) return;
-        pc.puts(snprintf_buffer);
+        pc.printf(snprintf_buffer);
     #endif
         
     #ifdef OUTPUT_ROLL_PITCH_YAW
         mpu.dmpGetQuaternion(&dmpData.q, fifoBuffer);
         mpu.dmpGetGravity(&dmpData.gravity, &dmpData.q);
+        mpu.dmpGetAccel(&dmpData.acc, fifoBuffer);
         float rollPitchYaw[3];
         mpu.dmpGetYawPitchRoll(rollPitchYaw, &dmpData.q, &dmpData.gravity);
         dmpData.roll = rollPitchYaw[2];
         dmpData.pitch = rollPitchYaw[1];
         dmpData.yaw = rollPitchYaw[0];
-        
-        if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Roll:%6.2fdeg, Pitch:%6.2fdeg, Yaw:%6.2fdeg\n", RAD_TO_DEG(dmpData.roll), RAD_TO_DEG(dmpData.pitch), RAD_TO_DEG(dmpData.yaw) ) < 0 ) return;
-        pc.puts(snprintf_buffer);
+        pc.printf("ax = %d,ay = %d,az = %d\r\n",dmpData.acc.x,dmpData.acc.y,dmpData.acc.z);
+//        if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Roll:%6.2fdeg, Pitch:%6.2fdeg, Yaw:%6.2fdeg\n", RAD_TO_DEG(dmpData.roll), RAD_TO_DEG(dmpData.pitch), RAD_TO_DEG(dmpData.yaw) ) < 0 ) return;
+//        pc.printf(snprintf_buffer);
     #endif
         
     #ifdef OUTPUT_FOR_TEAPOT
@@ -178,9 +181,9 @@
     #ifdef OUTPUT_TEMPERATURE
         float temp = mpu.getTemperature() / 340.0 + 36.53;
         if ( snprintf( snprintf_buffer, snprintf_buffer_size, "Temp:%4.1fdeg\n", temp ) < 0 ) return;
-        pc.puts(snprintf_buffer);
+        pc.printf(snprintf_buffer);
     #endif
         
-        pc.puts("\n");
+        pc.printf("\n");
     }
 }
\ No newline at end of file