diff --git a/.github/workflows/clang-tidy.yml b/.github/workflows/clang-tidy.yml
new file mode 100644
index 0000000..0374fb2
--- /dev/null
+++ b/.github/workflows/clang-tidy.yml
@@ -0,0 +1,51 @@
+name: Clang-Tidy Check
+
+on:
+  #triggers the workflow on pull requests
+  pull_request:
+    branches:
+      - '**'
+
+  #allows manual triggering of the workflow
+  workflow_dispatch:
+
+jobs:
+  clang-tidy-check:
+    runs-on: ubuntu-latest
+
+    steps:
+      #gets the current repo
+      - name: Checkout Avionics repo
+        uses: actions/checkout@v4
+        with:
+          path: Avionics
+      
+      #clones the native repo
+      - name: Clone Native repo
+        run: |
+          git clone https://github.com/CURocketEngineering/Native native
+
+      #replaces the linked Avionics repo with the current Avionics code
+      - name: Replace Native lib Avionics with checked-out Avionics code
+        run: |
+          rm -rf native/lib/Avionics
+          mkdir -p native/lib
+          mv Avionics native/lib/Avionics
+
+      #installed PlatformIO core
+      - name: Install PlatformIO Core
+        run: |
+          pip install -U platformio
+
+      #shows the file tree structure for debugging
+      - name: Show native directory structure
+        run: |
+          echo "===== native directory tree ====="
+          ls -R native
+
+      #runs clang-tidy check
+      - name: Run Clang-Tidy Check
+        working-directory: ./native
+        run: pio check --fail-on-defect=low --fail-on-defect=medium --fail-on-defect=high
+
+
diff --git a/hal/ArduinoHAL.h b/hal/ArduinoHAL.h
index b55d263..14137e0 100644
--- a/hal/ArduinoHAL.h
+++ b/hal/ArduinoHAL.h
@@ -3,30 +3,26 @@
 
 #ifdef ARDUINO
 #include "Arduino.h"
-#include <SPI.h>
-#include <Adafruit_Sensor.h>
 
 #include <Adafruit_SPIFlash.h>
+#include <Adafruit_Sensor.h>
 #include <SdFat.h>
 #include <SdFatConfig.h>
+#include <SPI.h>
 
 #else // Everything below will only be compiled if we are not on an Arduino
 
-#include <string>
-using String = std::string;
-
-#include <iostream>
-#include <cstring>
-
 #include <chrono>
-
-#include "spi_mock.h"
-
-// Within here we must define the mock functions for the Arduino functions
+#include <cstring>
+#include <iostream>
+#include <string>
 #include <thread>
 
 #include "Adafruit_SPIFlash_mock.h"
 #include "serial_mock.h"
+#include "spi_mock.h"
+
+using String = std::string;
 
 #define OUTPUT 1
 #define INPUT 0
