Diff: main.cpp

Revision:

1:efa4c7817836

Parent:

0:73a95126993e

--- a/main.cpp	Thu Oct 20 13:16:22 2016 +0000
+++ b/main.cpp	Thu Oct 20 13:19:15 2016 +0000
@@ -1,15 +1,19 @@
-// using NuMaker-PFM-NUC472 I2C0 to read 3-axis accelerometer & 3-axis gyroscope (MPU6500)
+// using NuMaker-PFM-NUC472 I2C0 to read IMU and calculate tilt angle
 #include "mbed.h"
+#include "math.h"
 #include "mpu6500.h"
 
+#define PI 3.14159265359
+
 I2C i2c0(PC_9, PA_15); // I2C0_SDA, I2C0_SCL
 
-MPU6500 IMU; // on-board IMU is MPU6500
+MPU6500 IMU; // IMU use on-board MPU6500
 
 int main() {
     int16_t accX,  accY,  accZ;
-    int16_t gyroX, gyroY, gyroZ;
-
+    float X2, Y2, Z2;
+    float theta, psi, phi;
+    
     i2c0.frequency(400000);    
     IMU.initialize();
     
@@ -17,10 +21,12 @@
        accX = IMU.getAccelXvalue();
        accY = IMU.getAccelYvalue();
        accZ = IMU.getAccelZvalue();
-       gyroX= IMU.getGyroXvalue();
-       gyroY= IMU.getGyroYvalue();
-       gyroZ= IMU.getGyroZvalue();
        printf("Acc: %6d, %6d, %6d, ",   accX, accY, accZ);
-       printf("Gyro:%6d, %6d, %6d\n\r", gyroX, gyroY, gyroZ);       
+       // calculate tilt (degree = radians *180 / PI)
+       X2 = pow(accX,2.0); Y2 = pow(accY, 2.0); Z2 = pow(accZ,2.0);
+       theta = atan(accX / sqrt(Y2 + Z2)) *180 /PI;
+       psi   = atan(accY / sqrt(Z2 + X2)) *180 /PI;
+       phi   = atan(sqrt(X2 + Y2) / accZ) *180 /PI;
+       printf("theta=%d, psi=%d, phi=%d\n\r", (int) theta, (int) psi, (int) phi);              
     }
 }