![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
mbed I2C MPU6500 tilt angle
Fork of NTOUEE-mbed-I2C_MPU6500_Tiltangle by
Revision 1:efa4c7817836, committed 2016-10-20
- Comitter:
- rkuo2000
- Date:
- Thu Oct 20 13:19:15 2016 +0000
- Parent:
- 0:73a95126993e
- Commit message:
- mbed I2C MPU6500 tilt angle
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 73a95126993e -r efa4c7817836 main.cpp --- a/main.cpp Thu Oct 20 13:16:22 2016 +0000 +++ b/main.cpp Thu Oct 20 13:19:15 2016 +0000 @@ -1,15 +1,19 @@ -// using NuMaker-PFM-NUC472 I2C0 to read 3-axis accelerometer & 3-axis gyroscope (MPU6500) +// 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; // on-board IMU is MPU6500 +MPU6500 IMU; // IMU use on-board MPU6500 int main() { int16_t accX, accY, accZ; - int16_t gyroX, gyroY, gyroZ; - + float X2, Y2, Z2; + float theta, psi, phi; + i2c0.frequency(400000); IMU.initialize(); @@ -17,10 +21,12 @@ accX = IMU.getAccelXvalue(); accY = IMU.getAccelYvalue(); accZ = IMU.getAccelZvalue(); - gyroX= IMU.getGyroXvalue(); - gyroY= IMU.getGyroYvalue(); - gyroZ= IMU.getGyroZvalue(); printf("Acc: %6d, %6d, %6d, ", accX, accY, accZ); - printf("Gyro:%6d, %6d, %6d\n\r", gyroX, gyroY, gyroZ); + // 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); } }