Revision 9:42b4d337d4cc, committed 2019-03-09

Comitter:

sashida_h

Date:

Sat Mar 09 12:23:07 2019 +0000

Parent:

8:15a1b22df82f

Child:

10:1a626929850e

Commit message:

2019_0309;

Changed in this revision

--- a/main.cpp	Thu Mar 07 12:13:33 2019 +0000
+++ b/main.cpp	Sat Mar 09 12:23:07 2019 +0000
@@ -28,6 +28,7 @@
 Madgwick MadgwickFilter;
 Serial pc(PA_2, PA_3);
 Serial gps(PA_9, PA_10);
+DigitalOut myled(PA_15);
 Timer timer_open;
 Timer timer_data;
 Ticker tic_data;
@@ -39,6 +40,7 @@
 void  _SendData();
 void  _SendGPS();
 float _DMS2DEG(float raw_data);
+int servo();
 
 enum PHASE{STANDBY=0,LAUNCH=1,RISE=3,FIRE=7,OPEN=15,RECOVERY=9,SEA=6} Phase;
 
@@ -46,21 +48,28 @@
 float t = 0;
 //地上高度
 float alt_gnd;
+float alt_max;
 //GPS
 int cnt_gps=0;
 
 int p = 1;
+//サーボ
+int i = 0;
+char c[516];
+char d [8];
+int len;
+char f[]="Receive";
 
-int main(){
+void main(){
     /*ローカル変数*/
     float acc[3],acc_buff[10],gyro[3],gyro_buff[10],acc_abs;
-    float alt_buff[10],alt_md,alt_max=0;
+    float alt_buff[10],alt_md;
     float time_judge;
     int cnt_data=0,cnt_judge=0;
-    int i=0;
-
+    char gps_data[256];
     /*センサの初期化等*/
     pc.baud(38400);
+    gps.baud(115200);
     mpu.setAcceleroRange(3);
     bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);
 
@@ -77,20 +86,21 @@
     pc.printf("Hello World!\r\n");
     wait(1.0);
     pc.printf("f:Flight_mode_on\r\n");
+    gps.printf("f:Flight_mode_on\r\n");
     wait(1.0);
-    
+    timer_data.start();
     while(1){
         switch(Phase){
             case STANDBY:
+                //gps.printf("Phase_STANDBY\r\n");
                 /*入力待ち*/
                 //tic_data.attach(&_SendData, 1.0/RATE_DATA);
-                char c = pc.getc(); 
-                if(c=='f'){
-                    pc.printf("Flight_mode\r\n");
-                    timer_data.start();
-                    Phase = LAUNCH;
-                }
-                wait(1.0);
+                servo();
+                //c[i] = pc.getc();
+                //gps.printf("%c",c[i]);
+                //i++;
+                //if(c[i] == '\n') i=0;
+                //wait(1.0);
                 break;
                 
             case LAUNCH:
@@ -204,11 +214,11 @@
                 //if((timer_open.read()-time_judge) - TIME_JUDGE_CNT > 0) cnt_judge=0;
                 if(cnt_judge == CNT_JUDGE || timer_open.read() > TIME_OPEN){
                     Phase = RECOVERY;
+                    tic_gps.attach(&_SendGPS, 1.0/RATE_GPS); 
                 }
                 break;
             case RECOVERY:
-                char gps_data[256];
-                while(1){
+                /*while(1){
                     if(gps.readable()){
                         gps_data[cnt_gps] = gps.getc();
                         if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
@@ -233,7 +243,7 @@
                                     break;
                                 }
                             }else{
-                                //ffpc.printf("No_Satellite_signal\r\n");
+                                //pc.printf("No_Satellite_signal\r\n");
                             }
                         }else{
                             cnt_gps++;
@@ -241,15 +251,9 @@
                 }
     }
  
-                Phase = SEA;
+                //Phase = SEA;*/
                 break;
             case SEA:
-                while(1){
-                    if(gps.readable()){
-                        c = gps.getc();
-                        pc.printf("%c",c);
-                    }
-                }
                     
                 break;
         }
@@ -306,6 +310,73 @@
     pc.printf("%d,%f,%f,%f\r\n",Phase,alt,Roll,Pitch);
 }
 
+float _DMS2DEG(float raw_data){
+    int d=(int)(raw_data/100);
+    float m=(raw_data-(float)d*100);
+    return (float)d+m/60;
+}
+
+
+float _median(float data[], int num){
+    float *data_cpy, ans;
+    data_cpy = new float[num];
+    memcpy(data_cpy,data,sizeof(float)*num);
+ 
+    for(i=0; i<num; i++){
+        for(int j=0; j<num-i-1; j++){
+            if(data_cpy[j]>data_cpy[j+1]){
+                float buff = data_cpy[j+1];
+                data_cpy[j+1] = data_cpy[j];
+                data_cpy[j] = buff;
+            }
+        }
+    }
+    
+    if(num%2!=0) ans = data_cpy[num/2];
+    else         ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
+    delete[] data_cpy;
+    return ans;
+}
+
+int servo(void){
+    do{
+        c[i] = pc.getc();
+        gps.printf("%c",c[i]);
+        i++;
+    }while(c[i-1] != '\n');
+    gps.printf("path\r\n");
+    //gps.printf("%s",c);
+    myled = 1;
+    //gps.printf("%s",c);
+    len = strlen(f);
+    i=0;
+    while(i != 4){
+        if (c[i] != f[i]) break;
+        i++;
+    }
+    if(i > 3){
+        i=0;
+        do{
+            d[i]=c[13+i];
+            i++;
+        }while(d[i-1] != ')');
+        d[i-1] = '\0';
+        gps.printf("d:%s",d);
+        i = 0;
+        if(d[0] == 'o') myled = 1;
+        if(d[0] == 'l') myled = 0;
+        if(d[0] == 'f'){
+        //pc.printf("flight_mode\r\n");
+        Phase = LAUNCH;
+        }
+    }
+        
+    i=0;
+    memset(c,'\0',64);
+    memset(d,'\0',8);
+    //pc.printf("2\r\n");
+    return 0;
+}
 
 void _SendGPS(){
     char gps_data[256];
@@ -326,7 +397,7 @@
                         //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,ntime:%d\r\n",lat_north,lon_east,world_time);
+                        pc.printf("Lat:%f,Lon:%f,MAX_ALT:%f\r\n",lat_north,lon_east,alt_max);
                         break;
                     }else{
                         //pc.printf("%s\r\n",gps_data);
@@ -341,33 +412,5 @@
             }
         }
     }
-
-}
-
-float _DMS2DEG(float raw_data){
-    int d=(int)(raw_data/100);
-    float m=(raw_data-(float)d*100);
-    return (float)d+m/60;
-}
-
-
-float _median(float data[], int num){
-    float *data_cpy, ans;
-    data_cpy = new float[num];
-    memcpy(data_cpy,data,sizeof(float)*num);
  
-    for(int i=0; i<num; i++){
-        for(int j=0; j<num-i-1; j++){
-            if(data_cpy[j]>data_cpy[j+1]){
-                float buff = data_cpy[j+1];
-                data_cpy[j+1] = data_cpy[j];
-                data_cpy[j] = buff;
-            }
-        }
-    }
-    
-    if(num%2!=0) ans = data_cpy[num/2];
-    else         ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0;
-    delete[] data_cpy;
-    return ans;
 }
\ No newline at end of file