Diff: main.cpp

Revision:

5:bb9c685fc1fe

Parent:

4:44e13dd5250b

Child:

6:e9004c78b394

--- a/main.cpp	Tue Feb 20 03:17:19 2018 +0000
+++ b/main.cpp	Tue Mar 06 06:56:28 2018 +0000
@@ -1,35 +1,46 @@
 #include "mbed.h"
+#include "SDFileSystem.h"
+#include "MPU6050.h"
+#include "math.h"
 #include "BMP180.h"
-#include "MPU6050.h"
-#include "SDFileSystem.h"
 
 #define UNLOCK 1
 #define LOCK 0
 #define TIMER 30 //開放タイマー
+#define RATE 50//判定周期
  
 SDFileSystem    sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
 BMP180          bmp(p28,p27);
 MPU6050         mpu(p28,p27);
-DigitalIn       fly(p14);
+DigitalIn       fly(p12);
+DigitalOut      myled(p20);
+DigitalOut      myled2(p19);
+DigitalOut      myled3(p18);
+DigitalOut      myled4(LED1);
+DigitalOut      myled5(LED2);
+DigitalOut      myled6(LED3);
+DigitalOut      myled7(LED4);
 Serial          twe(p9,p10);
 Serial          gps(p13,p14);
 Serial          pc(USBTX,USBRX);
-PwmOut          servo_para(p21);
+PwmOut          servo_para(p23);
 
 LocalFileSystem local("local");
 FILE *fp;
 FILE *lfp;
 
 Ticker kaihou;
+Ticker ochiru;
 Timer timer1;
+
 /*とりあえず全てグローバル変数化してます。*/
 //カウント変数
 int Cnt_buff = 0;
 int Cnt_para = 0;
 int Cnt_open = 0;
-int i, j, t;
+int i, j, t, t1;
 //高度取得
-float Alt_buff[10],Alt_gnd,Alt_now, Max_Alt;
+float Alt_buff[256],Alt_buff2[10],Alt_gnd,Alt_now, Max_Alt;
 float p0 = 1013.25;             //海面気圧
 float pressure,temperature, Alt;
 //位置情報取得
@@ -41,40 +52,42 @@
 float d[3];
 
 bool tf_para = true;
- 
+
 float median(float data[], int num);    //中央値求める
 float get_Alt(float press, float temp);
 void _open(void);                        //kaihou
 float _DMS2DEG(float raw_data);         //GPSデータ60進数→10進数
 void _para(int motion);
-
+void _recovery();
+ 
 int main() {
+    twe.baud(115200);
+    twe.printf("Hello World!\r\n");
     
-    twe.printf("Hello World!\r\n");   
-/*SDカード動作確認*/ 
-    mkdir("/sd/mydir", 0777);
-    FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
-    if(fp == NULL) {
-        error("Could not open file for write\n");
-    }
-    fprintf(fp, "Hello fun SD Card World!");
-    fclose(fp);
+    _para(LOCK);
     
-/*I2C初期化*/    
-    bmp.Initialize(60,BMP180_OSS_ULTRA_LOW_POWER); 
     mpu.setAcceleroRange(0);
     mpu.setGyroRange(0);
+    bmp.Initialize(60,BMP180_OSS_ULTRA_LOW_POWER);
     twe.printf("I2C_initialize_ok\r\n");
+    
 
-/*地上高度取得*/    
+//地上高度取得
+    Cnt_buff = 0;
     for(i=0; i<10; i++){
         bmp.ReadData(&temperature,&pressure);
         Alt_buff[i]=get_Alt(pressure, temperature);
     }
-    
     Alt_gnd = median(Alt_buff, 10);
     
-    fp = fopen("/sd/mydir/sdtest.txt", "w");
+    twe.printf("Alt_gnd:%f\r\n", Alt_gnd);
+    
+    lfp = fopen("/local/Alt.txt","a");
+    fprintf(lfp, "GND:%f\r\n",Alt_gnd);
+    fclose(lfp);
+    twe.printf("lacal_ok\r\n");
+    
+    fp = fopen("/sd/mydir/sdtest.txt", "a");
     if(fp == NULL) {
         error("Could not open file for write\n");
     }
@@ -83,21 +96,29 @@
     }
     
     fprintf(fp, "Alt_gnd:%f\r\n",Alt_gnd); 
-    fclose(fp);   
+    fclose(fp); 
     
-    twe.printf("Alt_gnd:%f\r\n",Alt_gnd); 
-    twe.printf("Standby ok!\r\n");
+    twe.printf("SD_write_OK!!\r\n");
+    
+    twe.printf("ALL_READY\r\n");
     
     while(fly == 1);        //フライトピン抜けるまで待機
     
+    i = 0;
+    Alt_buff[0] = 0;
+    fp = fopen("/sd/mydir/sdtest.txt", "a");
+    if(fp == NULL)twe.printf("ERROR\r\n");
     timer1.start();
