201803_oshima_jodam_test
Dependencies: BMP180 MPU6050 SDFileSystem mbed
Fork of SDFileSystem_HelloWorld by
main.cpp
- Committer:
- sashida_h
- Date:
- 2018-03-17
- Revision:
- 12:c32b14ca0196
- Parent:
- 11:0f8d26600248
- Child:
- 13:4c1633775de7
File content as of revision 12:c32b14ca0196:
#include "mbed.h" #include "SDFileSystem.h" #include "MPU6050.h" #include "math.h" #include "BMP180.h" #define UNLOCK 1 #define LOCK 0 #define TIMER 30 //開放タイマー #define RATE 50//判定周期 謎の不具合により、145行目のkaihou.attach(_open,1/RATE);はコメントアウトしてるので、直接数字をいじってください #define ALT 1 //落下判断高度 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(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(p22); 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; //高度取得 float Alt_buff[256],Alt_buff2[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); void _recovery(); int _input(char c); int main() { twe.baud(115200); twe.printf("Hello World!\r\n"); int ret; lfp = fopen("/local/Alt.txt","r"); if(lfp == NULL){ goto normal; } ret = fscanf(lfp,"GND:%f",&Alt_gnd); fclose(lfp); if(ret == -1){ goto normal; }else{ twe.printf("reboot now\r\n"); goto yabai; } normal: while(1){ char c = twe.getc(); if(_input(c) == 1){ _para(UNLOCK); twe.printf("servo_open\r\n"); } else if(_input(c) == 2){ _para(LOCK); twe.printf("servo_lock\r\n"); } else if(_input(c) == 3){ break; } } 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); 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"); } 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("SD_write_OK!!\r\n"); twe.printf("ALL_READY\r\n"); while(fly == 1); //フライトピン抜けるまで待機 yabai: twe.printf("SEPARATE!!!!!!!\r\n"); 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,1/RATE); kaihou.attach(_open,0.02); while(1){ /* twe.printf("MAX:%f,Cnt:%d\r\n",Max_Alt, Cnt_para); wait(1.0); */ /* 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,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){ //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] > ALT) Cnt_para++; //直前の値(0.2秒前より1ALTm降下)より小さければカウント+1 // twe.printf("Cnt_para:%d\r\n", Cnt_para); Cnt_buff++; if(Cnt_para == 10 || t > TIMER){ //頂点!!! kaihou.detach(); //SD閉じる前にサーボ動かす前にTicker止める fprintf(fp,"OPEN!\r\n"); fclose(fp); lfp = fopen("/local/Alt.txt","a"); fprintf(lfp, "MAX:%f\r\n",Max_Alt); fclose(lfp); _para(UNLOCK); tf_para = false; timer1.stop(); twe.printf("PARA_OPEN\r\n"); twe.printf("GPS_MODE_ON!!!!\r\n"); ochiru.attach(_recovery,0.2); //パラメータ保存のためのやつ } } } if(Cnt_buff == 250) Cnt_buff = 0; } void _recovery(){ if(i == 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; 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]); i++; if(i == 50){ fclose(fp); i = 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.001); // pulse servo out sita }else if(motion==LOCK){ servo_para.pulsewidth(0.002); // pulse servo outu sita } } int _input(char c){ if(c=='f'){ return 3; }else if(c=='l'){ return 2; }else if(c=='u'){ return 1; } return 0; }