Nuvoton
/
NuMaker-mbed-I2C0_MPU6500_Tiltangle
NuMaker-PFM-NUC472: I2C0 read on-board MPU6500 and calculate tilt angle
main.cpp@6:911c09e70fe4, 19 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?
User | Revision | Line number | New 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 | } |