Atsumi Toda / Mbed 2 deprecated Auto_pilot_mpu6050

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

Dependencies:   mbed MPU6050_alter

main.cpp@0:0a940ef5a5b2, 2019-12-30 (annotated)

Committer:
Joeatsumi
Date:
Mon Dec 30 08:45:22 2019 +0000
Revision:
0:0a940ef5a5b2
20191230

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Joeatsumi 0:0a940ef5a5b2 1 //==================================================
Joeatsumi 0:0a940ef5a5b2 2 //Inertial measurement
Joeatsumi 0:0a940ef5a5b2 3 //MPU board: mbed LPC1768
Joeatsumi 0:0a940ef5a5b2 4 //Accelerometer +Gyro sensor : GY-521
Joeatsumi 0:0a940ef5a5b2 5 //2019/10/11 A.Toda
Joeatsumi 0:0a940ef5a5b2 6 //==================================================
Joeatsumi 0:0a940ef5a5b2 7 #include "mbed.h"
Joeatsumi 0:0a940ef5a5b2 8 #include "MPU6050.h"
Joeatsumi 0:0a940ef5a5b2 9
Joeatsumi 0:0a940ef5a5b2 10 //==================================================
Joeatsumi 0:0a940ef5a5b2 11 #define RAD_TO_DEG 57.2957795f // 180 / π
Joeatsumi 0:0a940ef5a5b2 12
Joeatsumi 0:0a940ef5a5b2 13 //==================================================
Joeatsumi 0:0a940ef5a5b2 14 //Port Setting
Joeatsumi 0:0a940ef5a5b2 15 MPU6050 mpu(p9, p10); //Accelerometer + Gyro
Joeatsumi 0:0a940ef5a5b2 16 //(SDA,SCLK)
Joeatsumi 0:0a940ef5a5b2 17
Joeatsumi 0:0a940ef5a5b2 18 Serial pc(USBTX, USBRX); //UART
Joeatsumi 0:0a940ef5a5b2 19
Joeatsumi 0:0a940ef5a5b2 20 //==================================================
Joeatsumi 0:0a940ef5a5b2 21 //Accelerometer and gyro data
Joeatsumi 0:0a940ef5a5b2 22 //==================================================
Joeatsumi 0:0a940ef5a5b2 23 float acc[3]; //variables for accelerometer
Joeatsumi 0:0a940ef5a5b2 24 float gyro[3]; //variables for gyro
Joeatsumi 0:0a940ef5a5b2 25
Joeatsumi 0:0a940ef5a5b2 26 //==================================================
Joeatsumi 0:0a940ef5a5b2 27 //Main
Joeatsumi 0:0a940ef5a5b2 28 //==================================================
Joeatsumi 0:0a940ef5a5b2 29 int main() {
Joeatsumi 0:0a940ef5a5b2 30
Joeatsumi 0:0a940ef5a5b2 31 //UART initialization
Joeatsumi 0:0a940ef5a5b2 32 pc.baud(115200);
Joeatsumi 0:0a940ef5a5b2 33
Joeatsumi 0:0a940ef5a5b2 34 //Accelerometer and gyro setting
Joeatsumi 0:0a940ef5a5b2 35 mpu.setAcceleroRange(0);//acceleration range is +-2G
Joeatsumi 0:0a940ef5a5b2 36 mpu.setGyroRange(1);//gyro rate is +-500degree per second(dps)
Joeatsumi 0:0a940ef5a5b2 37
Joeatsumi 0:0a940ef5a5b2 38 while(1) {
Joeatsumi 0:0a940ef5a5b2 39
Joeatsumi 0:0a940ef5a5b2 40 //Aquisition sensor values
Joeatsumi 0:0a940ef5a5b2 41 mpu.getAccelero(acc);//get acceleration (Accelerometer)
Joeatsumi 0:0a940ef5a5b2 42 //x_axis acc[0]
Joeatsumi 0:0a940ef5a5b2 43 //y_axis acc[1]
Joeatsumi 0:0a940ef5a5b2 44 //z_axis acc[2]
Joeatsumi 0:0a940ef5a5b2 45 mpu.getGyro(gyro); //get rate of angle(Gyro)
Joeatsumi 0:0a940ef5a5b2 46 //x_axis gyro[0]
Joeatsumi 0:0a940ef5a5b2 47 //y_axis gyro[1]
Joeatsumi 0:0a940ef5a5b2 48 //z_axis gyro[2]
Joeatsumi 0:0a940ef5a5b2 49
Joeatsumi 0:0a940ef5a5b2 50 //Invertion for direction of Accelerometer axis
Joeatsumi 0:0a940ef5a5b2 51 acc[0]*=(-1.0);
Joeatsumi 0:0a940ef5a5b2 52 acc[2]*=(-1.0);
Joeatsumi 0:0a940ef5a5b2 53
Joeatsumi 0:0a940ef5a5b2 54 //Unit convertion of rate of angle(radian to degree)
Joeatsumi 0:0a940ef5a5b2 55 gyro[0]*=RAD_TO_DEG;
Joeatsumi 0:0a940ef5a5b2 56 gyro[0]*=(-1.0);
Joeatsumi 0:0a940ef5a5b2 57
Joeatsumi 0:0a940ef5a5b2 58 gyro[1]*=RAD_TO_DEG;
Joeatsumi 0:0a940ef5a5b2 59
Joeatsumi 0:0a940ef5a5b2 60 gyro[2]*=RAD_TO_DEG;
Joeatsumi 0:0a940ef5a5b2 61 gyro[2]*=(-1.0);
Joeatsumi 0:0a940ef5a5b2 62
Joeatsumi 0:0a940ef5a5b2 63 //Display inertial values
Joeatsumi 0:0a940ef5a5b2 64 pc.printf("a_x=%f,a_y=%f,a_z=%f,g_x=%f,g_y=%f,g_z=%f\r\n"
Joeatsumi 0:0a940ef5a5b2 65 ,acc[0],acc[1],acc[2],gyro[0],gyro[1],gyro[2]);
Joeatsumi 0:0a940ef5a5b2 66
Joeatsumi 0:0a940ef5a5b2 67 wait(0.01);
Joeatsumi 0:0a940ef5a5b2 68 }
Joeatsumi 0:0a940ef5a5b2 69 }