mbed I2C MPU6500 tilt angle

Fork of NTOUEE-mbed-I2C_MPU6500_Tiltangle by Richard Kuo

main.cpp

Committer:
rkuo2000
Date:
2016-10-20
Revision:
1:efa4c7817836
Parent:
0:73a95126993e

File content as of revision 1:efa4c7817836:

// 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; // IMU use on-board MPU6500

int main() {
    int16_t accX,  accY,  accZ;
    float X2, Y2, Z2;
    float theta, psi, phi;
    
    i2c0.frequency(400000);    
    IMU.initialize();
    
    while(true) {
       accX = IMU.getAccelXvalue();
       accY = IMU.getAccelYvalue();
       accZ = IMU.getAccelZvalue();
       printf("Acc: %6d, %6d, %6d, ",   accX, accY, accZ);
       // 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);              
    }
}