201903_14ISEで実際に使用した開放用プログラム. 使用マイコンがNUCLES-F303K8なので注意
Dependencies: mbed Madgwick MPU6050 Kalman BMP180
Diff: main.cpp
- Revision:
- 9:42b4d337d4cc
- Parent:
- 8:15a1b22df82f
- Child:
- 10:1a626929850e
diff -r 15a1b22df82f -r 42b4d337d4cc main.cpp --- a/main.cpp Thu Mar 07 12:13:33 2019 +0000 +++ b/main.cpp Sat Mar 09 12:23:07 2019 +0000 @@ -28,6 +28,7 @@ Madgwick MadgwickFilter; Serial pc(PA_2, PA_3); Serial gps(PA_9, PA_10); +DigitalOut myled(PA_15); Timer timer_open; Timer timer_data; Ticker tic_data; @@ -39,6 +40,7 @@ void _SendData(); void _SendGPS(); float _DMS2DEG(float raw_data); +int servo(); enum PHASE{STANDBY=0,LAUNCH=1,RISE=3,FIRE=7,OPEN=15,RECOVERY=9,SEA=6} Phase; @@ -46,21 +48,28 @@ float t = 0; //地上高度 float alt_gnd; +float alt_max; //GPS int cnt_gps=0; int p = 1; +//サーボ +int i = 0; +char c[516]; +char d [8]; +int len; +char f[]="Receive"; -int main(){ +void main(){ /*ローカル変数*/ float acc[3],acc_buff[10],gyro[3],gyro_buff[10],acc_abs; - float alt_buff[10],alt_md,alt_max=0; + float alt_buff[10],alt_md; float time_judge; int cnt_data=0,cnt_judge=0; - int i=0; - + char gps_data[256]; /*センサの初期化等*/ pc.baud(38400); + gps.baud(115200); mpu.setAcceleroRange(3); bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER); @@ -77,20 +86,21 @@ pc.printf("Hello World!\r\n"); wait(1.0); pc.printf("f:Flight_mode_on\r\n"); + gps.printf("f:Flight_mode_on\r\n"); wait(1.0); - + timer_data.start(); while(1){ switch(Phase){ case STANDBY: + //gps.printf("Phase_STANDBY\r\n"); /*入力待ち*/ //tic_data.attach(&_SendData, 1.0/RATE_DATA); - char c = pc.getc(); - if(c=='f'){ - pc.printf("Flight_mode\r\n"); - timer_data.start(); - Phase = LAUNCH; - } - wait(1.0); + servo(); + //c[i] = pc.getc(); + //gps.printf("%c",c[i]); + //i++; + //if(c[i] == '\n') i=0; + //wait(1.0); break; case LAUNCH: @@ -204,11 +214,11 @@ //if((timer_open.read()-time_judge) - TIME_JUDGE_CNT > 0) cnt_judge=0; if(cnt_judge == CNT_JUDGE || timer_open.read() > TIME_OPEN){ Phase = RECOVERY; + tic_gps.attach(&_SendGPS, 1.0/RATE_GPS); } break; case RECOVERY: - char gps_data[256]; - while(1){ + /*while(1){ if(gps.readable()){ gps_data[cnt_gps] = gps.getc(); if(gps_data[cnt_gps] == '$' || cnt_gps ==256){ @@ -233,7 +243,7 @@ break; } }else{ - //ffpc.printf("No_Satellite_signal\r\n"); + //pc.printf("No_Satellite_signal\r\n"); } }else{ cnt_gps++; @@ -241,15 +251,9 @@ } } - Phase = SEA; + //Phase = SEA;*/ break; case SEA: - while(1){ - if(gps.readable()){ - c = gps.getc(); - pc.printf("%c",c); - } - } break; } @@ -306,6 +310,73 @@ pc.printf("%d,%f,%f,%f\r\n",Phase,alt,Roll,Pitch); } +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(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; +} + +int servo(void){ + do{ + c[i] = pc.getc(); + gps.printf("%c",c[i]); + i++; + }while(c[i-1] != '\n'); + gps.printf("path\r\n"); + //gps.printf("%s",c); + myled = 1; + //gps.printf("%s",c); + len = strlen(f); + i=0; + while(i != 4){ + if (c[i] != f[i]) break; + i++; + } + if(i > 3){ + i=0; + do{ + d[i]=c[13+i]; + i++; + }while(d[i-1] != ')'); + d[i-1] = '\0'; + gps.printf("d:%s",d); + i = 0; + if(d[0] == 'o') myled = 1; + if(d[0] == 'l') myled = 0; + if(d[0] == 'f'){ + //pc.printf("flight_mode\r\n"); + Phase = LAUNCH; + } + } + + i=0; + memset(c,'\0',64); + memset(d,'\0',8); + //pc.printf("2\r\n"); + return 0; +} void _SendGPS(){ char gps_data[256]; @@ -326,7 +397,7 @@ //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); + pc.printf("Lat:%f,Lon:%f,MAX_ALT:%f\r\n",lat_north,lon_east,alt_max); break; }else{ //pc.printf("%s\r\n",gps_data); @@ -341,33 +412,5 @@ } } } - -} - -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