![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
mbed I2C MPU6500 tilt angle
Fork of NTOUEE-mbed-I2C_MPU6500_Tiltangle by
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); } }