201903_ISE
Dependencies: mbed Madgwick MPU6050 Kalman BMP180
Revision 6:f17310205c1f, committed 2019-02-12
- Comitter:
- Yukina
- Date:
- Tue Feb 12 11:20:33 2019 +0000
- Parent:
- 5:f6e956e8a060
- Child:
- 7:9953d922499d
- Commit message:
- add madgwick
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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++){