2017.11伊豆大島共同打ち上げ実験のデータ取得&保存用プログラム
Dependencies: BMP180 MPU6050 SDFileSystem mbed
Fork of SDFileSystem_HelloWorld by
main.cpp
- Committer:
- oichan
- Date:
- 2017-10-29
- Revision:
- 6:aae92602ef84
- Parent:
- 5:1e66d892109b
- Child:
- 7:5f8080485160
File content as of revision 6:aae92602ef84:
#include "mbed.h" #include "math.h" #include "MPU6050.h" #include "BMP180.h" #include "SDFileSystem.h" #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; DigitalOut oshirase1(p19); DigitalInOut oshirase2(p20); //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); int _input(char moji); /* グローバル変数 */ float a[3]; float Land_Alt; float Max_Alt = 0; float Alt_buff[10]; float Pressure,Temperature,Altitude; float Time; char gps_data[256]; char c; char command; int cnt_gps = 0; int Log_cnt = 0; int Phase = 0; int main() { // pc.printf("Hello World!\n"); oshirase2.output(); oshirase1 = 0; oshirase2 = 0; twe.baud(115200); mpu.setAcceleroRange(0); bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER); mkdir("/sd/mydir", 0777); while(1){ command = twe.getc(); if(_input(command)==-1){ //フライトモードへ移行 oshirase1 = 0; oshirase2 = 1; _flight(); break; }else if(_input(command)==1){ //サーボ動かすだけモードへ oshirase1 = 1; oshirase2 = 0; while(1){ command = twe.getc(); if(_input(command)==3){ //開放用サーボ(MG996)を動かす oshirase1 = 1; oshirase2 = 1; break; }else if(_input(command)==7){ //リーフィングのサーボ(SG92)を動かす oshirase1 = 0; oshirase2 = 0; break; } } } } while(1); } /* フライトモード用関数 */ void _flight(){ oshirase2.input(); twe.printf("flight mode on\r\n"); // pc.printf("flight mode on\r\n"); /* 地上高度取得 */ for(int i=0;i<10;i++){ bmp.ReadData(&Temperature,&Pressure); Alt_buff[i] = _getAlt(Pressure,Temperature); } Land_Alt = _median(Alt_buff,10); Max_Alt = Land_Alt; FILE *lfp = fopen("/local/alt.txt","a"); fprintf(lfp,"地上高度:%f\r\n",Land_Alt); fclose(lfp); twe.printf("地上高度:%f\r\n",Land_Alt); // for(int i=0;i<10;i++)pc.printf("%f\n\r",Alt_buff[i]); /* データ取得開始 */ timer.start(); logtimer.attach(_log,1.0/RATE); while(1){ /* フェーズ管理 */ if(Phase==0){ if(oshirase2==1){ Phase = 1; //上昇フェーズへ移行 alt_timer.start(); } }else if(Phase==1){ if(oshirase2==0){ Phase = 2; //降下1フェーズへ移行 } }else if(Phase==2){ if(oshirase2==1){ Phase = 3; //降下2フェーズへ移行 } } /* 30分ごとにタイマーをリセット(オーバーフロー対策) */ if(Time > 60*30){ timer.reset(); } /* 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); twe.printf("phase:%d,max altitude:%f\r\n",Phase,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("phase:%d,max altitude:%f\r\n",Phase,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(); }else if(alt_timer.read() > MAX_JUDGE_TIME_s){ alt_timer.stop(); alt_timer.reset(); FILE *lfp = fopen("/local/alt.txt","a"); fprintf(lfp,"最高高度:%f\r\n",Max_Alt); fclose(lfp); // pc.printf("最高高度:%f\r\n",Max_Alt); } /* データ保存 */ if(Log_cnt==0) { fp = fopen("/sd/mydir/sdtest.txt", "a"); } fprintf(fp, "%d,%f, %f, %f, %f, %f, %f, %f \r\n",Phase, Time, Temperature, Pressure, Altitude, a[0],a[1],a[2]); Log_cnt++; if(Log_cnt==RATE){ fclose(fp); Log_cnt = 0; } // pc.printf("%d,%f, %f, %f, %f, %f, %f, %f \r\n",Phase, 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; } /* コマンド判別用関数 */ int _input(char moji){ if(moji!='\0'){ twe.printf("%c\r\n",c); } if(moji=='f'){ return -1; }else if(moji=='s'){ return 1; }else if(moji=='p'){ return 3; }else if(moji=='l'){ return 7; } return 0; }