Diff: main.cpp

Revision:

2:0deade364b73

Parent:

0:bdbd3d6fc5d5

Child:

3:a7b39e55d100

--- a/main.cpp	Tue May 16 05:18:55 2017 +0000
+++ b/main.cpp	Tue Feb 20 03:02:02 2018 +0000
@@ -1,19 +1,217 @@
 #include "mbed.h"
+#include "BMP180.h"
+#include "MPU6050.h"
 #include "SDFileSystem.h"
- 
-SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
+
+#define UNLOCK 1
+#define LOCK 0
+#define TIMER 30
  
-int main() {
-    printf("Hello World!\n");   
+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(p20);
+Serial          twe(p9,p10);
+Serial          gps(p13,p14);
+Serial          pc(USBTX,USBRX);
+PwmOut          servo_para(p21);
+
+LocalFileSystem local("local");
+FILE *fp;
+FILE *lfp;
+
+Ticker kaihou;
+Timer timer1;
+
+//カウント変数
+int Cnt_buff = 0;
+int Cnt_para = 0;
+int Cnt_open = 0;
+int i, j, t;
+//高度取得
+float Alt_buff[10],Alt_gnd,Alt_now, Max_Alt;
+float p0 = 1013.25;             //海面気圧
+float pressure,temperature, Alt;
+//位置情報取得
+char gps_data[256];
+int cnt_gps =0;
+//加速度取得
+float a_abs;
+float a[3];
+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);
+
+int main() {
+    
+    twe.printf("Hello World!\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); 
+    fclose(fp);
+    
+    bmp.Initialize(60,BMP180_OSS_ULTRA_LOW_POWER); 
+    mpu.setAcceleroRange(0);
+    mpu.setGyroRange(0);
+    twe.printf("I2C_initialize_ok\r\n");
+    
+    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");
+    if(fp == NULL) {
+        error("Could not open file for write\n");
+    }
+    for(i=0; i<10; i++){
+        fprintf(fp, "%d:%f\r\n",i+1,Alt_buff[i]);
+    }
+    
+    fprintf(fp, "Alt_gnd:%f\r\n",Alt_gnd); 
+    fclose(fp);   
+    
+    twe.printf("Alt_gnd:%f\r\n",Alt_gnd); 
+    twe.printf("Standby ok!\r\n");
+    
+    while(fly == 1);        //フライトピン抜けるまで待機
+    
+    timer1.start();
+    kaihou.attach(_open, 0.05); 
+    
+    while(1){
+
  
-    printf("Goodbye World!\n");
+        /*  GPS取得&送信    */
+        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("phase:%d,max altitude:%f\r\n",Phase,Max_Alt);
+//                    twe.printf("%s\r\n",gps_data);
+                    twe.printf("Lat,Lon:%f,%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
+                }else{
+//                    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);
+            }
+        }else{
+            cnt_gps++;
+        }
+        
+    }
+    
+    
 }
+
+void _open(){
+    
+    if(Cnt_open == 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);
+    }
+    
+    if(Cnt_buff == 10 && tf_para == true){
+        t = timer1.read();                       
+        if(Cnt_para>=7 || t > TIMER){        //10回中6回小さけれ落下と判断（最初はぜってぇ大きいから実質6/9）
+            _para(UNLOCK);
+            tf_para = false;
+            timer1.stop();
+            twe.printf("PARA_OPEN\r\n");
+        }
+        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]);
+    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;
+}
+
+float median(float data[], int num){//todo:処理時間計測
+    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;
+}
+
+
+float _DMS2DEG(float raw_data){
+    int d = (int)(raw_data/100);
+    float m = (raw_data - (float)d*100);
+    return (float)d + m/60;
+}
+
+void _para(int motion){
+    if(motion==UNLOCK){
+            servo_para.pulsewidth(0.0005); // pulse servo out sita
+    }else if(motion==LOCK){
+            servo_para.pulsewidth(0.0025); // pulse servo outu sita     
+    }
+}
\ No newline at end of file