Diff: main.cpp

Revision:

8:15a1b22df82f

Parent:

7:9953d922499d

Child:

9:42b4d337d4cc

--- a/main.cpp	Mon Mar 04 02:50:16 2019 +0000
+++ b/main.cpp	Thu Mar 07 12:13:33 2019 +0000
@@ -5,13 +5,15 @@
 #include "MadgwickAHRS.h"
 
 /*しきい値など*/
-#define ACC_JUDGE_LAUNCH    3.0
-#define TIME_BURNING        6
+#define ACC_JUDGE_LAUNCH    3.0 //発射判定のしきい値
+#define TIME_BURNING        6   //開放判定しない時間（燃焼時間）
 #define ALT_JUDGE_FIRE      0
-#define ALT_JUDGE_OPEN      1
+#define ALT_JUDGE_OPEN      1   //落下判定のカウントを１増やす高度差
+#define TIME_OPEN           25  //強制的に開放させる時間
+#define TIME_SEND           1.0
 #define ANGLE_JUDGE_FIRE_MIN    15
 #define ANGLE_JUDGE_FIRE_MAX    70
-#define CNT_JUDGE           10
+#define CNT_JUDGE           10 
 #define TIME_JUDGE_CNT      1.5
 #define NUM_CNT_MEDIAN      10
 #define RATE_GPS            1.0
@@ -41,11 +43,7 @@
 enum PHASE{STANDBY=0,LAUNCH=1,RISE=3,FIRE=7,OPEN=15,RECOVERY=9,SEA=6} Phase;
 
 /*グローバル変数*/
-//カルマン用
-float gCalibrateX;
-float gCalibrateY;
-float gPrevMicros; 
-float degRoll,degPitch;
+float t = 0;
 //地上高度
 float alt_gnd;
 //GPS
@@ -69,12 +67,6 @@
     /*初期位置の設定*/
     mpu.getAccelero(acc);
     mpu.getGyro(gyro);
-    degRoll  = atan2(acc[1], acc[2]) * RadToDeg;
-    degPitch = atan(-acc[0] / sqrt(acc[1] * acc[1] + acc[2] * acc[2])) * RadToDeg;
-    gKfx.setAngle(degRoll);
-    gKfy.setAngle(degPitch);
-    gCalibrateY = degPitch;
-    gCalibrateX = degRoll;
 
     Phase = STANDBY;
     for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
@@ -83,96 +75,54 @@
     alt_gnd = _median(alt_buff,NUM_CNT_MEDIAN);
     wait(2.0);
     pc.printf("Hello World!\r\n");
-    timer_data.start();
-    int r = 0;
-    float r_time;
-    pc.printf("TimerStart,Standby_mode\r\n");
-
+    wait(1.0);
+    pc.printf("f:Flight_mode_on\r\n");
+    wait(1.0);
+    
     while(1){
         switch(Phase){
             case STANDBY:
                 /*入力待ち*/
-                timer_data.start();
                 //tic_data.attach(&_SendData, 1.0/RATE_DATA);
-                if(r == 0) r_time = timer_data.read() / 60.0;
-                if(r == 1) r_time = (timer_data.read() + 1800.0)/60;
-                if(r == 2) r_time = (timer_data.read() + 3600.0)/60;
-                if(r == 3) r_time = (timer_data.read() + 5400.0)/60;
-                if(r == 4){
-                    timer_data.reset();
-                    Phase = LAUNCH;
-                    pc.printf("Flight_mode\r\n");
-                }
-                pc.printf("TIME:%3f[min]",r_time);
-                if(timer_data.read() > 1800){
-                    timer_data.reset();
-                    r++;
-                }
-                /*char c = pc.getc(); 
+                char c = pc.getc(); 
                 if(c=='f'){
                     pc.printf("Flight_mode\r\n");
+                    timer_data.start();
                     Phase = LAUNCH;
                 }
-                */
                 wait(1.0);
                 break;
+                
             case LAUNCH:
                 for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
                     mpu.getAccelero(acc);
                     acc_buff[cnt_data] = sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0));
                 }
                 acc_abs = _median(acc_buff,NUM_CNT_MEDIAN);
