Diff: main.cpp

Revision:

11:b61d0fcc2ed3

Parent:

10:c1d1ff35a6a1

Child:

12:211d2c6d2afc

--- 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;
+}