![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
201903_14ISEで実際に使用した開放用プログラム. 使用マイコンがNUCLES-F303K8なので注意
Dependencies: mbed Madgwick MPU6050 Kalman BMP180
Diff: main.cpp
- Revision:
- 8:15a1b22df82f
- Parent:
- 7:9953d922499d
- Child:
- 9:42b4d337d4cc
diff -r 9953d922499d -r 15a1b22df82f main.cpp --- a/main.cpp Mon Mar 04 02:50:16 2019 +0000 +++ b/main.cpp Thu Mar 07 12:13:33 2019 +0000 @@ -5,13 +5,15 @@ #include "MadgwickAHRS.h" /*しきい値など*/ -#define ACC_JUDGE_LAUNCH 3.0 -#define TIME_BURNING 6 +#define ACC_JUDGE_LAUNCH 3.0 //発射判定のしきい値 +#define TIME_BURNING 6 //開放判定しない時間(燃焼時間) #define ALT_JUDGE_FIRE 0 -#define ALT_JUDGE_OPEN 1 +#define ALT_JUDGE_OPEN 1 //落下判定のカウントを1増やす高度差 +#define TIME_OPEN 25 //強制的に開放させる時間 +#define TIME_SEND 1.0 #define ANGLE_JUDGE_FIRE_MIN 15 #define ANGLE_JUDGE_FIRE_MAX 70 -#define CNT_JUDGE 10 +#define CNT_JUDGE 10 #define TIME_JUDGE_CNT 1.5 #define NUM_CNT_MEDIAN 10 #define RATE_GPS 1.0 @@ -41,11 +43,7 @@ enum PHASE{STANDBY=0,LAUNCH=1,RISE=3,FIRE=7,OPEN=15,RECOVERY=9,SEA=6} Phase; /*グローバル変数*/ -//カルマン用 -float gCalibrateX; -float gCalibrateY; -float gPrevMicros; -float degRoll,degPitch; +float t = 0; //地上高度 float alt_gnd; //GPS @@ -69,12 +67,6 @@ /*初期位置の設定*/ mpu.getAccelero(acc); mpu.getGyro(gyro); - degRoll = atan2(acc[1], acc[2]) * RadToDeg; - degPitch = atan(-acc[0] / sqrt(acc[1] * acc[1] + acc[2] * acc[2])) * RadToDeg; - gKfx.setAngle(degRoll); - gKfy.setAngle(degPitch); - gCalibrateY = degPitch; - gCalibrateX = degRoll; Phase = STANDBY; for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){ @@ -83,96 +75,54 @@ alt_gnd = _median(alt_buff,NUM_CNT_MEDIAN); wait(2.0); pc.printf("Hello World!\r\n"); - timer_data.start(); - int r = 0; - float r_time; - pc.printf("TimerStart,Standby_mode\r\n"); - + wait(1.0); + pc.printf("f:Flight_mode_on\r\n"); + wait(1.0); + while(1){ switch(Phase){ case STANDBY: /*入力待ち*/ - timer_data.start(); //tic_data.attach(&_SendData, 1.0/RATE_DATA); - if(r == 0) r_time = timer_data.read() / 60.0; - if(r == 1) r_time = (timer_data.read() + 1800.0)/60; - if(r == 2) r_time = (timer_data.read() + 3600.0)/60; - if(r == 3) r_time = (timer_data.read() + 5400.0)/60; - if(r == 4){ - timer_data.reset(); - Phase = LAUNCH; - pc.printf("Flight_mode\r\n"); - } - pc.printf("TIME:%3f[min]",r_time); - if(timer_data.read() > 1800){ - timer_data.reset(); - r++; - } - /*char c = pc.getc(); + char c = pc.getc(); if(c=='f'){ pc.printf("Flight_mode\r\n"); + timer_data.start(); Phase = LAUNCH; } - */ wait(1.0); break; + case LAUNCH: for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){ mpu.getAccelero(acc); acc_buff[cnt_data] = sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0)); } acc_abs = _median(acc_buff,NUM_CNT_MEDIAN); - if(i==50){ - pc.printf("LAUNCH,acc:%f,time:%f\r\n",acc_abs,timer_data.read()); - i=0; + if(timer_data.read() - t > TIME_SEND){ + pc.printf("LAUNCH,acc:%f,time:%3f,cnt:%d\r\n",acc_abs,timer_data.read(),cnt_judge); + t = timer_data.read(); } - i++; /*加速度判定*/ if(acc_abs>ACC_JUDGE_LAUNCH){ cnt_judge++; } - if(cnt_judge==CNT_JUDGE || timer_data.read()>30.0){ + if(cnt_judge==CNT_JUDGE){ cnt_judge=0; timer_open.start(); Phase = RISE; - pc.printf("LAUNCH!!!!\r\n"); } break; + case RISE: while(timer_open.read() < TIME_BURNING){ - pc.printf("RISE,time:%f\r\n",timer_open.read()); - wait(0.5); + pc.printf("RISE,time from launch:%f\r\n",timer_open.read()); + wait(1.0); i=0; - } - if(p == 1){ - pc.printf("FIRE_JUDGE_START\r\n"); - p=0; - } - for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){ - alt_buff[cnt_data] = _getAlt(); + timer_data.reset(); } - alt_md = _median(alt_buff,NUM_CNT_MEDIAN); - alt_md = alt_md - alt_gnd; - if(alt_md > ALT_JUDGE_FIRE){ - Phase = FIRE; - if(i == 5){ - //pc.printf("Time:%f,RISE,alt:%f\r\n",timer_open.read(),alt_md); - pc.printf("Time:%f,RISE,alt:%f\r\n",timer_data.read(),alt_md); - i = 0; - } - }else{ - if(i == 5){ - //pc.printf("Time:%f,RISE,alt:%f\r\n",timer_open.read(),alt_md); - pc.printf("Time:%f,RISE,alt:%f\r\n",timer_data.read(),alt_md); - i = 0; - } - } - i++; - //if(timer_open.read()>TIMER_NOTFIRE){ - if(timer_open.read()>60.0){ - Phase = OPEN; - //pc.printf("NOT_FIRE!\r\n"); - } + Phase = OPEN; + t = 0.0; break; case FIRE: @@ -238,37 +188,62 @@ } alt_md = _median(alt_buff,NUM_CNT_MEDIAN); alt_md = alt_md - alt_gnd; - if(i==10){ - pc.printf("OPEN,Cnt:%d,ALT_NOW:%f,ALT_MAX%f\r\n",cnt_judge,alt_md,alt_max); - i=0; + if(timer_open.read() - t > TIME_SEND){ + pc.printf("OPEN,alt:%f,time:%3f,cnt:%d\r\n",alt_md,timer_open.read(),cnt_judge); + t = timer_open.read(); } - i++; if(alt_md > alt_max){ alt_max = alt_md; cnt_judge = 0; } else if((alt_max-alt_md) > ALT_JUDGE_OPEN){ cnt_judge++; - time_judge = timer_open.read(); + //time_judge = timer_open.read(); } //if((timer_open.read()-time_judge) - TIME_JUDGE_CNT > 0) cnt_judge=0; - //if(cnt_judge == CNT_JUDGE){ - if(timer_data.read() > 70.0){ - cnt_judge = 0; + if(cnt_judge == CNT_JUDGE || timer_open.read() > TIME_OPEN){ Phase = RECOVERY; - pc.printf("Fall!!!\r\n"); - wait(1.0); - } break; case RECOVERY: - //tic_data.detach(); - //tic_gps.attach(&_SendGPS, 1.0/RATE_GPS); + char gps_data[256]; + while(1){ + 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); + //pc.printf("%s\r\n",gps_data); + //pc.printf("Lat:%f,Lon:%f\r\ntime:%f,sat_num:%d\r\n",lat_north,lon_east,world_time,sat_num); + int japan_time = int(world_time) - 9; + pc.printf("Lat:%f,Lon:%f,time:%d,max_alt:%f\r\n",lat_north,lon_east,world_time,alt_max); + break; + }else{ + //pc.printf("%s\r\n",gps_data); + pc.printf("NoGPSSignal,max_alt:%f\r\n",alt_max); + break; + } + }else{ + //ffpc.printf("No_Satellite_signal\r\n"); + } + }else{ + cnt_gps++; + } + } + } + Phase = SEA; break; case SEA: - char c; while(1){ if(gps.readable()){ c = gps.getc();