201903_ISE
Dependencies: mbed Madgwick MPU6050 Kalman BMP180
Diff: main.cpp
- Revision:
- 6:f17310205c1f
- Parent:
- 5:f6e956e8a060
- Child:
- 7:9953d922499d
--- a/main.cpp Sun Dec 30 13:28:35 2018 +0000 +++ b/main.cpp Tue Feb 12 11:20:33 2019 +0000 @@ -104,6 +104,9 @@ } break; case FIRE: + + //カルマン + /* gPrevMicros = timer_open.read(); mpu.getAccelero(acc); mpu.getGyro(gyro); @@ -125,6 +128,25 @@ if(degY>ANGLE_JUDGE_FIRE){ Phase = OPEN; } + */ + + //madgwick + float Roll,Pitch,Yaw; + MadgwickFilter.begin(2); + mpu.getAccelero(acc); + mpu.getGyro(gyro); + gyro[0] *= RadToDeg; + gyro[1] *= RadToDeg; + gyro[2] *= RadToDeg; + + MadgwickFilter.updateIMU(gyro[0],gyro[1],gyro[2],acc[0],acc[1],acc[2]); + Roll = MadgwickFilter.getRoll(); + Pitch = MadgwickFilter.getPitch(); + Yaw = MadgwickFilter.getYaw(); + if(Pitch>ANGLE_JUDGE_FIRE){ + Phase = OPEN; + } + break; case OPEN: for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){