-                if(i==50){
-                    pc.printf("LAUNCH,acc:%f,time:%f\r\n",acc_abs,timer_data.read());
-                    i=0;
+                if(timer_data.read() - t > TIME_SEND){
+                    pc.printf("LAUNCH,acc:%f,time:%3f,cnt:%d\r\n",acc_abs,timer_data.read(),cnt_judge);
+                    t = timer_data.read();
                 }
-                i++;
                 /*加速度判定*/
                 if(acc_abs>ACC_JUDGE_LAUNCH){
                     cnt_judge++;
                 }
-                if(cnt_judge==CNT_JUDGE || timer_data.read()>30.0){
+                if(cnt_judge==CNT_JUDGE){
                     cnt_judge=0;
                     timer_open.start();
                     Phase = RISE;
-                    pc.printf("LAUNCH!!!!\r\n");
                 }
                 break;
+                
             case RISE:
                 while(timer_open.read() < TIME_BURNING){
-                    pc.printf("RISE,time:%f\r\n",timer_open.read());
-                    wait(0.5);
+                    pc.printf("RISE,time from launch:%f\r\n",timer_open.read());
+                    wait(1.0);
                     i=0;
-                }
-                if(p == 1){
-                    pc.printf("FIRE_JUDGE_START\r\n");
-                    p=0;
-                }
-                for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
-                    alt_buff[cnt_data] = _getAlt();
+                    timer_data.reset();
                 }
-                alt_md = _median(alt_buff,NUM_CNT_MEDIAN);
-                alt_md = alt_md - alt_gnd;
-                if(alt_md > ALT_JUDGE_FIRE){
-                    Phase = FIRE;
-                    if(i == 5){
-                        //pc.printf("Time:%f,RISE,alt:%f\r\n",timer_open.read(),alt_md);
-                        pc.printf("Time:%f,RISE,alt:%f\r\n",timer_data.read(),alt_md);
-                        i = 0;
-                    }
-                }else{
-                    if(i == 5){
-                        //pc.printf("Time:%f,RISE,alt:%f\r\n",timer_open.read(),alt_md);
-                        pc.printf("Time:%f,RISE,alt:%f\r\n",timer_data.read(),alt_md);
-                        i = 0;
-                    }
-                }
-                i++;
-                //if(timer_open.read()>TIMER_NOTFIRE){
-                if(timer_open.read()>60.0){
-                    Phase = OPEN;
-                    //pc.printf("NOT_FIRE!\r\n");
-                }
+                Phase = OPEN;
+                t = 0.0;
                 break;
             case FIRE:
 
@@ -238,37 +188,62 @@
                 }
                 alt_md = _median(alt_buff,NUM_CNT_MEDIAN);
                 alt_md = alt_md - alt_gnd;
-                if(i==10){
-                    pc.printf("OPEN,Cnt:%d,ALT_NOW:%f,ALT_MAX%f\r\n",cnt_judge,alt_md,alt_max);
-                    i=0;
+                if(timer_open.read() - t > TIME_SEND){
+                    pc.printf("OPEN,alt:%f,time:%3f,cnt:%d\r\n",alt_md,timer_open.read(),cnt_judge);
+                    t = timer_open.read();
                 }
-                i++;
                 if(alt_md > alt_max){
                     alt_max = alt_md;
                     cnt_judge = 0;
                 }
                 else if((alt_max-alt_md) > ALT_JUDGE_OPEN){
                     cnt_judge++;
-                    time_judge = timer_open.read();
+                    //time_judge = timer_open.read();
                 }
 
                 //if((timer_open.read()-time_judge) - TIME_JUDGE_CNT > 0) cnt_judge=0;
-                //if(cnt_judge == CNT_JUDGE){
-                if(timer_data.read() > 70.0){
-                    cnt_judge = 0;
+                if(cnt_judge == CNT_JUDGE || timer_open.read() > TIME_OPEN){
                     Phase = RECOVERY;
-                    pc.printf("Fall!!!\r\n");
-                    wait(1.0);
-                    
                 }
                 break;
             case RECOVERY:
-                //tic_data.detach();
-                //tic_gps.attach(&_SendGPS, 1.0/RATE_GPS); 
+                char gps_data[256];
+                while(1){
+                    if(gps.readable()){
+                        gps_data[cnt_gps] = gps.getc();
+                        if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
+                            cnt_gps = 0;
+                            memset(gps_data,'\0',256);
+                        }else if(gps_data[cnt_gps] == '\r'){
+                            float world_time, lon_east, lat_north;
+                            int rlock, sat_num;
+                            char lat,lon;
+                            if(sscanf(gps_data,"GPGGA,%f,%f,%c,%f,%c,%d,%d",&world_time,&lat_north,&lat,&lon_east,&lon,&rlock,&sat_num)>=1){
+                                if(rlock==1){
+                                    lat_north = _DMS2DEG(lat_north);
+                                    lon_east = _DMS2DEG(lon_east);
+                                    //pc.printf("%s\r\n",gps_data);
+                                    //pc.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
+                                    int japan_time = int(world_time) - 9;
+                                    pc.printf("Lat:%f,Lon:%f,time:%d,max_alt:%f\r\n",lat_north,lon_east,world_time,alt_max);
+                                    break;
+                                }else{
+                                    //pc.printf("%s\r\n",gps_data);
+                                    pc.printf("NoGPSSignal,max_alt:%f\r\n",alt_max);
+                                    break;
+                                }
+                            }else{
+                                //ffpc.printf("No_Satellite_signal\r\n");
+                            }
+                        }else{
+                            cnt_gps++;
+                        }
+                }
+    }
+ 
                 Phase = SEA;
                 break;
             case SEA:
-                char c;
                 while(1){
                     if(gps.readable()){
                         c = gps.getc();