diff --git a/Side_Projects/Live Video/radio-interface/APRSMsg.h b/Side_Projects/Live Video/radio-interface/APRSMsg.h
index beb98909..0973dfd1 100644
--- a/Side_Projects/Live Video/radio-interface/APRSMsg.h	
+++ b/Side_Projects/Live Video/radio-interface/APRSMsg.h	
@@ -213,7 +213,7 @@ APRS Telemetry Data
 - t0
 - dao
 */
-struct APRSData
+struct APRSTelemData
 {
     char lat[16];
     char lng[16];
diff --git a/Side_Projects/Live Video/radio-interface/RFM69HCW.cpp b/Side_Projects/Live Video/radio-interface/RFM69HCW.cpp
index f0aa8433..ec243c4f 100644
--- a/Side_Projects/Live Video/radio-interface/RFM69HCW.cpp	
+++ b/Side_Projects/Live Video/radio-interface/RFM69HCW.cpp	
@@ -286,9 +286,9 @@ bool RFM69HCW::encode(char *message, EncodingType type, int len)
             return false;
 
         // holds the data to be assembled into the aprs body
-        APRSData data;
+        APRSTelemData data;
 
-        // find each value separated in order by a comma and put in the APRSData array
+        // find each value separated in order by a comma and put in the APRSTelemData array
         {
             char *currentVal = new char[this->msgLen];
             int currentValIndex = 0;
@@ -426,7 +426,7 @@ bool RFM69HCW::decode(char *message, EncodingType type, int len)
         char *bodyptr = body;
         strcpy(body, aprs.getBody()->getData());
         // decode body
-        APRSData data;
+        APRSTelemData data;
         int i = 0;
         int len = strlen(body);
 
diff --git a/Spaceport/23-24/Code/Rpi-Middleman/data7.log b/Spaceport/23-24/Code/Rpi-Middleman/data7.log
new file mode 100644
index 00000000..015f5ace
Binary files /dev/null and b/Spaceport/23-24/Code/Rpi-Middleman/data7.log differ
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/include/Radio.h b/Spaceport/23-24/Code/Teensy-Based Avionics/include/Radio.h
deleted file mode 100644
index 867253d4..00000000
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/include/Radio.h	
+++ /dev/null
@@ -1,45 +0,0 @@
-#ifndef RADIO_H
-#define RADIO_H
-
-#if defined(ARDUINO)
-#include <Arduino.h>
-#elif defined(_WIN32) || defined(_WIN64) // Windows
-#include <cstdint>
-#include <string>
-#include <cstring>
-#elif defined(__unix__)  // Linux
-// TODO
-#elif defined(__APPLE__) // OSX
-// TODO
-#endif
-
-/*
-Types:
-- ENCT_TELEMETRY: latitude,longitude,altitude,speed,heading,precision,stage,t0 <-> APRS message
-- ENCT_VIDEO: char* filled with raw bytes <-> Raw byte array
-- ENCT_GROUNDSTATION: Source:Value,Destination:Value,Path:Value,Type:Value,Body:Value <-> APRS message
-- ENCT_NONE: no encoding is applied, same as using tx()
-*/
-enum EncodingType
-{
-    ENCT_TELEMETRY,
-    ENCT_VIDEO,
-    ENCT_GROUNDSTATION,
-    ENCT_NONE
-};
-
-class Radio
-{
-public:
-    virtual ~Radio(){}; // Virtual descructor. Very important
-    virtual bool begin() = 0;
-    virtual bool tx(const char *message, int len) = 0;
-    virtual const char *rx() = 0;
-    virtual bool encode(char *message, EncodingType type) = 0;
-    virtual bool decode(char *message, EncodingType type) = 0;
-    virtual bool send(const char *message, EncodingType type) = 0;
-    virtual const char *receive(EncodingType type) = 0;
-    virtual int RSSI() = 0;
-};
-
-#endif // RADIO_H
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/lib/Kalman-Filter-main/src/Filters/LinearKalmanFilter/LinearKalmanFilter.cpp b/Spaceport/23-24/Code/Teensy-Based Avionics/lib/Kalman-Filter-main/src/Filters/LinearKalmanFilter/LinearKalmanFilter.cpp
index 37abd7a0..5a930f4c 100644
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/lib/Kalman-Filter-main/src/Filters/LinearKalmanFilter/LinearKalmanFilter.cpp	
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/lib/Kalman-Filter-main/src/Filters/LinearKalmanFilter/LinearKalmanFilter.cpp	
@@ -1,5 +1,4 @@
 #include "LinearKalmanFilter.h"
-#include "Matrix.h"
 LinearKalmanFilter::LinearKalmanFilter(Matrix X, Matrix U, Matrix P, Matrix F, Matrix G, Matrix R, Matrix Q)
 {
     state.X = X;
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/lib/RecordData/RecordData.cpp b/Spaceport/23-24/Code/Teensy-Based Avionics/lib/RecordData/RecordData.cpp
index b90935bc..fb24046f 100644
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/lib/RecordData/RecordData.cpp	
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/lib/RecordData/RecordData.cpp	
@@ -4,9 +4,9 @@ static const char *logTypeStrings[] = {"LOG", "ERROR", "WARNING", "INFO"};
 static Mode mode = GROUND;
 void recordFlightData(char *data)
 {
-    if(!isSDReady())
+    if (!isSDReady())
         return;
-    if (mode == GROUND)
+    if (mode == GROUND || !ram->isReady())
     {
         flightDataFile = sd.open(flightDataFileName, FILE_WRITE); // during preflight, print to SD card constantly. ignore PSRAM for this stage.
         if (flightDataFile)
@@ -15,7 +15,7 @@ void recordFlightData(char *data)
             flightDataFile.close();
         }
     }
-    else if(ram->isReady())
+    else
         ram->println(data); // while in flight, print to PSRAM for later dumping to SD card.
 }
 
@@ -32,15 +32,12 @@ void recordLogData(double timeStamp, LogType type, const char *data, Dest dest)
 
     if (dest == BOTH || dest == TO_USB)
     {
-        // if (!Serial){
-        //     Serial.begin(9600);
-        // }
         Serial.print(logPrefix);
         Serial.println(data);
     }
     if ((dest == BOTH || dest == TO_FILE) && isSDReady())
     {
-        if (mode == GROUND)
+        if (mode == GROUND || !ram->isReady())
         {
             logFile = sd.open(logFileName, FILE_WRITE); // during preflight, print to SD card constantly. ignore PSRAM for this stage. With both files printing, may be bad...
             if (logFile)
@@ -50,7 +47,7 @@ void recordLogData(double timeStamp, LogType type, const char *data, Dest dest)
                 logFile.close();
             }
         }
-        else if (ram->isReady()) // while in flight, print to PSRAM for later dumping to SD card.
+        else // while in flight, print to PSRAM for later dumping to SD card.
         {
             ram->print(logPrefix, false);
             ram->println(data, false);
@@ -60,10 +57,11 @@ void recordLogData(double timeStamp, LogType type, const char *data, Dest dest)
 
 void setRecordMode(Mode m)
 {
-    if(mode == FLIGHT && m == GROUND){
+    if (mode == FLIGHT && m == GROUND && ram->isReady())
+    {
         ram->dumpFlightData();
         ram->dumpLogData();
-        bb.aonoff(LED_BUILTIN, 2000);//turn on LED for 2 seconds to indicate that the PSRAM is being dumped to the SD card.
+        bb.aonoff(LED_BUILTIN, 2000); // turn on LED for 2 seconds to indicate that the PSRAM is being dumped to the SD card.
     }
     mode = m;
 }
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/lib/RecordData/psram.cpp b/Spaceport/23-24/Code/Teensy-Based Avionics/lib/RecordData/psram.cpp
index 33ab8c50..db600569 100644
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/lib/RecordData/psram.cpp	
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/lib/RecordData/psram.cpp	
@@ -15,7 +15,7 @@ bool PSRAM::init()
 {
     uint8_t size = external_psram_size;
     memBegin = cursorStart = reinterpret_cast<char *>(0x70000000);
-    memEnd = cursorEnd = memBegin + (size * 1048576);
+    memEnd = cursorEnd = memBegin + (size * 1048576) - 1;
 
     if (size > 0)
     {
@@ -73,7 +73,7 @@ bool PSRAM::dumpFlightData()
 bool PSRAM::dumpLogData()
 { // more complicated because data is stored in reverse order
     if (!isSDReady() || !ready){
-        bb.aonoff(33, 200);
+        bb.aonoff(BUZZER, 200);
         return false;
     }
 
@@ -83,11 +83,11 @@ bool PSRAM::dumpLogData()
     logFile = sd.open(logFileName, FILE_WRITE);
 
     if (!logFile){
-        bb.aonoff(33, 200, 3);
+        bb.aonoff(BUZZER, 200, 3);
         return false;
     }
     
-    while (writeCursor >= cursorEnd)
+    while (writeCursor > cursorEnd)
     {
 
         for (i = 0; i < 2048; i++)
@@ -106,7 +106,7 @@ bool PSRAM::dumpLogData()
     logFile.close();
     cursorEnd = memEnd;
     dumped = true;
-    bb.aonoff(33, 200, 5);
+    bb.aonoff(BUZZER, 200, 5);
     return true;
 }
 
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/lib/imumaths/imumaths.h b/Spaceport/23-24/Code/Teensy-Based Avionics/lib/imumaths/imumaths.h
deleted file mode 100644
index a168750e..00000000
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/lib/imumaths/imumaths.h	
+++ /dev/null
@@ -1,31 +0,0 @@
-
-//  Inertial Measurement Unit Maths Library
-//
-//  Copyright 2013-2021 Sam Cowen <samuel.cowen@camelsoftware.com>
-//
-//  Permission is hereby granted, free of charge, to any person obtaining a
-//  copy of this software and associated documentation files (the "Software"),
-//  to deal in the Software without restriction, including without limitation
-//  the rights to use, copy, modify, merge, publish, distribute, sublicense,
-//  and/or sell copies of the Software, and to permit persons to whom the
-//  Software is furnished to do so, subject to the following conditions:
-//
-//  The above copyright notice and this permission notice shall be included in
-//  all copies or substantial portions of the Software.
-//
-//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
-//  OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
-//  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
-//  DEALINGS IN THE SOFTWARE.
-
-#ifndef IMUMATH_H
-#define IMUMATH_H
-
-#include "matrix.h"
-#include "quaternion.h"
-#include "vector.h"
-
-#endif
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/lib/imumaths/matrix.h b/Spaceport/23-24/Code/Teensy-Based Avionics/lib/imumaths/matrix.h
deleted file mode 100644
index e71b2db9..00000000
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/lib/imumaths/matrix.h	
+++ /dev/null
@@ -1,185 +0,0 @@
-//  Inertial Measurement Unit Maths Library
-//
-//  Copyright 2013-2021 Sam Cowen <samuel.cowen@camelsoftware.com>
-//  Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com)
-//
-//  Permission is hereby granted, free of charge, to any person obtaining a
-//  copy of this software and associated documentation files (the "Software"),
-//  to deal in the Software without restriction, including without limitation
-//  the rights to use, copy, modify, merge, publish, distribute, sublicense,
-//  and/or sell copies of the Software, and to permit persons to whom the
-//  Software is furnished to do so, subject to the following conditions:
-//
-//  The above copyright notice and this permission notice shall be included in
-//  all copies or substantial portions of the Software.
-//
-//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
-//  OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
-//  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
-//  DEALINGS IN THE SOFTWARE.
-
-#ifndef IMUMATH_MATRIX_HPP
-#define IMUMATH_MATRIX_HPP
-
-#include <stdint.h>
-#include <string.h>
-
-#include "vector.h"
-
-namespace imu {
-
-template <uint8_t N> class Matrix {
-public:
-  Matrix() { memset(_cell_data, 0, N * N * sizeof(double)); }
-
-  Matrix(const Matrix &m) {
-    for (int ij = 0; ij < N * N; ++ij) {
-      _cell_data[ij] = m._cell_data[ij];
-    }
-  }
-
-  ~Matrix() {}
-
-  Matrix &operator=(const Matrix &m) {
-    for (int ij = 0; ij < N * N; ++ij) {
-      _cell_data[ij] = m._cell_data[ij];
-    }
-    return *this;
-  }
-
-  Vector<N> row_to_vector(int i) const {
-    Vector<N> ret;
-    for (int j = 0; j < N; j++) {
-      ret[j] = cell(i, j);
-    }
-    return ret;
-  }
-
-  Vector<N> col_to_vector(int j) const {
-    Vector<N> ret;
-    for (int i = 0; i < N; i++) {
-      ret[i] = cell(i, j);
-    }
-    return ret;
-  }
-
-  void vector_to_row(const Vector<N> &v, int i) {
-    for (int j = 0; j < N; j++) {
-      cell(i, j) = v[j];
-    }
-  }
-
-  void vector_to_col(const Vector<N> &v, int j) {
-    for (int i = 0; i < N; i++) {
-      cell(i, j) = v[i];
-    }
-  }
-
-  double operator()(int i, int j) const { return cell(i, j); }
-  double &operator()(int i, int j) { return cell(i, j); }
-
-  double cell(int i, int j) const { return _cell_data[i * N + j]; }
-  double &cell(int i, int j) { return _cell_data[i * N + j]; }
-
-  Matrix operator+(const Matrix &m) const {
-    Matrix ret;
-    for (int ij = 0; ij < N * N; ++ij) {
-      ret._cell_data[ij] = _cell_data[ij] + m._cell_data[ij];
-    }
-    return ret;
-  }
-
-  Matrix operator-(const Matrix &m) const {
-    Matrix ret;
-    for (int ij = 0; ij < N * N; ++ij) {
-      ret._cell_data[ij] = _cell_data[ij] - m._cell_data[ij];
-    }
-    return ret;
-  }
-
-  Matrix operator*(double scalar) const {
-    Matrix ret;
-    for (int ij = 0; ij < N * N; ++ij) {
-      ret._cell_data[ij] = _cell_data[ij] * scalar;
-    }
-    return ret;
-  }
-
-  Matrix operator*(const Matrix &m) const {
-    Matrix ret;
-    for (int i = 0; i < N; i++) {
-      Vector<N> row = row_to_vector(i);
-      for (int j = 0; j < N; j++) {
-        ret(i, j) = row.dot(m.col_to_vector(j));
-      }
-    }
-    return ret;
-  }
-
-  Matrix transpose() const {
-    Matrix ret;
-    for (int i = 0; i < N; i++) {
-      for (int j = 0; j < N; j++) {
-        ret(j, i) = cell(i, j);
-      }
-    }
-    return ret;
-  }
-
-  Matrix<N - 1> minor_matrix(int row, int col) const {
-    Matrix<N - 1> ret;
-    for (int i = 0, im = 0; i < N; i++) {
-      if (i == row)
-        continue;
-
-      for (int j = 0, jm = 0; j < N; j++) {
-        if (j != col) {
-          ret(im, jm++) = cell(i, j);
-        }
-      }
-      im++;
-    }
-    return ret;
-  }
-
-  double determinant() const {
-    // specialization for N == 1 given below this class
-    double det = 0.0, sign = 1.0;
-    for (int i = 0; i < N; ++i, sign = -sign)
-      det += sign * cell(0, i) * minor_matrix(0, i).determinant();
-    return det;
-  }
-
-  Matrix invert() const {
-    Matrix ret;
-    double det = determinant();
-
-    for (int i = 0; i < N; i++) {
-      for (int j = 0; j < N; j++) {
-        ret(i, j) = minor_matrix(j, i).determinant() / det;
-        if ((i + j) % 2 == 1)
-          ret(i, j) = -ret(i, j);
-      }
-    }
-    return ret;
-  }
-
-  double trace() const {
-    double tr = 0.0;
-    for (int i = 0; i < N; ++i)
-      tr += cell(i, i);
-    return tr;
-  }
-
-private:
-  double _cell_data[N * N];
-};
-
-template <> inline double Matrix<1>::determinant() const { return cell(0, 0); }
-
-}; // namespace imu
-
-#endif
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/lib/imumaths/quaternion.h b/Spaceport/23-24/Code/Teensy-Based Avionics/lib/imumaths/quaternion.h
deleted file mode 100644
index c5b907a1..00000000
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/lib/imumaths/quaternion.h	
+++ /dev/null
@@ -1,214 +0,0 @@
-//  Inertial Measurement Unit Maths Library
-//
-//  Copyright 2013-2021 Sam Cowen <samuel.cowen@camelsoftware.com>
-//  Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com)
-//
-//  Permission is hereby granted, free of charge, to any person obtaining a
-//  copy of this software and associated documentation files (the "Software"),
-//  to deal in the Software without restriction, including without limitation
-//  the rights to use, copy, modify, merge, publish, distribute, sublicense,
-//  and/or sell copies of the Software, and to permit persons to whom the
-//  Software is furnished to do so, subject to the following conditions:
-//
-//  The above copyright notice and this permission notice shall be included in
-//  all copies or substantial portions of the Software.
-//
-//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
-//  OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
-//  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
-//  DEALINGS IN THE SOFTWARE.
-
-#ifndef IMUMATH_QUATERNION_HPP
-#define IMUMATH_QUATERNION_HPP
-
-#include <math.h>
-#include <stdint.h>
-#include <stdlib.h>
-#include <string.h>
-
-#include "matrix.h"
-
-namespace imu {
-
-class Quaternion {
-public:
-  Quaternion() : _w(1.0), _x(0.0), _y(0.0), _z(0.0) {}
-
-  Quaternion(double w, double x, double y, double z)
-      : _w(w), _x(x), _y(y), _z(z) {}
-
-  Quaternion(double w, Vector<3> vec)
-      : _w(w), _x(vec.x()), _y(vec.y()), _z(vec.z()) {}
-
-  double &w() { return _w; }
-  double &x() { return _x; }
-  double &y() { return _y; }
-  double &z() { return _z; }
-
-  double w() const { return _w; }
-  double x() const { return _x; }
-  double y() const { return _y; }
-  double z() const { return _z; }
-
-  double magnitude() const {
-    return sqrt(_w * _w + _x * _x + _y * _y + _z * _z);
-  }
-
-  void normalize() {
-    double mag = magnitude();
-    *this = this->scale(1 / mag);
-  }
-
-  Quaternion conjugate() const { return Quaternion(_w, -_x, -_y, -_z); }
-
-  void fromAxisAngle(const Vector<3> &axis, double theta) {
-    _w = cos(theta / 2);
-    // only need to calculate sine of half theta once
-    double sht = sin(theta / 2);
-    _x = axis.x() * sht;
-    _y = axis.y() * sht;
-    _z = axis.z() * sht;
-  }
-
-  void fromMatrix(const Matrix<3> &m) {
-    double tr = m.trace();
-
-    double S;
-    if (tr > 0) {
-      S = sqrt(tr + 1.0) * 2;
-      _w = 0.25 * S;
-      _x = (m(2, 1) - m(1, 2)) / S;
-      _y = (m(0, 2) - m(2, 0)) / S;
-      _z = (m(1, 0) - m(0, 1)) / S;
-    } else if (m(0, 0) > m(1, 1) && m(0, 0) > m(2, 2)) {
-      S = sqrt(1.0 + m(0, 0) - m(1, 1) - m(2, 2)) * 2;
-      _w = (m(2, 1) - m(1, 2)) / S;
-      _x = 0.25 * S;
-      _y = (m(0, 1) + m(1, 0)) / S;
-      _z = (m(0, 2) + m(2, 0)) / S;
-    } else if (m(1, 1) > m(2, 2)) {
-      S = sqrt(1.0 + m(1, 1) - m(0, 0) - m(2, 2)) * 2;
-      _w = (m(0, 2) - m(2, 0)) / S;
-      _x = (m(0, 1) + m(1, 0)) / S;
-      _y = 0.25 * S;
-      _z = (m(1, 2) + m(2, 1)) / S;
-    } else {
-      S = sqrt(1.0 + m(2, 2) - m(0, 0) - m(1, 1)) * 2;
-      _w = (m(1, 0) - m(0, 1)) / S;
-      _x = (m(0, 2) + m(2, 0)) / S;
-      _y = (m(1, 2) + m(2, 1)) / S;
-      _z = 0.25 * S;
-    }
-  }
-
-  void toAxisAngle(Vector<3> &axis, double &angle) const {
-    double sqw = sqrt(1 - _w * _w);
-    if (sqw == 0) // it's a singularity and divide by zero, avoid
-      return;
-
-    angle = 2 * acos(_w);
-    axis.x() = _x / sqw;
-    axis.y() = _y / sqw;
-    axis.z() = _z / sqw;
-  }
-
-  Matrix<3> toMatrix() const {
-    Matrix<3> ret;
-    ret.cell(0, 0) = 1 - 2 * _y * _y - 2 * _z * _z;
-    ret.cell(0, 1) = 2 * _x * _y - 2 * _w * _z;
-    ret.cell(0, 2) = 2 * _x * _z + 2 * _w * _y;
-
-    ret.cell(1, 0) = 2 * _x * _y + 2 * _w * _z;
-    ret.cell(1, 1) = 1 - 2 * _x * _x - 2 * _z * _z;
-    ret.cell(1, 2) = 2 * _y * _z - 2 * _w * _x;
-
-    ret.cell(2, 0) = 2 * _x * _z - 2 * _w * _y;
-    ret.cell(2, 1) = 2 * _y * _z + 2 * _w * _x;
-    ret.cell(2, 2) = 1 - 2 * _x * _x - 2 * _y * _y;
-    return ret;
-  }
-
-  // Returns euler angles that represent the quaternion.  Angles are
-  // returned in rotation order and right-handed about the specified
-  // axes:
-  //
-  //   v[0] is applied 1st about z (ie, roll)
-  //   v[1] is applied 2nd about y (ie, pitch)
-  //   v[2] is applied 3rd about x (ie, yaw)
-  //
-  // Note that this means result.x() is not a rotation about x;
-  // similarly for result.z().
-  //
-  Vector<3> toEuler() const {
-    Vector<3> ret;
-    double sqw = _w * _w;
-    double sqx = _x * _x;
-    double sqy = _y * _y;
-    double sqz = _z * _z;
-
-    ret.x() = atan2(2.0 * (_x * _y + _z * _w), (sqx - sqy - sqz + sqw));
-    ret.y() = asin(-2.0 * (_x * _z - _y * _w) / (sqx + sqy + sqz + sqw));
-    ret.z() = atan2(2.0 * (_y * _z + _x * _w), (-sqx - sqy + sqz + sqw));
-
-    return ret;
-  }
-
-  Vector<3> toAngularVelocity(double dt) const {
-    Vector<3> ret;
-    Quaternion one(1.0, 0.0, 0.0, 0.0);
-    Quaternion delta = one - *this;
-    Quaternion r = (delta / dt);
-    r = r * 2;
-    r = r * one;
-
-    ret.x() = r.x();
-    ret.y() = r.y();
-    ret.z() = r.z();
-    return ret;
-  }
-
-  Vector<3> rotateVector(const Vector<2> &v) const {
-    return rotateVector(Vector<3>(v.x(), v.y()));
-  }
-
-  Vector<3> rotateVector(const Vector<3> &v) const {
-    Vector<3> qv(_x, _y, _z);
-    Vector<3> t = qv.cross(v) * 2.0;
-    return v + t * _w + qv.cross(t);
-  }
-
-  Quaternion operator*(const Quaternion &q) const {
-    return Quaternion(_w * q._w - _x * q._x - _y * q._y - _z * q._z,
-                      _w * q._x + _x * q._w + _y * q._z - _z * q._y,
-                      _w * q._y - _x * q._z + _y * q._w + _z * q._x,
-                      _w * q._z + _x * q._y - _y * q._x + _z * q._w);
-  }
-
-  Quaternion operator+(const Quaternion &q) const {
-    return Quaternion(_w + q._w, _x + q._x, _y + q._y, _z + q._z);
-  }
-
-  Quaternion operator-(const Quaternion &q) const {
-    return Quaternion(_w - q._w, _x - q._x, _y - q._y, _z - q._z);
-  }
-
-  Quaternion operator/(double scalar) const {
-    return Quaternion(_w / scalar, _x / scalar, _y / scalar, _z / scalar);
-  }
-
-  Quaternion operator*(double scalar) const { return scale(scalar); }
-
-  Quaternion scale(double scalar) const {
-    return Quaternion(_w * scalar, _x * scalar, _y * scalar, _z * scalar);
-  }
-
-private:
-  double _w, _x, _y, _z;
-};
-
-} // namespace imu
-
-#endif
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/lib/imumaths/vector.h b/Spaceport/23-24/Code/Teensy-Based Avionics/lib/imumaths/vector.h
deleted file mode 100644
index 10345c34..00000000
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/lib/imumaths/vector.h	
+++ /dev/null
@@ -1,184 +0,0 @@
-//  Inertial Measurement Unit Maths Library
-//
-//  Copyright 2013-2021 Sam Cowen <samuel.cowen@camelsoftware.com>
-//  Bug fixes and cleanups by Gé Vissers (gvissers@gmail.com)
-//
-//  Permission is hereby granted, free of charge, to any person obtaining a
-//  copy of this software and associated documentation files (the "Software"),
-//  to deal in the Software without restriction, including without limitation
-//  the rights to use, copy, modify, merge, publish, distribute, sublicense,
-//  and/or sell copies of the Software, and to permit persons to whom the
-//  Software is furnished to do so, subject to the following conditions:
-//
-//  The above copyright notice and this permission notice shall be included in
-//  all copies or substantial portions of the Software.
-//
-//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
-//  OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
-//  FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
-//  DEALINGS IN THE SOFTWARE.
-
-#ifndef IMUMATH_VECTOR_HPP
-#define IMUMATH_VECTOR_HPP
-
-#include <math.h>
-#include <stdint.h>
-#include <string.h>
-
-namespace imu {
-
-template <uint8_t N> class Vector {
-public:
-  Vector() { memset(p_vec, 0, sizeof(double) * N); }
-
-  Vector(double a) {
-    memset(p_vec, 0, sizeof(double) * N);
-    p_vec[0] = a;
-  }
-
-  Vector(double a, double b) {
-    memset(p_vec, 0, sizeof(double) * N);
-    p_vec[0] = a;
-    p_vec[1] = b;
-  }
-
-  Vector(double a, double b, double c) {
-    memset(p_vec, 0, sizeof(double) * N);
-    p_vec[0] = a;
-    p_vec[1] = b;
-    p_vec[2] = c;
-  }
-
-  Vector(double a, double b, double c, double d) {
-    memset(p_vec, 0, sizeof(double) * N);
-    p_vec[0] = a;
-    p_vec[1] = b;
-    p_vec[2] = c;
-    p_vec[3] = d;
-  }
-
-  Vector(const Vector<N> &v) {
-    for (int x = 0; x < N; x++)
-      p_vec[x] = v.p_vec[x];
-  }
-
-  ~Vector() {}
-
-  uint8_t n() { return N; }
-
-  double magnitude() const {
-    double res = 0;
-    for (int i = 0; i < N; i++)
-      res += p_vec[i] * p_vec[i];
-
-    return sqrt(res);
-  }
-
-  void normalize() {
-    double mag = magnitude();
-    if (isnan(mag) || mag == 0.0)
-      return;
-
-    for (int i = 0; i < N; i++)
-      p_vec[i] /= mag;
-  }
-
-  double dot(const Vector &v) const {
-    double ret = 0;
-    for (int i = 0; i < N; i++)
-      ret += p_vec[i] * v.p_vec[i];
-
-    return ret;
-  }
-
-  // The cross product is only valid for vectors with 3 dimensions,
-  // with the exception of higher dimensional stuff that is beyond
-  // the intended scope of this library.
-  // Only a definition for N==3 is given below this class, using
-  // cross() with another value for N will result in a link error.
-  Vector cross(const Vector &v) const;
-
-  Vector scale(double scalar) const {
-    Vector ret;
-    for (int i = 0; i < N; i++)
-      ret.p_vec[i] = p_vec[i] * scalar;
-    return ret;
-  }
-
-  Vector invert() const {
-    Vector ret;
-    for (int i = 0; i < N; i++)
-      ret.p_vec[i] = -p_vec[i];
-    return ret;
-  }
-
-  Vector &operator=(const Vector &v) {
-    for (int x = 0; x < N; x++)
-      p_vec[x] = v.p_vec[x];
-    return *this;
-  }
-
-  double &operator[](int n) { return p_vec[n]; }
-
-  double operator[](int n) const { return p_vec[n]; }
-
-  double &operator()(int n) { return p_vec[n]; }
-
-  double operator()(int n) const { return p_vec[n]; }
-
-  Vector operator+(const Vector &v) const {
-    Vector ret;
-    for (int i = 0; i < N; i++)
-      ret.p_vec[i] = p_vec[i] + v.p_vec[i];
-    return ret;
-  }
-
-  Vector operator-(const Vector &v) const {
-    Vector ret;
-    for (int i = 0; i < N; i++)
-      ret.p_vec[i] = p_vec[i] - v.p_vec[i];
-    return ret;
-  }
-
-  Vector operator*(double scalar) const { return scale(scalar); }
-
-  Vector operator/(double scalar) const {
-    Vector ret;
-    for (int i = 0; i < N; i++)
-      ret.p_vec[i] = p_vec[i] / scalar;
-    return ret;
-  }
-
-  void toDegrees() {
-    for (int i = 0; i < N; i++)
-      p_vec[i] *= 57.2957795131; // 180/pi
-  }
-
-  void toRadians() {
-    for (int i = 0; i < N; i++)
-      p_vec[i] *= 0.01745329251; // pi/180
-  }
-
-  double &x() { return p_vec[0]; }
-  double &y() { return p_vec[1]; }
-  double &z() { return p_vec[2]; }
-  double x() const { return p_vec[0]; }
-  double y() const { return p_vec[1]; }
-  double z() const { return p_vec[2]; }
-
-private:
-  double p_vec[N];
-};
-
-template <> inline Vector<3> Vector<3>::cross(const Vector &v) const {
-  return Vector(p_vec[1] * v.p_vec[2] - p_vec[2] * v.p_vec[1],
-                p_vec[2] * v.p_vec[0] - p_vec[0] * v.p_vec[2],
-                p_vec[0] * v.p_vec[1] - p_vec[1] * v.p_vec[0]);
-}
-
-} // namespace imu
-
-#endif
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/platformio.ini b/Spaceport/23-24/Code/Teensy-Based Avionics/platformio.ini
index 292bd841..3d46ab6b 100644
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/platformio.ini	
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/platformio.ini	
@@ -15,16 +15,15 @@ platform = teensy
 board = teensy41
 framework = arduino
 lib_extra_dirs = /lib/imumaths
-build_src_filter = +<*> -<StateTest/*> +<StateTest/TestUtils.h>
+build_src_filter = +<*> -<StateTest/*> +<StateTest/TestUtils.h> -<GroundStationReceiver/*>
 build_flags = -Wno-unknown-pragmas
 lib_compat_mode = strict
 lib_deps = 
 	adafruit/Adafruit BMP3XX Library@^2.1.2
 	adafruit/RTClib @ ^2.1.1
 	adafruit/Adafruit BNO055@^1.6.3
-	adafruit/Adafruit Unified Sensor@^1.1.14
 	sparkfun/SparkFun u-blox GNSS v3@^3.0.16
-	https://github.com/DrewBrandt/BlinkBuzz.git
+	https://github.com/DrewBrandt/BlinkBuzz.git@^1.0.2
 check_src_filters = -<*> +<src/*> +<include/*> +<lib/*> -<lib/RadioHead>
 check_flags =
 	cppcheck: --suppressions-list=cppcheck-suppressions.txt -j 12
@@ -34,14 +33,13 @@ platform = atmelavr
 board = uno
 framework = arduino
 lib_extra_dirs = /lib/imumaths
-build_src_filter = +<*> -<StateTest/*>
+build_src_filter = +<*> -<StateTest/*> -<GroundStationReceiver/*>
 build_flags = -Wno-unknown-pragmas
 lib_compat_mode = strict
 lib_deps = 
 	adafruit/Adafruit BMP3XX Library@^2.1.2
 	adafruit/RTClib @ ^2.1.1
 	adafruit/Adafruit BNO055@^1.6.3
-	adafruit/Adafruit Unified Sensor@^1.1.14
 	sparkfun/SparkFun u-blox GNSS v3@^3.0.16
 check_src_filters = -<*> +<src/*> +<include/*> +<lib/*> -<lib/RadioHead>
 check_flags =
@@ -52,18 +50,30 @@ platform = teensy
 board = teensy41
 framework = arduino
 lib_extra_dirs = /lib/imumaths
-build_src_filter = +<*> -<main.cpp>
+build_src_filter = +<*> -<main.cpp> -<GroundStationReceiver/*>
 build_flags = -Wno-unknown-pragmas
 lib_compat_mode = strict
 lib_deps = 
 	adafruit/Adafruit BMP3XX Library@^2.1.2
 	adafruit/RTClib @ ^2.1.1
 	adafruit/Adafruit BNO055@^1.6.3
-	adafruit/Adafruit Unified Sensor@^1.1.14
 	sparkfun/SparkFun u-blox GNSS v3@^3.0.16
-	https://github.com/DrewBrandt/BlinkBuzz.git
+	https://github.com/DrewBrandt/BlinkBuzz.git@^1.0.2
 check_src_filters = -<*> +<src/*> +<include/*> +<lib/*> -<lib/RadioHead>
 check_flags =
 	cppcheck: --suppressions-list=cppcheck-suppressions.txt -j 12
 
-
+[env:teensy41_GROUND_STATION_RECEIVER]
+platform = teensy
+board = teensy41
+framework = arduino
+lib_extra_dirs = /lib/imumaths
+build_src_filter = -<*> +<GroundStationReceiver/*> +<Radio/*> +<lib/RadioHead/*>
+build_flags = -Wno-unknown-pragmas
+lib_compat_mode = strict
+lib_deps = 
+	adafruit/Adafruit BNO055@^1.6.3 ; For the imumaths library
+	https://github.com/DrewBrandt/BlinkBuzz.git@^1.0.2
+check_src_filters = -<*> +<src/*> +<include/*> +<lib/*> -<lib/RadioHead>
+check_flags =
+	cppcheck: --suppressions-list=cppcheck-suppressions.txt -j 12
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/APRSEncodeFunctions.cpp b/Spaceport/23-24/Code/Teensy-Based Avionics/src/APRSEncodeFunctions.cpp
deleted file mode 100644
index 9a4e23b7..00000000
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/src/APRSEncodeFunctions.cpp	
+++ /dev/null
@@ -1,182 +0,0 @@
-/*
-MIT License
-
-Copyright (c) 2020 Peter Buchegger
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
-*/
-
-#include "APRSEncodeFunctions.h"
-
-// takes in decimal minutes and converts to MM.dddd
-char *sMin_nn(uint32_t min_nnnnn, int highPrecision)
-{
-    /* min_nnnnn: RawDegrees billionths is uint32_t by definition and is n'telth
-     * degree (-> *= 6 -> nn.mmmmmm minutes) highPrecision: 0: round at decimal
-     * position 2. 1: round at decimal position 4. 2: return decimal position 3-4
-     * as base91 encoded char
-     */
-
-    static char buf[6];
-
-    min_nnnnn = min_nnnnn * 0.006;
-
-    if (highPrecision)
-    {
-        if ((min_nnnnn % 10) >= 5 && min_nnnnn < 6000000 - 5)
-        {
-            // round up. Avoid overflow (59.999999 should never become 60.0 or more)
-            min_nnnnn = min_nnnnn + 5;
-        }
-    }
-    else
-    {
-        if ((min_nnnnn % 1000) >= 500 && min_nnnnn < (6000000 - 500))
-        {
-            // round up. Avoid overflow (59.9999 should never become 60.0 or more)
-            min_nnnnn = min_nnnnn + 500;
-        }
-    }
-
-    if (highPrecision < 2)
-        sprintf(buf, "%02u.%02u", (unsigned int)((min_nnnnn / 100000) % 100), (unsigned int)((min_nnnnn / 1000) % 100));
-    else
-        sprintf(buf, "%c", (char)((min_nnnnn % 1000) / 11) + 33);
-    // Like to verify? type in python for i.e. RawDegrees billions 566688333: i =
-    // 566688333; "%c" % (int(((i*.0006+0.5) % 100)/1.1) +33)
-    return buf;
-}
-
-// creates the latitude string for the APRS message based on whether the GPS coordinates are high precision
-void createLatAprs(char *lat, bool hp)
-{
-    bool negative = lat[0] == '-';
-
-    int len = strlen(lat);
-    int decimalPos = 0;
-    // use for loop in case there is no decimal
-    for (int i = 0; i < len; i++)
-    {
-        if (lat[i] == '.')
-        {
-            decimalPos = i;
-            break;
-        }
-    }
-    // we like sprintf's float up-rounding.
-    // but sprintf % may round to 60.00 -> 5360.00 (53° 60min is a wrong notation
-    // ;)
-
-    int decLen = strlen(lat + decimalPos + 1);
-    int dec = atoi(lat + decimalPos + 1);
-
-    // make sure there are nine digits
-    int missingDigits = 9 - decLen;
-    if (missingDigits < 0)
-        for (int i = 0; i < missingDigits * -1; i++)
-            dec /= 10;
-    if (missingDigits > 0)
-        for (int i = 0; i < missingDigits; i++)
-            dec *= 10;
-    sprintf(lat, "%02d%s%c", atoi(lat) * (negative ? -1 : 1), sMin_nn(dec, hp), negative ? 'S' : 'N');
-}
-
-// creates the longitude string for the APRS message based on whether the GPS coordinates are high precision
-void createLongAprs(char *lng, bool hp)
-{
-    bool negative = lng[0] == '-';
-
-    int len = strlen(lng);
-    int decimalPos = 0;
-    // use for loop in case there is no decimal
-    for (int i = 0; i < len; i++)
-    {
-        if (lng[i] == '.')
-        {
-            decimalPos = i;
-            break;
-        }
-    }
-    // we like sprintf's float up-rounding.
-    // but sprintf % may round to 60.00 -> 5360.00 (53° 60min is a wrong notation
-    // ;)
-
-    int decLen = strlen(lng + decimalPos + 1);
-    int dec = atoi(lng + decimalPos + 1);
-
-    // make sure there are nine digits
-    int missingDigits = 9 - decLen;
-    if (missingDigits < 0)
-        for (int i = 0; i < missingDigits * -1; i++)
-            dec /= 10;
-    if (missingDigits > 0)
-        for (int i = 0; i < missingDigits; i++)
-            dec *= 10;
-    sprintf(lng, "%02d%s%c", atoi(lng) * (negative ? -1 : 1), sMin_nn(dec, hp), negative ? 'W' : 'E');
-}
-
-// creates the dao at the end of aprs message based on latitude and longitude
-void createDaoAprs(char *lat, char *lng, char *dao)
-{
-    // !DAO! extension, use Base91 format for best precision
-    // /1.1 : scale from 0-99 to 0-90 for base91, int(... + 0.5): round to nearest
-    // integer https://metacpan.org/dist/Ham-APRS-FAP/source/FAP.pm
-    // http://www.aprs.org/aprs12/datum.txt
-
-    int len = strlen(lat);
-    int decimalPos = 0;
-    // use for loop in case there is no decimal
-    for (int i = 0; i < len; i++)
-    {
-        if (lat[i] == '.')
-            decimalPos = i;
-    }
-    sprintf(dao, "!w%s", sMin_nn(atoi(lat + decimalPos + 1), 2));
-
-    len = strlen(lng);
-    decimalPos = 0;
-    for (int i = 0; i < len; i++)
-    {
-        if (lng[i] == '.')
-            decimalPos = i;
-    }
-    sprintf(dao + 3, "%s!", sMin_nn(atoi(lng + decimalPos + 1), 2));
-}
-
-// adds a specified number of zeros to the begining of a number
-void padding(unsigned int number, unsigned int width, char *output, int offset)
-{
-    unsigned int numLen = numDigits(number);
-    if (numLen > width)
-    {
-        width = numLen;
-    }
-    for (unsigned int i = 0; i < width - numLen; i++)
-    {
-        output[offset + i] = '0';
-    }
-    sprintf(output + offset + width - numLen, "%u", number);
-}
-
-int numDigits(unsigned int num)
-{
-    if (num < 10)
-        return 1;
-    return 1 + numDigits(num / 10);
-}
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/APRSEncodeFunctions.h b/Spaceport/23-24/Code/Teensy-Based Avionics/src/APRSEncodeFunctions.h
deleted file mode 100644
index 2fd8fd33..00000000
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/src/APRSEncodeFunctions.h	
+++ /dev/null
@@ -1,47 +0,0 @@
-/*
-MIT License
-
-Copyright (c) 2020 Peter Buchegger
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
-*/
-
-#ifndef APRS_ENCODE_FUNCTIONS_H
-#define APRS_ENCODE_FUNCTIONS_H
-
-#if defined(ARDUINO)
-#include <Arduino.h>
-#elif defined(_WIN32) || defined(_WIN64) // Windows
-#include <cstdint>
-#include <string>
-#include <cstring>
-#elif defined(__unix__)  // Linux
-// TODO
-#elif defined(__APPLE__) // OSX
-// TODO
-#endif
-
-char *sMin_nn(uint32_t min_nnnnn, int highPrecision);//intentionally using snake_case for readability
-void createLatAprs(char *lat, bool hp);
-void createLongAprs(char *lng, bool hp);
-void createDaoAprs(char *lat, char *lng, char *dao);
-void padding(unsigned int number, unsigned int width, char *output, int offset = 0);
-int numDigits(unsigned int num);
-
-#endif
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/APRSMsg.cpp b/Spaceport/23-24/Code/Teensy-Based Avionics/src/APRSMsg.cpp
deleted file mode 100644
index e2763a41..00000000
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/src/APRSMsg.cpp	
+++ /dev/null
@@ -1,211 +0,0 @@
-/*
-MIT License
-
-Copyright (c) 2020 Peter Buchegger
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
-*/
-
-#include "APRSMsg.h"
-
-APRSMsg::APRSMsg() : _body()
-{
-}
-
-APRSMsg::APRSMsg(APRSMsg &otherMsg)
-    : _type(otherMsg.getType()), _body()
-{
-    strcpy(_source, otherMsg.getSource());
-    strcpy(_destination, otherMsg.getDestination());
-    strcpy(_path, otherMsg.getPath());
-    strcpy(_rawBody, otherMsg.getRawBody());
-    _body.setData(otherMsg.getBody()->getData());
-}
-
-APRSMsg &APRSMsg::operator=(APRSMsg &otherMsg)
-{
-    if (this != &otherMsg)
-    {
-        setSource(otherMsg.getSource());
-        setDestination(otherMsg.getDestination());
-        setPath(otherMsg.getPath());
-        _type = otherMsg.getType();
-        strcpy(_rawBody, otherMsg.getRawBody());
-        _body.setData(otherMsg.getBody()->getData());
-    }
-    return *this;
-}
-
-APRSMsg::~APRSMsg()
-{
-}
-
-const char *APRSMsg::getSource()
-{
-    return _source;
-}
-
-void APRSMsg::setSource(const char source[8])
-{
-    strcpy(_source, source);
-}
-
-const char *APRSMsg::getDestination()
-{
-    return _destination;
-}
-
-void APRSMsg::setDestination(const char destination[8])
-{
-    strcpy(_destination, destination);
-}
-
-const char *APRSMsg::getPath()
-{
-    return _path;
-}
-
-void APRSMsg::setPath(const char path[10])
-{
-    strcpy(_path, path);
-}
-
-APRSMessageType APRSMsg::getType()
-{
-    return _type;
-}
-
-const char *APRSMsg::getRawBody()
-{
-    return _rawBody;
-}
-
-APRSBody *APRSMsg::getBody()
-{
-    return &_body;
-}
-
-bool APRSMsg::decode(char *message)
-{
-    int len = strlen(message);
-    int posSrc = -1, posDest = -1, posPath = -1;
-    for (int i = 0; i < len; i++)
-    {
-        if (message[i] == '>' && posSrc == -1)
-            posSrc = i;
-        if (message[i] == ',' && posDest == -1)
-            posDest = i;
-        if (message[i] == ':' && posPath == -1)
-            posPath = i;
-    }
-    if (posSrc >= 8)
-        return false;
-    if (posDest - (posSrc + 1) >= 8)
-        return false;
-    if (posPath - (posDest + 1) >= 10)
-        return false;
-
-    if (posSrc >= 0)
-    {
-        strncpy(_source, message, posSrc);
-        _source[posSrc] = '\0';
-    }
-    else
-    {
-        _source[0] = '\0';
-    }
-
-    if (posDest != -1 && posDest < posPath)
-    {
-        strncpy(_path, message + posDest + 1, posPath - (posDest + 1));
-        strncpy(_destination, message + posSrc + 1, posDest - (posSrc + 1));
-        _path[posPath - (posDest + 1)] = '\0';
-        _destination[posDest - (posSrc + 1)] = '\0';
-    }
-    else
-    {
-        _path[0] = '\0';
-        if (posSrc >= 0 && posPath >= 0)
-        {
-
-            strncpy(_destination, message + posSrc + 1, posPath - (posSrc + 1));
-            _destination[posPath - (posSrc + 1)] = '\0';
-        }
-        else
-        {
-            _destination[0] = '\0';
-        }
-    }
-    strcpy(_rawBody, message + posPath + 1);
-    _rawBody[strlen(message + posPath + 1)] = '\0';
-    _type = APRSMessageType(_rawBody[0]);
-    _body.decode(_rawBody);
-    return bool(_type);
-}
-
-void APRSMsg::encode(char *message)
-{
-    sprintf(message, "%s>%s", _source, _destination);
-    if (strlen(_path) > 0)
-    {
-        sprintf(message + strlen(message), ",%s", _path);
-    }
-    sprintf(message + strlen(message), ":%s", _body.encode());
-}
-
-void APRSMsg::toString(char *str)
-{
-    char body[87];
-    _body.toString(body);
-    sprintf(str, "Source:%s,Destination:%s,Path:%s,Type:%s,%s", _source, _destination, _path, _type.toString(), body);
-}
-
-APRSBody::APRSBody()
-{
-}
-
-APRSBody::~APRSBody()
-{
-}
-
-const char *APRSBody::getData()
-{
-    return _data;
-}
-
-void APRSBody::setData(const char data[80])
-{
-    strcpy(_data, data);
-}
-
-bool APRSBody::decode(char *message)
-{
-    strcpy(_data, message);
-    return true;
-}
-
-const char *APRSBody::encode()
-{
-    return _data;
-}
-
-void APRSBody::toString(char *str)
-{
-    sprintf(str, "Data:%s", _data);
-}
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/APRSMsg.h b/Spaceport/23-24/Code/Teensy-Based Avionics/src/APRSMsg.h
deleted file mode 100644
index 1ceec249..00000000
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/src/APRSMsg.h	
+++ /dev/null
@@ -1,222 +0,0 @@
-/*
-MIT License
-
-Copyright (c) 2020 Peter Buchegger
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in all
-copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
-SOFTWARE.
-*/
-
-#ifndef APRSMSG_H
-#define APRSMSG_H
-
-#if defined(ARDUINO)
-#include <Arduino.h>
-#elif defined(_WIN32) || defined(_WIN64) // Windows
-#include <cstdint>
-#include <string>
-#include <cstring>
-#elif defined(__unix__)  // Linux
-// TODO
-#elif defined(__APPLE__) // OSX
-// TODO
-#endif
-
-class APRSMessageType
-{
-public:
-    enum Value : uint8_t
-    {
-        PositionWithoutTimestamp, // = and !
-        PositionWithTimestamp,    // @ and /
-        Status,                   // >
-        Query,                    // ?
-        Message,                  // :
-        Weather,                  // _
-        Telemetry,                // T
-        CurrentMicEData,          // `
-        // you can add more types ;)
-        Error,
-    };
-
-    APRSMessageType() = default;
-    // cppcheck-suppress noExplicitConstructor
-    APRSMessageType(char type)
-    {
-        switch (type)
-        {
-        case '=':
-        case '!':
-            value = PositionWithoutTimestamp;
-            break;
-        case '@':
-        case '/':
-            value = PositionWithTimestamp;
-            break;
-        case '>':
-            value = Status;
-            break;
-        case '?':
-            value = Query;
-            break;
-        case ':':
-            value = Message;
-            break;
-        case '_':
-            value = Weather;
-            break;
-        case 'T':
-            value = Telemetry;
-            break;
-        case '`':
-            value = CurrentMicEData;
-            break;
-        default:
-            value = Error;
-        }
-    }
-    // cppcheck-suppress noExplicitConstructor
-    constexpr APRSMessageType(Value aType) : value(aType) {}
-    constexpr bool operator==(APRSMessageType a) const { return value == a.value; }
-    constexpr bool operator!=(APRSMessageType a) const { return value != a.value; }
-    explicit operator bool() const { return value != Error; }
-
-    const char *toString() const
-    {
-        switch (value)
-        {
-        case PositionWithoutTimestamp:
-            return "Position Without Timestamp";
-        case PositionWithTimestamp:
-            return "Position With Timestamp";
-        case Status:
-            return "Status";
-        case Query:
-            return "Query";
-        case Message:
-            return "Message";
-        case Weather:
-            return "Weather";
-        case Telemetry:
-            return "Telemetry";
-        case CurrentMicEData:
-            return "Current Mic-E Data";
-        default:
-            return "Error";
-        }
-    }
-
-private:
-    Value value;
-};
-
-class APRSBody
-{
-public:
-    APRSBody();
-    virtual ~APRSBody();
-
-    const char *getData();
-    void setData(const char data[80]);
-
-    virtual bool decode(char *message);
-    virtual const char *encode();
-    virtual void toString(char *str);
-
-private:
-    char _data[80];
-};
-
-class APRSMsg
-{
-public:
-    APRSMsg();
-    APRSMsg(APRSMsg &otherMsg);
-    APRSMsg &operator=(APRSMsg &otherMsg);
-    virtual ~APRSMsg();
-
-    const char *getSource();
-    void setSource(const char source[8]);
-
-    const char *getDestination();
-    void setDestination(const char destination[8]);
-
-    const char *getPath();
-    void setPath(const char path[10]);
-
-    APRSMessageType getType();
-
-    const char *getRawBody();
-    APRSBody *getBody();
-
-    virtual bool decode(char *message);
-    virtual void encode(char *message);
-    virtual void toString(char *str);
-
-private:
-    char _source[8];
-    char _destination[8];
-    char _path[10];
-    APRSMessageType _type;
-    char _rawBody[80];
-    APRSBody _body;
-};
-
-/*
-APRS Configuration
-- CALLSIGN
-- TOCALL
-- PATH
-- SYMBOL
-- OVERLAY
-*/
-struct APRSConfig
-{
-    char CALLSIGN[8];
-    char TOCALL[8];
-    char PATH[10];
-    char SYMBOL;
-    char OVERLAY;
-};
-
-/*
-APRS Telemetry Data
-- lat
-- lng
-- alt
-- spd
-- hdg
-- precision
-- stage
-- t0
-- dao
-*/
-struct APRSData
-{
-    char lat[16];
-    char lng[16];
-    char alt[10];
-    char spd[4];
-    char hdg[4];
-    char precision;
-    char stage[3];
-    char t0[9];
-    char dao[6];
-};
-
-#endif // RADIO_H
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/BNO055.cpp b/Spaceport/23-24/Code/Teensy-Based Avionics/src/BNO055.cpp
index faf8ecf9..8a2405f8 100644
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/src/BNO055.cpp	
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/BNO055.cpp	
@@ -12,6 +12,18 @@ bool BNO055::initialize()
     {
         return initialized = false;
     }
+
+    // set to +-16g range
+    adafruit_bno055_opmode_t mode = bno.getMode();
+    bno.setMode(OPERATION_MODE_CONFIG);
+    delay(25);
+    Wire.beginTransmission(0x29);//BNO055 address
+    Wire.write(0x08);//ACC_CONFIG register address on BNO055
+    Wire.write(0b11);//set to +-16g range
+    Wire.endTransmission(true); // send stop
+    bno.setMode(mode);
+    delay(25);
+
     bno.setExtCrystalUse(true);
 
     initialMagField = bno.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
@@ -42,6 +54,10 @@ void BNO055::update()
     {
         accelerationVec = bno.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
     }
+    double x = accelerationVec.z();
+    accelerationVec.x() = accelerationVec.z();
+    accelerationVec.z() = x;
+
     orientation = bno.getQuat();
     orientationEuler = bno.getVector(Adafruit_BNO055::VECTOR_EULER);
     magnetometer = bno.getVector(Adafruit_BNO055::VECTOR_MAGNETOMETER);
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/GroundStationReceiver/EncodeAPRSForSerial.h b/Spaceport/23-24/Code/Teensy-Based Avionics/src/GroundStationReceiver/EncodeAPRSForSerial.h
new file mode 100644
index 00000000..dabc2c7a
--- /dev/null
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/GroundStationReceiver/EncodeAPRSForSerial.h	
@@ -0,0 +1,104 @@
+#ifndef ENCODE_APRS_FOR_SERIAL_H
+#define ENCODE_APRS_FOR_SERIAL_H
+
+#include "../Radio/APRS/APRSTelemMsg.h"
+
+namespace aprsToSerial
+{
+    const char OVERLAY = '/';
+    const char SYMBOL = '['; // Jogger symbol. I disagree with this usage.
+
+    void encodeLatLong(const APRSTelemMsg &msg, char *buffer, size_t buffer_size);
+    void encodeAltitude(const APRSTelemMsg &msg, char *buffer, size_t buffer_size);
+    void encodeFlags(const APRSTelemMsg &msg, char *buffer, size_t buffer_size);
+    /*
+     * Encodes an APRS message into a string for serial transmission
+     *   \param msg The APRS message to encode
+     *   \param buffer The buffer to write the encoded message to
+     *   \param buffer_size The size of the buffer
+     *   \return void
+     */
+    void encodeAPRSForSerial(const APRSTelemMsg &msg, char *buffer, size_t buffer_size, int RSSI)
+    {
+        char prefix[8] = "";
+
+        char header[100];
+        snprintf(header, 100,
+                 "Source:%s,Destination:%s,Path:%s,Type:%s,",
+                 msg.header.CALLSIGN,
+                 msg.header.TOCALL,
+                 msg.header.PATH,
+                 "Position Without Timestamp");
+
+        char latLong[20];
+        encodeLatLong(msg, latLong, 20);
+
+        char altitude[10];
+        encodeAltitude(msg, altitude, 10);
+
+        char flags[5];
+        encodeFlags(msg, flags, 5);
+
+        char suffix[10] = "!e";
+        // format: s\r\nSource:CALLSIGN,Destination:TOCALL,Path:PATH,Type:Position Without Timestamp,Data:!DDMM.MM[NS][OVERLAY]DDDMM.MM[WE][hhh/sss/A=DDDDDD/S[s]/zzz/yyy/xxx/fff,RSSI:RSSI\r\ne\r\n
+        snprintf(buffer, buffer_size,
+                 "%s%sData:!%s%c%03d/%03d/%s/S%d/%03d/%03d/%03d/%s,RSSI:%03d%s",
+                 prefix,
+                 header,
+                 latLong,
+                 SYMBOL,
+                 (int)msg.data.hdg,
+                 (int)msg.data.spd,
+                 altitude, msg.data.stage,
+                 (int)msg.data.orientation.z(),
+                 (int)msg.data.orientation.y(),
+                 (int)msg.data.orientation.x(),
+                 flags,
+                 RSSI,
+                 suffix);
+                 Serial.println(buffer);
+    }
+
+    void encodeLatLong(const APRSTelemMsg &msg, char *buffer, size_t buffer_size)
+    {
+        int lat_deg = (int)abs(msg.data.lat);
+        double lat_min = ((msg.data.lat - lat_deg) * 60);
+        char lat_dir = (msg.data.lat < 0) ? 'S' : 'N';
+
+        int long_deg = (int)abs(msg.data.lng);
+        double long_min = ((msg.data.lng - long_deg) * 60);
+        char long_dir = (msg.data.lng < 0) ? 'W' : 'E';
+
+        // format: DDMM.MM[NS][OVERLAY]DDDMM.MM[WE]
+        snprintf(buffer, buffer_size,
+                 "%02d%05.02f%c%c%03d%05.02f%c",
+                 lat_deg,
+                 lat_min,
+                 lat_dir,
+                 OVERLAY,
+                 long_deg,
+                 long_min,
+                 long_dir);
+    }
+
+    void encodeAltitude(const APRSTelemMsg &msg, char *buffer, size_t buffer_size)
+    {
+        // format: A=DDDDDD
+        int alt = (int)msg.data.alt;
+        if (alt < 0)
+            snprintf(buffer, buffer_size, "A=%07d", alt);
+        else
+            snprintf(buffer, buffer_size, "A=%06d", alt);
+    }
+
+    // print status flags as plain text
+    void encodeFlags(const APRSTelemMsg &msg, char *buffer, size_t buffer_size)
+    {
+        char f1 = (msg.data.statusFlags & PI_ON) ? '1' : '0';
+        char f2 = (msg.data.statusFlags & PI_VIDEO) ? '1' : '0';
+        char f3 = (msg.data.statusFlags & RECORDING_DATA) ? '1' : '0';
+        snprintf(buffer, buffer_size, "%c%c%c", f1, f2, f3);
+    }
+}
+
+#endif
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/GroundStationReceiver/main.cpp b/Spaceport/23-24/Code/Teensy-Based Avionics/src/GroundStationReceiver/main.cpp
new file mode 100644
index 00000000..8556a7a6
--- /dev/null
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/GroundStationReceiver/main.cpp	
@@ -0,0 +1,79 @@
+#include "EncodeAPRSForSerial.h"
+#include "Adafruit_BNO055.h"
+#include <Arduino.h>
+#include "../Radio/RFM69HCW.h"
+#include "../Radio/APRS/APRSCmdMsg.h"
+#include <BlinkBuzz.h>
+
+APRSHeader header = {"KC3UTM", "APRS", "WIDE1-1", '/', 'o'};
+APRSTelemMsg msg(header);
+APRSCmdMsg cmd(header);
+RadioSettings settings = {433.78, 0x02, 0x01, &hardware_spi, 15, 16, 14};
+RFM69HCW radio(&settings);
+
+int arr[] = {LED_BUILTIN, 33};
+BlinkBuzz bb(arr, 1, true);
+void setup()
+{
+
+    delay(2000); // Delay to allow the serial monitor to connect
+
+    if (!radio.init())
+        Serial.println("Radio failed to initialize");
+    else
+        Serial.println("Radio initialized");
+
+    // Test Case for Demo Purposes
+    cmd.data.MinutesUntilPowerOn = 0;
+    cmd.data.MinutesUntilDataRecording = -1;
+    cmd.data.MinutesUntilVideoStart = -2;
+    cmd.data.Launch = false;
+}
+
+int counter = 1;
+void loop()
+{
+    delay(5);
+    if (radio.update())
+    {
+        if (radio.dequeueReceive(&msg))
+        {
+            char buffer[255];
+            aprsToSerial::encodeAPRSForSerial(msg, buffer, 255, radio.RSSI());
+            Serial.println(buffer);
+            counter = 0;
+        }
+    }
+    else// reset the radio if we havent heard anything in a while
+    {
+        if (counter == 15000)
+        {
+            radio.init();
+            counter = 0;
+        }
+    }
+    counter ++;
+
+    if (Serial.available())
+    {
+        char buffer[255];
+        Serial.readBytesUntil('\n', buffer, 255);
+        char boolStr[6]; // "true" or "false" (max length 5 + 1 for null terminator)
+
+        // Parse the input string
+        sscanf(buffer, "%d,%d,%d,%5s", &cmd.data.MinutesUntilPowerOn, &cmd.data.MinutesUntilVideoStart, &cmd.data.MinutesUntilDataRecording, boolStr);
+
+        // Convert the string "true"/"false" to a boolean value
+        if (strcmp(boolStr, "true") == 0)
+        {
+            cmd.data.Launch = true;
+        }
+        else if (strcmp(boolStr, "false") == 0)
+        {
+            cmd.data.Launch = false;
+        }
+
+        // Send the command
+        radio.enqueueSend(&cmd);
+    }
+}
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/MAX_M10S.cpp b/Spaceport/23-24/Code/Teensy-Based Avionics/src/MAX_M10S.cpp
index e1883a67..63084f85 100644
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/src/MAX_M10S.cpp	
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/MAX_M10S.cpp	
@@ -70,7 +70,7 @@ void MAX_M10S::update()
     pos.x() = m10s.getLatitude() / 10000000.0;
     pos.y() = m10s.getLongitude() / 10000000.0;
 
-    heading = m10s.getHeading();
+    heading = m10s.getHeading() / 100000.0;
     altitude = m10s.getAltitude() / 1000.0;
     fixQual = m10s.getSIV();
     if (!hasFirstFix && fixQual >= 4)
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/Pi.cpp b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Pi.cpp
new file mode 100644
index 00000000..19b8258b
--- /dev/null
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Pi.cpp	
@@ -0,0 +1,40 @@
+#include "Pi.h"
+
+Pi::Pi(int pinControl, int pinVideo)
+{
+    this->pinControl = pinControl;
+    this->pinVideo = pinVideo;
+
+    pinMode(pinControl, OUTPUT);
+    pinMode(pinVideo, OUTPUT);
+
+    digitalWrite(pinVideo, HIGH); // Set video pin to high (off) by default
+
+    on = false;
+    recording = false;
+}
+
+void Pi::setOn(bool on)
+{
+    digitalWrite(this->pinControl, on ? HIGH : LOW);
+    this->on = on;
+}
+
+void Pi::setRecording(bool recording)
+{
+    if(this->recording == recording) return; // If the recording state is the same, do nothing
+
+    bb.aonoff(BUZZER, 100, 3); // Buzz 3 times (100ms on, 100ms off)
+    digitalWrite(pinVideo, recording ? LOW : HIGH);
+    this->recording = recording;
+}
+
+bool Pi::isOn()
+{
+    return on;
+}
+
+bool Pi::isRecording()
+{
+    return recording;
+}
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/Pi.h b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Pi.h
new file mode 100644
index 00000000..1f7bbfe6
--- /dev/null
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Pi.h	
@@ -0,0 +1,23 @@
+#ifndef PI_H
+#define PI_H
+
+#include <Arduino.h>
+#include <BlinkBuzz.h>
+
+class Pi
+{
+public:
+    Pi(int pinControl, int pinVideo);
+    void setOn(bool on);
+    void setRecording(bool recording);
+    bool isOn();
+    bool isRecording();
+
+private:
+    int pinControl;
+    int pinVideo;
+    bool on;
+    bool recording;
+};
+
+#endif // PI_H
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/RFM69HCW.cpp b/Spaceport/23-24/Code/Teensy-Based Avionics/src/RFM69HCW.cpp
deleted file mode 100644
index 03f04fc1..00000000
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/src/RFM69HCW.cpp	
+++ /dev/null
@@ -1,604 +0,0 @@
-#include "RFM69HCW.h"
-
-/*
-Constructor
-    - frequency in hertz
-    - transmitter true if the radio will be on the rocket
-    - highBitrate true for 300kbps, false for 4.8kbps
-    - config with APRS settings
-*/
-RFM69HCW::RFM69HCW(const RadioSettings s, const APRSConfig config) : radio(s.cs, s.irq, *s.spi)
-{
-    if (s.transmitter)
-    {
-        // addresses for transmitters
-        this->addr = 0x02;
-        this->toAddr = 0x01;
-        this->id = 0x00; // to fit the max number of bytes in a packet, this will change
-                         // based on message length, but it will be reset to this value
-                         // after each complete transmission
-    }
-    else
-    {
-        // addresses for receivers
-        this->addr = 0x01;
-        this->toAddr = 0x02;
-        this->id = 0x00;
-    }
-
-    this->avail = false;
-
-    this->settings = s;
-    this->cfg = config;
-}
-
-/*
-Initializer to call in setup
-    - s is the spi interface that should be used
-    - cs is the chip select pin
-    - irq is the interrupt pin
-*/
-bool RFM69HCW::begin()
-{
-    pinMode(this->settings.rst, OUTPUT);
-    digitalWrite(this->settings.rst, LOW);
-    delay(10);
-    digitalWrite(this->settings.rst, HIGH);
-    delay(10);
-    digitalWrite(this->settings.rst, LOW);
-
-    if (!this->radio.init())
-        return false;
-
-    // then use this to actually set the frequency
-    if (!this->radio.setFrequency(this->settings.frequency))
-        return false;
-    this->radio.setTxPower(this->txPower, true);
-
-    // always set FSK mode
-    this->radio.setModemConfig(RH_RF69::FSK_Rb4_8Fd9_6);
-
-    // set headers
-    this->radio.setHeaderTo(this->toAddr);
-    this->radio.setHeaderFrom(this->addr);
-    this->radio.setThisAddress(this->addr);
-    this->radio.setHeaderId(this->id);
-
-    if (this->settings.highBitrate)
-    {
-        // the default bitrate of 4.8kbps should be fine unless we want high bitrate for video
-        set300KBPS();
-        // remove as much overhead as possible
-        this->radio.setPreambleLength(0);
-        this->radio.setSyncWords();
-    }
-
-    return true;
-}
-
-/*
-Transmit function
-Most basic transmission method, simply transmits the string without modification
-    - message is the message to be transmitted, must be null terminated
-*/
-bool RFM69HCW::tx(const char *message, int len)
-{
-
-    // get number of packets for this message, and set this radio's id to that value so the receiver knows how many packets to expect
-    this->id = len / this->bufSize + (len % this->bufSize > 0);
-    this->radio.setHeaderId(this->id);
-
-// ignore same address warning
-#pragma GCC diagnostic push
-#pragma GCC diagnostic ignored "-Wrestrict"
-    strcpy(this->msg, message);
-#pragma GCC diagnostic pop
-
-    this->msgLen = len;
-    this->totalPackets = 0;
-    sendBuffer();
-    // fill the buffer repeatedly until the entire message has been sent
-    // for (unsigned int j = 0; j < this->id; j++)
-    // {
-    // memcpy(this->buf, message + j * this->bufSize, min(this->bufSize, len - j * this->bufSize));
-    // this->radio.send(this->buf, min(this->bufSize, len - j * this->bufSize));
-    // if (j != this->id - 1u)
-    //     this->radio.waitPacketSent();
-    // }
-
-    return true;
-}
-
-bool RFM69HCW::sendBuffer()
-{
-    if (this->totalPackets >= this->id)
-        return true;
-    memcpy(this->buf, this->msg + this->totalPackets * this->bufSize, min(this->bufSize, this->msgLen - this->totalPackets * this->bufSize));
-    this->radio.send(this->buf, min(this->bufSize, this->msgLen - this->totalPackets * this->bufSize));
-    if (this->radio.headerId() == this->totalPackets)
-        this->radio.setHeaderId(0xff);
-    this->totalPackets++;
-    return this->totalPackets == this->id;
-}
-
-void RFM69HCW::endtx()
-{
-    this->id = 0x00;
-    this->radio.setHeaderId(this->id);
-}
-
-RHGenericDriver::RHMode RFM69HCW::mode() { return radio.mode(); }
-
-/*
-Receive function
-Most basic receiving method, simply checks for a message and returns a pointer to it
-Note that the message may be incomplete, if the message is complete available() will return true
-The received message must be less than MSG_LEN
-*/
-const char *RFM69HCW::rx()
-{
-    if (this->radio.available())
-    {
-        // Should be a message for us now
-        uint8_t receivedLen = this->bufSize;
-        if (this->radio.recv(this->buf, &(receivedLen)))
-        {
-            // check if this is the first part of the message
-            if (this->totalPackets == 0)
-            {
-                this->totalPackets = this->radio.headerId();
-                if (this->totalPackets <= 0 || this->totalPackets == 0xff)
-                {
-                    this->totalPackets = 0;
-                    return 0;
-                }
-                memcpy(this->msg, this->buf, receivedLen);
-                this->msg[receivedLen] = '\0';
-            }
-            else // otherwise append to the end of the message, removing the \0 from last time
-            {
-                int len = strlen(this->msg);
-                memcpy(this->msg + len, this->buf, min(MSG_LEN - (len + receivedLen), receivedLen));
-                this->msg[min(MSG_LEN, len + receivedLen)] = '\0'; // make sure we don't go over MSG_LEN
-            }
-            this->id++;
-            // Debug
-            // Serial.print("Received [");
-            // Serial.print(this->bufSize);
-            // Serial.print("]: ");
-            // this->buf[this->bufSize] = 0;
-            // Serial.println((char *)this->buf);
-            // Serial.print("RSSI: ");
-            // Serial.println(this->radio.lastRssi(), DEC);
-            // Serial.println("");
-
-            this->rssi += radio.lastRssi();
-
-            // check if the full message has been received
-            // int msgLen = strlen(this->msg);
-            if (this->totalPackets == this->id) // this works for now, maybe find a better way later?
-            {
-                this->id = 0;
-                this->rssi /= this->totalPackets;
-                this->totalPackets = 0;
-                this->avail = true;
-            }
-
-            return this->msg;
-        }
-        return "Failed to receive message";
-    }
-    return "No message available";
-}
-
-/*
-Multi-purpose encoder function
-Encodes the message into a format selected by type
-char *message must be a dynamically allocated null terminated string
-- Telemetry:
-    message - input: latitude,longitude,altitude,speed,heading,precision,stage,t0 -> output: APRS message
-- Video: TODO
-    message - input: char* filled with raw bytes -> output: Raw byte array
-- Ground Station: TODO
-    message - input: Source:Value,Destination:Value,Path:Value,Type:Value,Body:Value -> output: APRS message
-*/
-bool RFM69HCW::encode(char *message, EncodingType type)
-{
-    if (type == ENCT_NONE)
-        return true;
-    if (type == ENCT_TELEMETRY)
-    {
-        int msgLen = strlen(message);
-
-        // holds the data to be assembled into the aprs body
-        APRSData data;
-
-        // find each value separated in order by a comma and put in the APRSData array
-        {
-            char currentVal[MSG_LEN];
-            int currentValIndex = 0;
-            int currentValCount = 0;
-            for (int i = 0; i < msgLen; i++)
-            {
-                if (message[i] != ',')
-                {
-                    currentVal[currentValIndex] = message[i];
-                    currentValIndex++;
-                }
-                if (message[i] == ',' || (currentValCount == 7 && i == msgLen - 1))
-                {
-                    currentVal[currentValIndex] = '\0';
-
-                    if (currentValCount == 0 && strlen(currentVal) < 16)
-                        strcpy(data.lat, currentVal);
-                    if (currentValCount == 1 && strlen(currentVal) < 16)
-                        strcpy(data.lng, currentVal);
-                    if (currentValCount == 2 && strlen(currentVal) < 10)
-                        strcpy(data.alt, currentVal);
-                    if (currentValCount == 3 && strlen(currentVal) < 4)
-                        strcpy(data.spd, currentVal);
-                    if (currentValCount == 4 && strlen(currentVal) < 4)
-                        strcpy(data.hdg, currentVal);
-                    if (currentValCount == 5)
-                        data.precision = currentVal[0];
-                    if (currentValCount == 6 && strlen(currentVal) < 3)
-                        strcpy(data.stage, currentVal);
-                    if (currentValCount == 7 && strlen(currentVal) < 9)
-                        strcpy(data.t0, currentVal);
-
-                    currentValIndex = 0;
-                    currentValCount++;
-                }
-            }
-        }
-        // get lat and long string for low or high precision
-        if (data.precision == 'L')
-        {
-            strcpy(data.dao, "");
-            createLatAprs(data.lat, 0);
-            createLongAprs(data.lng, 0);
-        }
-        else if (data.precision == 'H')
-        {
-            createDaoAprs(data.lat, data.lng, data.dao);
-            createLatAprs(data.lat, 1);
-            createLongAprs(data.lng, 1);
-        }
-        // get alt string
-        int altInt = max(-99999, min(999999, atoi(data.alt)));
-        if (altInt < 0)
-        {
-            strcpy(data.alt, "/A=-");
-            padding(altInt * -1, 5, data.alt, 4);
-        }
-        else
-        {
-            strcpy(data.alt, "/A=");
-            padding(altInt, 6, data.alt, 3);
-        }
-
-        // get course/speed strings
-        // TODO add speed zero counter (makes decoding more complex)
-        int spdInt = max(0, min(999, atoi(data.spd)));
-        int hdgInt = max(0, min(360, atoi(data.hdg)));
-        if (hdgInt == 0)
-            hdgInt = 360;
-        padding(spdInt, 3, data.spd);
-        padding(hdgInt, 3, data.hdg);
-
-        APRSMsg aprs;
-
-        aprs.setSource(this->cfg.CALLSIGN);
-        aprs.setPath(this->cfg.PATH);
-        aprs.setDestination(this->cfg.TOCALL);
-
-        // generate the aprs message
-        char body[80];
-        sprintf(body, "%c%s%c%s%c%s%c%s%s%s%s%c%s%c%s", '!', data.lat, this->cfg.OVERLAY, data.lng, this->cfg.SYMBOL,
-                data.hdg, '/', data.spd, data.alt, "/S", data.stage, '/',
-                data.t0, ' ', data.dao);
-        aprs.getBody()->setData(body);
-        aprs.encode(message);
-        return true;
-    }
-    return false;
-}
-
-/*
-Multi-purpose encoder function
-Decodes the message into a format selected by type
-char *message must be a dynamically allocated null terminated string
-- Telemetry:
-    message - output: latitude,longitude,altitude,speed,heading,precision,stage,t0 <- input: APRS message
-- Video: TODO
-    message - output: char* filled with raw bytes <- input: Raw byte array
-- Ground Station:
-    message - output: Source:Value,Destination:Value,Path:Value,Type:Value,Body:Value <- input: APRS message
-*/
-bool RFM69HCW::decode(char *message, EncodingType type)
-{
-    if (type == ENCT_NONE)
-        return true;
-    if (type == ENCT_TELEMETRY)
-    {
-        APRSMsg aprs;
-        aprs.decode(message);
-
-        char body[80];
-        char *bodyptr = body;
-        strcpy(body, aprs.getBody()->getData());
-        // decode body
-        APRSData data;
-        int i = 0;
-        int len = strlen(body);
-
-        // TODO this could probably be shortened
-        // body should start with '!'
-        if (body[0] != '!')
-            return false;
-        i++;
-        bodyptr = body + i;
-
-        // find latitude
-        while (body[i] != '/' && i != len)
-            i++;
-        if (i == len)
-            return false;
-        strncpy(data.lat, bodyptr, i - (bodyptr - body));
-        data.lat[i - (bodyptr - body)] = '\0';
-        i++;
-        bodyptr = body + i;
-
-        // find longitude
-        while (body[i] != '[' && i != len)
-            i++;
-        if (i == len)
-            return false;
-        strncpy(data.lng, bodyptr, i - (bodyptr - body));
-        data.lng[i - (bodyptr - body)] = '\0';
-        i++;
-        bodyptr = body + i;
-
-        // find heading
-        while (body[i] != '/' && i != len)
-            i++;
-        if (i == len)
-            return false;
-        strncpy(data.hdg, bodyptr, i - (bodyptr - body));
-        data.hdg[i - (bodyptr - body)] = '\0';
-        i++;
-        bodyptr = body + i;
-
-        // find speed
-        while (body[i] != '/' && i != len)
-            i++;
-        if (i == len)
-            return false;
-        strncpy(data.spd, bodyptr, i - (bodyptr - body));
-        data.spd[i - (bodyptr - body)] = '\0';
-        i++;
-        bodyptr = body + i;
-
-        // find altitude
-        if (body[i] != 'A' && body[i + 1] != '=')
-            return false;
-        i += 2;
-        bodyptr = body + i;
-        while (body[i] != '/' && i != len)
-            i++;
-        if (i == len)
-            return false;
-        strncpy(data.alt, bodyptr, i - (bodyptr - body));
-        data.alt[i - (bodyptr - body)] = '\0';
-        i++;
-        bodyptr = body + i;
-
-        // find stage
-        if (body[i] != 'S')
-            return false;
-        i++;
-        bodyptr = body + i;
-        while (body[i] != '/' && i != len)
-            i++;
-        if (i == len)
-            return false;
-        strncpy(data.stage, bodyptr, i - (bodyptr - body));
-        data.stage[i - (bodyptr - body)] = '\0';
-        i++;
-        bodyptr = body + i;
-
-        // find t0
-        while (body[i] != ' ' && i != len)
-            i++;
-        if (i == len)
-            return false;
-        strncpy(data.t0, bodyptr, i - (bodyptr - body));
-        data.t0[i - (bodyptr - body)] = '\0';
-        i++;
-        bodyptr = body + i;
-
-        // find dao
-        strncpy(data.dao, bodyptr, len - (bodyptr - body));
-        data.dao[len - (bodyptr - body)] = '\0';
-
-        // convert lat and lng to degrees
-
-        int latMult = (data.lat[strlen(data.lat) - 1] == 'N') ? 1 : -1;
-        int lngMult = (data.lat[strlen(data.lat) - 1] == 'E') ? 1 : -1;
-
-        int lenLat = strlen(data.lat);
-        int decimalPosLat = 0;
-        // use for loop in case there is no decimal
-        for (int i = 0; i < lenLat; i++)
-        {
-            if (data.lat[i] == '.')
-            {
-                decimalPosLat = i;
-                break;
-            }
-        }
-
-        int lenLng = strlen(data.lng);
-        int decimalPosLng = 0;
-        // use for loop in case there is no decimal
-        for (int i = 0; i < lenLng; i++)
-        {
-            if (data.lng[i] == '.')
-            {
-                decimalPosLng = i;
-                break;
-            }
-        }
-
-        double lat = 0;
-        for (int i = decimalPosLat - 3; i >= 0; i--)
-        {
-            int t = data.lat[i];
-            for (int j = 0; j < i; j++)
-                t *= 10;
-            lat += t;
-        }
-
-        double latMins = 0;
-        for (int i = lenLat - 2; i > decimalPosLat - 3; i--)
-        {
-            if (data.lat[i] == '.')
-                continue;
-            double t = data.lat[i];
-            for (int j = (lenLat - 2 - decimalPosLat) * -1; j < i - 2; j++)
-                t *= j < 0 ? 1 / 10 : 10;
-            latMins += t;
-        }
-        latMins /= 60;
-
-        lat += latMins;
-
-        double lng = 0;
-        for (int i = decimalPosLng - 3; i >= 0; i--)
-        {
-            int t = data.lng[i];
-            for (int j = 0; j < i; j++)
-                t *= 10;
-            lat += t;
-        }
-
-        double lngMins = 0;
-        for (int i = lenLng - 2; i > decimalPosLng - 3; i--)
-        {
-            if (data.lng[i] == '.')
-                continue;
-            double t = data.lng[i];
-            for (int j = (lenLng - 2 - decimalPosLng) * -1; j < i - 2; j++)
-                t *= j < 0 ? 1 / 10 : 10;
-            lngMins += t;
-        }
-        lngMins /= 60;
-
-        lng += lngMins;
-
-        lat *= latMult;
-        lng *= lngMult;
-
-        sprintf(message, "%f,%f,%s,%s,%s,%c,%s,%s", lat, lng, data.alt, data.spd, data.hdg, strlen(data.dao) > 0 ? 'H' : 'L', data.stage, data.t0);
-
-        return true;
-    }
-    if (type == ENCT_GROUNDSTATION)
-    {
-        // put the message into a APRSMessage object to decode it
-        APRSMsg aprs;
-        aprs.decode(message);
-
-        aprs.toString(message);
-        // add RSSI to the end of message
-        sprintf(message + strlen(message), "%s%d", ",RSSI:", this->rssi);
-        return true;
-    }
-    return false;
-}
-
-/*
-Comprehensive send function
-Encodes the message into the selected type, then sends it
-char *message must be a null terminated string
-Transmitted and encoded message length must not exceed MSG_LEN
-    - message is the message to be sent
-    - type is the encoding type
-*/
-bool RFM69HCW::send(const char *message, EncodingType type)
-{
-    strcpy(this->msg, message);
-    this->msg[MSG_LEN] = '\0'; // make sure msg is null terminated just in case
-    encode(this->msg, type);
-    tx(this->msg, strlen(this->msg));
-    return true;
-}
-
-/*
-Comprehensive receive function
-Should be called after verifying there is an available message by calling available()
-Decodes the last received message according to the type
-Received and decoded message length must not exceed MSG_LEN
-    - type is the decoding type
-*/
-const char *RFM69HCW::receive(EncodingType type)
-{
-    if (this->avail)
-    {
-        this->avail = false;
-        decode(this->msg, type);
-        return this->msg;
-    }
-    return "";
-}
-
-/*
-Returns true if a there is a new message
-*/
-bool RFM69HCW::available()
-{
-    rx();
-    return this->avail;
-}
-
-/*
-Returns the RSSI of the last message
-*/
-int RFM69HCW::RSSI()
-{
-    return this->rssi;
-}
-
-void RFM69HCW::set300KBPS()
-{
-    this->radio.spiWrite(0x03, 0x00);       // REG_BITRATEMSB: 300kbps (0x006B, see DS p20)
-    this->radio.spiWrite(0x04, 0x6B);       // REG_BITRATELSB: 300kbps (0x006B, see DS p20)
-    this->radio.spiWrite(0x19, 0x40);       // REG_RXBW: 500kHz
-    this->radio.spiWrite(0x1A, 0x80);       // REG_AFCBW: 500kHz
-    this->radio.spiWrite(0x05, 0x13);       // REG_FDEVMSB: 300khz (0x1333)
-    this->radio.spiWrite(0x06, 0x33);       // REG_FDEVLSB: 300khz (0x1333)
-    this->radio.spiWrite(0x29, 240);        // set REG_RSSITHRESH to -120dBm
-    this->radio.spiWrite(0x37, 0b10010000); // DC=WHITENING, CRCAUTOOFF=0
-                                            //                ^^->DC: 00=none, 01=manchester, 10=whitening
-}
-
-// utility functions
-#ifndef max
-int max(int a, int b)
-{
-    if (a > b)
-        return a;
-    return b;
-}
-
-#endif
-
-#ifndef min
-int min(int a, int b)
-{
-    if (a < b)
-        return a;
-    return b;
-}
-
-#endif
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/RFM69HCW.h b/Spaceport/23-24/Code/Teensy-Based Avionics/src/RFM69HCW.h
deleted file mode 100644
index d00a67cc..00000000
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/src/RFM69HCW.h	
+++ /dev/null
@@ -1,82 +0,0 @@
-#ifndef RFM69HCW_H
-#define RFM69HCW_H
-
-#if defined(ARDUINO)
-#define MSG_LEN 200
-#elif defined(TEENSYDUINO)
-#define MSG_LEN 10 * 1024
-#elif defined(RASPBERRY_PI)
-#define MSG_LEN 10 * 1024
-#endif
-
-#include "Radio.h"
-#include "APRSMsg.h"
-#include "RH_RF69.h"
-#include "APRSEncodeFunctions.h"
-
-/*
-Settings:
-- double frequency in Mhz
-- bool transmitter
-- bool highBitrate
-- RHGenericSPI *spi pointer to radiohead SPI object
-- uint8_t cs CS pin
-- uint8_t irq IRQ pin
-- uint8_t rst RST pin
-*/
-struct RadioSettings
-{
-    double frequency;
-    bool transmitter;
-    bool highBitrate;
-    RHGenericSPI *spi;
-    uint8_t cs;
-    uint8_t irq;
-    uint8_t rst;
-};
-
-class RFM69HCW : public Radio
-{
-public:
-    RFM69HCW(const RadioSettings s, const APRSConfig config);
-    bool begin() override;
-    bool tx(const char *message, int len) override;
-    bool sendBuffer();
-    void endtx();
-    const char *rx() override;
-    bool encode(char *message, EncodingType type) override;
-    bool decode(char *message, EncodingType type) override;
-    bool send(const char *message, EncodingType type) override;
-    const char *receive(EncodingType type) override;
-    int RSSI() override;
-    bool available();
-    void set300KBPS();
-    RHGenericDriver::RHMode mode();
-
-    // stores full messages, max length determined by platform
-    char msg[MSG_LEN + 1];
-    // length of msg for recieving binary messages
-    int msgLen = 0;
-
-private:
-    RH_RF69 radio;
-    // all radios should have the same networkID
-    const uint8_t networkID = 0x01;
-    // default to the highest transmit power
-    const int txPower = 20;
-    // set by constructor
-    uint8_t addr;
-    uint8_t toAddr;
-    uint8_t id;
-    RadioSettings settings;
-    // for sending/receiving data
-    // stores messages sent to radio, length determined by max radio message length
-    uint8_t buf[RH_RF69_MAX_MESSAGE_LEN + 1];
-    uint8_t bufSize = RH_RF69_MAX_MESSAGE_LEN;
-    APRSConfig cfg;
-    bool avail;
-    int rssi;
-    int totalPackets;
-};
-
-#endif // RFM69HCW_H
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/APRS/APRSCmdMsg.cpp b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/APRS/APRSCmdMsg.cpp
new file mode 100644
index 00000000..b8f87642
--- /dev/null
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/APRS/APRSCmdMsg.cpp	
@@ -0,0 +1,31 @@
+#include "APRSCmdMsg.h"
+
+APRSCmdMsg::APRSCmdMsg(APRSHeader header) : APRSMsgBase(header)
+{
+    data.MinutesUntilPowerOn = -1;
+    data.MinutesUntilVideoStart = -1;
+    data.MinutesUntilDataRecording = -1; // -1 so that we know if the value was set or not
+    data.Launch = false;
+}
+
+void APRSCmdMsg::encodeData(int cursor)
+{
+    snprintf((char *)&string[cursor], RADIO_MESSAGE_BUFFER_SIZE - cursor,
+             ":%-9s:PWR:%d VID:%d DAT:%d L:%d",
+             header.CALLSIGN,
+             data.MinutesUntilPowerOn,
+             data.MinutesUntilVideoStart,
+             data.MinutesUntilDataRecording,
+             data.Launch);
+    len = strlen((char *)string);
+}
+
+void APRSCmdMsg::decodeData(int cursor)
+{
+    sscanf((char *)&string[cursor + 11], // ignore the :TO_FIELD: part
+           "PWR:%d VID:%d DAT:%d L:%d",
+           &data.MinutesUntilPowerOn,
+           &data.MinutesUntilVideoStart,
+           &data.MinutesUntilDataRecording,
+           &data.Launch);
+}
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/APRS/APRSCmdMsg.h b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/APRS/APRSCmdMsg.h
new file mode 100644
index 00000000..0aacf074
--- /dev/null
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/APRS/APRSCmdMsg.h	
@@ -0,0 +1,25 @@
+#ifndef APRS_CMD_MSG_H
+#define APRS_CMD_MSG_H
+
+#include "APRSMsgBase.h"
+
+struct APRSCmdData final
+{
+    int MinutesUntilPowerOn;       // Minutes until the Pi turns on - 0 means turn on now
+    int MinutesUntilVideoStart;    // Minutes until the Pi starts recording video - 0 means start now
+    int MinutesUntilDataRecording; // Minutes until the FC starts recording data - 0 means start now
+    bool Launch;                   // Send the launch command to the FC - 0 means don't send
+};
+
+class APRSCmdMsg final : public APRSMsgBase 
+{
+public:
+    APRSCmdMsg(APRSHeader header);
+    APRSCmdData data;
+
+protected:
+    void decodeData(int cursor) override;
+    void encodeData(int cursor) override;
+} ;
+
+#endif // APRS_CMD_MSG_H
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/APRS/APRSMsgBase.cpp b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/APRS/APRSMsgBase.cpp
new file mode 100644
index 00000000..e5cfa9a2
--- /dev/null
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/APRS/APRSMsgBase.cpp	
@@ -0,0 +1,96 @@
+#include "APRSMsgBase.h"
+
+
+APRSMsgBase::APRSMsgBase(APRSHeader header)
+{
+    this->header = header;
+}
+
+bool APRSMsgBase::encode()
+{
+    int c = encodeHeader();
+    encodeData(c);
+    return true;
+}
+
+bool APRSMsgBase::decode()
+{
+    // message needs to be decoded and values reinserted into the header and data structs
+    int c = decodeHeader();
+    decodeData(c);
+    return true;
+}
+
+#pragma region APRSMsgBase Header Encoding/Decoding
+
+int APRSMsgBase::encodeHeader()
+{
+    // format: CALLSIGN>TOCALL,PATH:
+    int cursor = 0;
+
+    for (int i = 0; header.CALLSIGN[i] && i < 8; i++, cursor++)
+        string[cursor] = header.CALLSIGN[i];
+
+    string[cursor++] = '>';
+
+    for (int i = 0; header.TOCALL[i] && i < 8; i++, cursor++)
+        string[cursor] = header.TOCALL[i];
+
+    string[cursor++] = ',';
+
+    for (int i = 0; header.PATH[i] && i < 10; i++, cursor++)
+        string[cursor] = header.PATH[i];
+
+    string[cursor++] = ':'; // end of header
+    return cursor;
+}
+
+int APRSMsgBase::decodeHeader()
+{
+    // format: CALLSIGN>TOCALL,PATH:
+    int cursor = 0;
+
+    for (int i = 0; string[cursor] != '>'; i++, cursor++)
+        header.CALLSIGN[i] = string[cursor];
+    header.CALLSIGN[cursor] = '\0';
+
+    cursor++; // skip '>'
+
+    for (int i = 0; string[cursor] != ','; i++, cursor++)
+        header.TOCALL[i] = string[cursor];
+    header.TOCALL[cursor] = '\0';
+
+    cursor++; // skip ','
+
+    for (int i = 0; string[cursor] != ':'; i++, cursor++)
+        header.PATH[i] = string[cursor];
+    header.PATH[cursor] = '\0';
+
+    cursor++; // skip ':'
+    return cursor;
+}
+
+#pragma endregion
+
+#pragma region Base91 Encoding
+
+void APRSMsgBase::encodeBase91(uint8_t *message, int &cursor, int value, int precision)
+{
+    for (int i = precision - 1; i >= 0; i--)
+    {
+        int divisor = pow(91, i);
+        message[cursor++] = (uint8_t)((int)(value / divisor) + 33);
+        value %= divisor;
+    }
+}
+
+void APRSMsgBase::decodeBase91(const uint8_t *message, int &cursor, double &value, int precision)
+{
+    value = 0;
+    for (int i = 0; i < precision; i++)
+    {
+        value += (message[cursor++] - 33) * pow(91, precision - i - 1);
+    }
+}
+
+#pragma endregion
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/APRS/APRSMsgBase.h b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/APRS/APRSMsgBase.h
new file mode 100644
index 00000000..a5e7d0f0
--- /dev/null
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/APRS/APRSMsgBase.h	
@@ -0,0 +1,47 @@
+#ifndef APRS_MSG_BASE_H
+#define APRS_MSG_BASE_H
+
+#if defined(ARDUINO)
+#include <Arduino.h>
+#elif defined(_WIN32) || defined(_WIN64) // Windows
+#include <cstdint>
+#include <string>
+#include <cstring>
+#elif defined(__unix__)  // Linux
+// TODO
+#elif defined(__APPLE__) // OSX
+// TODO
+#endif
+
+#include "../RadioMessage.h"
+
+struct APRSHeader final
+{
+    char CALLSIGN[8];
+    char TOCALL[8];
+    char PATH[10];
+    char SYMBOL;
+    char OVERLAY;
+};
+
+class APRSMsgBase : public RadioMessage
+{
+public:
+    APRSMsgBase(APRSHeader header);
+    virtual ~APRSMsgBase(){};
+
+    virtual bool decode() override;
+    virtual bool encode() override;
+    APRSHeader header;
+
+protected:
+    int encodeHeader();
+    int decodeHeader();
+    virtual void encodeData(int cursor) = 0;
+    virtual void decodeData(int cursor) = 0;
+    void encodeBase91(uint8_t *message, int &cursor, int value, int precision);
+    void decodeBase91(const uint8_t *message, int &cursor, double &value, int precision);
+
+};
+
+#endif // RADIO_H
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/APRS/APRSTelemMsg.cpp b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/APRS/APRSTelemMsg.cpp
new file mode 100644
index 00000000..6721a11e
--- /dev/null
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/APRS/APRSTelemMsg.cpp	
@@ -0,0 +1,90 @@
+#include "APRSTelemMsg.h"
+
+APRSTelemMsg::APRSTelemMsg(APRSHeader header) : APRSMsgBase(header)
+{
+    data.lat = 0;
+    data.lng = 0;
+    data.alt = 0;
+    data.spd = 0;
+    data.hdg = 0;
+    data.stage = 0;
+    data.orientation = imu::Vector<3>(0, 0, 0);
+    data.statusFlags = 0;
+}
+
+void APRSTelemMsg::encodeData(int cursor)
+{
+    /* Small Aircraft symbol ( /' ) probably better than Jogger symbol ( /[ ) lol.
+     I'm going to use the Overlayed Aircraft symbol ( \^ ) for now, with the overlay of 'M' for UMD ( M^ ).
+     Uses base91 encoding to compress message as much as possible. APRS messages are apparently supposed to be only 256 bytes.
+
+     Takes the regular format of !DDMM.hhN/DDDMM.hhW^ALTSPD/HDG SSOrientationF
+
+     which expands to !(lat)/(lng)^altspd/hdg (stage #)(orientation)(status flags)
+
+     and compresses it to /YYYYXXXX^ ccssaasoooooof
+
+     which expands to /(lat)(lng)^ (course)(speed)(altitude)(stage)(orientation [as euler angles])(status flags)
+
+     Specifically does not use the csT compressed format because we don't need T and want better accuracy on course angle.
+     for more information on APRS, see http://www.aprs.org/doc/APRS101.PDF
+
+
+     lat and lng are in decimal degrees. no need to convert to DMS.
+     course is in degrees, speed is in knots, altitude is in feet, stage is an integer, orientation is in degrees, Pi status is a byte of flags.
+    */
+
+    // lat and lng
+    string[cursor++] = '!';                                          // Message type (position without timestamp)
+    string[cursor++] = 'M';                                          // overlay
+    encodeBase91(string, cursor, (int)380926 * (90 - data.lat), 4);  // 380926 is the magic number from the APRS spec (page 38)
+    encodeBase91(string, cursor, (int)190463 * (180 + data.lng), 4); // 190463 is the magic number from the APRS spec (page 38)
+    string[cursor++] = '^';                                          // end of lat and lng
+    string[cursor++] = ' ';                                          // space to start 'Comment' section
+
+    // course, speed, altitude
+    encodeBase91(string, cursor, (int)(data.hdg * HDG_SCALE), 2);                // (91^2/360) scale to fit in 2 base91 characters
+    encodeBase91(string, cursor, (int)(data.spd * SPD_SCALE), 2);                // (91^2/1000) scale to fit in 2 base91 characters. 1000 knots is the assumed max speed.
+    encodeBase91(string, cursor, (int)((data.alt + ALT_OFFSET) * ALT_SCALE), 2); // (91^2/15000) scale to fit in 2 base91 characters. 15000 feet is the assumed max altitude.
+
+    // stage and orientation
+    string[cursor++] = (uint8_t)(data.stage + (int)'0');                              // stage is just written in plaintext.
+    encodeBase91(string, cursor, (int)(data.orientation.x() * ORIENTATION_SCALE), 2); // same as course
+    encodeBase91(string, cursor, (int)(data.orientation.y() * ORIENTATION_SCALE), 2); // same as course
+    encodeBase91(string, cursor, (int)(data.orientation.z() * ORIENTATION_SCALE), 2); // same as course
+    string[cursor++] = (uint8_t)(data.statusFlags + 33);                              // all content is supposed to be printable ASCII characters, so we add 33 to the status flags to make sure it is.
+    len = cursor;
+}
+
+void APRSTelemMsg::decodeData(int cursor)
+{
+    // format is /YYYYXXXX^ ccssaasoooooof
+
+    // lat and lng
+    cursor++; // skip '!'
+    cursor++; // skip overlay
+    decodeBase91(string, cursor, data.lat, 4);
+    data.lat = 90 - data.lat / 380926.0;
+    decodeBase91(string, cursor, data.lng, 4);
+    data.lng = data.lng / 190463.0 - 180;
+    cursor++; // skip '^'
+    cursor++; // skip ' '
+
+    // course, speed, altitude
+    decodeBase91(string, cursor, data.hdg, 2);
+    data.hdg /= HDG_SCALE;
+    decodeBase91(string, cursor, data.spd, 2);
+    data.spd /= SPD_SCALE;
+    decodeBase91(string, cursor, data.alt, 2);
+    data.alt = data.alt / ALT_SCALE - ALT_OFFSET;
+
+    // stage and orientation
+    data.stage = string[cursor++] - (int)'0';
+    decodeBase91(string, cursor, data.orientation.x(), 2);
+    data.orientation.x() /= ORIENTATION_SCALE;
+    decodeBase91(string, cursor, data.orientation.y(), 2);
+    data.orientation.y() /= ORIENTATION_SCALE;
+    decodeBase91(string, cursor, data.orientation.z(), 2);
+    data.orientation.z() /= ORIENTATION_SCALE;
+    data.statusFlags = string[cursor++] - 33;
+}
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/APRS/APRSTelemMsg.h b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/APRS/APRSTelemMsg.h
new file mode 100644
index 00000000..2558ee0e
--- /dev/null
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/APRS/APRSTelemMsg.h	
@@ -0,0 +1,42 @@
+#ifndef APRS_TELEM_MSG_H
+#define APRS_TELEM_MSG_H
+
+#include "APRSMsgBase.h"
+#include <imumaths.h>
+
+const uint8_t PI_ON = 0b00000001;          // Pi is on
+const uint8_t PI_VIDEO = 0b00000010;       // Pi is recording video
+const uint8_t RECORDING_DATA = 0b00000100; // FC is recording data
+
+struct APRSTelemData final
+{
+    double lat;
+    double lng;
+    double alt;
+    double spd;
+    double hdg;
+    int stage;
+    imu::Vector<3> orientation;
+    uint8_t statusFlags;
+};
+
+class APRSTelemMsg final : public APRSMsgBase
+{
+public:
+    APRSTelemMsg(APRSHeader header);
+    APRSTelemData data;
+
+private:
+    void decodeData(int cursor) override;
+    void encodeData(int cursor) override;
+
+    // Scale factors for encoding/decoding ignoring lat/long
+    const double ALT_SCALE = (pow(91, 2) / 16000.0);       // (91^2/16000) scale to fit in 2 base91 characters
+    const double SPD_SCALE = (pow(91, 2) / 1000.0);        // (91^2/1000) scale to fit in 2 base91 characters
+    const double HDG_SCALE = (pow(91, 2) / 360.0);         // (91^2/360) scale to fit in 2 base91 characters
+    const double ORIENTATION_SCALE = (pow(91, 2) / 360.0); // same as course
+
+    const int ALT_OFFSET = +1000; // range of -1000 to 15000 ft.
+};
+
+#endif // APRS_TELEM_MSG_H
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/RFM69HCW.cpp b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/RFM69HCW.cpp
new file mode 100644
index 00000000..885fb80e
--- /dev/null
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/RFM69HCW.cpp	
@@ -0,0 +1,370 @@
+#include "RFM69HCW.h"
+
+#ifndef max
+int max(int a, int b);
+#endif
+
+#ifndef min
+int min(int a, int b);
+#endif
+
+/*
+Constructor
+    - frequency in hertz
+    - transmitter true if the radio will be on the rocket
+    - highBitrate true for 300kbps, false for 4.8kbps
+    - config with APRS settings
+*/
+RFM69HCW::RFM69HCW(const RadioSettings *s) : radio(s->cs, s->irq, *s->spi)
+{
+    settings = *s;
+}
+
+/*
+Initializer to call in setup
+*/
+bool RFM69HCW::init()
+{
+    // reset the radio
+    pinMode(settings.rst, OUTPUT);
+    digitalWrite(settings.rst, LOW);
+    delay(10);
+    digitalWrite(settings.rst, HIGH);
+    delay(10);
+    digitalWrite(settings.rst, LOW);
+
+    if (!radio.init())
+        return false;
+
+    // then use this to actually set the frequency
+    if (!radio.setFrequency(settings.frequency))
+        return false;
+
+    // set transmit power
+    radio.setTxPower(settings.txPower, true);
+
+    // always set FSK mode
+    radio.setModemConfig(RH_RF69::FSK_Rb4_8Fd9_6);
+
+    // set headers
+    radio.setHeaderTo(settings.toAddr);
+    radio.setHeaderFrom(settings.thisAddr);
+    radio.setThisAddress(settings.thisAddr);
+    radio.setHeaderId(0);
+
+    // configure unlimited packet length mode, don't do this for now
+    // this->radio.spiWrite(0x37, 0b00000000);                // Packet format (0x37) set to 00000000 (see manual for meaning of each bit)
+    // this->radio.spiWrite(RH_RF69_REG_38_PAYLOADLENGTH, 0); // Payload length (0x38) set to 0
+
+    // if (this->settings.highBitrate) // Note: not working
+    // {
+    //     // the default bitrate of 4.8kbps should be fine unless we want high bitrate for video
+    //     // set300KBPS();
+    //     this->radio.setModemConfig(RH_RF69::FSK_Rb4_8Fd9_6);
+    //     // remove as much overhead as possible
+    //     // this->radio.setPreambleLength(0);
+    //     // this->radio.setSyncWords();
+    // }
+
+    return initialized = true;
+}
+
+/*
+Most basic transmission method, simply transmits the string without modification
+    \param message is the message to be transmitted, must be shorter than RH_RF69_MAX_MESSAGE_LEN
+    \param len optional length of message, required if message is not a null terminated string
+    \param packetNum optional packet number of the message, given to the receiver to reassemble the message
+    \param lastPacket optional is this the last packet of a larger message or the only packet?
+    \return `true` if the message was sent, `false` otherwise
+*/
+bool RFM69HCW::tx(const uint8_t *message, int len, int packetNum, bool lastPacket)
+{
+    // figure out how long the message should be
+    if (len == -1)
+        len = strlen((char *)message);
+
+    radio.setHeaderId(packetNum);
+    radio.setHeaderFlags(lastPacket ? 0 : RADIO_FLAG_MORE_DATA);
+
+    return radio.send(message, len);
+}
+
+/*
+Receive function -
+Most basic receiving method, simply checks for a message and returns it
+Note that the message may be incomplete, if the message is complete update() will return true
+The received message will be truncated if it exceeds the maximum message length of the radio. Also updates the RSSI.
+    \param recvbuf the buffer to store the received message
+    \param len the length of the received message (updated by the function)
+    \return `true` if a message was received, `false` otherwise
+*/
+bool RFM69HCW::rx(uint8_t *recvbuf, uint8_t *len)
+{
+    if (!radio.recv(recvbuf, len))
+        return false;
+
+    rssi = radio.lastRssi();
+    return true;
+}
+
+#pragma region External Send and Receive
+
+/*
+Enqueue a message into the buffer
+    \param message the uint8_t[] to enqueue
+    \param len the length of the message
+    \return `true` if the message was enqueued, `false` otherwise
+*/
+bool RFM69HCW::enqueueSend(const uint8_t *message, uint8_t len)
+{
+    // determine if the message is too long to fit without the tail running into the head
+    if (sendBuffer.tail >= sendBuffer.head)
+    {
+        if (TRANSCEIVER_MESSAGE_BUFFER_SIZE - sendBuffer.tail + sendBuffer.head < len + 1) // +1 for the length byte
+            return false;
+    }
+    else if (sendBuffer.head - sendBuffer.tail < len + 1) // +1 for the length byte
+        return false;
+
+    // fill up the buffer with the message. buffer is a circular queue.
+    sendBuffer.data[sendBuffer.tail] = len; // store the length of the message in the first byte
+    inc(sendBuffer.tail);
+
+    copyToBuffer(sendBuffer, message, len);
+    return true;
+}
+
+/*
+Enqueue a message into the buffer
+    \param message the null terminated string to enqueue
+    \return `true` if the message was enqueued, `false` otherwise
+*/
+bool RFM69HCW::enqueueSend(const char *message)
+{
+    return enqueueSend((uint8_t *)message, strlen(message));
+}
+
+/*
+Encode the message and enqueue it in the buffer
+    \param message the RadioMessage to encode and enqueue
+    \return `true` if the message was encoded and enqueued, `false` otherwise
+*/
+bool RFM69HCW::enqueueSend(RadioMessage *message)
+{
+    if (message->encode())
+        return enqueueSend(message->getArr(), message->length());
+
+    return false;
+}
+
+/*
+Dequeue a message from the buffer and decode it into a uint8_t[].
+    \param message the uint8_t[] to copy the recieved message into
+    \return `true` if a message was copied, `false` otherwise
+*/
+bool RFM69HCW::dequeueReceive(uint8_t *message)
+{
+    if (recvBuffer.head == recvBuffer.tail) // buffer is empty
+        return false;
+
+    // get the length of the message
+    int len = recvBuffer.data[recvBuffer.head];
+    inc(recvBuffer.head);
+    // Empty the buffer up to the length of the expected message
+    copyFromBuffer(recvBuffer, message, len);
+    return true;
+}
+
+/*
+Dequeue a message from the buffer and decode it into a char[]. WIll break if the recieved message is not a null terminated string.
+    \param message the char[] to copy the recieved message into
+    \return `true` if a message was copied, `false` otherwise
+*/
+bool RFM69HCW::dequeueReceive(char *message)
+{
+    if (recvBuffer.head == recvBuffer.tail) // buffer is empty
+        return false;
+
+    // Empty the buffer up to the first null terminator
+    inc(recvBuffer.head); // skip the length byte, should be null terminated
+    int i = 0;
+    for (; recvBuffer.data[recvBuffer.head] != '\0' && recvBuffer.head != recvBuffer.tail - 1; i++) // same algo as copyFromBuffer but uses a null terminator
+    {
+        message[i] = recvBuffer.data[recvBuffer.head];
+        inc(recvBuffer.head);
+    }
+    message[i] = '\0';
+
+    return true;
+}
+
+/*
+Dequeue a message from the buffer and decode it into a RadioMessage
+    \param message the RadioMessage to give the encoded text to for decoding
+    \return `true` if a message was decoded, `false` if no message was decoded
+*/
+bool RFM69HCW::dequeueReceive(RadioMessage *message)
+{
+    if (recvBuffer.head == recvBuffer.tail) // buffer is empty
+        return false;
+
+    uint8_t len = recvBuffer.data[recvBuffer.head];
+    uint8_t *msg = new uint8_t[len];
+
+    bool worked = false;
+    if (dequeueReceive(msg))           // get the message from the buffer
+        if (message->setArr(msg, len)) // send the message to the RadioMessage
+            if (message->decode())     // decode the message
+                worked = true;
+
+    delete[] msg;
+    return worked;
+}
+
+#pragma endregion
+
+/*
+Update function
+    - checks if the radio is busy
+    - if the radio has received a message, it will store the message in the buffer, Will not transmit while a full message has been only partially received.
+    - if not receiving and the radio has a message to send, it will send a part of the message up to the max length the radio can handle.
+    \return `true` if the radio has received a full message, `false` otherwise (always false for transmiting)
+*/
+bool RFM69HCW::update()
+{
+    if (!initialized)
+        return false;
+    if (busy())
+        return false;
+
+    uint8_t rcvLen = RH_RF69_MAX_MESSAGE_LEN;
+    uint8_t rcvBuf[rcvLen];
+
+    if (rx(rcvBuf, &rcvLen))
+    {
+        // start of a new message
+        if (radio.headerId() == 0)
+        {
+            orignalBufferTail = recvBuffer.tail;
+            recvBuffer.data[recvBuffer.tail] = rcvLen; // store the length of the message
+            inc(recvBuffer.tail);
+        }
+        // continuing message
+        else
+            recvBuffer.data[orignalBufferTail] += rcvLen; // update the total length of the message
+
+        // store the message in the buffer
+        copyToBuffer(recvBuffer, rcvBuf, rcvLen);
+
+        // more data is coming, should not process yet. TODO: Add a timeout to prevent infinite loop
+        if (radio.headerFlags() & RADIO_FLAG_MORE_DATA)
+            return false;
+
+        return true;
+    }
+    // transmit
+    if (sendBuffer.head == sendBuffer.tail) // buffer is empty, nothing to send
+    {
+        remainingLength = 0;
+        return false;
+    }
+
+    int packetNum;
+
+    // start a new message
+    if (remainingLength == 0)
+    {
+        remainingLength = sendBuffer.data[sendBuffer.head]; // get the length of the message from the first byte of the message
+        inc(sendBuffer.head);
+        packetNum = 0;
+    }
+    // continue the message
+    else
+        packetNum = radio.headerId() + 1;
+
+    // load message to tx
+    int len = min(remainingLength, RH_RF69_MAX_MESSAGE_LEN); // how much of the message to send
+    uint8_t msgToTransmit[RH_RF69_MAX_MESSAGE_LEN];
+    copyFromBuffer(sendBuffer, msgToTransmit, len);
+    tx(msgToTransmit, len, packetNum, remainingLength <= RH_RF69_MAX_MESSAGE_LEN);
+    remainingLength -= len;
+    return false; // always false for transmitter
+}
+
+#pragma region Helpers
+
+/*
+\return `true` if the radio is currently transmitting a message
+*/
+bool RFM69HCW::busy()
+{
+    RHGenericDriver::RHMode mode = this->radio.mode();
+    if (mode == RHGenericDriver::RHModeTx)
+        return true;
+    return false;
+}
+/*
+Returns the RSSI of the last message
+*/
+int RFM69HCW::RSSI()
+{
+    return rssi;
+}
+
+void RFM69HCW::copyToBuffer(buffer &buffer, const uint8_t *src, int len)
+{
+    for (int i = 0; i < len; i++)
+    {
+        buffer.data[buffer.tail] = src[i];
+        inc(buffer.tail);
+    }
+}
+
+// pop is true by default. If pop is false, the buffer head will not be moved.
+void RFM69HCW::copyFromBuffer(buffer &buffer, uint8_t *dest, int len, bool pop)
+{
+    int originalHead = buffer.head;
+    for (int i = 0; i < len; i++)
+    {
+        dest[i] = buffer.data[buffer.head];
+        inc(buffer.head);
+    }
+    
+    if (!pop)
+        buffer.head = originalHead;
+}
+
+// probably broken
+// void RFM69HCW::set300KBPSMode()
+// {
+//     this->radio.spiWrite(0x03, 0x00);       // REG_BITRATEMSB: 300kbps (0x006B, see DS p20)
+//     this->radio.spiWrite(0x04, 0x6B);       // REG_BITRATELSB: 300kbps (0x006B, see DS p20)
+//     this->radio.spiWrite(0x19, 0x40);       // REG_RXBW: 500kHz
+//     this->radio.spiWrite(0x1A, 0x80);       // REG_AFCBW: 500kHz
+//     this->radio.spiWrite(0x05, 0x13);       // REG_FDEVMSB: 300khz (0x1333)
+//     this->radio.spiWrite(0x06, 0x33);       // REG_FDEVLSB: 300khz (0x1333)
+//     this->radio.spiWrite(0x29, 240);        // set REG_RSSITHRESH to -120dBm
+//     this->radio.spiWrite(0x37, 0b10010000); // DC=WHITENING, CRCAUTOOFF=0
+//                                             //                ^^->DC: 00=none, 01=manchester, 10=whitening
+// }
+
+// utility functions
+#ifndef max
+int max(int a, int b)
+{
+    if (a > b)
+        return a;
+    return b;
+}
+#endif
+
+#ifndef min
+int min(int a, int b)
+{
+    if (a < b)
+        return a;
+    return b;
+}
+#endif
+
+#pragma endregion
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/RFM69HCW.h b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/RFM69HCW.h
new file mode 100644
index 00000000..d55a2125
--- /dev/null
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/RFM69HCW.h	
@@ -0,0 +1,68 @@
+#ifndef RFM69HCW_H
+#define RFM69HCW_H
+
+#include "RH_RF69.h"
+#include "Radio.h"
+
+const int TRANSCEIVER_MESSAGE_BUFFER_SIZE = 512; // length of the buffer underpinning the queue of messages to send or receive. Radio makes 2 buffers this size for sending and receiving.
+#define RADIO_FLAG_MORE_DATA 0b00000001          // custom flag to indicate more packets are still to be sent
+
+#ifndef RH_RF69_MAX_MESSAGE_LEN
+#define RH_RF69_MAX_MESSAGE_LEN 66
+#endif // !RH_RF69_MAX_MESSAGE_LEN
+
+struct buffer final
+{
+    uint8_t data[TRANSCEIVER_MESSAGE_BUFFER_SIZE];
+    int head;
+    int tail;
+};
+
+class RFM69HCW final : public Radio
+{
+public:
+    RFM69HCW(const RadioSettings *s);
+
+    bool init() override;
+
+    bool tx(const uint8_t *message, int len = -1, int packetNum = 0, bool lastPacket = true) override; // designed to be used internally. cannot exceed 66 bytes including headers
+    bool rx(uint8_t *recvbuf, uint8_t *len) override;
+
+    bool enqueueSend(RadioMessage *message) override;               // designed to be used externally. can exceed 66 bytes.
+    bool enqueueSend(const char *message) override;                 // designed to be used externally. can exceed 66 bytes.
+    bool enqueueSend(const uint8_t *message, uint8_t len) override; // designed to be used externally. can exceed 66 bytes.
+
+    bool dequeueReceive(RadioMessage *message) override; // designed to be used externally. can exceed 66 bytes.
+    bool dequeueReceive(char *message) override;         // designed to be used externally. can exceed 66 bytes.
+    bool dequeueReceive(uint8_t *message) override;      // designed to be used externally. can exceed 66 bytes.
+
+    int RSSI() override;
+    bool update() override;
+    bool busy();
+
+
+    // increment the specified index, wrapping around if necessary
+    static void inc(int &i) { i = (i + 1) % TRANSCEIVER_MESSAGE_BUFFER_SIZE; }
+
+    // public during debugging
+    // stores queue of messages, with the length of the message as the first byte of each. no delimiter.
+    // [6xxxxxx3xxx1x2xx]
+    // Done to allow for messages that are not ascii encoded.
+    buffer sendBuffer;
+    buffer recvBuffer;
+    RH_RF69 radio;
+
+private:
+    // all radios should have the same networkID
+    const uint8_t networkID = 1;
+
+    int rssi;
+
+    int remainingLength = 0;   // how much message is left to send for transmitters
+    int orignalBufferTail = 0; // used to update the length of the message after recieving.
+
+    void copyToBuffer(buffer &buffer, const uint8_t *src, int len);
+    void copyFromBuffer(buffer &buffer, uint8_t *dest, int len, bool pop = true);
+};
+
+#endif // RFM69HCW_H
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/RFM69Helper.h b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/RFM69Helper.h
similarity index 94%
rename from Spaceport/23-24/Code/Teensy-Based Avionics/src/RFM69Helper.h
rename to Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/RFM69Helper.h
index 486d1066..3e1a8463 100644
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/src/RFM69Helper.h	
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/RFM69Helper.h	
@@ -1,171 +1,171 @@
-#ifndef RFM69HELPER_H
-#define RFM69HELPER_H
-
-#if defined(ARDUINO)
-#include <Arduino.h>
-#elif defined(_WIN32) || defined(_WIN64) // Windows
-
-#elif defined(__unix__)  // Linux
-// TODO
-#elif defined(__APPLE__) // OSX
-// TODO
-#endif
-
-#ifndef ARDUINO
-#include <iostream>
-#include <bitset>
-#include <chrono>
-#include <cstdint>
-#include <string>
-#include <cstring>
-
-#define SS 0
-
-static uint64_t startTime = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
-
-static uint32_t millis()
-{
-    return std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count() - startTime;
-};
-
-enum SPIDataMode
-{
-    SPI_MODE0
-};
-
-enum SPIBitOrder
-{
-    MSBFIRST,
-    LSBFIRST
-};
-
-enum SPIClockDivider
-{
-    SPI_CLOCK_DIV2,
-    SPI_CLOCK_DIV8,
-    SPI_CLOCK_DIV16
-};
-
-class SPISettings
-{
-public:
-    SPISettings(){};
-    SPISettings(uint32_t clock, uint8_t bitOrder, uint8_t dataMode)
-    {
-        this->clock = clock;
-        this->bitOrder = bitOrder;
-        this->dataMode = dataMode;
-    }
-    uint32_t clock = 4000000U;
-    uint8_t bitOrder = MSBFIRST;
-    uint8_t dataMode = SPI_MODE0;
-};
-
-// class SPIClass
-// {
-// public:
-//     SPIClass();
-//     void begin();
-
-//     // with transaction
-//     void beginTransaction(SPISettings settings);
-//     void endTransaction();
-
-//     // non transaction
-//     void setDataMode(SPIDataMode dm);
-//     void setBitOrder(SPIBitOrder bo);
-//     void setClockDivider(SPIClockDivider cd);
-//     uint8_t transfer(uint8_t data);
-// };
-
-// SPIClass SPI;
-
-enum PinState : bool
-{
-    LOW,
-    HIGH
-};
-
-enum PinMode : bool
-{
-    INPUT,
-    OUTPUT
-};
-
-enum InterruptTrigger : bool
-{
-    FALLING,
-    RISING
-};
-
-// void digitalWrite(int pin, PinState state);
-// void pinMode(int pin, PinMode mode);
-// void attachInterrupt(uint8_t pin, void (*function)(), InterruptTrigger mode);
-// void detachInterrupt(uint8_t pin);
-
-enum SerialPrintMode
-{
-    HEX,
-    BIN
-};
-
-class HardwareSerial
-{
-public:
-    HardwareSerial(){};
-    void print(const char *s) { std::cout << s; };
-    void print(uint8_t num, SerialPrintMode m)
-    {
-        std::ios_base::fmtflags f(std::cout.flags());
-        if (m == HEX)
-        {
-            std::cout << std::hex << num;
-            std::cout.flags(f);
-        }
-        if (m == BIN)
-            std::cout << std::bitset<sizeof(num)>(num);
-    };
-    void println() { std::cout << std::endl; };
-    void println(const char *s) { std::cout << s << std::endl; };
-    void println(uint8_t num, SerialPrintMode m)
-    {
-        print(num, m);
-        std::cout << std::endl;
-    };
-};
-
-static HardwareSerial Serial = HardwareSerial();
-
-#endif
-
-// add to RFM69.h
-/*
-#elif defined(_WIN32) || defined(_WIN64) // Windows
-#define RF69_PLATFORM RF69_PLATFORM_WINDOWS
-#else
-*/
-
-// TODO define the following
-/*
-digitalWrite
-pinMode
-SPIClass and necessary functions
-SPI
-HIGH
-LOW
-OUTPUT
-millis
-attachInterrupt
-RISING
-
-SPI_MODE0
-MSBFIRST    or SPI transaction
-
-SPI_CLOCK_DIV2
-detachInterrupt
-Serial print and println
-    HEX
-    BIN
-byte
-*/
+#ifndef RFM69HELPER_H
+#define RFM69HELPER_H
+
+#if defined(ARDUINO)
+#include <Arduino.h>
+#elif defined(_WIN32) || defined(_WIN64) // Windows
+
+#elif defined(__unix__)  // Linux
+// TODO
+#elif defined(__APPLE__) // OSX
+// TODO
+#endif
+
+#ifndef ARDUINO
+#include <iostream>
+#include <bitset>
+#include <chrono>
+#include <cstdint>
+#include <string>
+#include <cstring>
+
+#define SS 0
+
+static uint64_t startTime = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count();
+
+static uint32_t millis()
+{
+    return std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now().time_since_epoch()).count() - startTime;
+};
+
+enum SPIDataMode
+{
+    SPI_MODE0
+};
+
+enum SPIBitOrder
+{
+    MSBFIRST,
+    LSBFIRST
+};
+
+enum SPIClockDivider
+{
+    SPI_CLOCK_DIV2,
+    SPI_CLOCK_DIV8,
+    SPI_CLOCK_DIV16
+};
+
+class SPISettings
+{
+public:
+    SPISettings(){};
+    SPISettings(uint32_t clock, uint8_t bitOrder, uint8_t dataMode)
+    {
+        this->clock = clock;
+        this->bitOrder = bitOrder;
+        this->dataMode = dataMode;
+    }
+    uint32_t clock = 4000000U;
+    uint8_t bitOrder = MSBFIRST;
+    uint8_t dataMode = SPI_MODE0;
+};
+
+// class SPIClass
+// {
+// public:
+//     SPIClass();
+//     void begin();
+
+//     // with transaction
+//     void beginTransaction(SPISettings settings);
+//     void endTransaction();
+
+//     // non transaction
+//     void setDataMode(SPIDataMode dm);
+//     void setBitOrder(SPIBitOrder bo);
+//     void setClockDivider(SPIClockDivider cd);
+//     uint8_t transfer(uint8_t data);
+// };
+
+// SPIClass SPI;
+
+enum PinState : bool
+{
+    LOW,
+    HIGH
+};
+
+enum PinMode : bool
+{
+    INPUT,
+    OUTPUT
+};
+
+enum InterruptTrigger : bool
+{
+    FALLING,
+    RISING
+};
+
+// void digitalWrite(int pin, PinState state);
+// void pinMode(int pin, PinMode mode);
+// void attachInterrupt(uint8_t pin, void (*function)(), InterruptTrigger mode);
+// void detachInterrupt(uint8_t pin);
+
+enum SerialPrintMode
+{
+    HEX,
+    BIN
+};
+
+class HardwareSerial
+{
+public:
+    HardwareSerial(){};
+    void print(const char *s) { std::cout << s; };
+    void print(uint8_t num, SerialPrintMode m)
+    {
+        std::ios_base::fmtflags f(std::cout.flags());
+        if (m == HEX)
+        {
+            std::cout << std::hex << num;
+            std::cout.flags(f);
+        }
+        if (m == BIN)
+            std::cout << std::bitset<sizeof(num)>(num);
+    };
+    void println() { std::cout << std::endl; };
+    void println(const char *s) { std::cout << s << std::endl; };
+    void println(uint8_t num, SerialPrintMode m)
+    {
+        print(num, m);
+        std::cout << std::endl;
+    };
+};
+
+static HardwareSerial Serial = HardwareSerial();
+
+#endif
+
+// add to RFM69.h
+/*
+#elif defined(_WIN32) || defined(_WIN64) // Windows
+#define RF69_PLATFORM RF69_PLATFORM_WINDOWS
+#else
+*/
+
+// TODO define the following
+/*
+digitalWrite
+pinMode
+SPIClass and necessary functions
+SPI
+HIGH
+LOW
+OUTPUT
+millis
+attachInterrupt
+RISING
+
+SPI_MODE0
+MSBFIRST    or SPI transaction
+
+SPI_CLOCK_DIV2
+detachInterrupt
+Serial print and println
+    HEX
+    BIN
+byte
+*/
 #endif
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/Radio.h b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/Radio.h
new file mode 100644
index 00000000..b7423f8f
--- /dev/null
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/Radio.h	
@@ -0,0 +1,58 @@
+#ifndef RADIO_H
+#define RADIO_H
+
+#if defined(ARDUINO)
+#include <Arduino.h>
+#elif defined(_WIN32) || defined(_WIN64) // Windows
+#include <cstdint>
+#include <string>
+#include <cstring>
+#elif defined(__unix__)  // Linux
+// TODO
+#elif defined(__APPLE__) // OSX
+// TODO
+#endif
+
+#include "RadioMessage.h"
+#include "RHGenericSPI.h" //PIO doesn't like RadioHead without this
+
+struct RadioSettings final
+{
+    double frequency; // in Mhz
+    uint8_t thisAddr; // RadioHead address of this device
+    uint8_t toAddr;  // RadioHead address of the device to send to
+    RHGenericSPI *spi; // RadioHead SPI object
+    uint8_t cs; // Chip Select pin
+    uint8_t irq; // Interrupt Request pin
+    uint8_t rst; // Reset pin
+    int txPower = 12; // in dBm
+};
+
+class Radio
+{
+public:
+    virtual ~Radio(){};
+    virtual bool init() = 0;
+
+    virtual bool tx(const uint8_t *message, int len = -1, int packetNum = 0, bool lastPacket = true) = 0; // designed to be used internally. cannot exceed 66 bytes including headers
+    virtual bool rx(uint8_t *recvbuf, uint8_t *len) = 0;
+
+    virtual int RSSI() = 0;
+
+    virtual bool enqueueSend(const uint8_t *message, uint8_t len) = 0;
+    virtual bool enqueueSend(const char *message) = 0;
+    virtual bool enqueueSend(RadioMessage *message) = 0;
+
+    virtual bool dequeueReceive(RadioMessage *message) = 0;
+    virtual bool dequeueReceive(char *message) = 0;
+    virtual bool dequeueReceive(uint8_t *message) = 0;
+
+    virtual explicit operator bool() const { return initialized; }
+    virtual bool update() = 0; // should be called in the main loop
+
+protected:
+    RadioSettings settings;
+    bool initialized = false;
+};
+
+#endif // RADIO_H
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/RadioHandler.h b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/RadioHandler.h
new file mode 100644
index 00000000..2fbd0739
--- /dev/null
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/RadioHandler.h	
@@ -0,0 +1,80 @@
+#include "RFM69HCW.h"
+#include "APRS/APRSCmdMsg.h"
+#include "APRS/APRSTelemMsg.h"
+
+#include <BlinkBuzz.h>
+#include <RecordData.h>
+
+#include "../Pi.h"
+#include "../State.h"
+
+namespace radioHandler
+{
+    void processCmdData(APRSCmdMsg &cmd, APRSCmdData &old, APRSCmdData &currentCmdData, int time)
+    {
+        bb.aonoff(BUZZER, 100, 2);
+        char log[100];
+        if (cmd.data.Launch > 0)
+        {
+            snprintf(log, 100, "Launch Command Changed to %d.", cmd.data.Launch);
+            recordLogData(INFO, log);
+            currentCmdData.Launch = cmd.data.Launch;
+        }
+        if ( cmd.data.MinutesUntilPowerOn >= 0)
+        {
+            snprintf(log, 100, "Power On Time Changed: %d with %d minutes remaining.", cmd.data.MinutesUntilPowerOn, (int)(currentCmdData.MinutesUntilPowerOn - time) / 60000);
+            recordLogData(INFO, log);
+            currentCmdData.MinutesUntilPowerOn = cmd.data.MinutesUntilPowerOn * 60 * 1000 + time;
+        }
+        if (cmd.data.MinutesUntilVideoStart >= 0)
+        {
+            snprintf(log, 100, "Video Start Time Changed: %d with %d minutes remaining.", cmd.data.MinutesUntilVideoStart, (int)(currentCmdData.MinutesUntilVideoStart - time) / 60000);
+            recordLogData(INFO, log);
+            currentCmdData.MinutesUntilVideoStart = cmd.data.MinutesUntilVideoStart * 60 * 1000 + time;
+        }
+        if (cmd.data.MinutesUntilDataRecording >= 0)
+        {
+            snprintf(log, 100, "Data Recording Time Changed: %d with %d minutes remaining.", cmd.data.MinutesUntilDataRecording, (int)(currentCmdData.MinutesUntilDataRecording - time) / 60000);
+            recordLogData(INFO, log);
+            currentCmdData.MinutesUntilDataRecording = cmd.data.MinutesUntilDataRecording * 60 * 1000 + time;
+        }
+        // if(currentCmdData.Reset)
+        // {
+        //     recordLogData(INFO, "Reset Command Received. Restarting Flight Computer.");
+        //     currentCmdData.Reset = true;
+        // }
+    }
+
+    void processCurrentCmdData(APRSCmdData &currentCmdData, State &computer, Pi &rpi, int time)
+    {
+        // if(currentCmdData.Reset)
+        // {
+        //     computer.init();
+        //     computer.launch();
+        //     rpi.setOn(true);
+        //     rpi.setRecording(true);
+        //     computer.setRecordOwnFlightData(true);
+        //     currentCmdData.Reset = false;
+        // }
+        if (currentCmdData.Launch && computer.getStageNum() == 0)
+        {
+            recordLogData(INFO, "Launch Command Received. Launching Rocket.");
+            computer.launch();
+        }
+        if (time > currentCmdData.MinutesUntilPowerOn && !rpi.isOn())
+        {
+            recordLogData(INFO, "Power On RPI Time Reached. Turning on Raspberry Pi.");
+            rpi.setOn(true);
+        }
+        if (time > currentCmdData.MinutesUntilVideoStart && !rpi.isRecording())
+        {
+            recordLogData(INFO, "Video Start Time Reached. Starting Video Recording.");
+            rpi.setRecording(true);
+        }
+        if (time > currentCmdData.MinutesUntilDataRecording && !computer.getRecordOwnFlightData())
+        {
+            recordLogData(INFO, "Data Recording Time Reached. Starting Data Recording.");
+            computer.setRecordOwnFlightData(true);
+        }
+    }
+}
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/RadioMessage.cpp b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/RadioMessage.cpp
new file mode 100644
index 00000000..b4669621
--- /dev/null
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/RadioMessage.cpp	
@@ -0,0 +1,41 @@
+#include "RadioMessage.h"
+
+
+RadioMessage::RadioMessage()
+{
+    this->len = 0;
+}
+
+/*
+Copies a source array to the encoded message buffer
+    \param srcArr: the array to copy
+    \param len: the length of the source array
+    \return `false` if the length of the source array is greater than the buffer size, `true` otherwise
+*/
+bool RadioMessage::setArr(const uint8_t *srcArr, int len)
+{
+    if (len > RADIO_MESSAGE_BUFFER_SIZE)
+        return false;
+
+    memcpy(this->string, srcArr, len);
+    this->len = len;
+    return true;
+}
+
+/*
+Copies the encoded message buffer to a destination array
+    \param destArr: the array to copy into
+    \param len: the length of the dest array
+    \return `false` if the length of the dest array is smaller than the buffer size, `true` otherwise
+*/
+uint8_t *RadioMessage::getArr()
+{
+    return string;
+}
+/*
+Returns the length of the encoded message
+*/
+int RadioMessage::length() const
+{
+    return len;
+}
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/RadioMessage.h b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/RadioMessage.h
new file mode 100644
index 00000000..f918c9db
--- /dev/null
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/Radio/RadioMessage.h	
@@ -0,0 +1,25 @@
+#ifndef RADIO_MESSAGE_H
+#define RADIO_MESSAGE_H
+
+#include <cstring>
+#include <cstdint>
+
+const int RADIO_MESSAGE_BUFFER_SIZE = 255; // maximum length of an encoded `RadioMessage` in bytes.
+
+class RadioMessage
+{
+public:
+    RadioMessage();
+    virtual bool encode() = 0;                           // use stored variables to encode a message
+    virtual bool decode() = 0;                           // use `string` to set stored variables
+    virtual int length() const;                          // return the length of the message
+    virtual bool setArr(const uint8_t *srcArr, int len); // set the string to the given array
+    virtual uint8_t *getArr();                           // get the string as an array
+    virtual ~RadioMessage(){};
+
+protected:
+    uint8_t string[RADIO_MESSAGE_BUFFER_SIZE]; // buffer for the encoded message
+    uint8_t len;                               // length of the encoded message. modified by encode()
+};
+
+#endif // RADIO_MESSAGE_H
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/State.cpp b/Spaceport/23-24/Code/Teensy-Based Avionics/src/State.cpp
index bd7977ad..a23dc960 100644
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/src/State.cpp	
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/State.cpp	
@@ -7,15 +7,12 @@ State::State(bool useKalmanFilter, bool stateRecordsOwnFlightData)
     lastTimeAbsolute = 0;
     timeAbsolute = millis();
     timePreviousStage = 0;
-    position.x() = 0;
-    position.y() = 0;
-    position.z() = 0;
-    velocity.x() = 0;
-    velocity.y() = 0;
-    velocity.z() = 0;
-    acceleration.x() = 0;
-    acceleration.y() = 0;
-    acceleration.z() = 0;
+    
+    position = imu::Vector<3>(0, 0, 0);
+    velocity = imu::Vector<3>(0, 0, 0);
+    acceleration = imu::Vector<3>(0, 0, 0);
+    orientation = imu::Quaternion(0, 0, 0, 1);
+
     apogee = 0;
     stageNumber = 0;
     baroOldAltitude = 0;
@@ -32,6 +29,13 @@ State::State(bool useKalmanFilter, bool stateRecordsOwnFlightData)
 
     numSensors = 0;
     recordOwnFlightData = stateRecordsOwnFlightData;
+
+    ///
+
+    // recordOwnFlightData = false; // For radio testing. Will not record own data unless told to do so or if it detects a launch.
+
+    ///
+
     for (int i = 0; i < NUM_MAX_SENSORS; i++)
         sensors[i] = nullptr;
 
@@ -68,42 +72,34 @@ State::~State()
 
 bool State::init()
 {
-    char *logData = new char[100];
+    char logData[100];
     int good = 0, tryNumSensors = 0;
     for (int i = 0; i < NUM_MAX_SENSORS; i++)
     {
-        if (sensors[i])
+        if (sensors[i]) // not nullptr
         {
             tryNumSensors++;
             if (sensors[i]->initialize())
             {
                 good++;
-                strcpy(logData, sensors[i]->getTypeString()); // This is a lot for just some logging...
-                strcat(logData, " [");
-                strcat(logData, sensors[i]->getName());
-                strcat(logData, "] initialized.");
+                snprintf(logData, 100, "%s [%s] initialized.", sensors[i]->getTypeString(), sensors[i]->getName());
                 recordLogData(INFO, logData);
             }
             else
             {
-                strcpy(logData, sensors[i]->getTypeString());
-                strcat(logData, " [");
-                strcat(logData, sensors[i]->getName());
-                strcat(logData, "] failed to initialize.");
+                snprintf(logData, 100, "%s [%s] failed to initialize.", sensors[i]->getTypeString(), sensors[i]->getName());
                 recordLogData(ERROR, logData);
             }
         }
         else
         {
-            strcpy(logData, "Sensor [");
-            strcat(logData, SENSOR_NAMES[i]);
-            strcat(logData, "] was not added via addSensor().");
+            snprintf(logData, 100, "Sensor [%s] was not added via addSensor().", SENSOR_NAMES[i]);
             recordLogData(ERROR, logData);
         }
     }
     if (radio)
     {
-        if (!radio->begin())
+        if (!radio->init())
             radio = nullptr;
     }
     if (useKF)
@@ -142,10 +138,10 @@ void State::updateState(double newTimeAbsolute)
         return;
 
     lastTimeAbsolute = timeAbsolute;
-    if (newTimeAbsolute != -1)
-        timeAbsolute = newTimeAbsolute;
-    else
+    if (newTimeAbsolute == -1)
         timeAbsolute = millis() / 1000.0;
+    else
+        timeAbsolute = newTimeAbsolute;
 
     updateSensors();
     if (kfilter && sensorOK(gps) && gps->getHasFirstFix() && stageNumber > 0)
@@ -186,7 +182,7 @@ void State::updateState(double newTimeAbsolute)
 
         if (sensorOK(baro))
         {
-            baroVelocity = (baro->getRelAltM() - baroOldAltitude) / (millis() / 1000.0 - timeAbsolute);
+            baroVelocity = (baro->getRelAltM() - baroOldAltitude) / (timeAbsolute - lastTimeAbsolute);
             baroOldAltitude = baro->getRelAltM();
         }
     }
@@ -200,9 +196,9 @@ void State::updateState(double newTimeAbsolute)
         }
         if (sensorOK(baro))
         {
-            velocity.z() = (baro->getRelAltM() - baroOldAltitude) / (millis() / 1000.0 - lastTimeAbsolute);
+            velocity.z() = (baro->getRelAltM() - baroOldAltitude) / (timeAbsolute - lastTimeAbsolute);
             position.z() = baro->getRelAltM();
-            baroVelocity = (baro->getRelAltM() - baroOldAltitude) / (millis() / 1000.0 - lastTimeAbsolute);
+            baroVelocity = (baro->getRelAltM() - baroOldAltitude) / (timeAbsolute - lastTimeAbsolute);
             baroOldAltitude = baro->getRelAltM();
         }
         if (sensorOK(imu))
@@ -212,26 +208,24 @@ void State::updateState(double newTimeAbsolute)
         }
     }
 
-    if (stageNumber == 0)
-        timeSinceLaunch = 0;
-    else
-        timeSinceLaunch = timeAbsolute - timeOfLaunch;
-
     determineAccelerationMagnitude();
     determineStage();
+
     if (stageNumber > 0)
+    {
         timeSincePreviousStage = timeAbsolute - timePreviousStage;
+        timeSinceLaunch = timeAbsolute - timeOfLaunch;
+    }
+
     if (stageNumber < 3)
         apogee = position.z();
 
     // backup case to dump data (25 minutes)
-    if (stageNumber > 0 && timeSinceLaunch > 1500 && stageNumber < 5)
+    if (stageNumber > 0 && timeSinceLaunch > 25 * 60 && stageNumber < 5)
     {
         stageNumber = 5;
         setRecordMode(GROUND);
-        digitalWrite(LED_BUILTIN, HIGH);
-        delay(500);
-        digitalWrite(LED_BUILTIN, LOW);
+        bb.aonoff(LED, 5000);
         recordLogData(WARNING, "Dumping data after 25 minutes.");
     }
     setDataString();
@@ -239,6 +233,8 @@ void State::updateState(double newTimeAbsolute)
         recordFlightData(dataString);
 }
 
+#pragma region CSV Operations
+
 void State::setCsvHeader()
 {
     char csvHeaderStart[] = "Time,Stage,PX,PY,PZ,VX,VY,VZ,AX,AY,AZ,";
@@ -278,9 +274,54 @@ char *State::getStateString()
 
 char *State::getDataString() { return dataString; }
 char *State::getCsvHeader() { return csvHeader; }
-int State::getStageNum() { return stageNumber; }
 
-#pragma region Getters and Setters for Sensors
+void State::setCsvString(char *dest, const char *start, int startSize, bool header)
+{
+    int numCategories = numSensors + 1;
+    const char **str = new const char *[numCategories];
+    str[0] = start;
+    int cursor = 1;
+    delete[] dest;
+    //---Determine required size for string
+    int size = startSize + 1; // includes '\0' at end of string for the end of dataString to use
+    for (int i = 0; i < NUM_MAX_SENSORS; i++)
+    {
+        if (sensorOK(sensors[i]))
+        {
+            str[cursor] = header ? sensors[i]->getCsvHeader() : sensors[i]->getDataString();
+            size += strlen(str[cursor++]);
+        }
+    }
+    dest = new char[size];
+    if (header)
+        csvHeader = dest;
+    else
+        dataString = dest;
+    //---Fill data String
+    int j = 0;
+    for (int i = 0; i < numCategories; i++)
+    {
+        for (int k = 0; str[i][k] != '\0'; j++, k++)
+        { // append all the data strings onto the main string
+
+            dest[j] = str[i][k];
+        }
+        if (i >= 1 && !header)
+        {
+            delete[] str[i]; // delete all the heap arrays.
+        }
+    }
+    delete[] str;
+    dest[j - 1] = '\0'; // all strings have ',' at end so this gets rid of that and terminates it a character early.
+}
+
+#pragma endregion // CSV Operations
+
+#pragma region Getters and Setters
+
+void State::setRecordOwnFlightData(bool value) { recordOwnFlightData = value; }
+bool State::getRecordOwnFlightData() { return recordOwnFlightData; }
+int State::getStageNum() { return stageNumber; }
 
 void State::setRadio(Radio *r) { radio = r; }
 Radio *State::getRadio() { return radio; }
@@ -314,7 +355,8 @@ Sensor *State::getSensor(SensorType type, int sensorNum)
 Barometer *State::getBaro() { return baro; }
 GPS *State::getGPS() { return gps; }
 IMU *State::getIMU() { return imu; }
-#pragma endregion
+
+#pragma endregion // Getters and Setters
 
 #pragma region Helper Functions
 
@@ -358,52 +400,35 @@ bool State::applySensorType(int i, int sensorNum)
 
 void State::determineAccelerationMagnitude()
 {
-    accelerationMagnitude = sqrt((acceleration.x() * acceleration.x()) + (acceleration.y() * acceleration.y()) + (acceleration.z() * acceleration.z()));
+    accelerationMagnitude = acceleration.magnitude();
 }
 
 void State::determineStage()
 {
     if (stageNumber == 0 &&
         (sensorOK(imu) || sensorOK(baro)) &&
-        (sensorOK(imu) ? abs(imu->getAcceleration().z()) > 25 : true) &&
-        (sensorOK(baro) ? baro->getRelAltFt() > 60 : true))
+        (sensorOK(imu) ? accelerationMagnitude > 25 : true) &&
+        (sensorOK(baro) ? baro->getRelAltFt() > 30 : true))
     // if we are in preflight AND
     // we have either the IMU OR the barometer AND
-    // imu is ok AND the z acceleration is greater than 29 ft/s^2 OR imu is not ok AND
+    // imu is ok AND the accel Magnitude is greater than 25 ft/s^2 OR imu is not ok AND
     // barometer is ok AND the relative altitude is greater than 30 ft OR baro is not ok
 
     // essentially, if we have either sensor and they meet launch threshold, launch. Otherwise, it will never detect a launch.
     {
-        bb.aonoff(33, 200);
-        setRecordMode(FLIGHT);
-        stageNumber = 1;
-        timeOfLaunch = timeAbsolute;
-        timePreviousStage = timeAbsolute;
-        timeSinceLaunch = 0;
-        strcpy(launchTimeOfDay, gps->getTimeOfDay());
         recordLogData(INFO, "Launch detected.");
-        recordLogData(INFO, "Printing static data.");
-        for (int i = 0; i < NUM_MAX_SENSORS; i++)
-        {
-            if (sensorOK(sensors[i]))
-            {
-                char logData[200];
-                snprintf(logData, 200, "%s: %s", sensors[i]->getName(), sensors[i]->getStaticDataString());
-                recordLogData(INFO, logData);
-                sensors[i]->setBiasCorrectionMode(false);
-            }
-        }
+        launch();
     } // TODO: Add checks for each sensor being ok and decide what to do if they aren't.
-    else if (stageNumber == 1 && abs(acceleration.z()) < 10)
+    else if (stageNumber == 1 && accelerationMagnitude < 15)
     {
-        bb.aonoff(33, 200, 2);
+        bb.aonoff(BUZZER, 200, 2);
         timePreviousStage = timeAbsolute;
         stageNumber = 2;
         recordLogData(INFO, "Coasting detected.");
     }
     else if (stageNumber == 2 && baroVelocity <= 0 && timeSinceLaunch > 5)
     {
-        bb.aonoff(33, 200, 3);
+        bb.aonoff(BUZZER, 200, 3);
         char logData[100];
         snprintf(logData, 100, "Apogee detected at %.2f m.", position.z());
         recordLogData(INFO, logData);
@@ -413,66 +438,47 @@ void State::determineStage()
     }
     else if (stageNumber == 3 && baro->getRelAltFt() < 1000 && timeSinceLaunch > 10)
     {
-        bb.aonoff(33, 200, 4);
+        bb.aonoff(BUZZER, 200, 4);
         stageNumber = 4;
         timePreviousStage = timeAbsolute;
         recordLogData(INFO, "Main parachute conditions detected.");
     }
-    else if (stageNumber == 4 && baroVelocity > -1 && baro->getRelAltFt() < 66 && timeSinceLaunch > 15)
+    else if (stageNumber == 4 && baroVelocity > -3 && baro->getRelAltFt() < 66 && timeSinceLaunch > 15)
     {
-        bb.aonoff(33, 200, 5);
+        bb.aonoff(BUZZER, 200, 5);
         timePreviousStage = timeAbsolute;
         stageNumber = 5;
-        recordLogData(INFO, "Landing detected. Waiting for 5 seconds to dump data.");
+        recordLogData(INFO, "Landing detected. Waiting for 30 seconds to dump data.");
     }
-    else if (stageNumber == 5 && timeSinceLaunch > 20)
+    else if (stageNumber == 5 && timeSinceLaunch > 15)
     {
         if (landingCounter++ >= 300)
-        { // roughly 5 seconds of data after landing
+        { // roughly 30 seconds of data after landing
             setRecordMode(GROUND);
             recordLogData(INFO, "Dumped data after landing.");
         }
     }
 }
-
-void State::setCsvString(char *dest, const char *start, int startSize, bool header)
+void State::launch()
 {
-    int numCategories = numSensors + 1;
-    const char **str = new const char *[numCategories];
-    str[0] = start;
-    int cursor = 1;
-    delete[] dest;
-    //---Determine required size for string
-    int size = startSize + 1; // includes '\0' at end of string for the end of dataString to use
+    recordOwnFlightData = true; // just in case this wasnt already set
+    bb.aonoff(BUZZER, 200);
+    setRecordMode(FLIGHT);
+    stageNumber = 1;
+    timeOfLaunch = timePreviousStage = timeAbsolute;
+    timeSinceLaunch = 0;
+    strcpy(launchTimeOfDay, gps->getTimeOfDay());
+    recordLogData(INFO, "Printing static data.");
     for (int i = 0; i < NUM_MAX_SENSORS; i++)
     {
         if (sensorOK(sensors[i]))
         {
-            str[cursor] = header ? sensors[i]->getCsvHeader() : sensors[i]->getDataString();
-            size += strlen(str[cursor++]);
-        }
-    }
-    dest = new char[size];
-    if (header)
-        csvHeader = dest;
-    else
-        dataString = dest;
-    //---Fill data String
-    int j = 0;
-    for (int i = 0; i < numCategories; i++)
-    {
-        for (int k = 0; str[i][k] != '\0'; j++, k++)
-        { // append all the data strings onto the main string
-
-            dest[j] = str[i][k];
-        }
-        if (i >= 1 && !header)
-        {
-            delete[] str[i]; // delete all the heap arrays.
+            char logData[200];
+            snprintf(logData, 200, "%s: %s", sensors[i]->getName(), sensors[i]->getStaticDataString());
+            recordLogData(INFO, logData);
+            sensors[i]->setBiasCorrectionMode(false);
         }
     }
-    delete[] str;
-    dest[j - 1] = '\0'; // all strings have ',' at end so this gets rid of that and terminates it a character early.
 }
 
 bool State::sensorOK(const Sensor *sensor)
@@ -482,15 +488,24 @@ bool State::sensorOK(const Sensor *sensor)
     return false;
 }
 
-#pragma endregion
-
-bool State::transmit()
+void State::fillAPRSData(APRSTelemData &data)
 {
-    char data[200];
-    snprintf(data, 200, "%f,%f,%i,%i,%i,%c,%i,%s", sensorOK(gps) ? gps->getPos().x() : 0, sensorOK(gps) ? gps->getPos().y() : 0, sensorOK(baro) ? (int)baro->getRelAltFt() : 0, (int)(baroVelocity * 3.28), (int)headingAngle, 'H', stageNumber, launchTimeOfDay);
-    bool b = radio->send(data, ENCT_TELEMETRY);
-    return b;
+    data.lat = sensorOK(gps) ? gps->getPos().x() : 0;
+    data.lng = sensorOK(gps) ? gps->getPos().y() : 0;
+    data.alt = sensorOK(baro) ? baro->getRelAltFt() : 0;
+    data.hdg = (int)headingAngle;
+    data.spd = abs((int)(baroVelocity * 3.28));
+    data.stage = stageNumber;
+    data.orientation = imu->getOrientation().toEuler();
+    data.orientation.toDegrees();
+    for (int i = 0; i < 3; i++)
+    {
+        if (data.orientation[i] < 0)
+            data.orientation[i] += 360;
+    }
 }
+#pragma endregion // Helper Functions
+
 extern unsigned long _heap_start;
 extern unsigned long _heap_end;
 extern char *__brkval;
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/State.h b/Spaceport/23-24/Code/Teensy-Based Avionics/src/State.h
index 2bb88a72..a49903b2 100644
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/src/State.h	
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/State.h	
@@ -9,10 +9,14 @@
 #include "GPS.h"
 #include "IMU.h"
 #include "LightSensor.h"
-#include "Radio.h"
+#include "Radio/Radio.h"
 #include "RTC.h"
 #include "RecordData.h"
+#include "Radio/APRS/APRSTelemMsg.h"
+
 constexpr char STAGES[][15] = {"Pre-Flight", "Boosting", "Coasting", "Drogue Descent", "Main Descent", "Post-Flight"};
+
+extern APRSHeader header;
 class State
 {
 public:
@@ -20,6 +24,8 @@ class State
     // stateRecordsOwnData: whether or not the state should call recordFlightData() itself. If false, other funcitons must call recordFlightData() to record the state's data.
     explicit State(bool useKalmanFilter = true, bool stateRecordsOwnData = true);
     ~State();
+    
+    double baroVelocity;           // in m/s
 
     // to be called after all applicable sensors have been added.
     // Returns false if any sensor failed to init. check data log for failed sensor. Disables sensor if failed.
@@ -43,12 +49,15 @@ class State
     char *getCsvHeader();
     char *getStateString(); // This contains only the portions that define what the state thinks the rocket looks like.
 
-    bool transmit();
+    void fillAPRSData(APRSTelemData &data);
+    void setRecordOwnFlightData(bool val);
+    bool getRecordOwnFlightData();
+    void launch();
     double timeAbsolute; // in s since uC turned on
 
 private:
     int lastTimeAbsolute;
-    static constexpr int NUM_MAX_SENSORS = 3;                              // update with the max number of expected sensors.
+    static constexpr int NUM_MAX_SENSORS = 3;                            // update with the max number of expected sensors.
     SensorType SENSOR_ORDER[NUM_MAX_SENSORS] = {BAROMETER_, GPS_, IMU_}; // make this array the same length as NUM_MAX_SENSORS and fill it.
     // example if you have more than one of the same sensor type:
     // constexpr SensorType SENSOR_ORDER[] = {BAROMETER_, BAROMETER_, GPS_, IMU_}; or
@@ -97,13 +106,12 @@ class State
     imu::Vector<3> velocity;       // in m/s
     imu::Vector<3> acceleration;   // in m/s^2
     imu::Quaternion orientation;   // in quaternion
-    double baroVelocity;           // in m/s
     double baroOldAltitude;        // in m
-    double headingAngle;          // in degrees
+    double headingAngle;           // in degrees
 
     char launchTimeOfDay[9];
 
-    //Kalman Filter settings
+    // Kalman Filter settings
     bool useKF;
     LinearKalmanFilter *kfilter;
     // time pos x y z vel x y z acc x y z
@@ -114,7 +122,8 @@ class State
     double *inputs;
     uint32_t FreeMem();
 
-    
+    // APRS
+    APRSTelemData data;
 };
 
 #endif
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/src/main.cpp b/Spaceport/23-24/Code/Teensy-Based Avionics/src/main.cpp
index 160e5681..2f9af4d3 100644
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/src/main.cpp	
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/src/main.cpp	
@@ -1,35 +1,47 @@
 #include <Arduino.h>
 #include "State.h"
+
 #include "BMP390.h"
 #include "BNO055.h"
 #include "MAX_M10S.h"
-#include "DS3231.h"
-#include "RFM69HCW.h"
+
 #include "RecordData.h"
-#include "BlinkBuzz.h"
+#include <BlinkBuzz.h>
+#include "Pi.h"
+#include "Radio/RadioHandler.h"
+
+#define BMP_ADDR_PIN 36
+#define RPI_PWR 0
+#define RPI_VIDEO 1
 
 BNO055 bno(13, 12);         // I2C Address 0x29
 BMP390 bmp(13, 12);         // I2C Address 0x77
 MAX_M10S gps(13, 12, 0x42); // I2C Address 0x42
-DS3231 rtc();               // I2C Address 0x68
-APRSConfig config = {"KC3UTM", "APRS", "WIDE1-1", '[', '/'};
-RadioSettings settings = {433.775, true, false, &hardware_spi, 10, 31, 32};
-RFM69HCW radio = {settings, config};
+
+RadioSettings settings = {433.78, 0x01, 0x02, &hardware_spi, 10, 31, 32};
+RFM69HCW radio(&settings);
+APRSHeader header = {"KC3UTM", "APRS", "WIDE1-1", '^', 'M'};
+APRSCmdData currentCmdData = {800000, 800000, 800000, false};
+APRSCmdMsg cmd(header);
+APRSTelemMsg telem(header);
+int timeOfLastCmd = 0;
+const int CMD_TIMEOUT_SEC = 100; // 10 seconds
+void processCurrentCmdData(double time);
+
 State computer; // = useKalmanFilter = true, stateRecordsOwnData = true
 uint32_t radioTimer = millis();
-
+Pi rpi(RPI_PWR, RPI_VIDEO);
 PSRAM *ram;
 
-#define BUZZER 33
-#define BMP_ADDR_PIN 36
-#define RPI_PWR 0
-#define RPI_VIDEO 1
-
 static double last = 0; // for better timing than "delay(100)"
 
 // BlinkBuzz setup
-int allowedPins[] = {LED_BUILTIN, BUZZER};
+int BUZZER = 33;
+int LED = LED_BUILTIN;
+int allowedPins[] = {LED, BUZZER};
 BlinkBuzz bb(allowedPins, 2, true);
+
+// Free memory debug function
 extern unsigned long _heap_start;
 extern unsigned long _heap_end;
 extern char *__brkval;
@@ -41,26 +53,16 @@ void FreeMem()
     Serial.print(" ");
     free(heapTop);
 }
+// Free memory debug function
 
 void setup()
 {
 
-    pinMode(LED_BUILTIN, OUTPUT);
-    pinMode(BUZZER, OUTPUT); // its very loud during testing
-    bb.onoff(BUZZER, 100, 4, 100);
     recordLogData(INFO, "Initializing Avionics System. 5 second delay to prevent unnecessary file generation.", TO_USB);
-    delay(5000);
+    // delay(5000);
 
     pinMode(BMP_ADDR_PIN, OUTPUT);
     digitalWrite(BMP_ADDR_PIN, HIGH);
-
-    pinMode(RPI_PWR, OUTPUT);   // RASPBERRY PI TURN ON
-    pinMode(RPI_VIDEO, OUTPUT); // RASPBERRY PI TURN ON
-
-    digitalWrite(RPI_PWR, LOW);
-    digitalWrite(RPI_VIDEO, HIGH);
-
-    bb.onoff(LED_BUILTIN, 100);
     ram = new PSRAM(); // init after the SD card for better data logging.
 
     // The SD card MUST be initialized first to allow proper data logging.
@@ -103,55 +105,49 @@ void setup()
     }
     sendSDCardHeader(computer.getCsvHeader());
 }
