예제4) MPU9250 실습

*아래 MPU9250 라이브러리와 I2Cdev 라이브러리를 이용한다

 

MPU9250 라이브러리.zip

0.04MB

 

 

 

*아래 코드는 함수 및 변수에 주석 넣은 것이다.

 

#include "Wire.h"
#include "I2Cdev.h"
#include "MPU9250.h"

// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU9250 accelgyro;
I2Cdev   I2C_M;

//1. MPU9250 함수

void getHeading(void);  //N극에서 몇 도 회전해 있는가를 구함  
void getTiltHeading(void); //The clockwise angle between the magnetic north and the projection of the positive X-Axis in the horizontal plane:
void Mxyz_init_calibrated();  //Calibration 값 구하고 시리얼 출력하는 함수
void get_calibration_Data();  //calibration값 (mx_centre)구하는 함수
void get_one_sample_date_mxyz(); //Compass 데이터 1번 받고 샘플로 삼는다.
void getAccel_Data(void); //Accel 데이터 받기
void getGyro_Data(void);//Gyro 데이터 받기
void getCompass_Data(void);//Compass 데이터 받기
void getCompassDate_calibrated (); //Compass Calibrated 데이터 받기

//2. MPU9250 변수
uint8_t buffer_m[6];

int16_t ax, ay, az;//Accelation
int16_t gx, gy, gz;//Gyro
int16_t   mx, my, mz;//Compass

float heading; //N극에서 몇 도 회전해 있는가를 구함  
float tiltheading;//The clockwise angle between the magnetic north and the projection of the positive X-Axis in the horizontal plane:

float Axyz[3];//Accelation
float Gxyz[3];//Gyro
float Mxyz[3];//Compass

#define sample_num_mdate  5000  //sample 데이터 수집할 횟수

volatile float mx_sample[3]; //샘플 넣을 배열. mx_sample[]은 0:min, 1:max, 2:sample 이다.
volatile float my_sample[3]; //샘플 넣을 배열. my_sample[]은 0:min, 1:max, 2:sample 이다.
volatile float mz_sample[3]; //샘플 넣을 배열. mz_sample[]은 0:min, 1:max, 2:sample 이다.

static float mx_centre = 0; //Calibration 값 (설정값 아니다. 아래서 구할 것이다.) Mxyz_init_calibrated ()에서 구한다.
static float my_centre = 0; //Calibration 값 (설정값 아니다. 아래서 구할 것이다.) Mxyz_init_calibrated ()에서 구한다.
static float mz_centre = 0; //Calibration 값 (설정값 아니다. 아래서 구할 것이다.) Mxyz_init_calibrated ()에서 구한다.

volatile int mx_max = 0;  //샘플 데이터 중에서 최대값
volatile int my_max = 0;  //샘플 데이터 중에서 최대값
volatile int mz_max = 0;  //샘플 데이터 중에서 최대값

volatile int mx_min = 0;  //샘플 데이터 중에서 최소값
volatile int my_min = 0;  //샘플 데이터 중에서 최소값
volatile int mz_min = 0;  //샘플 데이터 중에서 최소값

float temperature;
float pressure;
float atm;
float altitude;

void setup()
{
    Wire.begin();
    Serial.begin(38400);                        // 통신속도 38400 bps

    Serial.println("Initializing I2C devices...");
    accelgyro.initialize();

    // verify connection
    Serial.println("Testing device connections...");
    Serial.println(accelgyro.testConnection() ? "MPU9250 connection successful" : "MPU9250 connection failed");

    delay(1000);
    Serial.println("     ");
    Mxyz_init_calibrated ();  //초기에 Calibration 값 구하고 동작해야한다.

}


void loop()
{
    getAccel_Data();
    getGyro_Data();
    getCompassDate_calibrated(); // compass data has been calibrated here
    getHeading();               //before we use this function we should run 'getCompassDate_calibrated()' frist, so that we can get calibrated data ,then we can get correct angle .
    getTiltHeading();

    Serial.println("calibration parameter: ");
    Serial.print(mx_centre);
    Serial.print("         ");
    Serial.print(my_centre);
    Serial.print("         ");
    Serial.println(mz_centre);
    Serial.println("     ");

    Serial.println("Acceleration(g) of X,Y,Z:");
    Serial.print(Axyz[0]);
    Serial.print(",");
    Serial.print(Axyz[1]);
    Serial.print(",");
    Serial.println(Axyz[2]);
    Serial.println("Gyro(degress/s) of X,Y,Z:");
    Serial.print(Gxyz[0]);
    Serial.print(",");
    Serial.print(Gxyz[1]);
    Serial.print(",");
    Serial.println(Gxyz[2]);
    Serial.println("Compass Value of X,Y,Z:");
    Serial.print(Mxyz[0]);
    Serial.print(",");
    Serial.print(Mxyz[1]);
    Serial.print(",");
    Serial.println(Mxyz[2]);
    Serial.println("The clockwise angle between the magnetic north and X-Axis:");
    Serial.print(heading);
    Serial.println(" ");
    Serial.println("The clockwise angle between the magnetic north and the projection of the positive X-Axis in the horizontal plane:");
    Serial.println(tiltheading);
    Serial.println("   ");
    Serial.println();
    delay(1000);
}


