Nuvoton / Mbed OS NuMaker-mbed-I2C0_MPU6500_Tiltangle

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

main.cpp@6:911c09e70fe4, 15 months ago (annotated)

Committer:
cyliang
Date:
Wed Mar 01 03:29:49 2023 +0000
Revision:
6:911c09e70fe4
Parent:
4:cf21ad364a28 
Update OS v6.17.0 for M467 target

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
cyliang 3:a5799a7d3bd1 7 #if TARGET_NUMAKER_PFM_NUC472
cyliang 3:a5799a7d3bd1 8 I2C i2c0(PC_9, PA_15); // I2C0_SDA, I2C0_SCL
cyliang 4:cf21ad364a28 9 #elif TARGET_NUMAKER_IOT_M467
cyliang 3:a5799a7d3bd1 10 I2C i2c0(PD_0, PD_1);
cyliang 4:cf21ad364a28 11 #else
cyliang 4:cf21ad364a28 12 I2C i2c0(I2C_SDA, I2C_SCL);
cyliang 3:a5799a7d3bd1 13 #endif
rkuo2000 0:d1cab7bc0553 14
rkuo2000 0:d1cab7bc0553 15 MPU6500 IMU; // IMU use on-board MPU6500
rkuo2000 0:d1cab7bc0553 16
rkuo2000 0:d1cab7bc0553 17 int main() {
rkuo2000 0:d1cab7bc0553 18 int16_t accX, accY, accZ;
rkuo2000 0:d1cab7bc0553 19 float X2, Y2, Z2;
rkuo2000 0:d1cab7bc0553 20 float theta, psi, phi;
rkuo2000 0:d1cab7bc0553 21
rkuo2000 0:d1cab7bc0553 22 i2c0.frequency(400000);
rkuo2000 0:d1cab7bc0553 23 IMU.initialize();
rkuo2000 0:d1cab7bc0553 24
rkuo2000 0:d1cab7bc0553 25 while(true) {
rkuo2000 0:d1cab7bc0553 26 accX = IMU.getAccelXvalue();
rkuo2000 0:d1cab7bc0553 27 accY = IMU.getAccelYvalue();
rkuo2000 0:d1cab7bc0553 28 accZ = IMU.getAccelZvalue();
rkuo2000 0:d1cab7bc0553 29 printf("Acc: %6d, %6d, %6d, ", accX, accY, accZ);
rkuo2000 0:d1cab7bc0553 30 // calculate tilt (degree = radians *180 / PI)
rkuo2000 0:d1cab7bc0553 31 X2 = pow(accX,2.0); Y2 = pow(accY, 2.0); Z2 = pow(accZ,2.0);
rkuo2000 0:d1cab7bc0553 32 theta = atan(accX / sqrt(Y2 + Z2)) *180 /PI;
rkuo2000 0:d1cab7bc0553 33 psi = atan(accY / sqrt(Z2 + X2)) *180 /PI;
rkuo2000 0:d1cab7bc0553 34 phi = atan(sqrt(X2 + Y2) / accZ) *180 /PI;
rkuo2000 0:d1cab7bc0553 35 printf("theta=%d, psi=%d, phi=%d\n\r", (int) theta, (int) psi, (int) phi);
rkuo2000 0:d1cab7bc0553 36 }
rkuo2000 0:d1cab7bc0553 37 }