-static bool more = false;
-static bool first = false;
-static bool second = false;
+
 void loop()
 {
-    bb.update();
     double time = millis();
+    bb.update();
 
-    if (time - radioTimer >= 500)
+    // Update the Radio
+    if (radio.update()) // if there is a message to be read
     {
-        more = computer.transmit();
-        radioTimer = time;
-    }
-    if (radio.mode() != RHGenericDriver::RHModeTx && more)
-    {
-        more = !radio.sendBuffer();
+        timeOfLastCmd = time;
+        APRSCmdData old = cmd.data;
+        if (radio.dequeueReceive(&cmd))
+            radioHandler::processCmdData(cmd, old, currentCmdData, time);
     }
+    radioHandler::processCurrentCmdData(currentCmdData, computer, rpi, time);
+
+    // Update the state of the rocket
     if (time - last < 100)
         return;
 
     last = time;
     computer.updateState();
     recordLogData(INFO, computer.getStateString(), TO_USB);
-    // RASPBERRY PI TURN ON
-    if (time / 1000.0 > 810)
-    {
-        digitalWrite(RPI_PWR, HIGH);
-        if (!first)
-        {
-            bb.aonoff(BUZZER, 100, 2);
-            first = true;
-        }
-    }
-    if (computer.getStageNum() >= 1)
+    Serial.println(computer.baroVelocity);
+    // Send Telemetry Data
+    if (time - radioTimer >= 500)
     {
-        digitalWrite(RPI_VIDEO, LOW);
-        if (!second)
-        {
-            bb.aonoff(BUZZER, 100, 3);
-            second = true;
-        }
+        computer.fillAPRSData(telem.data);
+
+        int status = PI_ON * rpi.isOn() +
+                     PI_VIDEO * rpi.isRecording() +
+                     RECORDING_DATA * computer.getRecordOwnFlightData();
+        telem.data.statusFlags = status;
+
+        radio.enqueueSend(&telem);
+        radioTimer = time;
     }
-    // if (time / 1000.0 > 180)
-    // {
-    //     digitalWrite(RPI_VIDEO, LOW);
-    //     if (!second)
-    //     {
-    //         bb.aonoff(BUZZER, 100, 3);
-    //         second = true;
-    //     }
-    // }
+
+    // RASPBERRY PI TURN ON/VIDEO BACKUP
+    if ((time / 1000.0 > 810 && time - timeOfLastCmd > CMD_TIMEOUT_SEC * 1000) || computer.getStageNum() >= 1)
+        rpi.setOn(true);
+    if ((computer.getStageNum() >= 1 || time - timeOfLastCmd > CMD_TIMEOUT_SEC * 1000) && rpi.isOn())
+        rpi.setRecording(true);
 }
