201803_oshima Team.F.C.
Dependencies: BMP180 MPU6050 SDFileSystem mbed
Fork of 201803_oshima_jodan by
main.cpp
- Committer:
- sashida_h
- Date:
- 2018-02-20
- Revision:
- 4:44e13dd5250b
- Parent:
- 3:a7b39e55d100
- Child:
- 5:bb9c685fc1fe
File content as of revision 4:44e13dd5250b:
#include "mbed.h" #include "BMP180.h" #include "MPU6050.h" #include "SDFileSystem.h" #define UNLOCK 1 #define LOCK 0 #define TIMER 30 //開放タイマー 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(p14); 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!\r\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); /*I2C初期化*/ 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){ /* 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){ //20回に一回保存(20回に1回openとclose) 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回中7回小さけれ落下と判断(最初はぜってぇ大きいから実質7/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]); //todo要らん場合は適宜コメントアウト 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 } }