CORE / Mbed 2 deprecated 201903_pf_lower_judgment

201903_14ISEで実際に使用した開放用プログラム． 使用マイコンがNUCLES-F303K8なので注意

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++){