adds apogee detection

nakujaproject · Dec 13, 2024 · 0465097 · 0465097
1 parent 79fe7e0
commit 0465097
Showing 1 changed file with 27 additions and 16 deletions.
diff --git a/n4-flight-software/src/main.cpp b/n4-flight-software/src/main.cpp
@@ -1096,13 +1096,15 @@ void checkFlightState(void* pvParameters) {
         // PREFLIGHT
         if(flight_data.alt_data.altitude < LAUNCH_DETECTION_THRESHOLD) {
             current_state = ARMED_FLIGHT_STATE::PRE_FLIGHT_GROUND;
+            debugln("PRE-FLIGHT");
         }
 
         // LAUNCH DETECTED -- LAUNCH DETECTED
         // POWERED FLIGHT STATE
         if(flight_data.alt_data.altitude > LAUNCH_DETECTION_THRESHOLD) {
             // launch detected
             current_state = ARMED_FLIGHT_STATE::POWERED_FLIGHT;
+            debugln("POW-FLIGHT");
         }
 
         // COASTING
@@ -1115,9 +1117,11 @@ void checkFlightState(void* pvParameters) {
         if((oldest_val - flight_data.alt_data.altitude) > APOGEE_DETECTION_THRESHOLD) {
             if(apogee_flag == 0) {
                 current_state = ARMED_FLIGHT_STATE::APOGEE;
+                debugln("APG");
                 // todo: deploy the drogue here ->
-//                delay(2); // simulate pyro firing
-//                current_state = ARMED_FLIGHT_STATE::DROGUE_DEPLOY;
+                delay(1500); // simulate pyro firing - remove this in production
+                current_state = ARMED_FLIGHT_STATE::DROGUE_DEPLOY;
+                debugln("DROG-DEP");
                 apogee_flag = 1;
             }
         }
@@ -1141,12 +1145,12 @@ void flightStateCallback(void* pvParameters) {
         switch (current_state) {
             // PRE_FLIGHT_GROUND
             case ARMED_FLIGHT_STATE::PRE_FLIGHT_GROUND:
-                debugln("PRE-FLIGHT STATE");
+                //debugln("PRE-FLIGHT STATE");
                 break;
 
             // POWERED_FLIGHT
             case ARMED_FLIGHT_STATE::POWERED_FLIGHT:
-                debugln("POWERED FLIGHT STATE");
+                //debugln("POWERED FLIGHT STATE");
                 break;
 
             // COASTING
@@ -1156,12 +1160,12 @@ void flightStateCallback(void* pvParameters) {
 
             // APOGEE
             case ARMED_FLIGHT_STATE::APOGEE:
-            //    debugln("APOGEE");
+                //debugln("APOGEE");
                 break;
 
             // DROGUE_DEPLOY
             case ARMED_FLIGHT_STATE::DROGUE_DEPLOY:
-                debugln("DROGUE DEPLOY");
+                //debugln("DROGUE DEPLOY");
                 drogueChuteDeploy();
                 break;
 
@@ -1445,6 +1449,10 @@ void mainChuteDeploy() {
     // }
 }
 
+void customCheckState() {
+
+}
+
 
 /*!****************************************************************************
  * @brief Setup - perform initialization of all hardware subsystems, create queues, create queue handles
@@ -1832,8 +1840,9 @@ void loop() {
         /**
          * Here is where we consume the test data stored in the data.txt file
          */
-
          if(current_test_state == TEST_STATES::DATA_CONSUME) {
+             vTaskResume(checkFlightStateTaskHandle);
+
              debugln("=============== Consuming test data ===============");
              telemetry_type_t test_data_packet;
 
@@ -1844,21 +1853,20 @@ void loop() {
 
                  if(col1 && col2) {
                      for(int row = 0; row < cp.getRowsCount(); row++) {
-                         //debug("row = ");
-                         //debug(row);
-                         //debug(", col_1 = ");
-                         //debug(col1[row]);
-                         //debug(", col_2 = ");
-                         //debugln(col2[row]);
+//                         debug("row = ");
+//                         debug(row);
+//                         debug(", col_1 = ");
+//                         debug(col1[row]);
+//                         debug(", col_2 = ");
+//                         debugln(col2[row]);
 
                          // set altitude as altitude read from queue
-                         //double alt = col2[row];
-                         double alt = col1[row];
+                         double alt = col2[row];
+                         debugln(alt);
                          test_data_packet.alt_data.altitude = alt;
 
                          // feed it into check-flight-state queue
                          xQueueSend(check_state_queue_handle, &test_data_packet, 0);
-
                      }
 
                      debugln("END OF FILE");
@@ -1872,6 +1880,9 @@ void loop() {
              }
 
          }
+         else if(current_test_state == TEST_STATES::DONE_TESTING) {
+             vTaskSuspend(checkFlightStateTaskHandle);
+         }
 
     //////////////////////////////////////////////////////////////////////////////////////////////
     //////////////////////////// END OF FLIGHT COMPUTER TESTING SYSTEM  ////////////////////////////