201903_14ISEで実際に使用した開放用プログラム. 使用マイコンがNUCLES-F303K8なので注意
Dependencies: mbed Madgwick MPU6050 Kalman BMP180
Diff: main.cpp
- Revision:
- 7:9953d922499d
- Parent:
- 6:f17310205c1f
- Child:
- 8:15a1b22df82f
diff -r f17310205c1f -r 9953d922499d main.cpp --- a/main.cpp Tue Feb 12 11:20:33 2019 +0000 +++ b/main.cpp Mon Mar 04 02:50:16 2019 +0000 @@ -5,24 +5,27 @@ #include "MadgwickAHRS.h" /*しきい値など*/ -#define ACC_JUDGE_LAUNCH 1.5 +#define ACC_JUDGE_LAUNCH 3.0 #define TIME_BURNING 6 -#define ALT_JUDGE_FIRE 1 -#define ALT_JUDGE_OPEN 2 -#define ANGLE_JUDGE_FIRE 90 -#define CNT_JUDGE 3 +#define ALT_JUDGE_FIRE 0 +#define ALT_JUDGE_OPEN 1 +#define ANGLE_JUDGE_FIRE_MIN 15 +#define ANGLE_JUDGE_FIRE_MAX 70 +#define CNT_JUDGE 10 #define TIME_JUDGE_CNT 1.5 #define NUM_CNT_MEDIAN 10 -#define RATE_GPS 1 -#define RATE_DATA 2 +#define RATE_GPS 1.0 +#define RATE_DATA 10 +#define TIMER_NOTFIRE 15.0 #define p0 1013.25f #define RadToDeg 57.295779513082320876798154814105 -MPU6050 mpu(dp5,dp27); -BMP180 bmp(dp5,dp27); +MPU6050 mpu(PB_7,PB_6); +BMP180 bmp(PB_7,PB_6); KalmanFilter gKfx, gKfy; Madgwick MadgwickFilter; -Serial pc(dp16,dp15); +Serial pc(PA_2, PA_3); +Serial gps(PA_9, PA_10); Timer timer_open; Timer timer_data; Ticker tic_data; @@ -43,16 +46,23 @@ float gCalibrateY; float gPrevMicros; float degRoll,degPitch; +//地上高度 +float alt_gnd; +//GPS +int cnt_gps=0; + +int p = 1; int main(){ /*ローカル変数*/ float acc[3],acc_buff[10],gyro[3],gyro_buff[10],acc_abs; - float alt_buff[10],alt_md,alt_max; + float alt_buff[10],alt_md,alt_max=0; float time_judge; int cnt_data=0,cnt_judge=0; + int i=0; /*センサの初期化等*/ - pc.baud(9600); + pc.baud(38400); mpu.setAcceleroRange(3); bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER); @@ -67,14 +77,44 @@ gCalibrateX = degRoll; Phase = STANDBY; + for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){ + alt_buff[cnt_data] = _getAlt(); + } + 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"); while(1){ switch(Phase){ case STANDBY: /*入力待ち*/ timer_data.start(); - tic_data.attach(&_SendData, 1.0/RATE_DATA); - Phase = LAUNCH; + //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(); + if(c=='f'){ + pc.printf("Flight_mode\r\n"); + Phase = LAUNCH; + } + */ + wait(1.0); break; case LAUNCH: for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){ @@ -82,25 +122,56 @@ 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; + } + i++; /*加速度判定*/ if(acc_abs>ACC_JUDGE_LAUNCH){ cnt_judge++; } - if(cnt_judge==CNT_JUDGE){ + if(cnt_judge==CNT_JUDGE || timer_data.read()>30.0){ cnt_judge=0; timer_open.start(); - Phase = FIRE; + 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); + 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(); } 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"); } break; case FIRE: @@ -143,16 +214,35 @@ Roll = MadgwickFilter.getRoll(); Pitch = MadgwickFilter.getPitch(); Yaw = MadgwickFilter.getYaw(); - if(Pitch>ANGLE_JUDGE_FIRE){ + i++; + if(i==400){ + //pc.printf("TIME:%f,Pitch:%f\r\n",timer_data.read(),Pitch); + pc.printf("FIRE:Pitch:%f,Time%f\r\n",Pitch,timer_open.read()); + i=0; + } + /*if(Pitch>ANGLE_JUDGE_FIRE_MIN && Pitch < ANGLE_JUDGE_FIRE_MAX){ Phase = OPEN; + i=0; } + */ + //if(timer_open.read()>TIMER_NOTFIRE){ + if(timer_data.read()>60.0){ + Phase = OPEN; + pc.printf("NOT_FIRE!!\r\n"); + } break; case OPEN: for(cnt_data=0;cnt_data<NUM_CNT_MEDIAN;cnt_data++){ alt_buff[cnt_data] = _getAlt(); } 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; + } + i++; if(alt_md > alt_max){ alt_max = alt_md; cnt_judge = 0; @@ -162,18 +252,30 @@ time_judge = timer_open.read(); } - if((timer_open.read()-time_judge) - TIME_JUDGE_CNT) cnt_judge=0; - if(cnt_judge == CNT_JUDGE){ + //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; Phase = RECOVERY; + pc.printf("Fall!!!\r\n"); + wait(1.0); + } break; case RECOVERY: - tic_data.detach(); + //tic_data.detach(); //tic_gps.attach(&_SendGPS, 1.0/RATE_GPS); Phase = SEA; break; case SEA: + char c; + while(1){ + if(gps.readable()){ + c = gps.getc(); + pc.printf("%c",c); + } + } + break; } } @@ -224,37 +326,47 @@ Yaw = MadgwickFilter.getYaw(); alt = _getAlt(); - + alt = alt - alt_gnd; //pc.printf("%d,%f,%f,%f\r\n",Phase,alt,degx,degy); pc.printf("%d,%f,%f,%f\r\n",Phase,alt,Roll,Pitch); } -/* + void _SendGPS(){ - int cnt_gps; - 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); + 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,ntime:%d\r\n",lat_north,lon_east,world_time); + break; + }else{ + //pc.printf("%s\r\n",gps_data); + pc.printf("NoGPSSignal\r\n"); + break; + } }else{ - twe.printf("%s\r\n",gps_data); + //ffpc.printf("No_Satellite_signal\r\n"); } + }else{ + cnt_gps++; } - }else{ - cnt_gps++; } } + } float _DMS2DEG(float raw_data){ @@ -262,7 +374,7 @@ float m=(raw_data-(float)d*100); return (float)d+m/60; } -*/ + float _median(float data[], int num){ float *data_cpy, ans; @@ -283,4 +395,4 @@ else ans = (data_cpy[num/2-1]+data_cpy[num/2])/2.0; delete[] data_cpy; return ans; -} \ No newline at end of file +} \ No newline at end of file