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

main.cpp

Committer:
cyliang
Date:
14 months ago
Revision:
6:911c09e70fe4
Parent:
4:cf21ad364a28

File content as of revision 6:911c09e70fe4:

// 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
#elif TARGET_NUMAKER_IOT_M467
I2C i2c0(PD_0, PD_1);
#else
I2C i2c0(I2C_SDA, I2C_SCL);
#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);              
    }
}