Richard Kuo / Mbed OS NTOUEE-mbed-I2C_MPU6500_Tiltangle

mbed I2C MPU6500 tilt angle

Fork of NTOUEE-mbed-I2C_MPU6500_Tiltangle by Richard Kuo

main.cpp@1:efa4c7817836, 2016-10-20 (annotated)

Committer:
rkuo2000
Date:
Thu Oct 20 13:19:15 2016 +0000
Revision:
1:efa4c7817836
Parent:
0:73a95126993e 
mbed I2C MPU6500 tilt angle

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rkuo2000 1:efa4c7817836 1 // using NuMaker-PFM-NUC472 I2C0 to read IMU and calculate tilt angle
rkuo2000 0:73a95126993e 2 #include "mbed.h"
rkuo2000 1:efa4c7817836 3 #include "math.h"
rkuo2000 0:73a95126993e 4 #include "mpu6500.h"
rkuo2000 0:73a95126993e 5
rkuo2000 1:efa4c7817836 6 #define PI 3.14159265359
rkuo2000 1:efa4c7817836 7
rkuo2000 0:73a95126993e 8 I2C i2c0(PC_9, PA_15); // I2C0_SDA, I2C0_SCL
rkuo2000 0:73a95126993e 9
rkuo2000 1:efa4c7817836 10 MPU6500 IMU; // IMU use on-board MPU6500
rkuo2000 0:73a95126993e 11
rkuo2000 0:73a95126993e 12 int main() {
rkuo2000 0:73a95126993e 13 int16_t accX, accY, accZ;
rkuo2000 1:efa4c7817836 14 float X2, Y2, Z2;
rkuo2000 1:efa4c7817836 15 float theta, psi, phi;
rkuo2000 1:efa4c7817836 16
rkuo2000 0:73a95126993e 17 i2c0.frequency(400000);
rkuo2000 0:73a95126993e 18 IMU.initialize();
rkuo2000 0:73a95126993e 19
rkuo2000 0:73a95126993e 20 while(true) {
rkuo2000 0:73a95126993e 21 accX = IMU.getAccelXvalue();
rkuo2000 0:73a95126993e 22 accY = IMU.getAccelYvalue();
rkuo2000 0:73a95126993e 23 accZ = IMU.getAccelZvalue();
rkuo2000 0:73a95126993e 24 printf("Acc: %6d, %6d, %6d, ", accX, accY, accZ);
rkuo2000 1:efa4c7817836 25 // calculate tilt (degree = radians *180 / PI)
rkuo2000 1:efa4c7817836 26 X2 = pow(accX,2.0); Y2 = pow(accY, 2.0); Z2 = pow(accZ,2.0);
rkuo2000 1:efa4c7817836 27 theta = atan(accX / sqrt(Y2 + Z2)) *180 /PI;
rkuo2000 1:efa4c7817836 28 psi = atan(accY / sqrt(Z2 + X2)) *180 /PI;
rkuo2000 1:efa4c7817836 29 phi = atan(sqrt(X2 + Y2) / accZ) *180 /PI;
rkuo2000 1:efa4c7817836 30 printf("theta=%d, psi=%d, phi=%d\n\r", (int) theta, (int) psi, (int) phi);
rkuo2000 0:73a95126993e 31 }
rkuo2000 0:73a95126993e 32 }