main.cpp

00001 // NuMaker-PFM-NUC472 : I2C0 reading IMU to calculate tilt angle
00002 #include "mbed.h"
00003 #include "math.h"
00004 #include "mpu6500.h"
00005 
00006 #define PI 3.14159265359
00007 #if TARGET_NUMAKER_PFM_NUC472
00008 I2C i2c0(PC_9, PA_15); // I2C0_SDA, I2C0_SCL
00009 #elif TARGET_NUMAKER_IOT_M467
00010 I2C i2c0(PD_0, PD_1);
00011 #else
00012 I2C i2c0(I2C_SDA, I2C_SCL);
00013 #endif
00014 
00015 MPU6500 IMU; // IMU use on-board MPU6500
00016 
00017 int main() {
00018     int16_t accX,  accY,  accZ;
00019     float X2, Y2, Z2;
00020     float theta, psi, phi;
00021     
00022     i2c0.frequency(400000);    
00023     IMU.initialize();
00024     
00025     while(true) {
00026        accX = IMU.getAccelXvalue();
00027        accY = IMU.getAccelYvalue();
00028        accZ = IMU.getAccelZvalue();
00029        printf("Acc: %6d, %6d, %6d, ",   accX, accY, accZ);
00030        // calculate tilt (degree = radians *180 / PI)
00031        X2 = pow(accX,2.0); Y2 = pow(accY, 2.0); Z2 = pow(accZ,2.0);
00032        theta = atan(accX / sqrt(Y2 + Z2)) *180 /PI;
00033        psi   = atan(accY / sqrt(Z2 + X2)) *180 /PI;
00034        phi   = atan(sqrt(X2 + Y2) / accZ) *180 /PI;
00035        printf("theta=%d, psi=%d, phi=%d\n\r", (int) theta, (int) psi, (int) phi);              
00036     }
00037 }