-    kaihou.attach(_open, 0.05); 
+    kaihou.attach(_open,1/RATE); 
+    
     
     while(1){
 
- 
+/*        twe.printf("MAX:%f,Cnt:%d\r\n",Max_Alt, Cnt_para);
+        wait(1.0); 
+*/        
         /*  GPS取得&送信    */
-        gps_data[cnt_gps] = gps.getc();
+       gps_data[cnt_gps] = gps.getc();
         if(gps_data[cnt_gps] == '$' || cnt_gps ==256){
             cnt_gps = 0;
             memset(gps_data,'\0',256);
@@ -116,68 +137,69 @@
 //                    pc.printf("max altitude:%f\r\n",Max_Alt);
                     twe.printf("%s\r\n",gps_data);
                 }
-                twe.printf("MAX:%f,%d\r\n",Max_Alt);
+                twe.printf("MAX:%f,Cnt:%d\r\n",Max_Alt,Cnt_para);
             }
         }else{
             cnt_gps++;
         }
         
+    }//while_gps    
+}//main
+
+void _open(){           
+    bmp.ReadData(&temperature,&pressure);
+    mpu.getAccelero(a);
+    mpu.getGyro(d);
+    Alt_buff2[i] = get_Alt(pressure, temperature);
+    i++;
+    if(i == 10){
+        Alt_now = median(Alt_buff2, 10);
+        Alt_now = Alt_now - Alt_gnd;
+        t = timer1.read();
+        fprintf(fp, "%f,%f,%f,%f,%f,%f,%f,%d,%d\r\n",Alt_now,a[0],a[1],a[2],d[0],d[1],d[2],t,Cnt_para); 
+        if(Alt_now > Max_Alt) Max_Alt = Alt_now;
+        i = 0;
+        if(tf_para == true){
+            Alt_buff[Cnt_buff+1] = Alt_now; 
+            if(Alt_buff[Cnt_buff]>Alt_buff[Cnt_buff+1]) Cnt_para++; //直前の値より小さければカウント+1
+//            twe.printf("Cnt_para:%d\r\n", Cnt_para); 
+            Cnt_buff++;                   
+            if(Cnt_para == 10 || t > TIMER){
+                kaihou.detach();
+                fprintf(fp,"OPEN!\r\n");
+                fclose(fp);        
+                _para(UNLOCK);
+                tf_para = false;
+                timer1.stop();
+                twe.printf("PARA_OPEN\r\n");
+                ochiru.attach(_recovery,0.1);
+                
+            }
+        }
     }
-    
-    
+                   
 }
 
-void _open(){
+void _recovery(){
     
-    if(Cnt_open == 0){  //20回に一回保存（20回に1回openとclose）
+    if(Cnt_buff == 0){
         fp = fopen("/sd/mydir/sdtest.txt", "a");
         if(fp == NULL)twe.printf("ERROR\r\n");
-    }        
-       
-    bmp.ReadData(&temperature,&pressure);
-    mpu.getAccelero(a); 
-    mpu.getGyro(d); 
-    
-    Alt_now = get_Alt(pressure, temperature);
-    
-    Alt_now = Alt_now - Alt_gnd;
-    
-    if(Alt_now > Max_Alt){
-        Max_Alt = Alt_now ;
     }
     
-    if(tf_para == true){    //パラ開く前は頂点判定する
-        Alt_buff[Cnt_buff+1] = Alt_now; 
-        if(Alt_buff[Cnt_buff]>Alt_buff[Cnt_buff+1]) Cnt_para++; //直前の値より小さければカウント+1
-        twe.printf("Cnt_para:%d\r\n", Cnt_para);
-    }
+    bmp.ReadData(&temperature,&pressure);
+    mpu.getAccelero(a);
+    mpu.getGyro(d);
+    Alt_now = get_Alt(pressure, temperature);
+    Alt_now = Alt_now - Alt_gnd;
+    fprintf(fp, "%f,%f,%f,%f,%f,%f,%f\r\n",Alt_now,a[0],a[1],a[2],d[0],d[1],d[2]);
+    Cnt_buff++;
     
-    if(Cnt_buff == 10 && tf_para == true){
-        t = timer1.read();                       
-        if(Cnt_para>=7 || t > TIMER){        //10回中7回小さけれ落下と判断（最初はぜってぇ大きいから実質7/9）
-            _para(UNLOCK);
-            tf_para = false;
-            timer1.stop();
-            twe.printf("PARA_OPEN\r\n");
-        }
+    if(Cnt_buff == 50){
+        fclose(fp);
         Cnt_buff = 0;
-        Cnt_para = 0;
     }
-    
-
-    fprintf(fp, "%f,%f,%f,%f,%f,%f,%f\r\n",Alt_now,a[0],a[1],a[2],d[0],d[1],d[2]); 
-    twe.printf("%f,%f,%f,%f,%f,%f,%f\r\n",Alt_now,a[0],a[1],a[2],d[0],d[1],d[2]);   //todo要らん場合は適宜コメントアウト
-    Cnt_buff++;
-    Cnt_open++;
-      
-    if(Cnt_open == 20){
-        fclose(fp);
-        
-        Cnt_open = 0;
-    }    
-
 }
-
 float get_Alt(float press, float temp){
     return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
 }