From a653a2b0344596b86d956251f4e48497c3496f9d Mon Sep 17 00:00:00 2001
From: WildWesley <86993555+WildWesley@users.noreply.github.com>
Date: Fri, 13 May 2022 14:44:15 -0500
Subject: [PATCH] Simplify Initialization.cpp

---
 Initialization.cpp | 32 +++++++++++++++++++++++---------
 1 file changed, 23 insertions(+), 9 deletions(-)

diff --git a/Initialization.cpp b/Initialization.cpp
index eea7902..9382b08 100644
--- a/Initialization.cpp
+++ b/Initialization.cpp
@@ -4,23 +4,37 @@
 
 namespace States
 {  
-  uint16_t EE_STATE = 0;
-  
   void Initialization()
   {
+    unsigned long start = millis();
+
     Common::GPS_Data gps_data;
     Common::Sensor_Data sensor_data;
-    
     Hardware::read_gps(gps_data);
     Hardware::read_sensors(sensor_data);
 
-    Hardware::start_recording();
-
     String packet;
-    Hardware::build_packet(packet, "INITIALIZATION", "N", gps_data, sensor_data); // build new packet
-    Serial.println(packet);
+    Common::build_packet(packet, "INITIALIZATION", 'N', gps_data, sensor_data);
   
+    Hardware::mtx.lock();
     Hardware::ground_packets.enqueue(packet);
-    //Hardware::payload_packets.enqueue("0");
-  }
+    Hardware::payload_packets.enqueue("0");
+    Hardware::mtx.unlock();
+
+    if (Common::altitudes.itemCount() == 3)
+    {
+      float previous_altitude = Common::altitudes.dequeue();
+      float current_altitude = gps_data.altitude);
+      Common::altitudes.enqueue(current_altitude);
+    }
+
+    else if (Common::altitudes.itemCount() < 3)
+    {
+      float current_altitude = gps_data.altitude);
+      Common::altitudes.enqueue(current_altitude);
+    }
+  
+    if (Common::TELEMETRY_DELAY > (millis() - start))
+      delay(Common::TELEMETRY_DELAY - (millis() - start));
+    }
 }