2017年3月,伊豆大島共同打上実験 CORE_缶ロケチーム電装
Dependencies: MPU6050 MS5607 mbed SDFileSystem
Diff: main.cpp
- Revision:
- 17:5f7808444d96
- Parent:
- 16:0a239047eb29
- Child:
- 18:5fb8ce45ce51
diff -r 0a239047eb29 -r 5f7808444d96 main.cpp --- a/main.cpp Mon Mar 13 12:41:09 2017 +0000 +++ b/main.cpp Tue Mar 14 12:13:28 2017 +0000 @@ -44,10 +44,10 @@ #include "MPU6050.h" #include "SDFileSystem.h" -#define ACC_LAUNCH 0//FIXME:本番は4g -#define TOP_DROP_AMOUNT 1.5 -#define ALT_UAV_DROP 1.0//FIXME:本番は150m -#define TIME_REACH_TOP 15 +#define ACC_LAUNCH 4.0f//FIXME:本番は4g +#define TOP_DROP_AMOUNT 1.5f +#define ALT_UAV_DROP 150.0f//FIXME:本番は150m +#define TIME_REACH_TOP 500.0f//FIXME:本番は15sec #define RATE_LOG 10 #define RATE_OPEN 10 @@ -81,13 +81,13 @@ Ticker loop_log; Ticker loop_open; /*ログカウンタ*/ +FILE *fp; bool row = 0; int8_t col = 0; /*ログ格納用*/ -float altitude[2][RATE_LOG],pressure[2][RATE_LOG],temperature[2][RATE_LOG]; +float pressure[2][RATE_LOG],temperature[2][RATE_LOG]; float acc[2][RATE_LOG][3],gyro[2][RATE_LOG][3]; float t[2][RATE_LOG]; -FILE *fp; /*フェーズ変数*/ int8_t Phase = SETUP; int8_t Mode = STANDBY; @@ -102,8 +102,8 @@ int8_t input_cnt = 0; /*関数*/ void _Open(); +void _Log(); void _Servo(int8_t door_num, int8_t motion); -void _Log(); void _Input(); float _Measure_Alt(float press, float temp); float _Median(float data[], int num); @@ -119,8 +119,8 @@ Door_2_2.period_ms(20); // _Servo(1,UNLOCK);//todo:当日は状態記憶に仕様変更予定? // _Servo(2,UNLOCK);// - fp = fopen("/sd/log.txt", "w"); - fprintf(fp, "pressure,temperature,ax,ay,az,gx,gy,gz\r\n"); + fp = fopen("/sd/log.txt", "a"); + fprintf(fp, "Phase,time,pressure,temperature,ax,ay,az,gx,gy,gz\r\n"); device.attach(&_Input,Serial::RxIrq); loop_open.attach(&_Open,1.0/RATE_OPEN); while(1); @@ -128,18 +128,17 @@ /*開放用関数 RATE_OPEN[Hz]で判定を行う*/ void _Open(){ myled = 1 << (Phase-1); - device.printf("%f,%f,%f\r\n",alt_buff[col_open],alt_launch,alt_max); switch (Phase) { case SETUP://セットアップモード(発射判定不可) break; case LAUNCH://点火モード(発射判定可) float acc_buff = (float)mpu.getAcceleroRawZ()/(ACC*0.981); - alt_buff[col_open] = _Measure_Alt(ms5607.getPressure()/100,ms5607.getTemperature()); - if(acc_buff>(float)ACC_LAUNCH){ + alt_buff[col_open] = ms5607.getAltitude(); + if(acc_buff>ACC_LAUNCH){ if(cnt_judge++==3){ - Phase = RISE; - alt_launch = _Median(alt_buff, RATE_OPEN); - cnt_judge = 0; + Phase = RISE; + alt_launch = _Median(alt_buff, RATE_OPEN); + cnt_judge = 0; } t_launch = timer.read(); alt_max = alt_launch; @@ -148,9 +147,9 @@ } break; case RISE://上昇中(パラシュート開放判定) - alt_buff[col_open] = _Measure_Alt(ms5607.getPressure()/100,ms5607.getTemperature()); - if(alt_buff[col_open]>alt_max) alt_max = alt_buff[col_open]; - if(alt_buff[col_open]<alt_max-(float)TOP_DROP_AMOUNT){ + float alt_rising = ms5607.getAltitude(); + if( alt_rising>alt_max && alt_rising-alt_max < 10.0 ) alt_max = alt_rising; + if(alt_rising<alt_max-TOP_DROP_AMOUNT){ if(cnt_judge++==3){ _Servo(2,UNLOCK); Phase = DROP; @@ -160,16 +159,15 @@ }else{ if(timer.read()>t_top+1.0) cnt_judge = 0; } - /*FIXME:当日は有効化 if(timer.read()-t_launch>TIME_REACH_TOP){ _Servo(2,UNLOCK); Phase = DROP; cnt_judge = 0; } - */ break; case DROP://降下中(缶サット開放判定) - if(alt_buff[col_open] < alt_launch+(float)ALT_UAV_DROP){ + float alt_droping = ms5607.getAltitude(); + if(alt_droping < alt_launch+ALT_UAV_DROP){ if(cnt_judge++==3){ Phase++; _Servo(1,UNLOCK); @@ -185,21 +183,24 @@ } /*記録用関数 RATE_LOG[Hz]で記録を行う*/ void _Log(){ - t[row][col] = timer.read(); + if(t[row][col] = timer.read()>=30.0*60.0){ + timer.reset(); + t[row][col] = timer.read(); + } pressure[row][col] = ms5607.getPressure()/100; temperature[row][col] = ms5607.getTemperature(); mpu.getAccelero(&acc[row][col][0]); mpu.getGyro(&gyro[row][col][0]); - fprintf(fp,"%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n", - t[row][col],pressure[row][col],temperature[row][col],acc[row][col][0]/10.0, + fprintf(fp,"%d,%f,%f,%f,%f,%f,%f,%f,%f,%f\r\n", + Phase,t[row][col],pressure[row][col],temperature[row][col],acc[row][col][0]/10.0, acc[row][col][1]/10.0,acc[row][col][2]/10.0,gyro[row][col][0],gyro[row][col][1],gyro[row][col][2] ); if(col++==RATE_LOG){ + col = 0; + row =! row; fclose(fp); fp = fopen("/sd/log.txt", "a"); - row =! row; - col = 0; - } + } } /*サーボ動作用関数 _Servo(int8_t 扉番号,int8_t 動作)*/ void _Servo(int8_t door_num, int8_t motion){ @@ -238,35 +239,32 @@ void _Input(){ input_buff[input_cnt] = device.getc(); device.printf("\r\n"); - device.printf("input_cnt:%d\r\n",input_cnt); - device.printf("input_buff:%s\r\n",input_buff); switch (Mode) { case STANDBY: if(input_cnt==0){ if(input_buff[0]=='S'){ - device.printf("P >> Parachute\r\n"); - device.printf("C >> CanSat\r\n"); + device.printf("P : Parachute\r\n"); + device.printf("C : CanSat\r\n"); }else if(input_buff[0]=='M'){ - device.printf("S >> STANDBY\r\n"); - device.printf("T >> TEST\r\n"); - device.printf("F >> FLIGHT\r\n"); + device.printf("S : STANDBY\r\n"); + device.printf("F : FLIGHT\r\n"); }else{ device.printf("This command is not found >> %c\r\n",input_buff[0]); device.printf(">>MAINMENU<<\r\n"); - device.printf("S >> Servo Operation\r\n"); - device.printf("M >> Mode Change\r\n"); + device.printf("S : Servo Operation\r\n"); + device.printf("M : Mode Change\r\n"); device.printf("-->>"); return; } }else if(input_cnt==1){ if(input_buff[0]=='S'){ if(input_buff[1]=='P'||input_buff[1]=='C'){ - device.printf("U >> UNLOCK\r\n"); - device.printf("L >> LOCK\r\n"); + device.printf("U : UNLOCK\r\n"); + device.printf("L : LOCK\r\n"); }else{ device.printf("This command is not found >> %c\r\n",input_buff[1]); - device.printf("P >> Parachute\r\n"); - device.printf("C >> CanSat\r\n"); + device.printf("P : Parachute\r\n"); + device.printf("C : CanSat\r\n"); device.printf("-->>"); return; } @@ -274,13 +272,8 @@ if(input_buff[1]=='S'){ Mode = STANDBY; device.printf(">>MAINMENU<<\r\n"); - device.printf("S >> Servo Operation\r\n"); - device.printf("M >> Mode Change\r\n"); - device.printf("-->>"); - }else if(input_buff[1]=='T'){ - Mode = TEST; - Phase = RISE; - alt_launch = _Measure_Alt(ms5607.getPressure()/100,ms5607.getTemperature()); + device.printf("S : Servo Operation\r\n"); + device.printf("M : Mode Change\r\n"); }else if(input_buff[1]=='F'){ Mode = FLIGHT; Phase = LAUNCH; @@ -290,9 +283,8 @@ device.printf("You will be able to reset only!\r\n"); }else{ device.printf("This command is not found >> %c\r\n",input_buff[1]); - device.printf("S >> STANDBY\r\n"); - device.printf("T >> TEST\r\n"); - device.printf("F >> FLIGHT\r\n"); + device.printf("S : STANDBY\r\n"); + device.printf("F : FLIGHT\r\n"); device.printf("-->>"); return; } @@ -307,23 +299,21 @@ if(input_buff[1]=='C') _Servo(1,LOCK); }else{ device.printf("This command is not found >> %c\r\n",input_buff[2]); - device.printf("U >> UNLOCK\r\n"); - device.printf("L >> LOCK\r\n"); + device.printf("U : UNLOCK\r\n"); + device.printf("L : LOCK\r\n"); device.printf("-->>"); return; } input_cnt = 0; device.printf(">>MAINMENU<<\r\n"); - device.printf("S >> Servo Operation\r\n"); - device.printf("M >> Mode Change\r\n"); + device.printf("S : Servo Operation\r\n"); + device.printf("M : Mode Change\r\n"); device.printf("-->>"); return; } device.printf("-->>"); input_cnt++; break; - case TEST: - break; case FLIGHT://reset only break; }