201903_14ISEで実際に使用した開放用プログラム. 使用マイコンがNUCLES-F303K8なので注意
Dependencies: mbed Madgwick MPU6050 Kalman BMP180
Diff: main.cpp
- Revision:
- 4:4b3ae90ec778
- Parent:
- 3:24a8901befb6
- Child:
- 5:f6e956e8a060
--- a/main.cpp Thu Nov 02 17:22:14 2017 +0000 +++ b/main.cpp Thu Aug 09 21:39:18 2018 +0000 @@ -1,47 +1,330 @@ -/* -説明 -Nucleo-F303K8とBMP180を使った気温・気圧・高度計算のサンプルプログラム - -ライブラリ -https://developer.mbed.org/users/spiridion/code/BMP180/ - -以下ピン配置 -Nucleo BMP180 -GND-----GND-------0V -+3V3----VIN -D4------SDA -D5------SCL - -*/ #include "mbed.h" #include "math.h" +#include "MPU6050.h" #include "BMP180.h" -#define p0 1013.25f//海面気圧 +#include "SDFileSystem.h" + +/*マクロ処理*/ +//動作レート +#define RATE_LOG 20 +#define RATE_OPEN 50 +//発射判定 +#define ACC_JUDGE_LAUNCH 1.5 +#define CNT_JUDGE_LAUNCH 5 +//待機時間 +#define TIME_GAP 3.0 +//頂点判定 +#define ALT_JUDGE_OPEN 1.5 +#define CNT_JUDGE_OPEN 5 +#define TIME_JUDGE_OPEN 30 +//放出判定 +#define TIME_JUDGE_OUT 60 +#define ALT_JUDGE_OUT -1.5 +#define CNT_JUDGE_OUT 5 +//共通 +#define TIME_JUDGE_CNT 1.5 +#define p0 1013.25f +//ログ //2018.06.28 edit +#define NUM_DATA 20 -DigitalOut myled(LED1); -Serial pc(USBTX,USBRX); -BMP180 bmp(p28, p27); -Timer timer; +MPU6050 mpu(p9,p10); +BMP180 bmp(p9,p10); +BusOut led(LED1,LED2,LED3,LED4); +Serial pc(USBTX,USBRX); +Serial twe(p28,p27); +Serial gps(p13,p14); +PwmOut servo1(p21); +PwmOut servo2(p22); +PwmOut servo3(p23); +PwmOut servo4(p24); +DigitalOut sw(p25); +SDFileSystem sd(p5,p6,p7,p8,"sd"); +LocalFileSystem local("local"); + +Timer timer_open; +Timer timer_log; +Ticker tic_open; +Ticker tic_log; -float getAlt(float press, float temp); +void _open(); +void _log(); +float _getAlt(); +float _DMS2DEG(float raw_data); +float _median(float data[],int num); +int _input(char cha); + +float Alt_buf[10], Alt_gnd, Alt_md, Alt_max=-300; //TODO:Alt_maxの値を調整すること +float Time_alt; +float Data[2][NUM_DATA][8]; +int Cnt=0, Cnt_acc=0, Cnt_alt=0; +int Cnt_data=0; +int cnt_gps; +char gps_data[256]; +bool Row; +FILE *lfp; +FILE *fp; + +enum PHASE{SETUP=0,LAUNCH=1,WAIT=3,RISE=7,DROP1=15,DROP2=9,SEA=10} Phase; +enum SERVO{LOCK=0,UNLOCK=1} Servo; -int main() { - float pressure,temperature,altitude; - float time; +int main(){ + /*センサ類初期化*/ bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER); - pc.printf("time, temperature ,pressure, altitude\r\n"); - timer.start(); - - while(1) { - bmp.ReadData(&temperature,&pressure); - altitude = getAlt(pressure,temperature); - time = timer.read(); - pc.printf("%f, %f, %f, %f \r\n",time, temperature, pressure, altitude); - myled =! myled; - wait(1); + twe.baud(115200); + sw=1; + /*ファイル作成*/ + mkdir("/sd/mydir",0777); + fp=fopen("/sd/mydir/data.txt","a"); + fprintf(fp,"phase,time,ax,ay,az,alt,temp,press\r\n"); + fclose(fp); + lfp=fopen("/local/data.txt","a"); + fclose(lfp); + /*サーボ調整*/ + servo1.period_ms(20); + servo2.period_ms(20); + servo3.period_ms(20); + servo4.period_ms(20); + /*初めの挨拶*/ + //pc.printf("POWER ON\r\n"); + twe.printf("POWER ON\r\n"); + /*入力待ち*/ + while(1){ + char cha = twe.getc(); + if(_input(cha)==-1){ + timer_log.start(); + tic_log.attach(&_log, 1.0/RATE_LOG); + break; + } + } + /*フライト準備*/ + Phase = SETUP; + tic_open.attach(&_open, 1.0/RATE_OPEN); + + /*GPS*/ + while(1){ + /*GPS取得*/ + if(Phase != RISE){ + 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); + 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("%s\r\n",gps_data); + } + } + }else{ + cnt_gps++; + } + } + } + + if(timer_log.read()>=30.0*60.0){ + timer_log.reset(); + /*なんかずっとこればっか書き込まれた 2018/7/1 + fp=fopen("/sd/mydir/data.txt","w"); + fprintf(fp,"phase,time,ax,ay,az,alt,temp,press\r\n"); + fclose(fp); + */ + } } } -float getAlt(float press, float temp){ - return (pow((p0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f; -} \ No newline at end of file +void _open(){ + led = Phase; + float acc[3],acc_buf[10],acc_abs,time_acc,time_alt; + switch(Phase){ + case SETUP: //地上高度取得 + //pc.printf("Hello,World!\r\n"); + twe.printf("Hello,World!\r\n"); + for(Cnt=0;Cnt<10;Cnt++){ + Alt_buf[Cnt] = _getAlt(); + } + Alt_gnd = _median(Alt_buf,10); + /*データをローカルファイルに保存*/ + lfp=fopen("/local/data.txt", "a"); + fprintf(lfp,"地上高度:%f\r\n",Alt_gnd); + fclose(lfp); + + timer_open.start(); + Phase = LAUNCH; + //pc.printf("Phase = LAUNCH\r\n"); + twe.printf("Phase = LAUNCH\r\n"); + break; + + case LAUNCH: //発射判定 + for(Cnt=0;Cnt<10;Cnt++){ + mpu.getAccelero(acc); + acc_buf[Cnt] = sqrt(pow(acc[0]/9.81,2)+pow(acc[1]/9.81,2)+pow(acc[2]/9.81,2)); + } + acc_abs = _median(acc_buf,10); + //pc.printf("acc:%f\r\n",acc_abs); + //pc.printf("cnt:%d\r\n",Cnt_acc); + //加速度判定 + if(acc_abs>ACC_JUDGE_LAUNCH){ + Cnt_acc++; + time_acc = timer_open.read(); + } + if(timer_open.read()-time_acc > TIME_JUDGE_CNT) Cnt_acc = 0; + if(Cnt_acc == CNT_JUDGE_LAUNCH){ + //pc.printf("Phase = WAIT\r\n"); + twe.printf("LAUNCHED\r\n"); + timer_open.reset(); + Phase = WAIT; + } + break; + + case WAIT: //待機時間 + if(timer_open.read()>TIME_GAP) Phase = RISE; + break; + + case RISE: //頂点判定 + for(Cnt=0;Cnt<5;Cnt++){ + Alt_buf[Cnt] = _getAlt(); + //pc.printf("alt:%f\r\n",Alt_buf[Cnt]); + } + Alt_md = _median(Alt_buf,5); + twe.printf("alt:%f\r\n",Alt_md); + //twe.printf("cnt:%d\r\n",Cnt_alt); + if(Alt_md > Alt_max){ + Alt_max = Alt_md; + Cnt_alt = 0; + } + else if((Alt_max-Alt_md) > ALT_JUDGE_OPEN){ + Cnt_alt++; + Time_alt = timer_open.read(); + } + if((timer_open.read()-Time_alt) > TIME_JUDGE_CNT){ + Cnt_alt = 0; + } + if(/*(*/Cnt_alt == CNT_JUDGE_OPEN/*)||(timer_open.read() > TIME_JUDGE_OPEN)*/){ + //pc.printf("Phase = DROP1\r\n"); + twe.printf("OPEN PARA\r\n"); + Cnt_alt = 0; + /*ローカルファイルに最高高度を記録*/ + lfp = fopen("/local/data.txt", "a"); + fprintf(lfp,"最高高度:%f\r\n",Alt_max); + fclose(lfp); + + servo1.pulsewidth(0.0006); //TODO:サーボホーンの角度調整 + servo2.pulsewidth(0.0006); //TODO:サーボホーンの角度調整 + Phase = DROP1; + } + break; + + case DROP1: //放出機構作動? + for(Cnt=0;Cnt<5;Cnt++){ + Alt_buf[Cnt] = _getAlt(); + } + Alt_md = _median(Alt_buf,5); + twe.printf("Alt:%f\r\n",Alt_md); + //twe.printf("Cnt:%d\r\n",Cnt_alt); + if((Alt_md-Alt_gnd) > ALT_JUDGE_OUT){ + Cnt_alt++; + } + if((Cnt_alt == CNT_JUDGE_OUT)||(timer_open.read() > TIME_JUDGE_OUT)){ //地上高度からの高度にする?到達高度低いかも? + //pc.printf("Phase = DROP2\r\n"); + twe.printf("GO\r\n"); + servo3.pulsewidth(0.0006); //TODO:サーボホーンの角度調整 + servo4.pulsewidth(0.0006); //TODO:サーボホーンの角度調整 + Phase = DROP2; + } + break; + + case DROP2: //着水検知.サーボの電源落とす. + //twe.printf("cnt:%d\r\n",Cnt_alt); + wait(3.0); + sw = 0; + //pc.printf("Goodbye Servo\r\n"); + twe.printf("FINISH\r\n"); + Phase=SEA; + break; + } +} + +void _log(){ + Data[Row][Cnt_data][0]=Phase; + Data[Row][Cnt_data][1]=timer_log.read(); + mpu.getAccelero(&Data[Row][Cnt_data][2]); + bmp.ReadData(&Data[Row][Cnt_data][5],&Data[Row][Cnt_data][6]); + Data[Row][Cnt_data][7] = (pow((p0/Data[Row][Cnt_data][6]), (1.0f/5.257f))-1.0f)*(Data[Row][Cnt_data][5]+273.15f)/0.0065f; + Cnt_data++; + //pc.printf("Cnt_data:%d\r\n",Cnt_data); + if(Cnt_data==NUM_DATA){ + Cnt_data = 0; + Row =! Row; + fp = fopen("/sd/mydir/data.txt","a"); + for(int i=0;i<NUM_DATA;i++){ + fprintf(fp,"%2.0f,%f,%f,%f,%f,%f,%f,%f\r\n",Data[!Row][i][0],Data[!Row][i][1],Data[!Row][i][2],Data[!Row][i][3],Data[!Row][i][4],Data[!Row][i][5],Data[!Row][i][6],Data[!Row][i][7]); + } + fclose(fp); + } +} + +float _getAlt(){ + float altitude,pressure,temperature; + bmp.ReadData(&temperature,&pressure); + altitude = (pow((p0/pressure), (1.0f/5.257f))-1.0f)*(temperature+273.15f)/0.0065f; + return altitude; +} + +int _input(char cha){ + twe.printf("%c\r\n",cha); + if(cha!='\0'){ +// pc.printf("%c\r\n",c); + } + if(cha=='F'){ + twe.printf("flight mode on\r\n"); + return -1; + }else if(cha=='u'){ //開放機構のUNLOCK + servo1.pulsewidth(0.0006); + servo2.pulsewidth(0.0006); + }else if(cha=='l'){ //開放機構のLOCK + servo1.pulsewidth(0.0024); + servo2.pulsewidth(0.0024); + }else if(cha=='a'){ //収穫機構のUNLOCK(AKERU) + servo3.pulsewidth(0.0010); + servo4.pulsewidth(0.0010); + }else if(cha=='s'){ //収穫機構のLOCK(SHIMERU) + servo3.pulsewidth(0.0024); + servo4.pulsewidth(0.0024); + } + return 0; +} + +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(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; +} \ No newline at end of file