+
+
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/test/ReceiverTest.cpp b/Spaceport/23-24/Code/Teensy-Based Avionics/test/ReceiverTest.cpp
index aafb5b27..57d80725 100644
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/test/ReceiverTest.cpp	
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/test/ReceiverTest.cpp	
@@ -6,12 +6,12 @@
 #define PATH "WIDE1-1"
 
 APRSConfig config = {CALLSIGN, TOCALL, PATH, '[', '/'};
-RadioSettings settings = {433.775, false, false, &hardware_spi, 10, 31, 32};
-RFM69HCW receive = {settings, config};
+RadioSettings settings = {915.0, false, false, &hardware_spi, 10, 31, 32};
+RFM69HCW receive = {&settings, &config};
 
 void setup()
 {
-    Serial.begin(9600);
+    Serial.begin(115200);
     while (!Serial)
         ;
 
@@ -25,6 +25,8 @@ void loop()
 {
     if (receive.available())
     {
+        Serial.println("s");
         Serial.println(receive.receive(ENCT_GROUNDSTATION));
+        Serial.println("e");
     }
 }
\ No newline at end of file
diff --git a/Spaceport/23-24/Code/Teensy-Based Avionics/test/TransmitterTest.cpp b/Spaceport/23-24/Code/Teensy-Based Avionics/test/TransmitterTest.cpp
index f30a9090..a59a1bf4 100644
--- a/Spaceport/23-24/Code/Teensy-Based Avionics/test/TransmitterTest.cpp	
+++ b/Spaceport/23-24/Code/Teensy-Based Avionics/test/TransmitterTest.cpp	
@@ -7,7 +7,7 @@
 
 APRSConfig config = {CALLSIGN, TOCALL, PATH, '[', '/'};
 RadioSettings settings = {433.775, true, false, &hardware_spi, 10, 2, 9};
-RFM69HCW transmit = {settings, config};
+RFM69HCW transmit = {&settings, &config};
 
 void setup()
 {