Nuvoton
/
NuMaker-mbed-I2C0_MPU6500_Tiltangle
NuMaker-PFM-NUC472: I2C0 read on-board MPU6500 and calculate tilt angle
main.cpp
- Committer:
- cyliang
- Date:
- 20 months ago
- Revision:
- 3:a5799a7d3bd1
- Parent:
- 0:d1cab7bc0553
- Child:
- 4:cf21ad364a28
File content as of revision 3:a5799a7d3bd1:
// NuMaker-PFM-NUC472 : I2C0 reading IMU to calculate tilt angle #include "mbed.h" #include "math.h" #include "mpu6500.h" #define PI 3.14159265359 #if TARGET_NUMAKER_PFM_NUC472 I2C i2c0(PC_9, PA_15); // I2C0_SDA, I2C0_SCL #endif #if TARGET_NUMAKER_IOT_M467 I2C i2c0(PD_0, PD_1); #endif 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); } }