![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
2017年3月,伊豆大島共同打上実験 CORE_缶ロケチーム電装
Dependencies: MPU6050 MS5607 mbed SDFileSystem
Diff: main.cpp
- Revision:
- 11:b61d0fcc2ed3
- Parent:
- 10:c1d1ff35a6a1
- Child:
- 12:211d2c6d2afc
diff -r c1d1ff35a6a1 -r b61d0fcc2ed3 main.cpp --- a/main.cpp Thu Feb 23 15:05:50 2017 +0000 +++ b/main.cpp Thu Mar 02 22:59:12 2017 +0000 @@ -61,10 +61,10 @@ #define DROP 3 #define P0 1013.25f//海面気圧[hPa] +#define ACC 4096.0f//加速度オフセット値 /*ピン指定*/ MS5607I2C ms5607(p9, p10, false); MPU6050 mpu(p9,p10); -DigitalIn sw(p21); BusOut myled(LED1,LED2,LED3,LED4); SDFileSystem sd(p11, p12, p13, p14, "sd"); Serial device(USBTX, USBRX); @@ -80,26 +80,29 @@ bool row = 0; int8_t col = 0; /*ログ格納用*/ -float pressure[2][RATE_LOG],temperature[2][RATE_LOG]; +float altitude[2][RATE_LOG],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 = STANDBY; /*判定用*/ -float a_buff; -float alt,alt_max,alt_launch; +float alt4open[RATE_OPEN],acc4open; +float alt_max,alt_launch; float t_drop,t_top,t_launch; -int8_t cnt_launch = 0; -int8_t cnt_top = 0; -int8_t cnt_drop = 0; +int8_t cnt_judge = 0; +int8_t col_open = 0; /*関数*/ -float getAlt(float press, float temp); void _Open(); void _Servo(int8_t door_num, float motion); void _Log(); +float _GetAlt(float press, float temp); +float _Median(float data[], int num); + /*---------------------------------------------------------------------*/ int main() { + mpu.setAcceleroRange(2); + mpu.setGyroRange(2); timer.start(); Door_1_1.period_ms(20); Door_1_2.period_ms(20); @@ -117,43 +120,56 @@ myled = 1 << (Phase-1); switch (Phase) { case STANDBY://スタンバイモード(発射判定不可) - if(sw==0)Phase = LAUNCH; + Phase = LAUNCH; break; case LAUNCH://点火モード(発射判定可) - alt_launch = alt; - if((-1)*a_buff>ACC_LAUNCH){ - if(cnt_launch++==3) Phase = FLIGHT; + acc4open = (float)mpu.getAcceleroRawZ()/(ACC*0.981); + alt4open[col_open] = _GetAlt(ms5607.getPressure()/100,ms5607.getTemperature()); + if(acc4open>ACC_LAUNCH){ + if(cnt_judge++==3){ + Phase = FLIGHT; + alt_launch = _Median(alt4open, RATE_OPEN); + cnt_judge = 0; + } t_launch = timer.read(); - alt_max = alt; + alt_max = alt_launch; }else{ - if(timer.read()>t_launch+1.0) cnt_launch = 0; + if(timer.read()>t_launch+1.0) cnt_judge = 0; } + device.printf("%f\r\n",acc4open); break; case FLIGHT://飛翔中(パラシュート開放判定) - if(alt>alt_max) alt_max = alt; - if(alt<alt_max-DROP_TOP){ - if(cnt_top++==3){ + alt4open[col_open] = _GetAlt(ms5607.getPressure()/100,ms5607.getTemperature()); + if(alt4open[col_open]>alt_max) alt_max = alt4open[col_open]; + if(alt4open[col_open]<alt_max-DROP_TOP){ + if(cnt_judge++==3){ _Servo(1,UNLOCK); Phase = DROP; + cnt_judge = 0; } t_top = timer.read(); }else{ - if(timer.read()>t_top+1.0) cnt_top = 0; + if(timer.read()>t_top+1.0) cnt_judge = 0; } if(timer.read()-t_launch>TIME_TOP){ _Servo(1,UNLOCK); Phase = DROP; + cnt_judge = 0; } break; case DROP://降下中(缶サット開放判定) - if(alt < alt_launch+ALT_CAN){ - if(cnt_drop++==3)_Servo(2,UNLOCK); + if(alt4open[col_open] < alt_launch+ALT_CAN){ + if(cnt_judge++==3){ + _Servo(2,UNLOCK); + cnt_judge = 0; + } t_drop = timer.read(); }else{ - if(timer.read()>t_drop+1.0)cnt_drop = 0; + if(timer.read()>t_drop+1.0)cnt_judge = 0; } break; } + if(col_open++==RATE_OPEN) col_open = 0; } /*記録用関数 RATE_LOG[Hz]で記録を行う*/ void _Log(){ @@ -162,11 +178,9 @@ temperature[row][col] = ms5607.getTemperature(); mpu.getAccelero(&acc[row][col][0]); mpu.getGyro(&gyro[row][col][0]); - a_buff = acc[row][col][2]; - alt = getAlt(pressure[row][col],temperature[row][col]); 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], - acc[row][col][1],acc[row][col][2],gyro[row][col][0],gyro[row][col][1],gyro[row][col][2] + 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){ fclose(fp); @@ -193,6 +207,27 @@ } } -float getAlt(float press, float temp){ +float _GetAlt(float press, float temp){ return (pow((P0/press), (1.0f/5.257f))-1.0f)*(temp+273.15f)/0.0065f; } + +float _Median(float data[], int num){ + float median; + float *sort = (float *)malloc(sizeof(float)*num); + for(int i=0; i<num; i++) sort[i] = data[i]; + for(int i=0; i<num; i++){ + for(int j=0; j<num-i-1; j++){ + if(sort[j]>sort[j+1]){ + float buff = sort[j+1]; + sort[j+1] = sort[j]; + sort[j] = buff; + } + } + } + + if(num%2!=0)median = sort[num/2]; + else median = (sort[num/2-1]+sort[num/2])/2.0; + + free(sort); + return median; +}