ドローン用計測制御基板の作り方 vol.1 ハードウェア編p.13掲載 角速度、加速度の表示

Dependencies:   mbed MPU6050_alter

main.cpp

Committer:
Joeatsumi
Date:
2019-12-30
Revision:
0:0a940ef5a5b2

File content as of revision 0:0a940ef5a5b2:

//==================================================
//Inertial measurement
//MPU board: mbed LPC1768
//Accelerometer +Gyro sensor : GY-521
//2019/10/11 A.Toda
//==================================================
#include "mbed.h"
#include "MPU6050.h"

//==================================================
#define RAD_TO_DEG          57.2957795f             // 180 / π

//==================================================
//Port Setting
MPU6050 mpu(p9, p10);  //Accelerometer + Gyro
                        //(SDA,SCLK)

Serial pc(USBTX, USBRX);    //UART

//==================================================
//Accelerometer and gyro data
//==================================================
float acc[3]; //variables for accelerometer
float gyro[3]; //variables for gyro

//==================================================
//Main
//==================================================
int main() {
    
    //UART initialization
    pc.baud(115200);
    
    //Accelerometer and gyro setting 
    mpu.setAcceleroRange(0);//acceleration range is +-2G
    mpu.setGyroRange(1);//gyro rate is +-500degree per second(dps)
    
    while(1) {
        
        //Aquisition sensor values
        mpu.getAccelero(acc);//get acceleration (Accelerometer)
                                //x_axis acc[0]
                                //y_axis acc[1]
                                //z_axis acc[2]
        mpu.getGyro(gyro);   //get rate of angle(Gyro)
                                //x_axis gyro[0]
                                //y_axis gyro[1]
                                //z_axis gyro[2]
        
        //Invertion for direction of Accelerometer axis
        acc[0]*=(-1.0);
        acc[2]*=(-1.0);
        
        //Unit convertion of rate of angle(radian to degree)
        gyro[0]*=RAD_TO_DEG;
        gyro[0]*=(-1.0);
        
        gyro[1]*=RAD_TO_DEG;
        
        gyro[2]*=RAD_TO_DEG;
        gyro[2]*=(-1.0);
        
        //Display inertial values 
        pc.printf("a_x=%f,a_y=%f,a_z=%f,g_x=%f,g_y=%f,g_z=%f\r\n"
                    ,acc[0],acc[1],acc[2],gyro[0],gyro[1],gyro[2]);
        
        wait(0.01);
    }
}