201803_oshima_jodam_test
Dependencies: BMP180 MPU6050 SDFileSystem mbed
Fork of SDFileSystem_HelloWorld by
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