Revision 0:aa715ea672cb, committed 2020-10-29

Comitter:

Zero0000

Date:

Thu Oct 29 10:23:16 2020 +0000

Commit message:

rocket

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BME280.lib	Thu Oct 29 10:23:16 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/MACRUM/code/BME280/#c1f1647004c4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250.lib	Thu Oct 29 10:23:16 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/Edutech/code/MPU9250/#98a0cccbc509

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem.lib	Thu Oct 29 10:23:16 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/teams/IZU2020/code/SDFileSystem/#7d0c7cd191a9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 29 10:23:16 2020 +0000
@@ -0,0 +1,305 @@
+#include "mbed.h"
+#include "math.h"
+#include "MPU9250.h"
+#include "BME280.h"
+#include "SDFileSystem.h"
+#define NUM_CNT_MEDIAN      10  //中央値をとる個数
+#define CNT_LAUNCH_TIMES 5//発射判定に必要な連続数
+#define TIME_SEND           1.0 //無線送信する間隔
+#define CNT_LAUNCH          5  //発射判定するときのしきい値(TBD)
+#define ACC_JUDGE_LAUNCH    3.0 //発射判定の合成加速度のしきい値
+#define TIME_BURNING        6   //燃焼時間(TBD)
+#define ALT_JUDGE_OPEN      5   //落下判定のカウントを１増やす高度差
+#define CNT_JUDGE           10  //頂点判定する時に落下のカウント数
+#define p0 1013.25f //海面気圧
+#define N 5
+#define sampleFreq 100.0f
+#define beta 0.33f // gain（大きいと加速度による補正が早い）
+#define PI 3.14159265358979323846f
+
+MPU9250 mpu = MPU9250(PB_7,PB_6);// pin30,29 SDA,SCL
+Serial gps(PB_4,PB_3, 9600);//pin19,20 tx,rx
+DigitalOut key(PA_8);//pin22
+Serial pc(PA_10,PA_9, 115200);//pin8,9 TX,RX
+BME280 bme = BME280(PB_7, PB_6);//pin30,29 SDA,SCL
+SDFileSystem sd(PA_7, PA_6, PA_5, PA_4, "sd");
+Timer timer_standby;
+Timer timer_flight;
+Timer timer_burning;
+FILE *fp;
+FILE *al;
+FILE *ac;
+//関数
+float _getAlt();
+float _median(float data[],int num);
+void  _SendGPS();
+float _DMS2DEG(float raw_data);
+void _calcurateAcc();
+
+
+void mpu_init(){
+    uint8_t whoami_MPU9250, whoami_AK8963;
+    float mag_init[3];
+    
+    whoami_MPU9250 = mpu.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
+    whoami_AK8963 = mpu.readByte(AK8963_ADDRESS, WHO_AM_I_AK8963);
+    pc.printf("MPU9250 IS 0x%x\n\r", whoami_MPU9250); // 0x71で正常
+    pc.printf("AK8963 IS 0x%x\n\r", whoami_AK8963); // 0x48で正常
+    
+    if (whoami_MPU9250 == 0x71 || whoami_AK8963 == 0x48){  
+        pc.printf("MPU9250 is detected.\n\r");
+        wait(0.1);
+        mpu.resetMPU9250();
+        mpu.initMPU9250();
+        wait(0.1);
+        mpu.initAK8963(mag_init);
+        mpu.getGres();
+        mpu.getAres();
+        mpu.getMres();
+        wait(0.1);
+    }
+   else{
+        pc.printf("Could not detect MPU9250.\n\r");
+        pc.printf("whoami_MPU9250 = 0x%x\n\rwhoami_AK8963 = 0x%x\r\n",
+                  whoami_MPU9250, whoami_AK8963);
+        while(1);
+    }
+}
+
+enum PHASE{Standby=0,Flight=1,Burning=3,Parachute=7} Phase;
+float alt_gnd;
+float alt_max;
+char c[516];
+int i = 0;
+int cnt_gps=0;
+int timeout;
+int cnt_data;
+int cnt_judge=0;
+int16_t acc[3];
+    
+float buff_ax[N], buff_ay[N], buff_az[N];
+float sum_ax = 0.0f, sum_ay = 0.0f, sum_az = 0.0f;
+float ax_new = 0.0f, ay_new = 0.0f, az_new = 0.0f;
+float ax = 0.0f, ay = 0.0f, az = 0.0f;
+int cnt = 0;
+float acc_abs;
+float alt_buff[10],alt_md;
+
+int main(){
+    
+    wait(0.1);
+    mpu_init();
+    bme.initialize();//BME初期化
+    mkdir("/sd/mydir",0777);//SDファイル作成
+    fp = fopen("/sd/mydir/gps.txt", "a");//最初のSDopen時間かかるのでwhile外デ
+    al = fopen("/sd/mydir/altitude.txt", "a");
+    ac = fopen("/sd/mydir/acc.txt", "a");
+    if(fp == NULL) {
+       error("Could not open file for write\n");
+       }
+
+    wait(0.1);
+
+    
+    switch(Phase){
+             case Standby: //待機モード
+                pc.printf("Standby\n\r");
+                key = 0;
+                if(pc.readable()){
+                    c[0]=pc.getc();
+                }
+
+                if(c[0] == 'f'){
+                pc.printf("Flight\r\n");
+                Phase = Flight;
+                timer_standby.stop();
+                timer_flight.start();
+                }
+                
+                break;
+                
+            case Flight:
+                     key = 0;
+                while(1){
+                     mpu.readAccelData(acc);
+                     _calcurateAcc();
+                     acc_abs = sqrt(pow(ax/9.81f,2)+pow(ay/9.81f,2)+pow(az/9.81f,2));//合成加速度
+                     fp = fopen("/sd/mydir/gps.txt", "a");
+                     _SendGPS();
+                     fclose(fp);
+                     timeout = timer_flight.read();
+                     ac = fopen("/sd/mydir/acc.txt","a");
+                     fprintf(ac,"%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
+                     fclose(ac);
+                     pc.printf("%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
+                    /*加速度判定*/
+                    if(acc_abs>ACC_JUDGE_LAUNCH){
+                        cnt_judge++;
+                     }
+                    if(cnt_judge==CNT_LAUNCH){
+                        cnt_judge=0;
+                        timer_burning.start();
+                        Phase = Burning;
+                        timer_flight.stop();
+                    }
+                  }
+                  break;
+            case Burning:
+                key = 0;
+                while(timer_flight.read() < TIME_BURNING){
+                    mpu.readAccelData(acc);
+                     _calcurateAcc();
+                    pc.printf("Burning\n\r");
+                    fp = fopen("/sd/mydir/gps.txt", "a");
+                    _SendGPS();
+                     fclose(fp);
+                     timeout = timer_flight.read();
+                     ac = fopen("/sd/mydir/acc.txt","a");
+                     fprintf(ac,"%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
+                     fclose(ac);
+                     pc.printf("%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
+                    
+                }
+                Phase = Parachute;
+                pc.printf("Parachute\n\r");
+                break;
+           
+               
+            case Parachute:
+                while(1){
+                    mpu.readAccelData(acc);
+                     _calcurateAcc();
+                    fp = fopen("/sd/mydir/gps.txt", "a");
+                    _SendGPS();
+                     fclose(fp);
+                     timeout = timer_flight.read();
+                     ac = fopen("/sd/mydir/acc.txt","a");
+                     fprintf(ac,"%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
+                     fclose(ac);
+                     pc.printf("%f\t%f\t%f\t%f\t%f\r\n",timeout,ax,ay,az,acc_abs);
+                    
+                    for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){
+                        alt_buff[cnt_data] = _getAlt();
+                    }
+                    alt_md = _median(alt_buff,NUM_CNT_MEDIAN);
+                    alt_md = alt_md - alt_gnd;
+                
+                    if(alt_md > alt_max){
+                    alt_max = alt_md;
+                    cnt_judge = 0;
+                }
+                else if((alt_max-alt_md) > ALT_JUDGE_OPEN){
+                    cnt_judge++;
+                }
+                //if((timer_open.read()-time_judge) - TIME_JUDGE_CNT > 0) cnt_judge=0;
+                 if(cnt_judge == CNT_JUDGE){
+                    key = 1;
+                    pc.printf("Open\n\r");
+                }
+                break;
+                
+             }
+           }    
+    }
+         
+    
+
+
+
+float _getAlt(){
+    float altitude,pressure,temperature;
+    temperature = bme.getTemperature();
+    pressure =  bme.getPressure();
+    altitude = (pow((p0/pressure), (1.0f/5.257f))-1.0f)*(temperature+273.15f)/0.0065f;
+    return altitude;
+}
+
+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;
+}
+
+
+void _SendGPS(){
+    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);
+                        pc.printf("Lat:%f,Lon:%f,MAX_ALT:%f\r\n",lat_north,lon_east,alt_max);
+                        for(i=0;i<2;i++){
+                            c[i]=pc.getc();
+                        }
+                        break;
+                    }else{
+                        //pc.printf("%s\r\n",gps_data);
+                        pc.printf("NoGPSSignal\r\n");
+                        break;
+                    }
+                }else{
+                    //pc.printf("No_Satellite_signal\r\n");
+                }
+            }else{
+                cnt_gps++;
+            }
+        }
+    }
+}
+
+void _calcurateAcc(){
+    /* 5回移動平均 */
+        sum_ax = sum_ax - buff_ax[cnt];
+        sum_ay = sum_ay - buff_ay[cnt];
+        sum_az = sum_az - buff_az[cnt];
+
+        ax_new = acc[0] / 2049.81;
+        ay_new = acc[1] / 2049.81;
+        az_new = acc[2] / 2049.81;
+
+        buff_ax[cnt] = ax_new;
+        buff_ay[cnt] = ay_new;
+        buff_az[cnt] = az_new;
+
+        sum_ax = sum_ax + buff_ax[cnt];
+        sum_ay = sum_ay + buff_ay[cnt];
+        sum_az = sum_az + buff_az[cnt];
+
+        cnt++;
+        if(cnt == N) cnt = 0;
+        
+        ax = sum_ax / N;
+        ay = sum_ay / N;
+        az = sum_az / N;
+    }
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Thu Oct 29 10:23:16 2020 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file