Nuvoton
/
NuMaker-mbed-I2C0_MPU6500_Tiltangle
NuMaker-PFM-NUC472: I2C0 read on-board MPU6500 and calculate tilt angle
Diff: main.cpp
- Revision:
- 0:d1cab7bc0553
- Child:
- 3:a5799a7d3bd1
diff -r 000000000000 -r d1cab7bc0553 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Mon Oct 17 06:34:27 2016 +0000 @@ -0,0 +1,32 @@ +// NuMaker-PFM-NUC472 : I2C0 reading IMU to 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); + } +}