void getHeading(void) //N극에서 몇 도 회전해 있는가를 구함    
{//before we use this function we should run 'getCompassDate_calibrated()' frist, 
//so that we can get calibrated data ,then we can get correct angle .
    heading = 180 * atan2(Mxyz[1], Mxyz[0]) / PI;
    if (heading < 0) heading += 360;
}
void getTiltHeading(void) //The clockwise angle between the magnetic north and the projection of the positive X-Axis in the horizontal plane:
{
    float pitch = asin(-Axyz[0]);
    float roll = asin(Axyz[1] / cos(pitch));

    float xh = Mxyz[0] * cos(pitch) + Mxyz[2] * sin(pitch);
    float yh = Mxyz[0] * sin(roll) * sin(pitch) + Mxyz[1] * cos(roll) - Mxyz[2] * sin(roll) * cos(pitch);
    float zh = -Mxyz[0] * cos(roll) * sin(pitch) + Mxyz[1] * sin(roll) + Mxyz[2] * cos(roll) * cos(pitch);
    tiltheading = 180 * atan2(yh, xh) / PI;
    if (yh < 0)    tiltheading += 360;
}
void Mxyz_init_calibrated () //Calibration 값 구하고 시리얼 출력하는 함수
{

    Serial.println(F("Before using 9DOF,we need to calibrate the compass frist,It will takes about 2 minutes."));
    Serial.print("  ");
    Serial.println(F("During  calibratting ,you should rotate and turn the 9DOF all the time within 2 minutes."));
    Serial.print("  ");
    Serial.println(F("If you are ready ,please sent a command data 'ready' to start sample and calibrate."));
    while (!Serial.find("ready"));
    Serial.println("  ");
    Serial.println("ready");
    Serial.println("Sample starting......");
    Serial.println("waiting ......");

    get_calibration_Data ();

    Serial.println("     ");
    Serial.println("compass calibration parameter ");
    Serial.print(mx_centre);
    Serial.print("     ");
    Serial.print(my_centre);
    Serial.print("     ");
    Serial.println(mz_centre);
    Serial.println("    ");
}
void get_calibration_Data () //calibration값 (mx_centre)구하는 함수
{
    for (int i = 0; i < sample_num_mdate; i++)
    {
        get_one_sample_date_mxyz();//샘플로 Compass Data받고 
        //샘플을 sample_num_mdate 만큼 받아서 최대값과 최소값을 구한다.
        //mx_sample[]은 0:min, 1:max, 2:sample 이다.
        if (mx_sample[2] >= mx_sample[1])mx_sample[1] = mx_sample[2];
        if (my_sample[2] >= my_sample[1])my_sample[1] = my_sample[2]; //find max value
        if (mz_sample[2] >= mz_sample[1])mz_sample[1] = mz_sample[2];

        if (mx_sample[2] <= mx_sample[0])mx_sample[0] = mx_sample[2];
        if (my_sample[2] <= my_sample[0])my_sample[0] = my_sample[2]; //find min value
        if (mz_sample[2] <= mz_sample[0])mz_sample[0] = mz_sample[2];
    }
    mx_max = mx_sample[1];
    my_max = my_sample[1];
    mz_max = mz_sample[1];

    mx_min = mx_sample[0];
    my_min = my_sample[0];
    mz_min = mz_sample[0];
    
    mx_centre = (mx_max + mx_min) / 2; //최대값과 최소값으로 중앙값 구함
    my_centre = (my_max + my_min) / 2;
    mz_centre = (mz_max + mz_min) / 2;
}
void get_one_sample_date_mxyz()//Compass 데이터 1번 받고 샘플로 삼는다.
{
    getCompass_Data();
    mx_sample[2] = Mxyz[0];
    my_sample[2] = Mxyz[1];
    mz_sample[2] = Mxyz[2];
}
void getAccel_Data(void) //Accel 데이터 받기
{
    accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
    Axyz[0] = (double) ax / 16384;
    Axyz[1] = (double) ay / 16384;
    Axyz[2] = (double) az / 16384;
}
void getGyro_Data(void) //Gyro 데이터 받기
{
    accelgyro.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);

    Gxyz[0] = (double) gx * 250 / 32768;
    Gxyz[1] = (double) gy * 250 / 32768;
    Gxyz[2] = (double) gz * 250 / 32768;
}
void getCompass_Data(void) //Compass 데이터 받기
{
    I2C_M.writeByte(MPU9150_RA_MAG_ADDRESS, 0x0A, 0x01); //enable the magnetometer
    delay(10);
    I2C_M.readBytes(MPU9150_RA_MAG_ADDRESS, MPU9150_RA_MAG_XOUT_L, 6, buffer_m);

    mx = ((int16_t)(buffer_m[1]) << 8) | buffer_m[0] ;
    my = ((int16_t)(buffer_m[3]) << 8) | buffer_m[2] ;
    mz = ((int16_t)(buffer_m[5]) << 8) | buffer_m[4] ;

    Mxyz[0] = (double) mx * 1200 / 4096;
    Mxyz[1] = (double) my * 1200 / 4096;
    Mxyz[2] = (double) mz * 1200 / 4096;
}
void getCompassDate_calibrated () //Compass Calibrated 데이터 받기
{
    getCompass_Data();
    Mxyz[0] = Mxyz[0] - mx_centre;
    Mxyz[1] = Mxyz[1] - my_centre;
    Mxyz[2] = Mxyz[2] - mz_centre;
}

 

*실행시 시리얼 통신

샘플링 대략 1분 정도 걸린다.