Nuvoton / Mbed OS NuMaker-mbed-I2C0_MPU6500_Tiltangle

NuMaker-PFM-NUC472: I2C0 read on-board MPU6500 and calculate tilt angle

main.cpp@0:d1cab7bc0553, 2016-10-17 (annotated)

Committer:
rkuo2000
Date:
Mon Oct 17 06:34:27 2016 +0000
Revision:
0:d1cab7bc0553
Child:
3:a5799a7d3bd1
NuMaker-PFM-NUC472: I2C0 read on-board MPU6500 and calculate tilt angle

Who changed what in which revision?

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