ドローン用計測制御基板の作り方 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?
User | Revision | Line number | New 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 | } |