Diff: main.cpp

Revision:

2:0cf17bba70ec

Parent:

0:bdbd3d6fc5d5

Child:

3:71a45bae8a37

--- a/main.cpp	Tue May 16 05:18:55 2017 +0000
+++ b/main.cpp	Fri Oct 20 03:53:03 2017 +0000
@@ -1,19 +1,164 @@
 #include "mbed.h"
+#include "math.h"
+#include "MPU6050.h"
+#include "BMP180.h"
 #include "SDFileSystem.h"
- 
-SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
- 
+
+#define RATE                20
+#define MAX_JUDGE_TIME_s    3.0
+#define p0                  1013.25f
+
+BMP180          bmp(p28,p27);
+MPU6050         mpu(p28,p27);
+Timer           timer;
+Timer           alt_timer;
+Ticker          logtimer;
+Serial          pc(USBTX,USBRX);
+Serial          gps(p13,p14);
+Serial          twe(p9,p10);
+SDFileSystem    sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
+LocalFileSystem local("local");
+FILE            *fp;
+
+/*  自作関数    */
+void    _flight();
+void    _log();
+float   _getAlt(float press, float temp);
+float   _DMS2DEG(float raw_data);
+float   _median(float data[], int num);
+
+/*  グローバル変数 */
+float   a[3];
+float   Land_Alt;
+float   Max_Alt=0;
+float   Alt_buff[10];
+float   Pressure,Temperature,Altitude;
+float   Time;
+char    gps_data[256];
+int     cnt_gps;
+int     Log_cnt=0;
+
+
 int main() {
-    printf("Hello World!\n");   
- 
+    printf("Hello World!\n");
+    twe.baud(115200); 
+    mpu.setAcceleroRange(0);
+    bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER);  
     mkdir("/sd/mydir", 0777);
+
+    _flight();
+}
+
+
+/*  フライトモード用関数  */
+void _flight(){
+
+    /*  地上高度取得  */
+    for(int i=0;i<10;i++){
+        bmp.ReadData(&Temperature,&Pressure);
+        Alt_buff[i] = _getAlt(Pressure,Temperature);
+    }
+    Land_Alt = _median(Alt_buff,10);
+    FILE *lfp = fopen("/local/alt.txt","a");
+    fprintf(lfp,"地上高度：%f\r\n",Land_Alt);
+    fclose(lfp);
+    for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]);
     
-    FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
-    if(fp == NULL) {
-        error("Could not open file for write\n");
+    /*  データ取得開始 */
+    timer.start();
+    logtimer.attach(_log,1.0/RATE);
+    alt_timer.start();
+
+    /*  GPS取得&送信    */
+    while(1){
+        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);
+                    twe.printf("max altitude:%f\r\n",Max_Alt);
+                    twe.printf("%s\r\n",gps_data);
+                    twe.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num);
+                }else{
+                    twe.printf("max altitude:%f\r\n",Max_Alt);
+                    twe.printf("%s\r\n",gps_data);
+                }
+            }
+        }else{
+            cnt_gps++;
+        }
+    }
+}
+
+
+/*  データ取得&保存関数  */
+void _log(){
+    Time = timer.read();
+    mpu.getAccelero(a);
+    bmp.ReadData(&Temperature,&Pressure);
+    Altitude = _getAlt(Pressure,Temperature);
+    if(Altitude > Max_Alt){
+        Max_Alt = Altitude;
+        alt_timer.reset();
     }
-    fprintf(fp, "Hello fun SD Card World!");
-    fclose(fp); 
- 
-    printf("Goodbye World!\n");
+    if(alt_timer.read() > MAX_JUDGE_TIME_s){
+        alt_timer.reset();
+        alt_timer.stop();
+        FILE *lfp = fopen("/local/alt.txt","a");
+        fprintf(lfp,"最高高度：%f\r\n",Max_Alt);
+        fclose(lfp);
+    }
+    if(Log_cnt==0) {
+        fp = fopen("/sd/mydir/sdtest.txt", "a");
+    }
+    fprintf(fp, "%f, %f, %f, %f, %f, %f, %f \r\n",Time, Temperature, Pressure, Altitude, a[0],a[1],a[2]);
+    Log_cnt++;
+    if(Log_cnt==RATE){
+        fclose(fp);
+        Log_cnt = 0;
+    }
+    pc.printf("%f, %f, %f, %f, %f, %f, %f \r\n",Time, Temperature, Pressure, Altitude, a[0],a[1],a[2]);
+}
+
+
+/*  高度計算関数  */
+float _getAlt(float press, float temp){
+    return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f;
 }
+
+
+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){//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;
+}