201903_14ISEで実際に使用した開放用プログラム. 使用マイコンがNUCLES-F303K8なので注意
Dependencies: mbed Madgwick MPU6050 Kalman BMP180
main.cpp
- Committer:
- sashida_h
- Date:
- 2019-03-13
- Revision:
- 14:f932a8a297ff
- Parent:
- 13:3e6411bfd581
- Child:
- 15:a47cf038d601
File content as of revision 14:f932a8a297ff:
#include "mbed.h" #include "MPU6050.h" #include "BMP180.h" /*しきい値など*/ #define CNT_LAUNCH 10 //発射判定するときのしきい値 #define ACC_JUDGE_LAUNCH 3.0 //発射判定の合成加速度のしきい値 #define TIME_BURNING 6 //開放判定しない時間(燃焼時間) #define ALT_JUDGE_OPEN 1 //落下判定のカウントを1増やす高度差 #define TIME_OPEN 25 //強制的に開放させる時間 #define TIME_SEND 1.0 //無線送信する間隔 #define CNT_JUDGE 10 //頂点判定する時に落下のカウント数 #define NUM_CNT_MEDIAN 10 //中央値をとる個数 #define RATE_GPS 1.0 //GPSのTickerを動作させるタイミング #define p0 1013.25f //海面気圧 MPU6050 mpu(PB_7,PB_6); BMP180 bmp(PB_7,PB_6); Serial pc(PA_2, PA_3); Serial gps(PA_9, PA_10); DigitalOut myled(PA_15); DigitalOut ES920_RST(PA_5); PwmOut servo1(PB_4); PwmOut servo2(PB_5); DigitalOut servo1_signal(PA_0); DigitalOut servo2_signal(PA_1); Timer timer_open; Timer timer_data; Ticker tic_gps; /*自作関数*/ float _getAlt(); float _median(float data[],int num); void _SendGPS(); float _DMS2DEG(float raw_data); enum PHASE{STANDBY=0,LAUNCH=1,RISE=3,FIRE=7,OPEN=15,RECOVERY=9,SEA=6} Phase; /*グローバル変数*/ //地上高度 float alt_gnd; float alt_max; char c[516]; int i = 0; int cnt_gps=0; void main(){ ES920_RST = 0; myled =0; /*ローカル変数*/ float acc[3],acc_buff[10],gyro[3],gyro_buff[10],acc_abs; float alt_buff[10],alt_md; float time_judge; int cnt_data=0,cnt_judge=0; float t = 0; float t_hour = 0; float t_send = 0; /*センサの初期化等*/ pc.baud(38400); mpu.setAcceleroRange(3); bmp.Initialize(64,BMP180_OSS_ULTRA_LOW_POWER); /*初期位置の設定*/ mpu.getAccelero(acc); mpu.getGyro(gyro); 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("0,0,0,0,0\r\n"); wait(1.0); timer_data.start(); servo1_signal = 1; servo2_signal = 1; servo1.pulsewidth(0.0015);//close servo2.pulsewidth(0.0006);//close while(1){ switch(Phase){ case STANDBY: /*サーボ入力待ち*/ //servo(); if(pc.readable()){ c[0]=pc.getc(); } t = timer_data.read(); if(t > 1800){ t_hour =+ 1800; timer_data.reset(); } t = t + t_hour; t = t/60.0; if(c[0] == 'o'){ myled = 1; servo1.pulsewidth(0.0006);//open servo2.pulsewidth(0.0015);//open mpu.getAccelero(acc); //pc.printf("OPEN:%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0))); pc.printf("0,%f,%f,0,%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0)),(acc[1]/9.81),t); } if(c[0] == 'l'){ myled = 0; servo1.pulsewidth(0.0015); servo2.pulsewidth(0.0006); mpu.getAccelero(acc); //pc.printf("LOCK:%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0))); pc.printf("0,%f,%f,0,%f\r\n",sqrt(pow(acc[0]/9.81,2.0)+pow(acc[1]/9.81,2.0)+pow(acc[2]/9.81,2.0)),(acc[1]/9.81),t); } if(c[0] == 'f'){ //pc.printf("flight_mode\r\n"); Phase = LAUNCH; servo1_signal = 0; servo2_signal = 0; } if(c[0] == 'g'){ pc.printf("9,0,0,0,0\r\n"); wait(0.5); tic_gps.attach(&_SendGPS, 1.0/RATE_GPS); Phase = RECOVERY; } 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(timer_data.read() - t > TIME_SEND){ //pc.printf("LAUNCH,acc:%f,time:%3f,cnt:%d\r\n",acc_abs,timer_data.read(),cnt_judge); pc.printf("1,%f,%f,0,%3f\r\n",acc_abs,acc_abs,timer_data.read()); cnt_judge = 0; t = timer_data.read(); } /*加速度判定*/ if(acc_abs>ACC_JUDGE_LAUNCH){ cnt_judge++; } if(cnt_judge==CNT_LAUNCH){ servo1_signal = 1; servo1_signal = 1; cnt_judge=0; timer_open.start(); Phase = RISE; } break; case RISE: while(timer_open.read() < TIME_BURNING){ 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; 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); //pc.printf("RISE,time from launch:%f\r\n",timer_open.read()); pc.printf("3,%f,%f,%f,%3f\r\n",acc_abs,acc_abs,alt_md,timer_open.read()); wait(0.9); i=0; timer_data.reset(); } Phase = OPEN; t = 0.0; break; case FIRE: 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(timer_open.read() - t > TIME_SEND){ //pc.printf("OPEN,alt:%f,time:%3f,cnt:%d\r\n",alt_md,timer_open.read(),cnt_judge); pc.printf("15,%f,%f,%f,%3f\r\n",acc_abs,acc_abs,alt_md,timer_open.read()); t = timer_open.read(); } if(alt_md > alt_max){ alt_max = alt_md; cnt_judge = 0; } else if((alt_max-alt_md) > ALT_JUDGE_OPEN){ cnt_judge++; } //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; servo1_signal = 1; servo2_signal = 1; wait(0.1); servo1.pulsewidth(0.0006);//open servo2.pulsewidth(0.0015);//open pc.printf("9,0,0,0,%d\r\n",cnt_judge); wait(0.5); tic_gps.attach(&_SendGPS, 1.0/RATE_GPS); } break; case RECOVERY: break; case SEA: break; } } } 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; } 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; } void _SendGPS(){ 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,MAX_ALT:%f\r\n",lat_north,lon_east,alt_max); for(i=0;i<2;i++){ c[i]=pc.getc(); } //pc.printf("%c",c[1]); if(c[1]!='O'){ ES920_RST = 1; wait(0.1); ES920_RST = 0; wait(1.0); myled = 1; }else{ myled = 0; } break; }else{ //pc.printf("%s\r\n",gps_data); //pc.printf("NoGPSSignal\r\n"); break; } }else{ //pc.printf("No_Satellite_signal\r\n"); } }else{ cnt_gps++; } } } }