@@ -55,4 +51,4 @@ inline void delay(unsigned long ms) { // NOLINT
 
 
 #endif // ARDUINO
-#endif // ARDUINOHAL_H
\ No newline at end of file
+#endif // ARDUINOHAL_H
diff --git a/include/data_handling/CircularArray.h b/include/data_handling/CircularArray.h
index aedc98a..024e817 100644
--- a/include/data_handling/CircularArray.h
+++ b/include/data_handling/CircularArray.h
@@ -2,11 +2,14 @@
 #define CIRCULARARRAY_H
 
 #include <algorithm>
+#include <array>
+#include <assert.h>
 #include <cstdint>
-#include <vector>
 
-template<typename T>
-int partition(std::vector<T>& array, int left, int right, int pivotIndex) {
+constexpr std::size_t MAX_CIRCULAR_ARRAY_CAPACITY = 255;
+
+template<typename T, std::size_t N>
+int partition(std::array<T, N>& array, int left, int right, int pivotIndex) {
     T pivotValue = array[pivotIndex];
     std::swap(array[pivotIndex], array[right]); // Move pivot to end
     int storeIndex = left;
@@ -26,8 +29,8 @@ int partition(std::vector<T>& array, int left, int right, int pivotIndex) {
 // O(n) average time complexity
 // O(n^2) worst case time complexity
 // If we were to just use bubble sort, that would be O(n^2) time complexity
-template<typename T>
-T quickSelect(std::vector<T> &array, int left, int right, int k){
+template<typename T, std::size_t N>
+T quickSelect(std::array<T, N> &array, int left, int right, int k){
     while (left < right){
         int pivotIndex = (left + right) / 2;
         int pivotNewIndex = partition(array, left, right, pivotIndex);
@@ -43,7 +46,7 @@ T quickSelect(std::vector<T> &array, int left, int right, int k){
 }
 
 
-template<typename T>
+template<typename T, std::size_t Capacity>
 /**
  * @brief Fixed-size circular buffer with median helper and head tracking.
  * @note When to use: maintain a rolling window of recent samples for filters
@@ -51,38 +54,40 @@ template<typename T>
  */
 class CircularArray {
   protected:
-    std::vector<T> array; 
-    uint8_t head;       // 0 to 255
+    std::array<T, Capacity> array; 
+    std::array<T, Capacity> scratchArray; // For median calculation
     uint8_t maxSize;    // 0 to 255
-    uint16_t pushCount; // 0 to 65535
+    uint8_t head;       // 0 to 255
+    uint8_t currentSize; // 0 to 255
 
   public:
-    CircularArray(uint8_t maxSize){
-        this->maxSize = maxSize;
+    CircularArray(uint8_t maxSize = Capacity) : maxSize(maxSize) {
+        static_assert(Capacity > 0, "CircularArray capacity must be greater than 0");
+        static_assert(Capacity <= MAX_CIRCULAR_ARRAY_CAPACITY, "CircularArray capacity must be less than or equal to 255 b/c of head being uint8_t");
+        assert(maxSize > 0 && maxSize <= Capacity);
         this->head = 0;
-        this->pushCount = 0;
-        this->array = std::vector<T>(maxSize, T());
-    }
-
-    ~CircularArray(){
-        array.clear();
+        this->currentSize = 0; // How full is the circular buffer? 
     }
 
     void push(T data){
         // After the first push, start moving the head
-        if (pushCount)
+        if (currentSize)
             head = (head + 1) % maxSize;
         array[head] = data;
-        pushCount++;
+
+        // Cap current size at maxSize
+        if (currentSize < maxSize){
+            currentSize++;
+        }
     }
 
     T pop(){
-        if (pushCount == 0){
+        if (currentSize == 0){
             return T();
         }
         T data = array[head];
         head = (head + maxSize - 1) % maxSize;
-        pushCount--;
+        currentSize--;
         return data;
     }
 
@@ -93,11 +98,11 @@ class CircularArray {
 
     // Has the circular array been filled          
     bool isFull(){
-        return pushCount >= maxSize;
+        return currentSize >= maxSize;
     }
     
     bool isEmpty(){
-        return pushCount == 0;
+        return currentSize == 0;
     }
 
     uint8_t getHead(){
@@ -109,28 +114,27 @@ class CircularArray {
     }
 
     T getMedian(){
-        if (pushCount == 0) {
+        if (currentSize == 0) {
         // Handle the case when the array is empty
         return T();
         }
 
-        size_t count = std::min(static_cast<size_t>(pushCount), static_cast<size_t>(maxSize));
-        std::vector<T> copyArray(count);
+        size_t count = std::min(static_cast<size_t>(currentSize), static_cast<size_t>(maxSize));
 
         // Collect the valid elements from the circular array
-        // TODO: Optimize (or just copy everything and restrict this function to only running when the array is full)
         for (size_t i = 0; i < count; ++i) {
-            copyArray[i] = array[(head + maxSize - i) % maxSize];
+            scratchArray[i] = array[(head + maxSize - i) % maxSize];
         }
 
         // Find the median
-        return quickSelect(copyArray, 0, count - 1, count / 2);
+        int n = static_cast<int>(count);
+        return quickSelect(scratchArray, 0, n - 1, n / 2);
     }
 
     void clear(){
         head = 0;
-        pushCount = 0;
-        for (int i = 0; i < maxSize; i++){
+        currentSize = 0;
+        for (uint8_t i = 0; i < maxSize; i++){
             array[i] = T();
         }
     }
@@ -139,4 +143,4 @@ class CircularArray {
 
 
 
-#endif
\ No newline at end of file
+#endif
diff --git a/include/data_handling/DataSaver.h b/include/data_handling/DataSaver.h
index d40a0e8..a0266b9 100644
--- a/include/data_handling/DataSaver.h
+++ b/include/data_handling/DataSaver.h
@@ -53,6 +53,11 @@ class IDataSaver {
             // Default implementation does nothing
         }
 
+        // default method that does nothing, can be overridden
+        virtual void clearPostLaunchMode(){
+            // default implementation does nothing
+        }
+
 };
 
 
diff --git a/include/data_handling/Telemetry.h b/include/data_handling/Telemetry.h
index 3715251..763d745 100644
--- a/include/data_handling/Telemetry.h
+++ b/include/data_handling/Telemetry.h
@@ -1,98 +1,237 @@
 #ifndef TELEMETRY_H
 #define TELEMETRY_H
 
-#include "data_handling/DataPoint.h"
-#include "data_handling/SensorDataHandler.h"
+#include <array>
+#include <cstddef>
 #include <cstdint>
-#include <unordered_map>
-#include <vector>
+#include <cstring>
+
 #include "ArduinoHAL.h"
+#include "data_handling/SensorDataHandler.h"
+
+/**
+ * @file Telemetry.h
+ * @brief Packs SensorDataHandler values into a fixed-size byte packet and streams over a Stream (UART).
+ *
+ * Design goals:
+ * - Keep Telemetry as a normal class (non-templated) so implementation lives in Telemetry.cpp.
+ * - Allow callers to provide the list of telemetry streams as either:
+ *   - pointer + count (most general)
+ *   - std::array (compile-time sized, convenient)
+ *   - C array (compile-time sized, convenient)
+ *
+ * Lifetime rule:
+ * - Telemetry stores a non-owning pointer to the caller-provided stream list.
+ *   The stream list must outlive the Telemetry instance (use static storage in embedded code).
+ */
+
+namespace TelemetryFmt {
+
+/** Maximum packet size (bytes). Must match your radio/modem configuration. */
+constexpr std::size_t kPacketCapacity = 120;
+
+/** Header markers: 3 sync zeros followed by a start byte. */
+constexpr std::size_t kSyncZeros = 3;
+
+/** Number of bytes in a packed 32-bit value. */
+constexpr std::size_t kU32Bytes = 4;
+
+/** Header layout: [0..2]=0, [3]=START, [4..7]=timestamp (big-endian). */
+constexpr std::size_t kStartByteIndex = kSyncZeros;          // 3
+constexpr std::size_t kTimestampIndex = kStartByteIndex + 1; // 4
+constexpr std::size_t kHeaderBytes    = kSyncZeros + 1 + kU32Bytes;
+
+/** End marker layout: 3 zeros followed by an end byte. */
+constexpr std::size_t kEndMarkerBytes = kSyncZeros + 1;
+
+/** Start-of-packet marker byte value. */
+constexpr std::uint8_t kStartByteValue = 51;
+
+/** End-of-packet marker byte value. */
+constexpr std::uint8_t kEndByteValue = 52;
+
+/** 32-bit helper constants */
+constexpr std::size_t kBytesIn32Bit = 4;
+constexpr unsigned kBitsPerByte = 8;
+constexpr std::uint8_t kAllOnesByte = 0xFF;
+
+/** Assumptions used by float packing. */
+static_assert(sizeof(std::uint32_t) == 4, "Expected 32-bit uint32_t");
+static_assert(sizeof(float) == 4, "Expected 32-bit float");
+
+/**
+ * @brief Write a 32-bit value in big-endian order to dst[0..3].
+ */
+inline void write_u32_be(std::uint8_t* dst, std::uint32_t v) {
+
+    for (std::size_t i = 0; i < kBytesIn32Bit; ++i) {
+        const unsigned shift = static_cast<unsigned>((kBytesIn32Bit - 1 - i) * kBitsPerByte);
+        dst[i] = static_cast<std::uint8_t>(v >> shift);
+    }
+}
 
-#define START_BYTE 51
-#define END_BYTE 52
 
 /**
- * @struct SendableSensorData
- * @brief Bundles one or more SensorDataHandler pointers for telemetry packing.
- * @param singleSDH        Single handler to send when not using an array.
- * @param multiSDH         Array of handlers to send together (optional).
- * @param multiSDHLength   Length of the multiSDH array.
- * @param multiSDHDataLabel Label used for the multi array payload.
- * @param sendFrequencyHz  Desired transmission rate in hertz.
- * @note When to use: define the telemetry mix (single vs. grouped streams) fed
- *       to Telemetry before calling tick().
+ * @brief Convert frequency (Hz) to period (ms), using integer math.
+ *
+ * Uses ceil(1000 / Hz). If Hz == 0, returns 1000ms as a safe fallback.
+ */
+inline std::uint16_t hz_to_period_ms(std::uint16_t hz) {
+    return (hz == 0) ? 1000u
+                     : static_cast<std::uint16_t>((1000u + hz - 1u) / hz);
+}
+
+} // namespace TelemetryFmt
+
+// Backwards-compatible names if existing code uses START_BYTE / END_BYTE.
+static const std::uint8_t START_BYTE = TelemetryFmt::kStartByteValue;
+static const std::uint8_t END_BYTE   = TelemetryFmt::kEndByteValue;
+
+/**
+ * @brief Declares one telemetry "stream" to include in packets.
+ *
+ * A stream can be either:
+ * - single SDH: write the SDH name followed by its float value
+ * - multi SDH group: write a group label followed by N float values
+ *
+ * This type does not own any SensorDataHandler objects.
+ * Pointers must remain valid while Telemetry uses them.
+ *
+ * For multi groups, you can provide either:
+ * - raw pointer + count (most general)
+ * - std::array (preferred when size is fixed at compile time)
  */
 struct SendableSensorData {
+    // --- Payload configuration ---
+
+    /** Single-value stream. If non-null, this stream will send this SDH. */
     SensorDataHandler* singleSDH;
-    SensorDataHandler** multiSDH;
-    int multiSDHLength;
-    int multiSDHDataLabel;
-    int sendFrequencyHz;
-    uint32_t lastSentTimestamp;
-
-    SendableSensorData(SensorDataHandler* _singleSDH, SensorDataHandler** _multiSDH, int _multiSDHLength, int _multiSDHDataLabel, uint8_t _sendFrequencyHz) {
-        singleSDH = _singleSDH;
-        multiSDH = _multiSDH;
-        multiSDHLength = _multiSDHLength;
-        multiSDHDataLabel = _multiSDHDataLabel;
-        sendFrequencyHz = _sendFrequencyHz;
-        lastSentTimestamp = 0;
-    }
-    
+
+    /** Multi-value group. Pointer to an external array of SDHs. */
+    SensorDataHandler* const* multiSDH;
+
+    /** Length of multiSDH array. */
+    // Shall not be changed after construction
+    // Used to iterate over multiSDH
+    const std::size_t multiSDHLength;
+
+    /** Label written once before the multi SDH values. */
+    std::uint8_t multiSDHDataLabel;
+
+    // --- Scheduling state ---
+
+    /** Minimum time between sends for this stream. */
+    std::uint16_t periodMs;
+
+    /** Last send time in ms (same time base as tick()). */
+    std::uint32_t lastSentTimestamp;
+
+    /**
+     * @brief Create a single SDH stream.
+     * @param sdh SensorDataHandler to send (non-owning).
+     * @param sendFrequencyHz Desired send rate in Hz.
+     */
+    SendableSensorData(SensorDataHandler* sdh, std::uint16_t sendFrequencyHz)
+        : singleSDH(sdh),
+          multiSDH(0),
+          multiSDHLength(0),
+          multiSDHDataLabel(0),
+          periodMs(TelemetryFmt::hz_to_period_ms(sendFrequencyHz)),
+          lastSentTimestamp(0) {}
+
+    /**
+     * @brief Create a multi SDH stream from a std::array.
+     *
+     * This is convenient when the group size is fixed at compile time.
+     * The std::array passed in must outlive this SendableSensorData object.
+     */
+    template <std::size_t M>
+    SendableSensorData(const std::array<SensorDataHandler*, M>& sdhList,
+                       std::uint8_t label,
+                       std::uint16_t sendFrequencyHz)
+        : singleSDH(0),
+          multiSDH(sdhList.data()),
+          multiSDHLength(M),
+          multiSDHDataLabel(label),
+          periodMs(TelemetryFmt::hz_to_period_ms(sendFrequencyHz)),
+          lastSentTimestamp(0) {}
+
     /**
-     * @brief True if the packet should be sent
+     * @brief Return true if enough time has elapsed such that this stream wants to be sent again.
      */
-    bool shouldBeSent(uint32_t time) {
-        uint32_t delta = time-lastSentTimestamp;
-        return delta >= (1000.0/sendFrequencyHz);
+    bool shouldBeSent(std::uint32_t now) const {
+        return (now - lastSentTimestamp) >= periodMs;
     }
 
     /**
-     * @brief Run when the packet is sent
+     * @brief Update internal state after sending.
      */
-    void markWasSent(uint32_t time) {
-        lastSentTimestamp = time;
+    void markWasSent(std::uint32_t now) {
+        lastSentTimestamp = now;
     }
+
+    /** @brief Convenience: true if configured as a single SDH stream. */
+    bool isSingle() const { return singleSDH != 0; }
+
+    /** @brief Convenience: true if configured as a multi SDH stream. */
+    bool isMulti() const { return (multiSDH != 0) && (multiSDHLength != 0); }
 };
 
 /**
- * @brief Packages sensor data into fixed-size packets and streams over UART.
- * @note When to use: periodic downlink of key channels during flight; call
- *       tick() every loop to honor per-stream send rates.
+ * @brief Packetizes telemetry streams and sends them out over a Stream.
+ *
+ * Usage pattern:
+ * - Construct Telemetry with a stable list of SendableSensorData pointers.
+ * - Call tick(currentTimeMs) every loop.
+ *
+ * Lifetime rule:
+ * - Telemetry stores a pointer to the provided list of streams (non-owning).
+ *   The list must outlive Telemetry.
  */
 class Telemetry {
-    public:
-    
-        /**
-         * @brief Initialize this object
-         * @param ssdArray          Array of pointers to SendableSensorData.
-         * @param ssdArrayLength    Number of entries in ssdArray.
-         * @param rfdSerialConnection Stream connected to the radio/modem.
-         * @note When to use: instantiate once during setup with the telemetry
-         *       layout you need for the current flight profile.
-         */
-        Telemetry(SendableSensorData* ssdArray[], int ssdArrayLength, Stream &rfdSerialConnection);
-
-        /**
-         * @attention MUST BE RUN EVERY LOOP
-         * @brief No argument tick function that handles sending data at
-         * specified send frequencies.
-         * @return true if a packet was sent after calling this function.
-         */
-        bool tick(uint32_t currentTime);
-
-    private:
-        void preparePacket(uint32_t timestamp);
-        void addSingleSDHToPacket(SensorDataHandler* sdh);
-        void addSSDToPacket(SendableSensorData* ssd);
-        void setPacketToZero();
-        void addEndMarker();
-
-        SendableSensorData** ssdArray;
-        int ssdArrayLength;
-        Stream &rfdSerialConnection;
-        int nextEmptyPacketIndex;
-        uint8_t packet[120]; //rfd settings indicate that 120 is the max packet size
+public:
+
+    /**
+     * @brief Construct from std::array (convenient and compile-time sized).
+     * The std::array must outlive the Telemetry instance.
+     */
+    template <std::size_t N>
+    Telemetry(const std::array<SendableSensorData*, N>& streams,
+              Stream& rfdSerialConnection)
+        : streams(streams.data()),
+          streamCount(N),
+          rfdSerialConnection(rfdSerialConnection),
+          nextEmptyPacketIndex(0),
+          packet{} {}
+    /**
+     * @brief Call every loop to send due telemetry streams.
+     * @param currentTimeMs Current time in milliseconds.
+     * @return true if a packet was sent on this tick.
+     */
+    bool tick(std::uint32_t currentTimeMs);
+
+private:
+    // Packet building helpers
+    void preparePacket(std::uint32_t timestamp);
+    void addSingleSDHToPacket(SensorDataHandler* sdh);
+    void addSSDToPacket(SendableSensorData* ssd);
+    void setPacketToZero();
+    void addEndMarker();
+
+    // Non-owning view of the stream list
+    SendableSensorData* const* streams;
+
+    // Number of streams in the list
+    // Shall not be changed after construction
+    // Used to iterate over streams
+    const std::size_t streamCount;
+
+    // Output
+    Stream& rfdSerialConnection;
+
+    // Packet state
+    std::size_t nextEmptyPacketIndex;
+    std::array<std::uint8_t, TelemetryFmt::kPacketCapacity> packet;
 };
 
-#endif
\ No newline at end of file
+#endif
diff --git a/include/state_estimation/BurnoutStateMachine.h b/include/state_estimation/BurnoutStateMachine.h
index 0c6aa16..a4dfe8e 100644
--- a/include/state_estimation/BurnoutStateMachine.h
+++ b/include/state_estimation/BurnoutStateMachine.h
@@ -5,10 +5,10 @@
 #include "data_handling/DataSaver.h"
 
 #include "state_estimation/ApogeeDetector.h"
+#include "state_estimation/BaseStateMachine.h"
 #include "state_estimation/LaunchDetector.h"
 #include "state_estimation/StateEstimationTypes.h"
 #include "state_estimation/States.h"
-#include "state_estimation/BaseStateMachine.h"
 
 /**
  * @brief State machine variant that explicitly models motor burnout before coast.
@@ -52,4 +52,4 @@ class BurnoutStateMachine : public BaseStateMachine {
 };
 
 
-#endif
\ No newline at end of file
+#endif
diff --git a/include/state_estimation/FastLaunchDetector.h b/include/state_estimation/FastLaunchDetector.h
new file mode 100644
index 0000000..7c4e7cf
--- /dev/null
+++ b/include/state_estimation/FastLaunchDetector.h
@@ -0,0 +1,45 @@
+#ifndef FAST_LAUNCH_DETECTOR_H
+#define FAST_LAUNCH_DETECTOR_H
+ // need to figure out how to impliment checking for launch from LaunchDetector
+#include "data_handling/CircularArray.h"
+#include "data_handling/DataPoint.h"
+#include "state_estimation/StateEstimationTypes.h"
+
+// Potential returns from the update function
+// Positive values are errors
+// Negative values are warnings
+enum FastLaunchDetectorStatus {
+    FLD_LAUNCH_DETECTED = 0,
+    FLD_ALREADY_LAUNCHED = -1,
+    FLD_ACL_TOO_LOW = -2, // The acceleration is too low for launch
+    FLD_DEFAULT_FAIL = 2,
+};
+
+
+class FastLaunchDetector
+{
+public:
+    /**
+    * Constructor
+    * @param accelerationThreshold_ms2: The threshold for acceleration to be considered a launch
+    * @param confirmationWindow_ms: The time window in ms to confirm the launch using LaunchDetector
+    */
+    FastLaunchDetector(float accelerationThreshold, uint32_t confirmationWindow_ms = 500);
+
+    int update(AccelerationTriplet accel);
+
+    bool hasLaunched() const { return launched; }
+    uint32_t getLaunchedTime() const { return launchedTime_ms; }
+    uint32_t getConfirmationWindow() const { return confirmationWindow_ms; }
+    void reset();
+
+private:
+    float accelerationThresholdSq_ms2;
+
+    bool launched;
+    uint32_t launchedTime_ms;
+    uint32_t confirmationWindow_ms;
+};
+
+
+#endif
\ No newline at end of file
diff --git a/include/state_estimation/GroundLevelEstimator.h b/include/state_estimation/GroundLevelEstimator.h
index 4c32af5..571eebe 100644
--- a/include/state_estimation/GroundLevelEstimator.h
+++ b/include/state_estimation/GroundLevelEstimator.h
@@ -1,6 +1,9 @@
 #ifndef AGL_DETECTOR_H
 #define AGL_DETECTOR_H
 #include <cstdint>
+
+#define DEFAULT_GLE_ALPHA 0.1F
+
 /*
 TWO RULES - 2 input functions, 1 output:
 
@@ -17,7 +20,7 @@ class GroundLevelEstimator{
         /**
          * @brief Constructs a GroundLevelEstimator.
          */
-        GroundLevelEstimator();
+        GroundLevelEstimator(float alpha = DEFAULT_GLE_ALPHA);
 
         /**
          * @brief Updates the ground level estimate or converts ASL to AGL.
@@ -46,10 +49,10 @@ class GroundLevelEstimator{
 
     private:
 
-        bool launched = false; //Turned true if launch is detected
-        float estimatedGroundLevel_m = 0.0F; //EGL in meters
-        uint32_t sampleCount = 0; //Number of samples used for ground level estimate
-        float alpha; //Determines how much weight the most recent number added has on the current EGL
+        bool launched = false; // Turned true if launch is detected
+        float estimatedGroundLevel_m = 0.0F; // EGL in meters
+        uint32_t sampleCount = 0; // Number of samples used for ground level estimate
+        float alpha; // Determines how much weight the most recent number added has on the current EGL
 
 };
 
diff --git a/include/state_estimation/LaunchDetector.h b/include/state_estimation/LaunchDetector.h
index 40ce71e..5d7e49b 100644
--- a/include/state_estimation/LaunchDetector.h
+++ b/include/state_estimation/LaunchDetector.h
@@ -12,6 +12,7 @@
 #include "state_estimation/StateEstimationTypes.h"
 
 constexpr float ACCEPTABLE_PERCENT_DIFFERENCE_WINDOW_INTERVAL = 0.5F;
+constexpr std::size_t CIRCULAR_ARRAY_ALLOCATED_SLOTS = 100; // 100 slots allocated for the circular array (100 * sizeof(DataPoint)) = 800 bytes allocated)
 
 // Potential returns from the update function
 // Positive values are errors
@@ -77,7 +78,7 @@ class LaunchDetector
     // Testing Methods
     // --------------
     // Gives a pointer to the window
-    CircularArray<DataPoint>* getWindowPtr() {return &AclMagSqWindow_ms2;}
+    CircularArray<DataPoint, CIRCULAR_ARRAY_ALLOCATED_SLOTS>* getWindowPtr() {return &AclMagSqWindow_ms2;}
     // Gives the threshold in ms^2 squared
     float getThreshold() {return accelerationThresholdSq_ms2;}
     // Gives the window interval in ms
@@ -96,7 +97,7 @@ class LaunchDetector
 
     uint16_t acceptableTimeDifference_ms;
     // The window holding the acceleration magnitude squared b/c sqrt is expensive
-    CircularArray<DataPoint> AclMagSqWindow_ms2;
+    CircularArray<DataPoint, CIRCULAR_ARRAY_ALLOCATED_SLOTS> AclMagSqWindow_ms2;
     bool launched;
     uint32_t launchedTime_ms;
 
diff --git a/include/state_estimation/StateMachine.h b/include/state_estimation/StateMachine.h
index 22375f0..0e0a738 100644
--- a/include/state_estimation/StateMachine.h
+++ b/include/state_estimation/StateMachine.h
@@ -1,14 +1,14 @@
 #ifndef FLIGHT_STATE_MACHINE_H
 #define FLIGHT_STATE_MACHINE_H
 
-#include "state_estimation/States.h"
+#include "data_handling/DataPoint.h"
+#include "data_handling/DataSaver.h"
 #include "state_estimation/ApogeeDetector.h"
+#include "state_estimation/BaseStateMachine.h"
+#include "state_estimation/FastLaunchDetector.h"
 #include "state_estimation/LaunchDetector.h"
+#include "state_estimation/States.h"
 #include "state_estimation/VerticalVelocityEstimator.h"
-#include "state_estimation/BaseStateMachine.h"
-
-#include "data_handling/DataPoint.h"
-#include "data_handling/DataSaver.h"
 
 /**
  * @brief Nominal flight state machine using launch/apogee detection and VVE.
@@ -27,7 +27,7 @@ class StateMachine : public BaseStateMachine {
      *       estimator/detector instances.
      */
     StateMachine(IDataSaver* dataSaver, LaunchDetector* launchDetector, ApogeeDetector* apogeeDetector, 
-                 VerticalVelocityEstimator* verticalVelocityEstimator);
+                 VerticalVelocityEstimator* verticalVelocityEstimator, FastLaunchDetector* fastLaunchDetector);
 
     /**
      * @brief Process new sensor data and transition states if thresholds are met.
@@ -49,7 +49,9 @@ class StateMachine : public BaseStateMachine {
     LaunchDetector* launchDetector;
     ApogeeDetector* apogeeDetector;
     VerticalVelocityEstimator* verticalVelocityEstimator;
+    FastLaunchDetector* fastLaunchDetector;
+    uint32_t fldLaunchTime_ms = 0;
 };
 
 
-#endif
\ No newline at end of file
+#endif
diff --git a/include/state_estimation/States.h b/include/state_estimation/States.h
index 4a3d389..5fe7140 100644
--- a/include/state_estimation/States.h
+++ b/include/state_estimation/States.h
@@ -12,13 +12,14 @@
 enum FlightState {
     STATE_UNARMED,
     STATE_ARMED,
+    STATE_SOFT_ASCENT,
     STATE_ASCENT, // Don't use the ascent state if you are already using powered ascent and coast ascent
     STATE_POWERED_ASCENT,
     STATE_COAST_ASCENT,
     STATE_DESCENT,
     STATE_DROGUE_DEPLOYED,
     STATE_MAIN_DEPLOYED,
-    STATE_LANDED
+    STATE_LANDED,
 };
 
 #endif
\ No newline at end of file
diff --git a/src/data_handling/Telemetry.cpp b/src/data_handling/Telemetry.cpp
index 7105c11..ccbd7ef 100644
--- a/src/data_handling/Telemetry.cpp
+++ b/src/data_handling/Telemetry.cpp
@@ -2,88 +2,123 @@
 #include "ArduinoHAL.h"
 #include <algorithm>
 
-Telemetry::Telemetry(SendableSensorData* ssdArray[], int ssdArrayLength, Stream &rfdSerialConnection)
-    : rfdSerialConnection(rfdSerialConnection)
-{
-    this->ssdArray = ssdArray;
-    this->ssdArrayLength = ssdArrayLength;
-    //TODO: it would be nice if we could throw an error at compile time
-    //if a user's desired max packet size as specified from what they put in
-    //ssdArray is larger than this value.
+// Helpers for checking if the packet has room for more data
+std::size_t bytesNeededForSSD(const SendableSensorData* ssd) {
+    // Each SSD writes 1 label byte (name/label) plus 4 bytes per float value.
+    if (ssd->isSingle()) {
+        return 1U + TelemetryFmt::kBytesIn32Bit;
+    }
+    if (ssd->isMulti()) {
+        // label + N floats
+        return 1U + (static_cast<std::size_t>(ssd->multiSDHLength) * TelemetryFmt::kBytesIn32Bit);
+    }
+    return 0U;
 }
 
-void Telemetry::preparePacket(uint32_t timestamp) {
-    this->packet[0] = 0;
-    this->packet[1] = 0;
-    this->packet[2] = 0;
-    this->packet[3] = START_BYTE;
-    this->packet[4] = (timestamp >> 24) & 0xFF;
-    this->packet[5] = (timestamp >> 16) & 0xFF;
-    this->packet[6] = (timestamp >> 8) & 0xFF;
-    this->packet[7] = timestamp & 0xFF;
-    nextEmptyPacketIndex = 8;
+bool hasRoom(std::size_t nextIndex, std::size_t bytesToAdd) {
+    return nextIndex + bytesToAdd <= TelemetryFmt::kPacketCapacity;
+}
+
+void Telemetry::preparePacket(std::uint32_t timestamp) {
+    // This write the header of the packet with sync bytes, start byte, and timestamp.
+    // Only clear what we own in the header (whole-packet clearing happens in setPacketToZero()).
+
+    // Fill sync bytes with 0
+    std::fill_n(&this->packet[0], TelemetryFmt::kSyncZeros, static_cast<std::uint8_t>(0));
+
+    // Set the start byte after the sync bytes
+    this->packet[TelemetryFmt::kStartByteIndex] = TelemetryFmt::kStartByteValue;
+
+    // Write the timestamp in big-endian format
+    TelemetryFmt::write_u32_be(&this->packet[TelemetryFmt::kTimestampIndex], timestamp);
+
+    nextEmptyPacketIndex = TelemetryFmt::kHeaderBytes;
 }
 
 void Telemetry::addSingleSDHToPacket(SensorDataHandler* sdh) {
     float floatData = sdh->getLastDataPointSaved().data; 
-    uint32_t data;
-    memcpy(&data, &floatData, sizeof(data));
-    for (int i = 3; i > -1; i--) {
-        this->packet[nextEmptyPacketIndex+(3-i)] = (data >> (i*8)) & 0xFF;
-    }
-    nextEmptyPacketIndex += 4;
+    uint32_t data = 0;
+    memcpy(&data, &floatData, sizeof(data)); // Move float data into an uint32_t for bytewise access
+    TelemetryFmt::write_u32_be(&this->packet[nextEmptyPacketIndex], data);
+    nextEmptyPacketIndex += TelemetryFmt::kBytesIn32Bit;
 }
 
 void Telemetry::addSSDToPacket(SendableSensorData* ssd) {
-    if (ssd->singleSDH != nullptr) {
+    if (ssd->isSingle()) {
         this->packet[nextEmptyPacketIndex] = ssd->singleSDH->getName();
         nextEmptyPacketIndex += 1;
         this->addSingleSDHToPacket(ssd->singleSDH);
     }
-    if (ssd->multiSDH != nullptr) {
+    if (ssd->isMulti()) {
         this->packet[nextEmptyPacketIndex] = ssd->multiSDHDataLabel;
         nextEmptyPacketIndex += 1;
-        for (int i = 0; i < ssd->multiSDHLength; i++) {
-            this->addSingleSDHToPacket(ssd->multiSDH[i]);
+        for (int i = 0; i < ssd->multiSDHLength; i++) { 
+            // multiSDHLength comes directly from the array passed in by the client
+            // So, we can ignore this raw pointer indexing warning
+            this->addSingleSDHToPacket(ssd->multiSDH[i]); // NOLINT(cppcoreguidelines-pro-bounds-pointer-arithmetic)
         }
     }
 }
 
 void Telemetry::setPacketToZero() {
-    for (int i = 0; i < 120; i++) { //Completely clear packet
+    for (int i = 0; i < TelemetryFmt::kPacketCapacity; i++) { //Completely clear packet
         this->packet[i] = 0;
     }
 }
 
 void Telemetry::addEndMarker() {
-    this->packet[nextEmptyPacketIndex] = 0;
-    this->packet[nextEmptyPacketIndex+1] = 0;
-    this->packet[nextEmptyPacketIndex+2] = 0;
-    this->packet[nextEmptyPacketIndex+3] = END_BYTE;
-    nextEmptyPacketIndex += 4;
+    // Adds the following 4 bytes to the end of the packet: 0x00 0x00 0x00 (kEndByteValue)
+
+    std::fill_n(&this->packet[nextEmptyPacketIndex], TelemetryFmt::kSyncZeros, static_cast<std::uint8_t>(0));
+    this->packet[nextEmptyPacketIndex+TelemetryFmt::kSyncZeros] = TelemetryFmt::kEndByteValue;
+    nextEmptyPacketIndex += TelemetryFmt::kEndMarkerBytes;
 }
 
 bool Telemetry::tick(uint32_t currentTime) {
     bool sendingPacketThisTick = false;
-    int currentPacketIndex = 0;
-    for (int i = 0; i < this->ssdArrayLength; i++) {
-        if (ssdArray[i]->shouldBeSent(currentTime)) {
+
+    for (std::size_t i = 0; i < streamCount; i++) {
+        // i is safe because streamCount comes from the array passed in by the client
+        if (streams[i]->shouldBeSent(currentTime)) { // NOLINT(cppcoreguidelines-pro-bounds-pointer-arithmetic)
+
             if (!sendingPacketThisTick) {
                 setPacketToZero();
                 preparePacket(currentTime);
-                addSSDToPacket(ssdArray[i]);
                 sendingPacketThisTick = true;
+            }
+
+            // Compute how many bytes we need for this stream's payload.
+            const std::size_t payloadBytes = bytesNeededForSSD(streams[i]);  // NOLINT(cppcoreguidelines-pro-bounds-pointer-arithmetic)
+            const std::size_t totalBytesIfAdded = payloadBytes + TelemetryFmt::kEndMarkerBytes;
+
+            // Only add if it fits (payload + end marker).
+            if (hasRoom(nextEmptyPacketIndex, totalBytesIfAdded)) {
+                addSSDToPacket(streams[i]); // NOLINT(cppcoreguidelines-pro-bounds-pointer-arithmetic)
+                streams[i]->markWasSent(currentTime); // NOLINT(cppcoreguidelines-pro-bounds-pointer-arithmetic)
             } else {
-                addSSDToPacket(ssdArray[i]);
+                // Not enough room. Skip this stream for now.
+                // It will be sent on the next tick as long as the packet isn't filled before reaching it again.
+                // If we have too many high-frequency streams, the stream as the end of the list may be starved.
             }
-            ssdArray[i]->markWasSent(currentTime);
         }
     }
-    if (sendingPacketThisTick) {
-        addEndMarker();
-        for (int i = 0; i < nextEmptyPacketIndex; i++) {
-            this->rfdSerialConnection.write(this->packet[i]);
+
+    // Only send if we actually added any payload beyond the header.
+    if (sendingPacketThisTick && nextEmptyPacketIndex > TelemetryFmt::kHeaderBytes) {
+        // Ensure end marker itself fits
+        if (hasRoom(nextEmptyPacketIndex, TelemetryFmt::kEndMarkerBytes)) {
+            addEndMarker();
+
+            // Send used portion
+            for (std::size_t i = 0; i < nextEmptyPacketIndex; i++) {
+                rfdSerialConnection.write(packet[i]);
+            }
+            return true;
         }
+
+        // If somehow we can't fit the end marker, drop the packet.
+        // (This shouldn't happen with the checks above.)
     }
-    return sendingPacketThisTick;
-}
\ No newline at end of file
+
+    return false;
+}
diff --git a/src/state_estimation/ApogeePredictor.cpp b/src/state_estimation/ApogeePredictor.cpp
index 0edb2e8..0ef0cb2 100644
--- a/src/state_estimation/ApogeePredictor.cpp
+++ b/src/state_estimation/ApogeePredictor.cpp
@@ -1,6 +1,7 @@
 #include "state_estimation/ApogeePredictor.h"
 
 #include <algorithm>
+#include <array>
 #include <cmath>
 #include <cstdint>
 #include <cstdio>
@@ -108,12 +109,10 @@ void ApogeePredictor::poly_update() {
     const float velocity_ms = vve_.getEstimatedVelocity();
     const float acceleration_ms2 = vve_.getInertialVerticalAcceleration();
 
-    
-
-    const size_t featureCount = 10;
+    constexpr size_t FEATURE_COUNT = 10; // NOLINT(cppcoreguidelines-init-variables)
 
     // Polynomial Regression Coefficients for C++
-    const float coeffs[] = {
+    const std::array<float, FEATURE_COUNT> coeffs = {
         /* 1 */ 0.00000000,
         /* vertical_velocity */ 5.06108448,
         /* vertical_acceleration */ 63.94744144,
@@ -129,13 +128,13 @@ void ApogeePredictor::poly_update() {
 
     // ───────────────────────────────────────────────────────
     // Compute delta_h_simple = v^2 / (2 * decel), with decel > 0
-    double decel = std::fabs(acceleration_ms2);
-    double delta_h_simple = (velocity_ms * velocity_ms) / (2.0 * decel);
+    const float decel = std::fabs(acceleration_ms2);
+    const float delta_h_simple = MAGIC_HALF * (velocity_ms * velocity_ms) / decel;
 
     // ───────────────────────────────────────────────────────
     // Evaluate the regression model
-    const double inputs[featureCount] = {
-        1.0,
+    const std::array<float, FEATURE_COUNT> inputs = {
+        1.0F,
         velocity_ms,                // vertical_velocity
         acceleration_ms2,           // vertical_acceleration
         delta_h_simple,
@@ -147,8 +146,8 @@ void ApogeePredictor::poly_update() {
         delta_h_simple * delta_h_simple, // delta_h_simple^2
     };
 
-    double apogeeRemaining_m = intercept;
-    for (size_t i = 0; i < featureCount; ++i) {
+    float apogeeRemaining_m = intercept;
+    for (size_t i = 0; i < FEATURE_COUNT; ++i) { // NOLINT(cppcoreguidelines-init-variables)
         apogeeRemaining_m += coeffs[i] * inputs[i];
     }
 
@@ -166,8 +165,8 @@ void ApogeePredictor::poly_update() {
     lastVel_ = velocity_ms;
     numWarmups_ = std::min(numWarmups_ + 1, MAX_WARMUPS);
 
-    printf("Current Timestamp: %u, Altitude: %.2f, Velocity: %.2f, Acceleration: %.2f, Predicted Apogee Remaining: %.2f, Delta H: %.2f\n",
-           currentTimestamp_ms, altitude_m, velocity_ms, acceleration_ms2, apogeeRemaining_m, delta_h_simple);
+    // printf("Current Timestamp: %u, Altitude: %.2f, Velocity: %.2f, Acceleration: %.2f, Predicted Apogee Remaining: %.2f, Delta H: %.2f\n",
+    //        currentTimestamp_ms, altitude_m, velocity_ms, acceleration_ms2, apogeeRemaining_m, delta_h_simple);
 }
 
 
diff --git a/src/state_estimation/BurnoutStateMachine.cpp b/src/state_estimation/BurnoutStateMachine.cpp
index 7c955f1..8622e5d 100644
--- a/src/state_estimation/BurnoutStateMachine.cpp
+++ b/src/state_estimation/BurnoutStateMachine.cpp
@@ -2,8 +2,9 @@
 
 #include "data_handling/DataNames.h"
 #include "data_handling/DataPoint.h"
-#include "state_estimation/StateEstimationTypes.h"
 #include "state_estimation/BurnoutStateMachine.h"
+#include "state_estimation/StateEstimationTypes.h"
+
 
 constexpr float GRAVITY = 9.8;
 
diff --git a/src/state_estimation/FastLaunchDetector.cpp b/src/state_estimation/FastLaunchDetector.cpp
new file mode 100644
index 0000000..a8779cc
--- /dev/null
+++ b/src/state_estimation/FastLaunchDetector.cpp
@@ -0,0 +1,53 @@
+#include "state_estimation/FastLaunchDetector.h"
+
+// #define DEBUG
+#ifdef DEBUG
+#include "ArduinoHAL.h"
+#endif
+
+FastLaunchDetector::FastLaunchDetector(float accelerationThreshold_ms2, uint32_t confirmationWindow_ms) //NOLINT(bugprone-easily-swappable-parameters)
+    : accelerationThresholdSq_ms2(accelerationThreshold_ms2 * accelerationThreshold_ms2),
+      launched(false),
+      launchedTime_ms(0),
+      confirmationWindow_ms(confirmationWindow_ms)
+{}
+
+int FastLaunchDetector::update(AccelerationTriplet accel){
+
+    // Calculate the magnitude of the acceleration squared
+    const float aclMagSq = accel.x.data * accel.x.data + accel.y.data * accel.y.data + accel.z.data * accel.z.data;
+
+    // Take the average of the timestamps
+    // Ideally these should all be the same
+    const uint32_t time_ms = (accel.x.timestamp_ms + accel.y.timestamp_ms + accel.z.timestamp_ms) / 3;
+
+    //if launch already detected, ignore further data
+    if (launched){
+        #ifdef DEBUG
+        Serial.println("FastLaunchDetector: Data point ignored because already launched");
+        #endif
+        return FLD_ALREADY_LAUNCHED;
+    }
+
+    //if accel higher than threshold, launch detected
+    if (aclMagSq > accelerationThresholdSq_ms2){
+        launched = true;
+        launchedTime_ms = time_ms;
+        return FLD_LAUNCH_DETECTED;
+    }
+
+    //if accel lower than threshold, acl too low
+    if (aclMagSq < accelerationThresholdSq_ms2) {
+        #ifdef DEBUG
+        Serial.println("FastLaunchDetector: Acceloration below threshold");
+        #endif
+        return FLD_ACL_TOO_LOW;
+    }
+
+    return FLD_DEFAULT_FAIL;
+}
+
+void FastLaunchDetector::reset(){
+    launched = false;
+    launchedTime_ms = 0;
+}
\ No newline at end of file
diff --git a/src/state_estimation/GroundLevelEstimator.cpp b/src/state_estimation/GroundLevelEstimator.cpp
index 88c4d6b..7931742 100644
--- a/src/state_estimation/GroundLevelEstimator.cpp
+++ b/src/state_estimation/GroundLevelEstimator.cpp
@@ -1,8 +1,8 @@
 #include "state_estimation/GroundLevelEstimator.h"
 
 // Constructor
-GroundLevelEstimator::GroundLevelEstimator()
-: launched(false), estimatedGroundLevel_m(0.0F), sampleCount(0), alpha(0.1F)
+GroundLevelEstimator::GroundLevelEstimator(float alpha)
+: alpha(alpha)
 {}
 
 // Update the ground level estimate or convert ASL to AGL - Altitude ABOVE ground level
diff --git a/src/state_estimation/StateMachine.cpp b/src/state_estimation/StateMachine.cpp
index 4899da3..360a0f8 100644
--- a/src/state_estimation/StateMachine.cpp
+++ b/src/state_estimation/StateMachine.cpp
@@ -8,11 +8,13 @@
 StateMachine::StateMachine(IDataSaver* dataSaver,
                            LaunchDetector* launchDetector,
                            ApogeeDetector* apogeeDetector,
-                           VerticalVelocityEstimator* verticalVelocityEstimator)
+                           VerticalVelocityEstimator* verticalVelocityEstimator,
+                           FastLaunchDetector* fastLaunchDetector)
     : dataSaver(dataSaver),
       launchDetector(launchDetector),
       apogeeDetector(apogeeDetector),
       verticalVelocityEstimator(verticalVelocityEstimator),
+      fastLaunchDetector(fastLaunchDetector),
       state(STATE_ARMED)
 {
 }
@@ -20,13 +22,31 @@ StateMachine::StateMachine(IDataSaver* dataSaver,
 int StateMachine::update(const AccelerationTriplet& accel, const DataPoint& alt) {
     // Update the state
     int lpStatus = LP_DEFAULT_FAIL; 
+    int fldStatus = FLD_DEFAULT_FAIL;
 
     switch (state) {
         case STATE_ARMED:
             // Serial.println("lp update");
             lpStatus = launchDetector->update(accel);
+            fldStatus = fastLaunchDetector->update(accel);
             // Serial.println(lpStatus);
-            if (launchDetector->isLaunched()) {
+            if (fastLaunchDetector->hasLaunched()) {
+                // Change state to soft ascent
+                state = STATE_SOFT_ASCENT;
+
+                // Save the FLD launch time
+                fldLaunchTime_ms = fastLaunchDetector->getLaunchedTime();
+
+                // Log the state change
+                dataSaver->saveDataPoint(
+                    DataPoint(accel.x.timestamp_ms, STATE_SOFT_ASCENT),
+                    STATE_CHANGE
+                );
+
+                // Put the data saver into post-launch mode
+                dataSaver->launchDetected(fastLaunchDetector->getLaunchedTime());
+            }
+            else if (launchDetector->isLaunched()) {
                 // Change state to ascent
                 state = STATE_ASCENT;
 
@@ -46,6 +66,51 @@ int StateMachine::update(const AccelerationTriplet& accel, const DataPoint& alt)
                 verticalVelocityEstimator->update(accel, alt);
             }
             break;
+
+        case STATE_SOFT_ASCENT:
+        /*
+        * In soft ascent, we are waiting for confirmation of launch from the LaunchDetector.
+        * If LaunchDetector confirms launch within the confirmation window, we transition to ASCENT.
+        * If the confirmation window passes without confirmation, we revert to ARMED
+        * and clear post-launch mode.
+        */
+            // Serial.println("lp update");
+            lpStatus = launchDetector->update(accel);
+            // Serial.println(lpStatus);
+            if (launchDetector->isLaunched()) {
+                // Change state to ascent
+                state = STATE_ASCENT;
+
+                // Log the state change
+                dataSaver->saveDataPoint(
+                    DataPoint(accel.x.timestamp_ms, STATE_ASCENT),
+                    STATE_CHANGE
+                );
+
+                // Start the apogee detection system
+                apogeeDetector->init({alt.data, alt.timestamp_ms});
+
+                // Update the vertical velocity estimator
+                verticalVelocityEstimator->update(accel, alt);
+            }
+            else if (accel.x.timestamp_ms - fldLaunchTime_ms > fastLaunchDetector->getConfirmationWindow()) {
+                // If the confirmation window has passed without launch detected by LaunchDetector,
+                // revert to ARMED state
+                state = STATE_ARMED;
+                fldLaunchTime_ms = 0;
+                fastLaunchDetector->reset();
+
+                // Log the state change
+                dataSaver->saveDataPoint(
+                    DataPoint(accel.x.timestamp_ms, STATE_ARMED),
+                    STATE_CHANGE
+                );
+
+                // Clear post-launch mode
+                dataSaver->clearPostLaunchMode();
+            }
+            break;
+            
         case STATE_ASCENT:
             // Serial.println("apogee update");
             // Update the vertical velocity estimator