Diff: main.cpp

Revision:

5:b71a554ab4c4

Parent:

4:b878dce9c247

Child:

6:f2016544b9e4

--- a/main.cpp	Fri Feb 22 12:21:23 2019 +0000
+++ b/main.cpp	Fri Feb 22 13:02:57 2019 +0000
@@ -18,6 +18,12 @@
 設定値
 投下前に必ず確認！！
 *******************************************************************************/
+const float START_CONTROLL_TIME = 1.0f;
+const float START_CONTROLL_ALT = -5.0f;
+
+const float RECOVERY_TIME = 120.0f;
+const float RECOVERY_SPEED = -2.0f;
+
 bool wait_GPS = true;
 bool ok_PC = false;
 
@@ -54,8 +60,7 @@
 
 DigitalIn FlightPin(p15);
 
-Timeout timeout_stop;
-Timeout timeout_sep;
+Timeout timeout_start_controll;
 Timeout timeout_recovery;
 
 Timer time_main;
@@ -73,6 +78,8 @@
 void setup();   //無線あり
 
 void modeChange();  //無線あり
+void changePhase_RECOVERY();
+void startControllAttitude();
 
 void controllAttitude();
 
@@ -115,6 +122,7 @@
 int speed_time_old;
 float alt_buf[10], alt_ave, alt_ave_old, speed, speed_buf[10], speed_ave;
 int alt_count = 0, speed_count = 0;
+float alt_flight_start;
 
 int CO2, TVOCs;
 
@@ -234,32 +242,98 @@
 *******************************************************************************/
 void modeChange(){
     switch(CanSat_phase){
-        case CANSAT_SAFETY:
+        case CANSAT_SAFETY:////////////////////安全モード
         if(!do_first){
+            es920_using = true;
+            es920 << HEADER_START;
+            es920 << 'S';
+            es920.send();
+            es920_using = false;
             
+            do_first = true;
+        }
+        break;
+        
+        case CANSAT_WAIT:////////////////////待機モード
+        if(!do_first){
+            es920_using = true;
+            es920 << HEADER_START;
+            es920 << 'W';
+            es920.send();
+            es920_using = false;
+            
+            do_first = true;
+        }
+        if(flight_pin){
+            CanSat_phase = CANSAT_FLIGHT;
+            do_first = false;
         }
         break;
         
-        case CANSAT_WAIT:
+        case CANSAT_FLIGHT:////////////////////フライトモード
         if(!do_first){
+            es920_using = true;
+            es920 << HEADER_START;
+            es920 << 'F';
+            es920.send();
+            es920_using = false;
             
+            if(save_slow && !save_fast){
+                stopRec();
+                startRecFast();
+            }
+            else if(!save_slow && !save_fast){
+                startRecFast();
+            }
+            
+            time_flight.reset();
+            time_flight.start();
+            
+            alt_flight_start = alt;
+            timeout_start_controll.attach(&startControllAttitude, START_CONTROLL_TIME);
+            
+            timeout_recovery.attach(&changePhase_RECOVERY, RECOVERY_TIME);
+            
+            do_first = true;
         }
-        break;
-        
-        case CANSAT_FLIGHT:
-        if(!do_first){
-            
+        if(alt - alt_flight_start > START_CONTROLL_ALT){
+            startControllAttitude();
+        }
+        if(speed_ave > RECOVERY_SPEED){
+            changePhase_RECOVERY();
         }
         break;
         
         case CANSAT_RECOVERY:
         if(!do_first){
+            es920_using = true;
+            es920 << HEADER_START;
+            es920 << 'R';
+            es920.send();
+            es920_using = false;
             
+            stopRec();
+            startRecSlow();
+            
+            controll_yn = false;
+            
+            Buzzer.fire_on();
+            
+            do_first = true;
         }
         break;
     }
 }
 
+void changePhase_RECOVERY(){
+    CanSat_phase = CANSAT_RECOVERY;
+    do_first = false;
+}
+
+void startControllAttitude(){
+    controll_yn = true;
+}
+
 /*******************************************************************************
 姿勢制御
 *******************************************************************************/