Where to begin…. (#13)

+/- Reworked collection of altimeter related functions into altimeter class +/- Reworked bno055 class to be imu class with minimal functionality \- Removed external Kalman filter implementations in favor of own in house version \- Removed any/unused files \+ Added buffer logger for when sitting on pad for extended period of time in effort to prevent filling of flash chip \+ Added heartbeat LED for alive status
author: Dawsyn Schraiber <[email protected]> 2024-06-13 14:30:58 -0400
committer: GitHub <[email protected]> 2024-06-13 14:30:58 -0400
commit: 58b4bc754bbb9f5197119cd0c124e49c05acff46 (patch)
tree: 8a65e23756374626e2c9cb997af9d8ed6f892390
parent: 8fbd08fe29bbc2246a78b481b219c241f62ff420 (diff)
download: active-drag-system-58b4bc754bbb9f5197119cd0c124e49c05acff46.tar.gz
active-drag-system-58b4bc754bbb9f5197119cd0c124e49c05acff46.tar.bz2
active-drag-system-58b4bc754bbb9f5197119cd0c124e49c05acff46.zip
85 files changed, 1176 insertions, 7601 deletions
diff --git a/.gitignore b/.gitignore
index affb9f0..4bc92b4 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,7 +2,7 @@ Testing/*
 .DS_Store
 .vagrant/*
 .idea/
-./vscode
+.vscode/
 CMakeFiles/
 cmake_install.cmake
 CMakeCache.txt
diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
deleted file mode 100644
index 63e048a..0000000
--- a/.vscode/c_cpp_properties.json
+++ /dev/null
@@ -1,22 +0,0 @@
-{
-    "configurations": [
-        {
-            "name": "Win32",
-            "includePath": [
-                "${workspaceFolder}/**",
-                "${workspaceFolder}/include/math"
-            ],
-            "defines": [
-                "_DEBUG",
-                "UNICODE",
-                "_UNICODE"
-            ],
-            "windowsSdkVersion": "10.0.22621.0",
-            "compilerPath": "cl.exe",
-            "cStandard": "c17",
-            "cppStandard": "c++17",
-            "intelliSenseMode": "windows-msvc-x64"
-        }
-    ],
-    "version": 4
-}
-\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4973b9d..933710e 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -21,6 +21,7 @@ endif()
 pico_sdk_init()
 
 add_subdirectory(src)
+add_subdirectory(tools)
 
 
 add_compile_options(-Wall
diff --git a/Dockerfile b/Dockerfile
deleted file mode 100644
index 62b9af6..0000000
--- a/Dockerfile
+++ /dev/null
@@ -1,15 +0,0 @@
-FROM alpine:3.17.0
-
-# Install toolchain
-RUN apk update && \
-    apk upgrade && \
-    apk add git \
-            python3 \
-            py3-pip \
-            cmake \
-            build-base \
-            libusb-dev \
-            bsd-compat-headers \
-            newlib-arm-none-eabi \
-            gcc-arm-none-eabi \
-            valgrind
-\ No newline at end of file
diff --git a/README.md b/README.md
index ae9dcec..5ebaf1d 100644
--- a/README.md
+++ b/README.md
@@ -2,27 +2,30 @@
 This is the main codebase for Rocketry at Virginia Tech's Active Drag System, also known colloquially as the ADS, for the 2023-2024 competition year. It runs primarily on a Raspberry Pi Pico, and its goal is to autonomously control the ADS' deployment during flight.
 
 `Eigen` Library, `cmake`, and `arm-none-eabi-gcc` tooling required for successful build.
-## Before Build
+
+## Clone
+
+**Note: You must initialize the git submodules prior to utilizing CMake for a proper build.**
+
 ```shell
 git clone https://github.com/RocketryVT/active-drag-system.git
 cd active-drag-system/
 git submodule update --init --recursive
 ```
 
-## BUILD
+## Build (Linux)
 ```shell
-vagrant up
-vagrant ssh
-cmake /vagrant
-cmake --build .
+cmake -B build
+cmake --build build
 ```
+In the event that your preferred IDE has trouble locating header files and/or is displaying incorrect errors, pass ```-DCMAKE_EXPORT_COMPILE_COMMANDS=true``` to the first CMake command above. Similarly, if you wish to compile the additional tools (e.g. reading flash, calibrating the IMU, etc.), pass ```-DCOMPILE_TOOLS=true``` to the first CMake command above as well.
 
-## BUILD Alternative (Windows)
+## Build (Windows)
 Enable WSL2 in windows
 Install Ubuntu 22 LTS from Windows Store
 ```shell
 sudo apt update && upgrade
-sudo apt install build-essential cmake valgrind gcc-arm-none-eabi
+sudo apt install build-essential cmake gcc-arm-none-eabi
 ```
 Then to actually build:
 ```shell
@@ -30,7 +33,7 @@ cmake -B build
 cmake --build build
 ```
 
-## BUILD Alternative (Mac)
+## Build Alternative (Mac)
 ```shell
 brew install arm-none-eabi-gcc
 ```
diff --git a/Vagrantfile b/Vagrantfile
deleted file mode 100644
index ff65b09..0000000
--- a/Vagrantfile
+++ /dev/null
@@ -1,24 +0,0 @@
-#-----------------------------------------------------------------------
-$bootstrap = <<BOOTSTRAP
-export DEBIAN_FRONTEND=noninteractive
-apt-get update && apt-get upgrade
-
-# install base development tools
-apt-get -y install build-essential
-apt-get -y install cmake valgrind gcc-arm-none-eabi
-
-BOOTSTRAP
-#-----------------------------------------------------------------------
-
-
-# Configuration
-Vagrant.configure("2") do |config|
-  config.vm.box = "debian/bullseye64"
-  config.vm.box_version = "11.20230615.1"
-
-  # (default timeout is 300 s)
-  config.vm.boot_timeout = 600
-
-  # set up the VM
-  config.vm.provision "shell", inline: $bootstrap
-end
diff --git a/build.sh b/build.sh
deleted file mode 100755
index 6c2ece8..0000000
--- a/build.sh
+++ /dev/null
@@ -1,3 +0,0 @@
-#!/bin/bash
-cmake -B build
-cmake --build build
-\ No newline at end of file
diff --git a/compose-dev.yaml b/compose-dev.yaml
deleted file mode 100644
index 81a2ce4..0000000
--- a/compose-dev.yaml
+++ /dev/null
@@ -1,14 +0,0 @@
-services:
-  app:
-    build:
-      context: .
-      dockerfile: Dockerfile
-    entrypoint:
-    - sleep
-    - infinity
-    init: true
-    volumes:
-    - type: bind
-      source: /var/run/docker.sock
-      target: /var/run/docker.sock
-
diff --git a/docs/Rocketry-ADS-UML.pdf b/docs/Rocketry-ADS-UML.pdf
deleted file mode 100644
index 41b54f4..0000000
--- a/docs/Rocketry-ADS-UML.pdf
+++ /dev/null
diff --git a/docs/Rocketry-ADS-UML.png b/docs/Rocketry-ADS-UML.png
deleted file mode 100644
index 1a491e5..0000000
--- a/docs/Rocketry-ADS-UML.png
+++ /dev/null
diff --git a/docs/TestLaunch2-Critial.txt b/docs/TestLaunch2-Critial.txt
deleted file mode 100644
index 35f03ed..0000000
--- a/docs/TestLaunch2-Critial.txt
+++ /dev/null
@@ -1,255 +0,0 @@
-1898.572  -2.313 120.00 0.22 -0.59 9.45 0.00 -0.00 0.01 8.31 1.81 3.44 
-1898.984  -2.313 120.00 0.24 -0.60 9.43 0.00 -0.00 0.01 8.31 1.81 3.44 
-1899.397  -2.188 120.00 0.23 -0.59 9.43 0.00 -0.00 -0.00 8.31 1.81 3.44 
-1899.808  -2.313 120.00 0.24 -0.56 9.44 0.00 -0.00 0.01 8.31 1.81 3.44 
-1900.221  -2.313 120.00 0.27 -0.58 9.48 0.00 0.00 -0.00 8.31 1.81 3.44 
-1900.634  -2.188 120.00 0.23 -0.57 9.45 0.00 -0.00 0.00 8.31 1.81 3.44 
-1901.047  -2.313 120.00 0.24 -0.57 9.43 -0.00 -0.00 -0.00 8.31 1.81 3.44 
-1901.459  -2.438 120.00 0.21 -0.59 9.46 -0.00 0.00 0.00 8.31 1.81 3.44 
-1901.872  -2.625 120.00 0.23 -0.60 9.46 -0.00 0.00 0.00 8.31 1.81 3.44 
-1902.283  -2.563 120.00 0.22 -0.60 9.42 -0.00 -0.00 0.00 8.31 1.81 3.44 
-1902.696  -2.750 120.00 0.23 -0.60 9.44 -0.00 -0.01 0.00 8.31 1.81 3.44 
-1903.106  -2.188 120.00 0.25 -0.57 9.46 0.00 -0.00 -0.00 8.31 1.81 3.44 
-1903.521  -2.188 120.00 0.23 -0.56 9.44 -0.00 0.00 -0.01 8.31 1.81 3.44 
-1903.932  -2.313 120.00 0.22 -0.59 9.46 0.00 0.00 -0.00 8.31 1.81 3.44 
-1904.345  -2.000 120.00 0.25 -0.60 9.43 0.00 -0.00 0.01 8.31 1.81 3.44 
-1904.755  -2.000 120.00 0.26 -0.56 9.44 0.00 0.00 0.01 8.31 1.81 3.44 
-1905.159  -1.875 120.00 0.22 -0.56 9.43 0.00 -0.00 -0.00 8.31 1.81 3.44 
-1905.571  -2.125 120.00 0.24 -0.56 9.44 -0.00 0.00 0.00 8.31 1.81 3.44 
-1905.984  -1.938 120.00 0.24 -0.57 9.41 0.00 0.00 0.00 8.31 1.81 3.44 
-1906.395  -1.813 120.00 0.23 -0.57 9.44 -0.00 0.00 0.00 8.31 1.81 3.44 
-1906.806  -2.125 120.00 0.23 -0.61 9.46 0.00 0.00 0.00 8.31 1.81 3.44 
-1907.209  -1.938 120.00 0.22 -0.59 9.45 -0.00 -0.00 0.00 8.31 1.81 3.44 
-1907.622  -2.188 120.00 0.24 -0.60 9.47 -0.00 0.00 0.01 8.31 1.81 3.44 
-1908.034  -2.250 120.00 0.21 -0.56 9.43 0.00 -0.00 0.01 8.31 1.81 3.44 
-1908.448  -2.000 120.00 0.23 -0.61 9.46 -0.00 0.00 -0.00 8.31 1.81 3.44 
-1908.849  -1.875 120.00 0.24 -0.57 9.47 0.00 0.00 0.00 8.25 1.81 3.44 
-1909.261  -1.750 120.00 0.24 -0.58 9.43 0.00 0.00 0.00 8.25 1.81 3.44 
-1909.672  -2.000 120.00 0.21 -0.59 9.45 0.00 -0.00 0.00 8.25 1.81 3.44 
-1910.085  -2.000 120.00 0.24 -0.59 9.45 -0.00 0.00 0.00 8.25 1.81 3.44 
-1910.497  -1.938 120.00 0.23 -0.60 9.45 0.00 -0.00 0.01 8.25 1.81 3.44 
-1910.912  -2.125 120.00 0.22 -0.58 9.46 0.00 -0.00 0.00 8.31 1.81 3.44 
-1911.323  -2.063 120.00 0.22 -0.56 9.47 0.00 -0.00 0.01 8.31 1.81 3.44 
-1911.736  -2.000 120.00 0.22 -0.54 9.43 -0.00 0.00 0.00 8.31 1.81 3.44 
-1912.148  -1.813 120.00 0.21 -0.57 9.42 0.00 0.00 -0.01 8.38 1.81 3.44 
-1912.550  -1.938 120.00 0.23 -0.59 9.46 0.00 0.00 -0.01 8.31 1.81 3.44 
-1912.962  -2.188 120.00 0.24 -0.59 9.44 0.00 -0.00 -0.00 8.31 1.81 3.44 
-1913.377  -1.813 120.00 0.21 -0.59 9.46 -0.00 -0.00 0.01 8.31 1.81 3.44 
-1913.789  -1.938 120.00 0.27 -0.58 9.45 0.00 -0.00 0.00 8.31 1.81 3.44 
-1914.203  -1.750 120.00 0.23 -0.62 9.44 -0.00 0.00 0.01 8.31 1.81 3.44 
-1914.615  -2.000 120.00 0.21 -0.56 9.44 -0.00 -0.00 -0.00 8.31 1.81 3.44 
-1915.029  -1.625 120.00 0.23 -0.56 9.49 0.00 -0.00 0.01 8.31 1.81 3.44 
-1915.442  -1.875 120.00 0.24 -0.56 9.42 -0.00 -0.00 -0.00 8.25 1.81 3.44 
-1915.853  -1.750 120.00 0.23 -0.58 9.46 0.00 0.00 -0.01 8.25 1.81 3.44 
-1916.256  -1.750 120.00 0.25 -0.57 9.44 0.00 -0.00 -0.01 8.25 1.81 3.44 
-1916.669  -2.000 120.00 0.24 -0.59 9.45 0.00 -0.00 0.00 8.25 1.81 3.44 
-1917.081  -1.750 120.00 0.25 -0.57 9.45 -0.00 -0.00 0.00 8.25 1.81 3.44 
-1917.495  -1.750 120.00 0.25 -0.56 9.42 -0.00 0.00 -0.00 8.25 1.81 3.44 
-1917.907  -1.563 120.00 0.21 -0.61 9.47 -0.00 0.00 -0.00 8.25 1.81 3.44 
-1918.320  -1.500 120.00 0.24 -0.56 9.46 0.00 0.00 0.01 8.31 1.81 3.44 
-1918.732  -1.313 120.00 0.23 -0.57 9.45 0.00 -0.00 0.01 8.25 1.81 3.44 
-1919.145  -1.313 120.00 0.26 -0.54 9.43 -0.00 -0.00 -0.01 8.25 1.81 3.44 
-1919.556  -1.313 120.00 0.23 -0.58 9.44 0.00 0.00 -0.01 8.31 1.81 3.44 
-1919.971  -1.438 120.00 0.22 -0.59 9.48 -0.00 0.00 -0.00 8.38 1.81 3.44 
-1920.382  -1.313 120.00 0.23 -0.57 9.46 0.00 0.00 -0.00 8.38 1.81 3.44 
-1920.794  -1.250 120.00 0.22 -0.56 9.44 0.00 -0.00 0.00 8.38 1.81 3.44 
-1921.206  -1.125 120.00 0.22 -0.58 9.44 0.00 0.00 0.01 8.38 1.81 3.44 
-1921.610  -1.188 120.00 0.25 -0.58 9.45 0.00 -0.00 -0.00 8.38 1.81 3.44 
-1922.022  -1.313 120.00 0.25 -0.59 9.47 -0.00 0.00 -0.00 8.38 1.81 3.44 
-1922.434  -0.938 120.00 0.22 -0.55 9.47 0.00 -0.00 0.00 8.38 1.81 3.44 
-1922.846  -1.188 120.00 0.24 -0.55 9.46 0.00 -0.00 0.01 8.38 1.81 3.44 
-1923.258  -1.438 120.00 0.26 -0.56 9.42 -0.00 0.00 0.00 8.38 1.81 3.44 
-1923.672  -1.063 120.00 0.26 -0.60 9.44 -0.00 -0.00 0.01 8.38 1.81 3.44 
-1924.084  -1.375 120.00 0.24 -0.56 9.43 -0.00 -0.00 -0.01 8.38 1.81 3.44 
-1924.496  -1.063 120.00 0.23 -0.58 9.44 0.00 0.00 0.00 8.38 1.81 3.44 
-1924.908  -1.250 120.00 0.20 -0.60 9.44 0.00 -0.00 -0.00 8.38 1.81 3.44 
-1925.321  -1.438 120.00 0.22 -0.61 9.45 0.00 0.00 -0.01 8.38 1.81 3.44 
-1925.735  -1.625 120.00 0.18 -0.56 9.41 -0.00 0.00 -0.00 8.38 1.81 3.44 
-1926.147  -1.188 120.00 0.27 -0.58 9.44 0.00 -0.00 -0.00 8.38 1.81 3.44 
-1926.559  -1.438 120.00 0.26 -0.58 9.46 0.00 0.00 0.00 8.38 1.81 3.44 
-1926.972  -1.313 120.00 0.27 -0.59 9.46 -0.01 -0.00 0.00 8.44 1.88 3.44 
-1927.385  -1.750 120.00 0.22 -0.58 9.43 0.00 0.00 -0.01 8.44 1.88 3.44 
-1927.798  -1.438 120.00 0.23 -0.56 9.45 0.00 -0.00 0.01 8.44 1.88 3.44 
-1928.211  -1.438 120.00 0.21 -0.57 9.48 -0.00 0.00 -0.01 8.44 1.88 3.44 
-1928.622  -1.500 120.00 0.25 -0.52 9.42 0.00 0.00 -0.01 8.44 1.88 3.44 
-1929.026  -2.000 120.00 0.26 -0.59 9.44 0.00 -0.00 -0.01 8.44 1.88 3.44 
-1929.438  -1.813 120.00 0.22 -0.60 9.44 0.00 -0.00 -0.00 8.50 1.88 3.44 
-1929.850  -1.563 120.00 0.23 -0.61 9.44 0.00 0.00 0.00 8.50 1.88 3.44 
-1930.262  -2.000 120.00 0.24 -0.60 9.46 0.00 0.00 0.01 8.50 1.88 3.44 
-1930.677  -1.875 120.00 0.21 -0.59 9.48 0.00 -0.00 0.00 8.50 1.88 3.44 
-1931.089  -1.500 120.00 0.22 -0.60 9.46 0.00 0.00 -0.01 8.50 1.88 3.44 
-1931.501  -1.688 120.00 0.23 -0.58 9.44 -0.00 0.00 -0.01 8.50 1.88 3.44 
-1931.913  -1.750 120.00 0.22 -0.61 9.43 0.00 0.00 -0.00 8.50 1.88 3.44 
-1932.325  -2.000 120.00 0.23 -0.63 9.45 -0.00 0.00 -0.00 8.50 1.88 3.44 
-1932.740  -1.750 120.00 0.15 -1.07 9.04 0.00 0.00 0.01 8.50 1.88 3.44 
-1933.152  -1.813 120.00 0.31 -0.64 9.48 0.00 -0.00 -0.00 8.50 1.88 3.44 
-1933.564  -2.000 120.00 0.24 -0.56 9.45 0.00 -0.00 0.00 8.50 1.88 3.44 
-1933.977  -1.875 120.00 0.20 -0.63 9.47 0.00 -0.00 0.00 8.50 1.88 3.44 
-1934.390  -1.875 120.00 0.25 -0.58 9.45 -0.00 0.00 -0.01 8.50 1.88 3.44 
-1934.801  -2.000 120.00 0.25 -0.56 9.48 0.00 -0.00 -0.00 8.50 1.88 3.44 
-1935.213  -1.750 120.00 0.24 -0.54 9.45 -0.00 -0.00 -0.00 8.50 1.88 3.44 
-1935.627  -1.688 120.00 0.25 -0.58 9.43 0.00 0.00 0.00 8.50 1.88 3.44 
-1936.040  -1.750 120.00 0.26 -0.56 9.44 -0.00 0.00 0.00 8.50 1.88 3.44 
-1936.450  -1.938 120.00 0.29 -0.57 9.44 -0.00 0.00 -0.00 8.50 1.88 3.44 
-1936.862  -2.000 120.00 0.24 -0.56 9.45 0.00 -0.00 -0.00 8.50 1.88 3.44 
-1937.275  -1.938 120.00 0.24 -0.56 9.45 0.00 0.00 -0.00 8.50 1.88 3.44 
-1937.687  -1.875 120.00 0.24 -0.60 9.44 -0.00 -0.00 -0.00 8.50 1.88 3.44 
-1938.089  -1.875 120.00 0.22 -0.56 9.46 0.00 -0.00 -0.00 8.50 1.88 3.44 
-1938.500  -1.875 120.00 0.23 -0.59 9.47 -0.00 0.00 0.00 8.50 1.88 3.44 
-1938.913  -2.188 120.00 0.22 -0.60 9.45 -0.00 0.00 -0.00 8.50 1.88 3.44 
-1939.325  -2.125 120.00 0.30 -0.56 9.50 -0.00 0.00 -0.01 8.50 1.88 3.44 
-1939.739  -2.063 120.00 0.32 -0.50 9.42 0.00 0.00 0.00 8.56 1.88 3.44 
-1940.149  -1.813 120.00 0.23 -0.59 9.41 -0.00 -0.00 0.01 8.56 1.88 3.44 
-1940.563  -1.875 120.00 0.26 -0.51 9.49 -0.00 -0.00 0.00 8.56 1.88 3.44 
-1940.975  -2.063 120.00 0.29 -0.51 9.41 -0.00 0.00 -0.00 8.56 1.88 3.44 
-1941.387  -2.188 120.00 0.31 -0.51 9.46 0.00 0.00 0.00 8.50 1.88 3.44 
-1941.800  -2.188 120.00 0.30 -0.53 9.43 -0.00 -0.00 -0.00 8.44 1.88 3.44 
-1942.213  -2.313 120.00 0.29 -0.60 9.49 -0.00 0.00 0.01 8.38 1.81 3.44 
-1942.624  -1.875 120.00 0.22 -0.54 9.54 -0.00 -0.00 0.00 8.38 1.81 3.44 
-1943.038  -2.000 120.00 0.22 -0.54 9.47 0.00 -0.00 0.01 8.38 1.81 3.44 
-1943.450  -1.875 120.00 0.25 -0.62 9.36 -0.00 -0.00 0.01 8.38 1.81 3.44 
-1943.864  -1.688 120.00 0.19 -0.67 9.38 -0.00 0.00 0.01 8.31 1.81 3.44 
-1944.275  -2.000 120.00 0.21 -0.51 9.53 -0.00 0.00 0.00 8.31 1.81 3.44 
-1944.688  -1.750 120.00 0.30 -0.51 9.52 0.00 0.00 0.00 8.31 1.81 3.44 
-1945.099  -1.625 120.00 0.29 -0.57 9.49 0.00 -0.00 0.00 8.25 1.81 3.44 
-1945.513  -1.500 120.00 0.30 -0.54 9.46 -0.00 -0.00 0.01 8.12 1.81 3.50 
-1945.925  -1.438 120.00 0.36 -0.66 9.39 -0.00 -0.00 -0.00 8.12 1.81 3.50 
-1946.338  -1.625 120.00 0.16 -0.62 9.28 0.00 0.00 -0.00 8.12 1.81 3.50 
-1946.749  -1.500 120.00 0.31 -0.42 9.83 0.00 0.00 0.01 8.12 1.81 3.50 
-1947.164  -1.563 120.00 0.32 -0.45 9.53 -0.00 0.00 0.02 8.00 1.81 3.50 
-1947.575  -1.500 120.00 0.38 -0.51 9.52 0.00 -0.00 -0.00 8.00 1.75 3.50 
-1947.988  -0.688 120.00 0.15 -0.51 9.87 0.00 -0.00 -0.01 8.00 1.75 3.50 
-1948.398  2.188 120.00 -0.35 1.03 32.08 0.00 0.00 0.28 6.00 1.62 3.44 
-1948.810  9.250 120.00 -1.58 -0.62 36.21 -0.02 0.06 -0.15 5.50 1.50 3.25 
-1949.222  22.688 120.00 -0.43 -0.28 34.87 -0.26 0.35 -0.37 12.19 -3.19 6.75 
-1949.635  41.063 120.00 1.98 1.71 35.88 -0.42 0.27 -0.76 26.56 -8.38 16.50 
-1950.046  56.563 120.00 -2.13 8.33 35.49 -0.02 0.09 -4.24 72.75 7.69 24.94 
-1950.457  78.439 120.00 -2.34 4.88 34.23 -0.60 0.44 -5.97 209.19 7.75 -13.38 
-1950.871  105.127 120.00 -8.42 4.96 36.94 -0.47 0.29 -5.85 340.06 -6.44 9.81 
-1951.284  136.565 120.00 -5.20 9.33 34.35 -0.57 0.27 -5.73 114.94 20.38 9.75 
-1951.695  172.753 120.00 -1.75 10.43 33.26 -0.42 0.53 -6.07 251.06 -4.94 -11.06 
-1952.106  211.753 120.00 -1.06 15.10 31.32 -0.15 0.78 -6.60 32.44 -3.88 22.25 
-1952.519  254.566 120.00 -3.26 22.65 23.92 -0.13 0.90 -9.20 214.56 5.06 -11.00 
-1952.931  303.130 120.00 -6.78 15.66 10.96 -0.48 0.59 -8.72 71.25 10.94 21.31 
-1953.344  351.693 120.00 -5.43 9.68 -2.99 -0.33 0.41 -6.96 248.06 -3.12 -14.88 
-1953.755  399.069 120.00 -2.44 8.77 -3.14 -0.18 0.42 -6.18 35.19 -2.81 22.19 
-1954.167  443.569 120.00 -0.05 8.89 -2.72 -0.04 0.43 -6.25 174.75 15.75 -7.81 
-1954.581  488.195 120.00 0.52 8.67 -2.33 -0.11 0.45 -6.74 327.50 -19.00 7.94 
-1954.993  530.946 120.00 -1.44 6.65 -2.89 -0.19 0.40 -6.36 119.19 20.75 10.69 
-1955.395  570.634 120.00 -2.64 6.27 -2.32 -0.16 0.35 -5.94 259.06 -8.31 -14.56 
-1955.805  606.697 120.00 -2.31 5.05 -1.80 -0.26 0.31 -5.46 24.56 -6.12 23.25 
-1956.220  641.635 120.00 -2.01 4.01 -1.48 -0.23 0.23 -5.15 146.38 25.00 -0.06 
-1956.632  674.948 120.00 -1.95 3.69 -1.66 -0.21 0.25 -4.93 263.44 -10.25 -16.38 
-1957.045  703.198 120.00 -2.31 2.91 -1.11 -0.28 0.22 -4.70 12.12 -10.88 22.19 
-1957.455  730.824 120.00 -2.18 2.42 -1.33 -0.24 0.13 -4.46 117.06 26.44 12.25 
-1957.870  757.324 120.00 -1.36 1.56 -0.97 -0.25 0.12 -4.19 216.44 8.38 -23.19 
-1958.282  780.512 120.00 -2.56 1.17 -1.07 -0.19 0.17 -3.91 309.94 -21.88 -2.81 
-1958.694  804.012 120.00 -1.82 0.88 -0.76 -0.37 0.12 -3.57 34.25 -3.56 29.06  
-1959.104  822.825 120.00 -1.99 0.70 -0.67 -0.27 0.02 -3.39 114.44 31.50 16.44 
-1959.516  841.075 120.00 -1.15 0.77 -0.61 -0.23 0.03 -3.16 190.62 24.06 -24.31 
-1959.929  858.326 120.00 -2.02 0.71 -0.61 -0.09 0.09 -2.98 260.31 -11.00 -28.62 
-1960.343  872.201 120.00 -1.81 -0.14 -0.44 -0.27 0.25 -2.87 326.75 -32.62 5.19 
-1960.753  884.889 120.00 -1.23 0.00 -0.43 -0.43 0.04 -2.73 29.50 -8.56 41.00 
-1961.164  895.576 120.00 -1.35 -0.45 -0.33 -0.33 -0.12 -2.51 89.69 33.44 40.31 
-1961.577  904.076 120.00 -0.67 -0.46 -0.34 -0.18 -0.17 -2.28 146.19 55.25 -9.56 
-1961.990  912.701 120.00 -0.69 -0.23 -0.40 -0.01 -0.14 -2.11 197.81 30.12 -52.50 
-1962.400  918.639 120.00 -1.28 -0.37 -0.28 0.08 0.04 -2.02 245.25 -8.56 -59.50 
-1962.811  922.077 120.00 -1.63 -0.74 -0.23 -0.09 0.31 -2.00 288.88 -48.56 -46.62 
-1963.225  924.139 120.00 -1.52 -0.84 -0.11 -0.44 0.33 -2.29 332.44 -63.38 41.75 
-1963.639  924.702 120.00 -1.06 -1.05 -0.07 -0.67 -0.03 -2.59 24.44 -13.19 83.00 
-1964.048  925.014 120.00 -0.53 -1.58 -0.57 -0.35 -0.41 -2.52 79.31 43.94 100.12 
-1964.459  924.827 120.00 2.55 -0.85 8.47 -0.11 0.18 -2.50 131.44 74.00 -162.62 
-1964.870  929.827 120.00 1.39 4.57 0.42 -0.78 -0.12 -1.64 194.62 23.00 -94.25 
-1965.285  923.827 120.00 3.42 6.27 -1.06 -0.99 1.16 2.85 196.62 23.31 -67.31 
-1965.696  917.889 120.00 6.35 -7.18 1.43 1.06 -0.26 7.92 72.31 29.94 15.56 
-1966.106  910.264 120.00 -0.28 4.39 -0.87 0.93 -1.75 9.52 252.19 -13.00 -7.50 
-1966.518  904.514 120.00 -7.76 -0.71 -1.33 -0.81 2.15 11.66 19.62 -45.56 14.38 
-1966.932  893.701 120.00 0.09 -8.09 0.43 0.39 -0.77 15.43 42.44 -31.88 55.56 
-1967.344  882.201 120.00 -5.64 4.65 0.37 -0.05 -0.37 20.07 339.62 -31.88 -3.69 
-1967.755  877.451 120.00 -0.55 10.84 -1.17 1.95 0.78 18.63 263.88 -4.94 -50.31 
-1968.167  868.888 120.00 12.21 4.23 -0.51 0.63 0.37 23.15 163.88 79.88 -16.00 
-1968.580  848.075 120.00 -7.08 9.55 1.30 -0.18 -1.29 26.35 318.62 -52.06 -27.31 
-1968.993  835.013 120.00 -2.13 -10.67 9.63 -0.82 0.40 15.20 172.00 12.88 24.12 
-1969.404  827.075 120.00 -10.37 1.39 7.11 -0.18 -0.02 7.19 300.31 -23.00 -24.19 
-1969.816  820.075 120.00 7.46 -7.93 6.36 -0.14 -1.05 1.74 201.75 39.12 17.88 
-1970.229  810.075 120.00 2.82 4.08 5.78 0.41 0.18 0.92 291.75 31.69 -22.94 
-1970.643  806.262 120.00 -1.15 -10.63 4.44 -1.12 0.87 1.94 233.88 14.94 48.62 
-1971.053  794.700 120.00 2.78 -4.33 4.60 0.94 -1.21 -1.39 233.31 27.81 36.31 
-1971.463  786.950 120.00 8.94 -0.16 4.55 -1.20 0.09 -2.09 261.44 52.31 16.12 
-1971.877  780.887 120.00 8.25 2.24 6.15 -0.04 -0.11 -3.71 334.12 42.38 -42.44 
-1972.289  771.324 120.00 -7.32 2.43 4.58 -0.21 0.22 -6.07 89.62 -50.62 -29.50 
-1972.701  761.199 120.00 3.29 -6.83 3.56 -0.01 0.27 -6.98 239.25 28.50 53.19 
-1973.111  751.511 120.00 -4.56 5.45 3.29 0.26 -0.15 -10.20 98.25 -47.38 -36.06 
-1973.524  741.449 120.00 3.18 8.63 4.09 0.12 -0.15 -13.64 7.88 21.00 -57.19 
-1973.936  732.011 120.00 8.93 1.91 3.12 -0.21 -0.65 -15.94 341.75 47.38 -40.88 
-1974.348  722.199 120.00 8.18 6.51 3.20 0.01 -0.15 -18.15 10.31 35.50 -48.12 
-1974.757  713.761 120.00 -9.03 9.44 4.25 0.44 -0.09 -19.88 84.19 -34.06 -59.00 
-1975.170  704.323 120.00 -3.27 -1.36 2.78 0.90 -0.32 -21.34 213.62 -19.06 51.69 
-1975.583  696.448 120.00 10.41 9.02 3.38 0.56 -1.06 -23.49 7.69 43.50 -49.31 
-1975.995  684.260 120.00 -3.97 1.48 5.19 0.07 -0.87 -24.27 188.31 -39.12 45.12 
-1976.406  674.385 120.00 1.66 14.80 2.38 -0.05 -0.87 -25.11 34.25 23.62 -57.75 
-1976.819  664.385 120.00 3.42 -3.06 5.06 -0.14 -0.87 -25.35 241.19 -0.88 56.19 
-1977.231  654.197 120.00 -5.94 10.18 2.88 0.23 -1.10 -25.24 110.81 -46.62 -49.56 
-1977.644  645.072 120.00 9.47 6.95 4.42 0.09 -0.32 -25.00 327.75 53.44 11.62 
-1978.055  634.135 120.00 -10.57 0.18 5.11 -0.02 -0.61 -23.42 157.06 -66.38 15.88 
-1978.467  624.510 120.00 7.92 9.02 2.69 0.71 -0.64 -23.82 332.12 49.94 -17.31 
-1978.878  614.697 120.00 -6.97 1.07 3.16 0.89 -0.54 -22.72 141.75 -60.06 -9.56 
-1979.287  605.822 120.00 9.39 4.43 4.10 0.45 -0.82 -22.84 311.81 53.50 22.44 
-1979.698  594.884 120.00 -9.20 6.59 3.33 0.17 -0.71 -22.13 81.00 -30.12 -57.88 
-1980.109  585.384 120.00 3.49 -1.80 4.67 -0.09 -0.97 -22.55 229.06 -2.75 51.94 
-1980.521  574.884 120.00 -2.01 12.91 2.54 -0.26 -1.07 -23.27 32.69 11.81 -61.25 
-1980.934  567.196 120.00 3.65 -2.22 4.18 -0.62 -1.12 -22.86 210.88 -14.44 49.31 
-1981.346  553.321 120.00 -1.22 16.89 2.20 -0.87 -0.73 -23.20 0.94 33.31 -56.94 
-1981.759  545.571 120.00 -1.35 -0.52 6.29 -0.92 -0.21 -20.54 144.12 -44.56 2.31 
-1982.170  533.446 120.00 9.24 -1.67 4.11 0.33 0.69 -19.42 222.56 5.44 60.31 
-1982.583  526.571 120.00 1.23 8.51 2.32 1.25 0.10 -19.95 334.81 41.50 -15.19 
-1982.995  513.383 120.00 -8.37 2.31 0.64 0.54 -1.87 -19.60 64.50 -33.25 -51.31 
-1983.397  504.320 120.00 0.97 -2.07 2.73 -0.14 -1.19 -19.96 175.81 -33.50 19.81 
-1983.807  494.133 120.00 15.46 2.71 1.52 -0.38 -0.47 -19.99 226.69 29.06 41.50 
-1984.220  481.882 120.00 0.51 7.48 3.54 -0.72 0.42 -19.83 327.19 38.44 -0.94 
-1984.632  475.320 120.00 -10.24 13.98 6.53 2.01 0.31 -14.37 350.12 7.69 -37.12 
-1985.046  459.257 120.00 2.23 7.06 2.71 -2.92 -0.67 -15.96 1.06 30.44 -25.50 
-1985.458  454.382 120.00 4.39 12.62 3.16 1.61 -2.85 -7.55 274.19 19.88 -6.94 
-1985.872  436.694 120.00 6.99 -2.31 0.74 -1.51 2.83 6.69 261.94 16.69 31.19 
-1986.284  434.882 120.00 2.83 -4.76 2.77 -1.29 -1.46 2.44 166.69 -23.38 -4.31 
-1986.697  420.069 120.00 -0.37 -16.55 3.27 1.66 1.20 -0.20 99.88 -27.81 20.62 
-1987.109  409.069 120.00 -0.32 0.77 4.67 -0.12 -1.27 -0.51 96.81 -24.50 -28.12 
-1987.521  397.319 120.00 19.83 -5.74 4.49 -1.02 0.36 -7.74 142.94 12.62 32.50 
-1987.933  386.506 120.00 -0.15 1.49 2.54 -0.89 0.18 -7.86 313.56 37.75 -10.69 
-1988.346  380.256 120.00 0.90 -16.31 6.25 -0.66 0.43 -3.49 69.81 -20.44 11.38 
-1988.758  363.818 120.00 4.80 -0.93 3.44 -0.73 1.22 -7.32 179.88 -2.88 39.12 
-1989.170  358.943 120.00 -15.73 0.80 5.63 0.73 1.15 -4.98 316.88 2.94 -29.00 
-1989.582  340.943 120.00 -2.38 2.13 5.90 0.07 -1.49 2.15 316.62 23.75 -36.00 
-1989.995  337.380 120.00 3.52 8.10 2.86 2.16 1.11 4.65 236.56 45.31 2.31 
-1990.406  321.942 120.00 8.09 -0.80 2.16 0.70 2.08 2.65 120.75 -8.69 15.44 
-1990.818  316.567 120.00 -2.51 -3.11 4.54 -0.96 1.05 2.84 47.62 -32.00 -29.62 
-1991.231  303.942 120.00 -9.31 -8.24 3.56 -0.35 -0.70 1.48 347.06 -20.94 -15.81 
-1991.643  292.504 120.00 -3.67 0.31 7.84 0.81 0.09 1.12 344.62 0.50 -37.00 
-1992.053  285.942 120.00 -2.17 14.13 3.41 0.71 1.06 5.37 243.25 26.12 -29.31 
-1992.466  273.379 120.00 5.81 1.26 7.42 -0.22 1.14 7.56 115.25 -16.88 24.12 
-1992.878  265.317 120.00 -11.95 7.46 4.45 0.58 1.00 9.86 276.44 14.88 -54.75 
-1993.291  255.566 120.00 -0.67 -2.90 5.64 0.62 0.05 13.14 7.62 -26.94 -31.19 
-1993.702  245.191 120.00 -8.67 -8.81 4.57 -0.79 0.61 16.25 16.94 -62.12 -19.75 
-1994.113  235.004 120.00 -4.25 4.18 3.41 0.51 -0.62 18.61 334.31 -1.81 -43.19 
-1994.527  228.628 120.00 -5.47 14.60 2.89 1.19 1.72 18.12 276.50 21.56 -65.62 
-1994.939  214.316 120.00 2.24 7.38 3.62 -2.31 1.04 20.07 204.44 45.56 33.56 
-1995.351  207.628 120.00 9.44 -10.74 3.69 -0.81 -1.83 18.29 113.44 -14.62 82.25 
-1995.762  194.628 120.00 2.19 -1.20 2.85 2.03 0.38 19.57 55.44 -55.19 -6.75 
-1996.175  186.753 120.00 -16.19 3.75 2.35 -1.13 2.27 17.15 356.06 -43.00 -98.44  
-1996.588  177.565 120.00 0.38 1.68 5.04 0.43 -2.28 17.63 312.06 24.19 -68.50 
-1997.000  167.440 120.00 10.25 13.77 0.91 0.82 2.12 18.60 245.69 68.62 -144.81 
-1997.411  156.940 120.00 3.83 2.44 2.09 -1.43 1.46 19.03 175.00 11.56 65.44 
-1997.823  148.315 120.00 -4.36 -16.21 3.57 -1.01 0.02 17.34 114.31 -37.06 129.62 
-1998.236  138.190 120.00 -2.48 3.32 4.78 0.92 0.10 19.86 61.81 -63.50 -78.75 
-1998.648  129.127 120.00 11.40 12.20 3.03 0.78 1.06 23.28 291.12 47.44 -139.75 
-1999.059  118.877 120.00 -2.21 6.21 3.70 1.54 0.73 25.85 82.00 -78.56 -90.38 
-1999.469  108.752 120.00 6.12 -2.59 1.76 0.70 -0.31 28.28 205.12 11.38 116.06 
-1999.884  100.064 120.00 7.30 3.30 2.89 -1.25 0.63 29.83 260.38 66.25 91.94 
-2000.296  90.376 120.00 15.85 5.45 3.09 0.15 0.63 32.77 287.81 64.38 172.75 
-2000.706  80.564 120.00 1.56 3.41 3.18 -0.63 1.75 34.70 250.12 42.88 96.75 
-2001.116  69.251 120.00 -11.55 12.91 5.31 -0.29 1.14 -35.42 193.88 -37.38 102.94 
-2001.531  59.251 120.00 12.33 11.40 3.34 -0.45 0.74 -35.42 87.69 -48.25 -94.12 
-2001.943  49.938 120.00 -1.70 20.14 2.93 -0.10 0.33 -35.42 340.38 63.31 -96.81 
-2002.355  40.063 120.00 -1.09 8.30 3.00 -0.33 1.00 -35.42 261.00 25.25 103.44  
-2002.765  30.688 120.00 11.02 12.45 3.78 -0.85 1.18 -35.42 158.81 -60.81 134.25 
-2003.177  20.063 120.00 15.61 19.47 2.56 -0.45 1.24 -35.42 58.75 -15.50 -117.50 
-\ No newline at end of file
diff --git a/include/SimpleKalmanFilter.h b/include/SimpleKalmanFilter.h
deleted file mode 100644
index 61b682e..0000000
--- a/include/SimpleKalmanFilter.h
+++ /dev/null
@@ -1,32 +0,0 @@
-/*
- * SimpleKalmanFilter - a Kalman Filter implementation for single variable models.
- * Created by Denys Sene, January, 1, 2017.
- * Released under MIT License - see LICENSE file for details.
- */
-
-#ifndef SimpleKalmanFilter_h
-#define SimpleKalmanFilter_h
-
-class SimpleKalmanFilter
-{
-
-public:
-  SimpleKalmanFilter(float mea_e, float est_e, float q);
-  float updateEstimate(float mea);
-  void setMeasurementError(float mea_e);
-  void setEstimateError(float est_e);
-  void setProcessNoise(float q);
-  float getKalmanGain();
-  float getEstimateError();
-
-private:
-  float _err_measure;
-  float _err_estimate;
-  float _q;
-  float _current_estimate = 0;
-  float _last_estimate = 0;
-  float _kalman_gain = 0;
-
-};
-
-#endif
diff --git a/include/actuationPlan.hpp b/include/actuationPlan.hpp
deleted file mode 100644
index 8d1ef26..0000000
--- a/include/actuationPlan.hpp
+++ /dev/null
@@ -1,36 +0,0 @@
-#pragma once
-#include <algorithm>
-#include <ctime>
-#include "surfaceFitModel.hpp"
-#include "rocketUtils.hpp"
-#include "sensorIMU.hpp"
-#include "sensorAltimeter.hpp"
-
-class ActuationPlan {
-
-    private:
-        SurfaceFitModel sFitModel;
-        
-    public:
-
-        /**
-         * @brief Construct a new Actuation Plan object
-         * 
-         */
-        ActuationPlan();
-
-        /**
-         * @brief Construct a new Actuation Plan object
-         * 
-         * @param sFitModel 
-         */
-        ActuationPlan(SurfaceFitModel sFitModel);
-
-        /**
-         * @brief Run the Fin Actuation Plan.
-         *  Adjusts the fin angle values depending on the current vehicle state during the launch
-         * 
-         * @param rocket Provides current rocket status and hold updated fin angle value.
-         */
-        void runPlan(Vehicle& rocket);
-};
-\ No newline at end of file
diff --git a/include/actuator.hpp b/include/actuator.hpp
deleted file mode 100644
index e7c4120..0000000
--- a/include/actuator.hpp
+++ /dev/null
@@ -1,30 +0,0 @@
-#pragma once
-
-
-class Actuator {
-
-    private:
-
-
-    public:
-
-        /**
-         * @brief Initialize the actuator. 
-         * 
-         * @param data Data for initializing the actuator
-         * 
-         * @return true Initialization Success
-         * @return false Initialization Failure
-         */
-        virtual bool init(void* data) = 0;
-
-        /**
-         * @brief Pass data to the actuator. 
-         * 
-         * @param data Data to sent to the actuator
-         * 
-         * @return true Actuator write Success
-         * @return false Actuator write Failure
-         */
-        virtual bool writeData(void* data) = 0;
-};
-\ No newline at end of file
diff --git a/include/ads.hpp b/include/ads.hpp
deleted file mode 100644
index b16c4fb..0000000
--- a/include/ads.hpp
+++ /dev/null
@@ -1,66 +0,0 @@
-#pragma once
-#include <iostream>
-#include <exception>
-#include <stdexcept>
-#include <chrono>
-#include <thread>
-#include "kalmanfilter.hpp"
-#include "logger.hpp"
-#include "actuationPlan.hpp"
-#include "rocketUtils.hpp"
-#include "sensorIMU.hpp"
-#include "sensorAltimeter.hpp"
-#include "motor.hpp"
-#include "eigen3/Eigen/Dense"
-
-using namespace Eigen;
-
-class ADS {
-
-    private:
-
-        KalmanFilter kf;      
-        ActuationPlan plan;
-
-        IMUSensor imu;
-        AltimeterSensor altimeter;
-        Motor motor;
-        Vehicle rocket;
-
-        /**
-         * @brief Logs a summary of all pertinent current rocket data
-         *  (e.g. Altitude, Velocity, Acceleration)
-         */
-        virtual void logSummary();
-        
-        /**
-         * @brief Performs a routine to calculate the average altitude
-         *      while the vehicle is waiting on the pad.
-         */
-        virtual void updateOnPadAltitude();
-
-        /**
-         * @brief Update the vehicle with the current sensor (IMU & Altimeter) readings
-         */
-        virtual void updateSensorData();
-
-        /**
-         * @brief Update the rocket state based on its current telemetry readings.
-         *     Also Log pertinent telemetry and rocket state data
-         */
-        virtual void updateRocketState();
-
-    public:
-
-        /**
-         * @brief Construct a new ADS object
-         * 
-         * @param plan The Actuation Plan for the Rocket
-         */
-        ADS(ActuationPlan plan);
-
-        /**
-         * @brief Run the full active drag system from launch to landing.
-         */
-        virtual void run();
-};
-\ No newline at end of file
diff --git a/include/altimeter.hpp b/include/altimeter.hpp
new file mode 100644
index 0000000..bf7effd
--- /dev/null
+++ b/include/altimeter.hpp
@@ -0,0 +1,31 @@
+#pragma once
+
+#include "pico/stdlib.h"
+#include "hardware/i2c.h"
+#include "hardware/gpio.h"
+#include <cstdint>
+
+class altimeter {
+    private:
+        uint8_t altitude_buffer[4];
+        uint8_t buffer[4];
+        uint8_t addr;
+        i2c_inst_t* inst;
+    public:
+        altimeter(i2c_inst_t* inst, uint8_t addr);
+
+        void initialize();
+
+        void initialize(float threshold_altitude, uint8_t interrupt_pin, gpio_irq_callback_t callback);
+
+        void set_threshold_altitude(float threshold_altitude, uint8_t interrupt_pin, gpio_irq_callback_t callback);
+
+        void unset_threshold_altitude(uint8_t interrupt_pin);
+
+        float get_altitude_converted();
+
+        void get_altitude_raw(uint8_t* buffer);
+
+        uint32_t expose_buffer(uint8_t** buffer);
+};
+
diff --git a/include/bno055.hpp b/include/bno055.hpp
deleted file mode 100644
index 7fbac4d..0000000
--- a/include/bno055.hpp
+++ /dev/null
@@ -1,292 +0,0 @@
-#pragma once
-
-#include <vector>
-#include <string>
-
-#include "boards/pico_w.h"
-#include "hardware/gpio.h"
-#include "hardware/i2c.h"
-#include "pico/stdlib.h"
-#include "pico/time.h"
-#include "pico/types.h"
-
-#include <Eigen/Geometry>
-
-//Register addresses and data structs copied from Adafruit implementation
-/** BNO055 Address A **/
-#define BNO055_ADDRESS_A (0x28)
-/** BNO055 Address B **/
-#define BNO055_ADDRESS_B (0x29)
-/** BNO055 ID **/
-#define BNO055_ID (0xA0)
-
-/** Offsets registers **/
-#define NUM_BNO055_OFFSET_REGISTERS (22)
-
-
-/** A structure to represent offsets **/
-typedef struct {
-    int16_t accel_offset_x; /**< x acceleration offset */
-    int16_t accel_offset_y; /**< y acceleration offset */
-    int16_t accel_offset_z; /**< z acceleration offset */
-
-    int16_t mag_offset_x; /**< x magnetometer offset */
-    int16_t mag_offset_y; /**< y magnetometer offset */
-    int16_t mag_offset_z; /**< z magnetometer offset */
-
-    int16_t gyro_offset_x; /**< x gyroscrope offset */
-    int16_t gyro_offset_y; /**< y gyroscrope offset */
-    int16_t gyro_offset_z; /**< z gyroscrope offset */
-
-    int16_t accel_radius; /**< acceleration radius */
-
-    int16_t mag_radius; /**< magnetometer radius */
-} bno055_offsets_t;
-
-/** Operation mode settings **/
-typedef enum {
-    OPERATION_MODE_CONFIG = 0X00,
-    OPERATION_MODE_ACCONLY = 0X01,
-    OPERATION_MODE_MAGONLY = 0X02,
-    OPERATION_MODE_GYRONLY = 0X03,
-    OPERATION_MODE_ACCMAG = 0X04,
-    OPERATION_MODE_ACCGYRO = 0X05,
-    OPERATION_MODE_MAGGYRO = 0X06,
-    OPERATION_MODE_AMG = 0X07,
-    OPERATION_MODE_IMUPLUS = 0X08,
-    OPERATION_MODE_COMPASS = 0X09,
-    OPERATION_MODE_M4G = 0X0A,
-    OPERATION_MODE_NDOF_FMC_OFF = 0X0B,
-    OPERATION_MODE_NDOF = 0X0C
-} bno055_opmode_t;
-
-/** BNO055 Registers **/
-typedef enum {
-    /* Page id register definition */
-    BNO055_PAGE_ID_ADDR = 0X07,
-
-    /* PAGE0 REGISTER DEFINITION START*/
-    BNO055_CHIP_ID_ADDR = 0x00,
-    BNO055_ACCEL_REV_ID_ADDR = 0x01,
-    BNO055_MAG_REV_ID_ADDR = 0x02,
-    BNO055_GYRO_REV_ID_ADDR = 0x03,
-    BNO055_SW_REV_ID_LSB_ADDR = 0x04,
-    BNO055_SW_REV_ID_MSB_ADDR = 0x05,
-    BNO055_BL_REV_ID_ADDR = 0X06,
-
-    /* Accel data register */
-    BNO055_ACCEL_DATA_X_LSB_ADDR = 0X08,
-    BNO055_ACCEL_DATA_X_MSB_ADDR = 0X09,
-    BNO055_ACCEL_DATA_Y_LSB_ADDR = 0X0A,
-    BNO055_ACCEL_DATA_Y_MSB_ADDR = 0X0B,
-    BNO055_ACCEL_DATA_Z_LSB_ADDR = 0X0C,
-    BNO055_ACCEL_DATA_Z_MSB_ADDR = 0X0D,
-
-    /* Mag data register */
-    BNO055_MAG_DATA_X_LSB_ADDR = 0X0E,
-    BNO055_MAG_DATA_X_MSB_ADDR = 0X0F,
-    BNO055_MAG_DATA_Y_LSB_ADDR = 0X10,
-    BNO055_MAG_DATA_Y_MSB_ADDR = 0X11,
-    BNO055_MAG_DATA_Z_LSB_ADDR = 0X12,
-    BNO055_MAG_DATA_Z_MSB_ADDR = 0X13,
-
-    /* Gyro data registers */
-    BNO055_GYRO_DATA_X_LSB_ADDR = 0X14,
-    BNO055_GYRO_DATA_X_MSB_ADDR = 0X15,
-    BNO055_GYRO_DATA_Y_LSB_ADDR = 0X16,
-    BNO055_GYRO_DATA_Y_MSB_ADDR = 0X17,
-    BNO055_GYRO_DATA_Z_LSB_ADDR = 0X18,
-    BNO055_GYRO_DATA_Z_MSB_ADDR = 0X19,
-
-    /* Euler data registers */
-    BNO055_EULER_H_LSB_ADDR = 0X1A,
-    BNO055_EULER_H_MSB_ADDR = 0X1B,
-    BNO055_EULER_R_LSB_ADDR = 0X1C,
-    BNO055_EULER_R_MSB_ADDR = 0X1D,
-    BNO055_EULER_P_LSB_ADDR = 0X1E,
-    BNO055_EULER_P_MSB_ADDR = 0X1F,
-
-    /* Quaternion data registers */
-    BNO055_QUATERNION_DATA_W_LSB_ADDR = 0X20,
-    BNO055_QUATERNION_DATA_W_MSB_ADDR = 0X21,
-    BNO055_QUATERNION_DATA_X_LSB_ADDR = 0X22,
-    BNO055_QUATERNION_DATA_X_MSB_ADDR = 0X23,
-    BNO055_QUATERNION_DATA_Y_LSB_ADDR = 0X24,
-    BNO055_QUATERNION_DATA_Y_MSB_ADDR = 0X25,
-    BNO055_QUATERNION_DATA_Z_LSB_ADDR = 0X26,
-    BNO055_QUATERNION_DATA_Z_MSB_ADDR = 0X27,
-
-    /* Linear acceleration data registers */
-    BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR = 0X28,
-    BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR = 0X29,
-    BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR = 0X2A,
-    BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR = 0X2B,
-    BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR = 0X2C,
-    BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR = 0X2D,
-
-    /* Gravity data registers */
-    BNO055_GRAVITY_DATA_X_LSB_ADDR = 0X2E,
-    BNO055_GRAVITY_DATA_X_MSB_ADDR = 0X2F,
-    BNO055_GRAVITY_DATA_Y_LSB_ADDR = 0X30,
-    BNO055_GRAVITY_DATA_Y_MSB_ADDR = 0X31,
-    BNO055_GRAVITY_DATA_Z_LSB_ADDR = 0X32,
-    BNO055_GRAVITY_DATA_Z_MSB_ADDR = 0X33,
-
-    /* Temperature data register */
-    BNO055_TEMP_ADDR = 0X34,
-
-    /* Status registers */
-    BNO055_CALIB_STAT_ADDR = 0X35,
-    BNO055_SELFTEST_RESULT_ADDR = 0X36,
-    BNO055_INTR_STAT_ADDR = 0X37,
-
-    BNO055_SYS_CLK_STAT_ADDR = 0X38,
-    BNO055_SYS_STAT_ADDR = 0X39,
-    BNO055_SYS_ERR_ADDR = 0X3A,
-
-    /* Unit selection register */
-    BNO055_UNIT_SEL_ADDR = 0X3B,
-
-    /* Mode registers */
-    BNO055_OPR_MODE_ADDR = 0X3D,
-    BNO055_PWR_MODE_ADDR = 0X3E,
-
-    BNO055_SYS_TRIGGER_ADDR = 0X3F,
-    BNO055_TEMP_SOURCE_ADDR = 0X40,
-
-    /* Axis remap registers */
-    BNO055_AXIS_MAP_CONFIG_ADDR = 0X41,
-    BNO055_AXIS_MAP_SIGN_ADDR = 0X42,
-
-    /* SIC registers */
-    BNO055_SIC_MATRIX_0_LSB_ADDR = 0X43,
-    BNO055_SIC_MATRIX_0_MSB_ADDR = 0X44,
-    BNO055_SIC_MATRIX_1_LSB_ADDR = 0X45,
-    BNO055_SIC_MATRIX_1_MSB_ADDR = 0X46,
-    BNO055_SIC_MATRIX_2_LSB_ADDR = 0X47,
-    BNO055_SIC_MATRIX_2_MSB_ADDR = 0X48,
-    BNO055_SIC_MATRIX_3_LSB_ADDR = 0X49,
-    BNO055_SIC_MATRIX_3_MSB_ADDR = 0X4A,
-    BNO055_SIC_MATRIX_4_LSB_ADDR = 0X4B,
-    BNO055_SIC_MATRIX_4_MSB_ADDR = 0X4C,
-    BNO055_SIC_MATRIX_5_LSB_ADDR = 0X4D,
-    BNO055_SIC_MATRIX_5_MSB_ADDR = 0X4E,
-    BNO055_SIC_MATRIX_6_LSB_ADDR = 0X4F,
-    BNO055_SIC_MATRIX_6_MSB_ADDR = 0X50,
-    BNO055_SIC_MATRIX_7_LSB_ADDR = 0X51,
-    BNO055_SIC_MATRIX_7_MSB_ADDR = 0X52,
-    BNO055_SIC_MATRIX_8_LSB_ADDR = 0X53,
-    BNO055_SIC_MATRIX_8_MSB_ADDR = 0X54,
-
-    /* Accelerometer Offset registers */
-    ACCEL_OFFSET_X_LSB_ADDR = 0X55,
-    ACCEL_OFFSET_X_MSB_ADDR = 0X56,
-    ACCEL_OFFSET_Y_LSB_ADDR = 0X57,
-    ACCEL_OFFSET_Y_MSB_ADDR = 0X58,
-    ACCEL_OFFSET_Z_LSB_ADDR = 0X59,
-    ACCEL_OFFSET_Z_MSB_ADDR = 0X5A,
-
-    /* Magnetometer Offset registers */
-    MAG_OFFSET_X_LSB_ADDR = 0X5B,
-    MAG_OFFSET_X_MSB_ADDR = 0X5C,
-    MAG_OFFSET_Y_LSB_ADDR = 0X5D,
-    MAG_OFFSET_Y_MSB_ADDR = 0X5E,
-    MAG_OFFSET_Z_LSB_ADDR = 0X5F,
-    MAG_OFFSET_Z_MSB_ADDR = 0X60,
-
-    /* Gyroscope Offset register s*/
-    GYRO_OFFSET_X_LSB_ADDR = 0X61,
-    GYRO_OFFSET_X_MSB_ADDR = 0X62,
-    GYRO_OFFSET_Y_LSB_ADDR = 0X63,
-    GYRO_OFFSET_Y_MSB_ADDR = 0X64,
-    GYRO_OFFSET_Z_LSB_ADDR = 0X65,
-    GYRO_OFFSET_Z_MSB_ADDR = 0X66,
-
-    /* Radius registers */
-    ACCEL_RADIUS_LSB_ADDR = 0X67,
-    ACCEL_RADIUS_MSB_ADDR = 0X68,
-    MAG_RADIUS_LSB_ADDR = 0X69,
-    MAG_RADIUS_MSB_ADDR = 0X6A,
-
-    /* Interrupt registers*/
-    RST_INT = 0x01,
-
-    NO_MOTION_INT_ADDR = 0x00,
-    SLOW_NO_MOTION_INT_ADDR = 0x01,
-    THRESHOLD_INT_ADDR = 0x02,
-} bno055_reg_t;
-
-/** BNO055 power settings */
-typedef enum {
-    POWER_MODE_NORMAL = 0X00,
-    POWER_MODE_LOWPOWER = 0X01,
-    POWER_MODE_SUSPEND = 0X02
-} bno055_powermode_t;
-
-/** Vector Mappings **/
-typedef enum {
-    VECTOR_ACCELEROMETER = BNO055_ACCEL_DATA_X_LSB_ADDR,
-    VECTOR_MAGNETOMETER = BNO055_MAG_DATA_X_LSB_ADDR,
-    VECTOR_GYROSCOPE = BNO055_GYRO_DATA_X_LSB_ADDR,
-    VECTOR_EULER = BNO055_EULER_H_LSB_ADDR,
-    VECTOR_LINEARACCEL = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR,
-    VECTOR_GRAVITY = BNO055_GRAVITY_DATA_X_LSB_ADDR
-} vector_type_t;
-
-struct quarternion {
-    float w;
-    float x;
-    float y;
-    float z;
-};
-
-struct vector3f {
-    float x;
-    float y;
-    float z;
-};
-
-struct CALIB_STATUS {
-    uint8_t sys;
-    uint8_t gyro;
-    uint8_t accel;
-    uint8_t mag;
-};
-
-extern volatile vector3f linear_acceleration;
-extern volatile vector3f acceleration;
-extern volatile quarternion abs_quaternion;
-extern volatile CALIB_STATUS calib_status;
-
-extern volatile vector3f euler_angles;
-extern volatile vector3f abs_lin_accel;
-extern volatile vector3f rot_y_vec;
-extern volatile vector3f vel_at_angle;
-
-extern volatile vector3f accel_gravity;
-
-extern uint8_t accel[6];
-extern uint8_t quat[8];
-
-class BNO055 {
-    public:
-        BNO055();
-        //Sanity check for factory device ID 
-        void reset_bno055();
-        void init();
-        void read_lin_accel();
-        void read_abs_quaternion();
-        void read_euler_angles();
-        void read_accel();
-        void read_calib_status();
-        void calculate_abs_linear_acceleration();
-        void accel_to_gravity();
-        void quaternion_to_euler(); 
-        void get_rotation_vector();
-        void clamp_close_zero(volatile float &val);
-        
-    private:
-        unsigned char bno055_address;
-        int32_t _sensorID;
-        bno055_opmode_t default_mode;
-};
diff --git a/include/imu.hpp b/include/imu.hpp
new file mode 100644
index 0000000..a1b72ef
--- /dev/null
+++ b/include/imu.hpp
@@ -0,0 +1,207 @@
+#pragma once
+
+#include <stdint.h>
+#include "hardware/i2c.h"
+
+#include <Eigen/Dense>
+
+
+#define BNO055_NUM_OFFSET_REGISTERS 22
+
+
+typedef enum {
+    CONFIG = 0x00,
+    ACCELERATON_ONLY = 0x01,
+    MAGNETOMETER_ONLY = 0x02,
+    GYROSCOPE_ONLY = 0x03,
+    ACCEL_MAG = 0x04,
+    ACCEL_GYRO = 0x05,
+    MAG_GYRO = 0x06,
+    ACCEL_MAG_GYRO = 0x07,
+    IMU_PLUS = 0x08,
+    COMPASS = 0x09,
+    M4G = 0x0A,
+    NDOF_FMC_OFF = 0x0B,
+    NDOF = 0x0C
+} imu_opmode_t;
+
+typedef enum {
+    PAGE_ID = 0x07,
+
+    CHIP_ID = 0x00,
+    ACCEL_REV_ID = 0x01,
+    MAG_REV_ID = 0x02,
+    GYRO_REV_ID = 0x03,
+    SW_REV_ID_LSB = 0x04,
+    SW_REV_ID_MSB = 0x05,
+    BL_REV_ID = 0x06,
+
+    ACCELERATION_X_LSB = 0x08,
+    ACCELERATION_X_MSB = 0x09,
+    ACCELERATION_Y_LSB = 0x0A,
+    ACCELERATION_Y_MSB = 0x0B,
+    ACCELERATION_Z_LSB = 0x0C,
+    ACCELERATION_Z_MSB = 0x0D,
+
+    MAGNETOMETER_X_LSB = 0x0E,
+    MAGNETOMETER_X_MSB = 0x0F,
+    MAGNETOMETER_Y_LSB = 0x10,
+    MAGNETOMETER_Y_MSB = 0x11,
+    MAGNETOMETER_Z_LSB = 0x12,
+    MAGNETOMETER_Z_MSB = 0x13,
+
+    GYROSCOPE_X_LSB = 0x14,
+    GYROSCOPE_X_MSB = 0x15,
+    GYROSCOPE_Y_LSB = 0x16,
+    GYROSCOPE_Y_MSB = 0x17,
+    GYROSCOPE_Z_LSB = 0x18,
+    GYROSCOPE_Z_MSB = 0x19,
+
+    EULER_H_LSB = 0x1A,
+    EULER_H_MSB = 0x1B,
+    EULER_R_LSB = 0x1C,
+    EULER_R_MSB = 0x1D,
+    EULER_P_LSB = 0x1E,
+    EULER_P_MSB = 0x1F,
+
+    QUATERNION_W_LSB = 0x20,
+    QUATERNION_W_MSB = 0x21,
+    QUATERNION_X_LSB = 0x22,
+    QUATERNION_X_MSB = 0x23,
+    QUATERNION_Y_LSB = 0x24,
+    QUATERNION_Y_MSB = 0x25,
+    QUATERNION_Z_LSB = 0x26,
+    QUATERNION_Z_MSB = 0x27,
+
+    LINEAR_ACCELERATION_X_LSB = 0x28,
+    LINEAR_ACCELERATION_X_MSB = 0x29,
+    LINEAR_ACCELERATION_Y_LSB = 0x2A,
+    LINEAR_ACCELERATION_Y_MSB = 0x2B,
+    LINEAR_ACCELERATION_Z_LSB = 0x2C,
+    LINEAR_ACCELERATION_Z_MSB = 0x2D,
+
+    GRAVITY_X_LSB = 0x2E,
+    GRAVITY_X_MSB = 0x2F,
+    GRAVITY_Y_LSB = 0x30,
+    GRAVITY_Y_MSB = 0x31,
+    GRAVITY_Z_LSB = 0x32,
+    GRAVITY_Z_MSB = 0x33,
+
+    TEMPERATURE = 0x34,
+
+    CALIBRATION_STATUS = 0x35,
+    SELF_TEST_RESULT = 0x36,
+    INTERRUPT_STATUS = 0x37,
+
+    SYS_CLK_STATUS = 0x38,
+    SYS_STATUS = 0x39,
+    SYS_ERROR = 0x3A,
+
+    UNIT_SELECTION = 0x3B,
+
+    OPERATION_MODE = 0x3D,
+    POWER_MODE = 0x3E,
+
+    SYS_TRIGGER = 0x3F,
+    TEMP_SOURCE = 0x40,
+
+    AXIS_MAP_CONFIG = 0x41,
+    AXIS_MAP_SIGN = 0x42,
+
+    SIC_MATRIX_0_LSB = 0x43,
+    SIC_MATRIX_0_MSB = 0x44,
+    SIC_MATRIX_1_LSB = 0x45,
+    SIC_MATRIX_1_MSB = 0x46,
+    SIC_MATRIX_2_LSB = 0x47,
+    SIC_MATRIX_2_MSB = 0x48,
+    SIC_MATRIX_3_LSB = 0x49,
+    SIC_MATRIX_3_MSB = 0x4A,
+    SIC_MATRIX_4_LSB = 0x4B,
+    SIC_MATRIX_4_MSB = 0x4C,
+    SIC_MATRIX_5_LSB = 0x4D,
+    SIC_MATRIX_5_MSB = 0x4E,
+    SIC_MATRIX_6_LSB = 0x4F,
+    SIC_MATRIX_6_MSB = 0x50,
+    SIC_MATRIX_7_LSB = 0x51,
+    SIC_MATRIX_7_MSB = 0x52,
+    SIC_MATRIX_8_LSB = 0x53,
+    SIC_MATRIX_8_MSB = 0x54,
+
+    ACCELERATION_OFFSET_X_LSB = 0x55,
+    ACCELERATION_OFFSET_X_MSB = 0x56,
+    ACCELERATION_OFFSET_Y_LSB = 0x57,
+    ACCELERATION_OFFSET_Y_MSB = 0x58,
+    ACCELERATION_OFFSET_Z_LSB = 0x59,
+    ACCELERATION_OFFSET_Z_MSB = 0x5A,
+
+    MAGNETOMETER_OFFSET_X_LSB = 0x5B,
+    MAGNETOMETER_OFFSET_X_MSB = 0x5C,
+    MAGNETOMETER_OFFSET_Y_LSB = 0x5D,
+    MAGNETOMETER_OFFSET_Y_MSB = 0x5E,
+    MAGNETOMETER_OFFSET_Z_LSB = 0x5F,
+    MAGNETOMETER_OFFSET_Z_MSB = 0x60,
+
+    GYROSCOPE_OFFSET_X_LSB = 0x61,
+    GYROSCOPE_OFFSET_X_MSB = 0x62,
+    GYROSCOPE_OFFSET_Y_LSB = 0x63,
+    GYROSCOPE_OFFSET_Y_MSB = 0x64,
+    GYROSCOPE_OFFSET_Z_LSB = 0x65,
+    GYROSCOPE_OFFSET_Z_MSB = 0x66,
+
+    ACCEL_RADIUS_LSB = 0x67,
+    ACCEL_RADIUS_MSB = 0x68,
+    MAG_RADIUS_LSB = 0x69,
+    MAG_RADIUS_MSB = 0x6A,
+
+    RESET_INTERRUPT = 0x01,
+
+    NO_MOTION_INTERRUPT = 0x00,
+    SLOW_NO_MOTION_INTERRUPT = 0x01,
+    THRESHOLD_INTERRUPT = 0x02,
+} imu_reg_t;
+
+typedef enum {
+    NORMAL = 0x00,
+    LOW_POWER = 0x01,
+    SUSPEND = 0x02
+} imu_power_mode_t;
+
+typedef struct {
+    uint8_t sys;
+    uint8_t gyro;
+    uint8_t accel;
+    uint8_t mag;
+} calibration_status_t ;
+
+class imu {
+    private:
+        i2c_inst_t* inst;
+        uint8_t addr;
+        uint8_t id;
+        imu_opmode_t mode;
+
+        uint8_t buffer[10];
+        uint8_t accel_buffer[6];
+        uint8_t quat_buffer[8];
+
+        void read_register(uint8_t reg, size_t len, uint8_t* buffer);
+
+    public:
+        imu(i2c_inst_t* inst, uint8_t addr, uint8_t id, imu_opmode_t mode);
+
+        void initialize();
+
+        void reset();
+
+        void linear_acceleration(Eigen::Vector3f& vec);
+
+        void quaternion(Eigen::Vector4f& vec);
+
+        void quaternion_euler(Eigen::Vector3f& angles, Eigen::Vector4f& quat);
+
+        void calibration_status(calibration_status_t* status);
+
+        uint32_t expose_acceleration_buffer(uint8_t** buffer);
+
+        uint32_t expose_quaternion_buffer(uint8_t** buffer);
+};
diff --git a/include/kalmanfilter.hpp b/include/kalman_filter.hpp
index 8b9e942..26d46cc 100644
--- a/include/kalmanfilter.hpp
+++ b/include/kalman_filter.hpp
@@ -1,11 +1,9 @@
 #pragma once
 #include <Eigen/Dense>
-#include <iostream>
-#include <cmath>
 
 using namespace Eigen;
 
-class KalmanFilter {
+class kalman_filter {
 
     private:
 
@@ -25,20 +23,20 @@ class KalmanFilter {
         int p; // Control Vector Dimension
         int m; // Measurement Vector Dimension
 
-        double dt; // timestep
+        float dt; // timestep
 
         /**
          * @brief Initialize all necessary matrices.
          * 
          */
-        void matrixInit();
+        void matrix_initialize();
 
         /**
          * @brief Update any existing variable elements in your State Transition 
          * & Control Input matrices.
          * 
          */
-        void updateMatrices();
+        void matrix_update();
 
         /**
          * @brief Predict current State Vector & State Covariance 
@@ -47,7 +45,7 @@ class KalmanFilter {
          * @param control_vec The control input to be applied to the
          * previous State Vector
          */
-        void prediction(VectorXf control_vec);
+        void predict(VectorXf control_vec);
 
         /**
          * @brief Correct the State Vector & State Covariance predictions
@@ -59,7 +57,7 @@ class KalmanFilter {
 
     public:
 
-        KalmanFilter();
+        kalman_filter();
         
         /**
          * @brief Construct a new Kalman Filter object
@@ -70,7 +68,7 @@ class KalmanFilter {
          * @param measurement_dim Measurement Vector Dimension. i.e. dim(z)
          * @param dt timestep
          */
-        KalmanFilter(int state_dim, int control_dim, int measurement_dim, double dt);
+        kalman_filter(int state_dim, int control_dim, int measurement_dim, float dt);
 
 
         /**
@@ -82,7 +80,7 @@ class KalmanFilter {
          * 
          * @return Whether state initialization was successful
          */
-        bool setInitialState(VectorXf state_vec, MatrixXf state_cov);
+        bool state_initialize(VectorXf state_vec, MatrixXf state_cov);
 
         /**
          * @brief Perform Kalman Filter operation with given control input vector
@@ -94,5 +92,5 @@ class KalmanFilter {
          * 
          * @return Filtered state vector
          */
-        VectorXf run(VectorXf control, VectorXf measurement, double _dt);
-};
-\ No newline at end of file
+        VectorXf run(VectorXf control, VectorXf measurement, float _dt);
+};
diff --git a/include/logger.hpp b/include/logger.hpp
deleted file mode 100644
index 90fa05a..0000000
--- a/include/logger.hpp
+++ /dev/null
@@ -1,83 +0,0 @@
-#pragma once
-#include <iostream>
-#include <fstream>
-#include <ctime>
-
-class Logger {
-
-    private:
-        std::fstream file;
-        std::string filename;
-        time_t t;
-        tm* now;
-        bool file_open;
-
-        std::string infoTag = "-> [INFO]: ";
-        std::string errorTag = "-> [ERROR]: ";
-        std::string months[12] = {"Jan", "Feb", "Mar", "Apr", "May", "Jun", "Jul", "Aug", "Sept", "Oct", "Nov", "Dec"};
-        std::string days[7] = {"Sun", "Mon", "Tues", "Wed", "Thur", "Frid", "Sat"};
-
-        /**
-         * @brief Create formatted current-date tag 
-         * 
-         * @return string 
-         */
-        std::string getDate();
-
-        /**
-         * @brief Create formatted current-time tag 
-         * 
-         * @return string 
-         */
-        std::string getTime();
-    
-
-    public:
-        
-        /**
-         * @brief 
-         * 
-         */
-        static Logger& Get();
-
-        /**
-         * @brief Open a Log File for writing
-         * 
-         * @param _filename Name of file to open
-         * @return true Successful Open
-         * @return false Unsuccessful Open
-         */
-        bool openLog(std::string _filename);
-
-        /**
-         * @brief Close the Log File
-         * 
-         */
-        void closeLog();
-
-        /**
-         * @brief Write data to Log file
-         * 
-         * @param data Data to log
-         * @return true Data Successfully Logged
-         * @return false Data Logging Unsuccessful
-         */
-        bool log(std::string data);
-
-        /**
-         * @brief Write error data to Log file
-         * 
-         * @param data Error Data to log
-         * @return true Data Successfully Logged
-         * @return false Data Logging Unsuccessful
-         */
-        bool logErr(std::string data);
-
-        /**
-         * @brief Print Log Data to Terminal
-         * 
-         * @return true Successful Print
-         * @return false Unsuccessful Print
-         */
-        bool printLog();
-};
-\ No newline at end of file
diff --git a/include/math/imumath.h b/include/math/imumath.h
deleted file mode 100644
index 831df60..0000000
--- a/include/math/imumath.h
+++ /dev/null
@@ -1,30 +0,0 @@
-//  Inertial Measurement Unit Maths Library
-//
-//  Copyright 2013-2021 Sam Cowen <[email protected]>
-//
-//  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/include/math/matrix.h b/include/math/matrix.h
deleted file mode 100644
index e71b2db..0000000
--- a/include/math/matrix.h
+++ /dev/null
@@ -1,185 +0,0 @@
-//  Inertial Measurement Unit Maths Library
-//
-//  Copyright 2013-2021 Sam Cowen <[email protected]>
-//  Bug fixes and cleanups by Gé Vissers ([email protected])
-//
-//  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/include/math/quaternion.h b/include/math/quaternion.h
deleted file mode 100644
index c5b907a..0000000
--- a/include/math/quaternion.h
+++ /dev/null
@@ -1,214 +0,0 @@
-//  Inertial Measurement Unit Maths Library
-//
-//  Copyright 2013-2021 Sam Cowen <[email protected]>
-//  Bug fixes and cleanups by Gé Vissers ([email protected])
-//
-//  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/include/math/vector.h b/include/math/vector.h
deleted file mode 100644
index 10345c3..0000000
--- a/include/math/vector.h
+++ /dev/null
@@ -1,184 +0,0 @@
-//  Inertial Measurement Unit Maths Library
-//
-//  Copyright 2013-2021 Sam Cowen <[email protected]>
-//  Bug fixes and cleanups by Gé Vissers ([email protected])
-//
-//  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/include/motor.hpp b/include/motor.hpp
deleted file mode 100644
index cb95b6a..0000000
--- a/include/motor.hpp
+++ /dev/null
@@ -1,36 +0,0 @@
-#pragma once
-#include "actuator.hpp"
-#include "logger.hpp"
-#include "rocketUtils.hpp"
-
-class Motor : public Actuator {
-
-    private:
-
-
-    public:
-
-        /**
-         * @brief Construct a new Motor object
-         * 
-         */
-        Motor();
-
-        /**
-         * @brief Initialize the motor. 
-         * 
-         * @param data Data for initializing the motor
-         * @return true Initialization Success
-         * @return false Initialization Failure
-         */
-        virtual bool init(void* data) override;
-
-        /**
-         * @brief Write data to the motor.
-         * 
-         * @param data Data to be writen to the motor
-         * @return true Motor write Success
-         * @return false Motor write Failure
-         */
-        virtual bool writeData(void* data) override;
-};
-\ No newline at end of file
diff --git a/include/rocketUtils.hpp b/include/rocketUtils.hpp
deleted file mode 100644
index 65b9cea..0000000
--- a/include/rocketUtils.hpp
+++ /dev/null
@@ -1,121 +0,0 @@
-#pragma once
-#include <iostream>
-#include <vector>
-#include <ctime>
-#include <vector>
-#include <iomanip>
-#include <sstream>
-#include <fstream>
-
-// Deployment angle limits
-#define MAX_ANGLE 100
-#define MIN_ANGLE 120
-
-// Altimeter initialization count limit
-#define COUNT_LIMIT 50
-
-// Constants
-#define G_0 9.8066
-
-// Threshold limits
-#define BOOST_ACCEL_THRESH 3
-#define BOOST_HEIGHT_THRESH 20
-#define GLIDE_ACCEL_THRESH 0.5
-
-#define ALTI_DEPL_THRESHOLD 0.5
-#define RATE_ALTI_DEPL 1/50
-#define FSM_DONE_SURFACE_ALTITUDE 200
-#define APOGEE_FSM_CHANGE 3
-
-#define INIT_DEPLOYMENT 0 
-
-#define TIME_BO 8
-#define TIME_APO 25
-#define TIME_END 120
-
-#define PAD_PRESSURE 102250
-
-#define DUTY_MAX 14.5
-#define DUTY_MIN 3
-
-#define LAUNCH_DATE "4-15-2023"
-#define LOG_FILENAME "DataLog_" LAUNCH_DATE ".txt"
-
-#define LED_GAP_TIME 0.5
-#define LED_ONE_PATH "/sys/class/leds/beaglebone:green:usr1"
-#define LED_BRIGHTNESS_FILE "brightness"
-#define LED_FILENAME LED_ONE_PATH LED_BRIGHTNESS_FILE
-
-#define TEST_MODE false
-
-
-enum VehicleState {ON_PAD, BOOST, GLIDE, APOGEE, DONE};
-extern std::string state_for_log[5];
-
-struct Vehicle {
-
-	int status;
-
-	std::vector<double> acceleration; 
-    std::vector<double> linear_acceleration;
-
-    double apogee_altitude;
-	double previous_altitude;
-	double current_altitude;
-	double filtered_altitude;
-
-	double filtered_velocity;
-
-	double deployment_angle;
-
-	bool imuInitFail;
-	bool imuReadFail;
-	bool altiInitFail;
-	bool altiReadFail;
-
-	double ON_PAD_altitude;
-	bool ON_PAD_fail;
-
-	double duty_span;
-
-	double dt;
-
-	int led_brightness;
-
-	time_t start_time;
-	time_t fail_time; // For failure termination
-	time_t liftoff_time;
-	time_t relog_time;
-	time_t deploy_time; // NOT INITIALIZED YET
-	time_t loop_time;
-	time_t led_time;
-};
-
-/**
- * @brief Convert fin deployment percentage to fin rotation angle
- * 
- * @param percentage Fin deployment percentage
- * @return double 
- */
-double deploy_percentage_to_angle(double percentage);
-
-/**
- * @brief Set the decimal precision of the given data, and return it 
- * 		as a formatted string with a prefix containing a relevant description of the data. 
- * 
- * @param prefix Identifying or clarifying information about the loggef data
- * @param data Data to Log
- * @param precision The decimal precision value for the data
- * 
- * @return A string with the formatted data.
- */
-std::string format_data(std::string prefix, double data, int precision);
-
-/**
- * @brief Blink Beaglebone LED 1
- * 
- * @param vehicle Holds settings pertinent to the Beaglebone LED
- * @return true Successful Blink
- * @return false Unsuccessful Blink
- */
-bool led_out(Vehicle *vehicle);
diff --git a/include/sensorAltimeter.hpp b/include/sensorAltimeter.hpp
deleted file mode 100644
index aa3a3cd..0000000
--- a/include/sensorAltimeter.hpp
+++ /dev/null
@@ -1,165 +0,0 @@
-#pragma once
-#include "sensorI2C.hpp"
-#include <string>
-
-/*
-Header file for driver for MPL3115a2 breakout, based heavily on https://github.com/adafruit/Adafruit_MPL3115A2_Library/tree/master
-Based on register declarations found https://cdn-shop.adafruit.com/datasheets/1893_datasheet.pdf
-Designed using subclass for I2C handler for Beaglebone Black
-*/
-
-class AltimeterSensor : public sensorI2C {
-
-
-    private:
-        double internalTemperature = 0;
-        double internalAltitude = 0;
-        
-        //ENUMS COPIED DIRECTLY FROM ADAFRUIT IMPLEMENTATION
-        /** MPL3115A2 registers **/
-        enum {
-            MPL3115A2_REGISTER_STATUS = (0x00),
-
-            MPL3115A2_REGISTER_PRESSURE_MSB = (0x01),
-            MPL3115A2_REGISTER_PRESSURE_CSB = (0x02),
-            MPL3115A2_REGISTER_PRESSURE_LSB = (0x03),
-
-            MPL3115A2_REGISTER_TEMP_MSB = (0x04),
-            MPL3115A2_REGISTER_TEMP_LSB = (0x05),
-
-            MPL3115A2_REGISTER_DR_STATUS = (0x06),
-
-            MPL3115A2_OUT_P_DELTA_MSB = (0x07),
-            MPL3115A2_OUT_P_DELTA_CSB = (0x08),
-            MPL3115A2_OUT_P_DELTA_LSB = (0x09),
-
-            MPL3115A2_OUT_T_DELTA_MSB = (0x0A),
-            MPL3115A2_OUT_T_DELTA_LSB = (0x0B),
-
-            MPL3115A2_WHOAMI = (0x0C),
-            //This is hard-coded in the device from the factory
-            MPL3115A2_WHOAMI_EXPECTED = (0xC4),
-
-            MPL3115A2_BAR_IN_MSB = (0x14),
-            MPL3115A2_BAR_IN_LSB = (0x15),
-
-            MPL3115A2_OFF_H = (0x2D),
-        };
-
-        /** MPL3115A2 status register BITS **/
-        enum {
-            MPL3115A2_REGISTER_STATUS_TDR = 0x02,
-            MPL3115A2_REGISTER_STATUS_PDR = 0x04,
-            MPL3115A2_REGISTER_STATUS_PTDR = 0x08,
-        };
-
-        /** MPL3115A2 PT DATA register BITS **/
-        enum {
-            MPL3115A2_PT_DATA_CFG = 0x13,
-            MPL3115A2_PT_DATA_CFG_TDEFE = 0x01,
-            MPL3115A2_PT_DATA_CFG_PDEFE = 0x02,
-            MPL3115A2_PT_DATA_CFG_DREM = 0x04,
-        };
-
-        /** MPL3115A2 control registers **/
-        enum {
-            MPL3115A2_CTRL_REG1 = (0x26),
-            MPL3115A2_CTRL_REG2 = (0x27),
-            MPL3115A2_CTRL_REG3 = (0x28),
-            MPL3115A2_CTRL_REG4 = (0x29),
-            MPL3115A2_CTRL_REG5 = (0x2A),
-        };
-
-        /** MPL3115A2 control register BITS **/
-        enum {
-            MPL3115A2_CTRL_REG1_SBYB = 0x01,
-            MPL3115A2_CTRL_REG1_OST = 0x02,
-            MPL3115A2_CTRL_REG1_RST = 0x04,
-            MPL3115A2_CTRL_REG1_RAW = 0x40,
-            MPL3115A2_CTRL_REG1_ALT = 0x80,
-            MPL3115A2_CTRL_REG1_BAR = 0x00,
-        };
-
-        /** MPL3115A2 oversample values **/
-        enum {
-            MPL3115A2_CTRL_REG1_OS1 = 0x00,
-            MPL3115A2_CTRL_REG1_OS2 = 0x08,
-            MPL3115A2_CTRL_REG1_OS4 = 0x10,
-            MPL3115A2_CTRL_REG1_OS8 = 0x18,
-            MPL3115A2_CTRL_REG1_OS16 = 0x20,
-            MPL3115A2_CTRL_REG1_OS32 = 0x28,
-            MPL3115A2_CTRL_REG1_OS64 = 0x30,
-            MPL3115A2_CTRL_REG1_OS128 = 0x38,
-        };
-
-        /** MPL3115A2 measurement modes **/
-        typedef enum {
-            MPL3115A2_BAROMETER = 0,
-            MPL3115A2_ALTIMETER,
-        } mpl3115a2_mode_t;
-
-        /** MPL3115A2 measurement types **/
-        typedef enum {
-            MPL3115A2_PRESSURE,
-            MPL3115A2_ALTITUDE,
-            MPL3115A2_TEMPERATURE,
-        } mpl3115a2_meas_t;
-
-        //This never actually gets used, and I can't find anything in the datasheet about it??
-        #define MPL3115A2_REGISTER_STARTCONVERSION (0x12) ///< start conversion
-
-        //Store current operating mode, sent to device during startup procedure
-        //This is why an enum is used rather than raw #define statements
-        mpl3115a2_mode_t currentMode;
-
-        //Struct for storing ctrl register contents, copied from adafruit implementation
-        typedef union {
-            struct {
-                uint8_t SBYB : 1;
-                uint8_t OST : 1;
-                uint8_t RST : 1;
-                uint8_t OS : 3;
-                uint8_t RAW : 1;
-                uint8_t ALT : 1;
-            } bit;
-            uint8_t reg;
-        } CTRL_REG_1_STRUCT;
-        //Create instance of this register config to use during device startup and operation
-        CTRL_REG_1_STRUCT ctrl_reg1;
-
-    public:
-
-        /**
-         * @brief Construct a new Altimeter Sensor object
-         * 
-         */
-        AltimeterSensor(std::string I2C_FILE);
-
-        /**
-         * @brief Initialize the Altimeter
-         * 
-         * @param data Data for initializing the sensor
-         * @return true Initialization Success
-         * @return false Initialization Failure
-         */
-        bool init() override;
-
-        //Data getters and setters
-        // double getPressure();
-        double getAltitude();
-        double getTemperature();
-        double setSeaLevelPressure(double pressure);
-
-        //Data and mode handlers
-        //Use altimeter mode by default as this is what rocket logger wants
-        void setMode(mpl3115a2_mode_t mode = MPL3115A2_ALTIMETER);
-        void requestOneShotReading();
-        bool isNewDataAvailable();
-        void updateCurrentDataBuffer();
-
-
-};
-
-
-
-
diff --git a/include/sensorI2C.hpp b/include/sensorI2C.hpp
deleted file mode 100644
index bd497a5..0000000
--- a/include/sensorI2C.hpp
+++ /dev/null
@@ -1,105 +0,0 @@
-#pragma once
-
-#include <stdio.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <sys/ioctl.h>
-#include <linux/i2c.h>
-#include <linux/i2c-dev.h>
-#include <cstdint>
-#include <string>
-
-class sensorI2C {
-    
-
-    //Predominantly used for I2C handler functions; implement high-level functions in sensor classes
-    //Implemented as a combination of Jazz' implementation and Derek Malloy's code
-    public:
-        
-        //Initial single byte write, used at beginning of read operation
-        //Returns 0 if successful, -1 if not
-        int initialWrite(unsigned char registerAddress) {
-            unsigned char *convertedAddressBuffer = new unsigned char[1];
-            convertedAddressBuffer[0] = registerAddress;
-            //Expect 1 byte response from 1 byte write
-            if (write(i2c_bus, convertedAddressBuffer, 1) != 1) {
-                fprintf(stderr, "ERROR DOING INITIAL READ TRANSACTION WRITE TO REGISTER %x FOR DEVICE %x\n", registerAddress, deviceAddress);
-                return 0;
-            }
-            return 1;
-        }
-
-        int writeRegister(unsigned char registerAddress, unsigned char value) {
-            //initialWrite() not used here because it's easier to just pack it into one buffer for file writing
-            unsigned char writeBuffer[2];
-            writeBuffer[0] = registerAddress;
-            writeBuffer[1] = value;
-
-            //Expect 2 byte output
-            if (write(i2c_bus, writeBuffer, 2) != 2) {
-                //These error messages are kind of obtuse but I'd rather have too much information than not enough
-                fprintf(stderr, "ERROR WRITING %x TO REGISTER %x ON DEVICE %x\n", value, registerAddress, deviceAddress);
-                return -1;
-            }
-            return 0;
-        }
-
-        //Could probably be uint8_t but Derek Malloy does it with unsigned chars and that's what worked during testing so I don't want to touch it
-        unsigned char readSingleRegister(unsigned char registerAddress) {
-            printf("reg addr: %X\n", registerAddress);
-
-            initialWrite(registerAddress);
-            unsigned char* readBuffer = new unsigned char[1];
-            if (read(i2c_bus, readBuffer, 1) != 1){
-                fprintf(stderr, "FAILED TO READ VALUE FROM REGISTER %x ON DEVICE %x\n", registerAddress, deviceAddress);
-                return -1;
-            }
-            printf("readBuffer: %X\n", readBuffer[0]);
-            return readBuffer[0];
-        }
-
-        unsigned char* readMultipleRegisters(unsigned char startingRegisterAddress, int numberOfRegisters) {            
-            initialWrite(startingRegisterAddress);
-            unsigned char* readBuffer = new unsigned char[numberOfRegisters];
-            if (read(i2c_bus, readBuffer, numberOfRegisters) != numberOfRegisters) {
-                fprintf(stderr, "ERROR TRYING TO READ %d REGISTERS STARTING AT ADDRESS %x ON DEVICE %x\n", 
-                    numberOfRegisters, startingRegisterAddress, deviceAddress);
-            }
-            return readBuffer;
-        }
-
-
-        //Intakes device address and file 
-        //Private because IT'S ASSUMED PROGRAMMER WILL CALL THIS METHOD DURING INIT() ROUTINE
-        int setupI2C(std::string I2C_FILE) {
-            // Open i2c driver file
-            i2c_bus = open("/dev/i2c-2", O_RDWR);
-            if(i2c_bus < 0){
-                fprintf(stderr, "FAILED TO OPEN I2C BUS USING FILE %s\n", "/dev/i2c-2");
-                close(i2c_bus);
-                return -1;
-            }
-
-            // Identify slave device address (MODIFIED FROM INITIAL IMPLEMENTATION, USES INTERNAL deviceAddress INSTEAD OF PARAMETER)
-            if(ioctl(i2c_bus, I2C_SLAVE, deviceAddress) < 0){
-                fprintf(stderr, "FAILED TO CONNECT TO DEVICE AT ADDRESS %x VIA I2C\n", deviceAddress);
-                close(i2c_bus);
-                return -1;
-            }
-            return 0;
-        }
-
-        /**
-         * @brief Initialize the sensor. 
-         * 
-         * @param data Data for initializing the sensor
-         * 
-         * @return true Initialization Success
-         * @return false Initialization Failure
-         */
-        virtual bool init() = 0;
-
-        unsigned char deviceAddress;
-        int i2c_bus;
-        std::string I2C_FILE;
-};
diff --git a/include/sensorIMU.hpp b/include/sensorIMU.hpp
deleted file mode 100644
index caddbe4..0000000
--- a/include/sensorIMU.hpp
+++ /dev/null
@@ -1,312 +0,0 @@
-#pragma once
-#include <vector>
-#include "sensorI2C.hpp"
-#include "logger.hpp"
-#include "rocketUtils.hpp"
-#include "imumath.h"
-#include <string>
-
-struct IMUData {
-    std::vector<double> acceleration[3];
-    std::vector<double> linear_acceleration[3];
-};
-
-//Register addresses and data structs copied from Adafruit implementation
-/** BNO055 Address A **/
-#define BNO055_ADDRESS_A (0x28)
-/** BNO055 Address B **/
-#define BNO055_ADDRESS_B (0x29)
-/** BNO055 ID **/
-#define BNO055_ID (0xA0)
-
-/** Offsets registers **/
-#define NUM_BNO055_OFFSET_REGISTERS (22)
-
-/** A structure to represent offsets **/
-typedef struct {
-    int16_t accel_offset_x; /**< x acceleration offset */
-    int16_t accel_offset_y; /**< y acceleration offset */
-    int16_t accel_offset_z; /**< z acceleration offset */
-
-    int16_t mag_offset_x; /**< x magnetometer offset */
-    int16_t mag_offset_y; /**< y magnetometer offset */
-    int16_t mag_offset_z; /**< z magnetometer offset */
-
-    int16_t gyro_offset_x; /**< x gyroscrope offset */
-    int16_t gyro_offset_y; /**< y gyroscrope offset */
-    int16_t gyro_offset_z; /**< z gyroscrope offset */
-
-    int16_t accel_radius; /**< acceleration radius */
-
-    int16_t mag_radius; /**< magnetometer radius */
-} adafruit_bno055_offsets_t;
-
-/** Operation mode settings **/
-typedef enum {
-    OPERATION_MODE_CONFIG = 0X00,
-    OPERATION_MODE_ACCONLY = 0X01,
-    OPERATION_MODE_MAGONLY = 0X02,
-    OPERATION_MODE_GYRONLY = 0X03,
-    OPERATION_MODE_ACCMAG = 0X04,
-    OPERATION_MODE_ACCGYRO = 0X05,
-    OPERATION_MODE_MAGGYRO = 0X06,
-    OPERATION_MODE_AMG = 0X07,
-    OPERATION_MODE_IMUPLUS = 0X08,
-    OPERATION_MODE_COMPASS = 0X09,
-    OPERATION_MODE_M4G = 0X0A,
-    OPERATION_MODE_NDOF_FMC_OFF = 0X0B,
-    OPERATION_MODE_NDOF = 0X0C
-} adafruit_bno055_opmode_t;
-class IMUSensor : public sensorI2C {
-
-    private:
-        deviceAddress = BNO055_ADDRESS_A;
-
-        adafruit_bno055_opmode_t default_mode = OPERATION_MODE_NDOF;
-        /** BNO055 Registers **/
-        typedef enum {
-            /* Page id register definition */
-            BNO055_PAGE_ID_ADDR = 0X07,
-
-            /* PAGE0 REGISTER DEFINITION START*/
-            BNO055_CHIP_ID_ADDR = 0x00,
-            BNO055_ACCEL_REV_ID_ADDR = 0x01,
-            BNO055_MAG_REV_ID_ADDR = 0x02,
-            BNO055_GYRO_REV_ID_ADDR = 0x03,
-            BNO055_SW_REV_ID_LSB_ADDR = 0x04,
-            BNO055_SW_REV_ID_MSB_ADDR = 0x05,
-            BNO055_BL_REV_ID_ADDR = 0X06,
-
-            /* Accel data register */
-            BNO055_ACCEL_DATA_X_LSB_ADDR = 0X08,
-            BNO055_ACCEL_DATA_X_MSB_ADDR = 0X09,
-            BNO055_ACCEL_DATA_Y_LSB_ADDR = 0X0A,
-            BNO055_ACCEL_DATA_Y_MSB_ADDR = 0X0B,
-            BNO055_ACCEL_DATA_Z_LSB_ADDR = 0X0C,
-            BNO055_ACCEL_DATA_Z_MSB_ADDR = 0X0D,
-
-            /* Mag data register */
-            BNO055_MAG_DATA_X_LSB_ADDR = 0X0E,
-            BNO055_MAG_DATA_X_MSB_ADDR = 0X0F,
-            BNO055_MAG_DATA_Y_LSB_ADDR = 0X10,
-            BNO055_MAG_DATA_Y_MSB_ADDR = 0X11,
-            BNO055_MAG_DATA_Z_LSB_ADDR = 0X12,
-            BNO055_MAG_DATA_Z_MSB_ADDR = 0X13,
-
-            /* Gyro data registers */
-            BNO055_GYRO_DATA_X_LSB_ADDR = 0X14,
-            BNO055_GYRO_DATA_X_MSB_ADDR = 0X15,
-            BNO055_GYRO_DATA_Y_LSB_ADDR = 0X16,
-            BNO055_GYRO_DATA_Y_MSB_ADDR = 0X17,
-            BNO055_GYRO_DATA_Z_LSB_ADDR = 0X18,
-            BNO055_GYRO_DATA_Z_MSB_ADDR = 0X19,
-
-            /* Euler data registers */
-            BNO055_EULER_H_LSB_ADDR = 0X1A,
-            BNO055_EULER_H_MSB_ADDR = 0X1B,
-            BNO055_EULER_R_LSB_ADDR = 0X1C,
-            BNO055_EULER_R_MSB_ADDR = 0X1D,
-            BNO055_EULER_P_LSB_ADDR = 0X1E,
-            BNO055_EULER_P_MSB_ADDR = 0X1F,
-
-            /* Quaternion data registers */
-            BNO055_QUATERNION_DATA_W_LSB_ADDR = 0X20,
-            BNO055_QUATERNION_DATA_W_MSB_ADDR = 0X21,
-            BNO055_QUATERNION_DATA_X_LSB_ADDR = 0X22,
-            BNO055_QUATERNION_DATA_X_MSB_ADDR = 0X23,
-            BNO055_QUATERNION_DATA_Y_LSB_ADDR = 0X24,
-            BNO055_QUATERNION_DATA_Y_MSB_ADDR = 0X25,
-            BNO055_QUATERNION_DATA_Z_LSB_ADDR = 0X26,
-            BNO055_QUATERNION_DATA_Z_MSB_ADDR = 0X27,
-
-            /* Linear acceleration data registers */
-            BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR = 0X28,
-            BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR = 0X29,
-            BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR = 0X2A,
-            BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR = 0X2B,
-            BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR = 0X2C,
-            BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR = 0X2D,
-
-            /* Gravity data registers */
-            BNO055_GRAVITY_DATA_X_LSB_ADDR = 0X2E,
-            BNO055_GRAVITY_DATA_X_MSB_ADDR = 0X2F,
-            BNO055_GRAVITY_DATA_Y_LSB_ADDR = 0X30,
-            BNO055_GRAVITY_DATA_Y_MSB_ADDR = 0X31,
-            BNO055_GRAVITY_DATA_Z_LSB_ADDR = 0X32,
-            BNO055_GRAVITY_DATA_Z_MSB_ADDR = 0X33,
-
-            /* Temperature data register */
-            BNO055_TEMP_ADDR = 0X34,
-
-            /* Status registers */
-            BNO055_CALIB_STAT_ADDR = 0X35,
-            BNO055_SELFTEST_RESULT_ADDR = 0X36,
-            BNO055_INTR_STAT_ADDR = 0X37,
-
-            BNO055_SYS_CLK_STAT_ADDR = 0X38,
-            BNO055_SYS_STAT_ADDR = 0X39,
-            BNO055_SYS_ERR_ADDR = 0X3A,
-
-            /* Unit selection register */
-            BNO055_UNIT_SEL_ADDR = 0X3B,
-
-            /* Mode registers */
-            BNO055_OPR_MODE_ADDR = 0X3D,
-            BNO055_PWR_MODE_ADDR = 0X3E,
-
-            BNO055_SYS_TRIGGER_ADDR = 0X3F,
-            BNO055_TEMP_SOURCE_ADDR = 0X40,
-
-            /* Axis remap registers */
-            BNO055_AXIS_MAP_CONFIG_ADDR = 0X41,
-            BNO055_AXIS_MAP_SIGN_ADDR = 0X42,
-
-            /* SIC registers */
-            BNO055_SIC_MATRIX_0_LSB_ADDR = 0X43,
-            BNO055_SIC_MATRIX_0_MSB_ADDR = 0X44,
-            BNO055_SIC_MATRIX_1_LSB_ADDR = 0X45,
-            BNO055_SIC_MATRIX_1_MSB_ADDR = 0X46,
-            BNO055_SIC_MATRIX_2_LSB_ADDR = 0X47,
-            BNO055_SIC_MATRIX_2_MSB_ADDR = 0X48,
-            BNO055_SIC_MATRIX_3_LSB_ADDR = 0X49,
-            BNO055_SIC_MATRIX_3_MSB_ADDR = 0X4A,
-            BNO055_SIC_MATRIX_4_LSB_ADDR = 0X4B,
-            BNO055_SIC_MATRIX_4_MSB_ADDR = 0X4C,
-            BNO055_SIC_MATRIX_5_LSB_ADDR = 0X4D,
-            BNO055_SIC_MATRIX_5_MSB_ADDR = 0X4E,
-            BNO055_SIC_MATRIX_6_LSB_ADDR = 0X4F,
-            BNO055_SIC_MATRIX_6_MSB_ADDR = 0X50,
-            BNO055_SIC_MATRIX_7_LSB_ADDR = 0X51,
-            BNO055_SIC_MATRIX_7_MSB_ADDR = 0X52,
-            BNO055_SIC_MATRIX_8_LSB_ADDR = 0X53,
-            BNO055_SIC_MATRIX_8_MSB_ADDR = 0X54,
-
-            /* Accelerometer Offset registers */
-            ACCEL_OFFSET_X_LSB_ADDR = 0X55,
-            ACCEL_OFFSET_X_MSB_ADDR = 0X56,
-            ACCEL_OFFSET_Y_LSB_ADDR = 0X57,
-            ACCEL_OFFSET_Y_MSB_ADDR = 0X58,
-            ACCEL_OFFSET_Z_LSB_ADDR = 0X59,
-            ACCEL_OFFSET_Z_MSB_ADDR = 0X5A,
-
-            /* Magnetometer Offset registers */
-            MAG_OFFSET_X_LSB_ADDR = 0X5B,
-            MAG_OFFSET_X_MSB_ADDR = 0X5C,
-            MAG_OFFSET_Y_LSB_ADDR = 0X5D,
-            MAG_OFFSET_Y_MSB_ADDR = 0X5E,
-            MAG_OFFSET_Z_LSB_ADDR = 0X5F,
-            MAG_OFFSET_Z_MSB_ADDR = 0X60,
-
-            /* Gyroscope Offset register s*/
-            GYRO_OFFSET_X_LSB_ADDR = 0X61,
-            GYRO_OFFSET_X_MSB_ADDR = 0X62,
-            GYRO_OFFSET_Y_LSB_ADDR = 0X63,
-            GYRO_OFFSET_Y_MSB_ADDR = 0X64,
-            GYRO_OFFSET_Z_LSB_ADDR = 0X65,
-            GYRO_OFFSET_Z_MSB_ADDR = 0X66,
-
-            /* Radius registers */
-            ACCEL_RADIUS_LSB_ADDR = 0X67,
-            ACCEL_RADIUS_MSB_ADDR = 0X68,
-            MAG_RADIUS_LSB_ADDR = 0X69,
-            MAG_RADIUS_MSB_ADDR = 0X6A
-        } adafruit_bno055_reg_t;
-
-        /** BNO055 power settings */
-        typedef enum {
-            POWER_MODE_NORMAL = 0X00,
-            POWER_MODE_LOWPOWER = 0X01,
-            POWER_MODE_SUSPEND = 0X02
-        } adafruit_bno055_powermode_t;
-
-        /** Remap settings **/
-        typedef enum {
-            REMAP_CONFIG_P0 = 0x21,
-            REMAP_CONFIG_P1 = 0x24, // default
-            REMAP_CONFIG_P2 = 0x24,
-            REMAP_CONFIG_P3 = 0x21,
-            REMAP_CONFIG_P4 = 0x24,
-            REMAP_CONFIG_P5 = 0x21,
-            REMAP_CONFIG_P6 = 0x21,
-            REMAP_CONFIG_P7 = 0x24
-        } adafruit_bno055_axis_remap_config_t;
-
-        /** Remap Signs **/
-        typedef enum {
-            REMAP_SIGN_P0 = 0x04,
-            REMAP_SIGN_P1 = 0x00, // default
-            REMAP_SIGN_P2 = 0x06,
-            REMAP_SIGN_P3 = 0x02,
-            REMAP_SIGN_P4 = 0x03,
-            REMAP_SIGN_P5 = 0x01,
-            REMAP_SIGN_P6 = 0x07,
-            REMAP_SIGN_P7 = 0x05
-        } adafruit_bno055_axis_remap_sign_t;
-
-        /** A structure to represent revisions **/
-        typedef struct {
-            uint8_t accel_rev; /**< acceleration rev */
-            uint8_t mag_rev;   /**< magnetometer rev */
-            uint8_t gyro_rev;  /**< gyroscrope rev */
-            uint16_t sw_rev;   /**< SW rev */
-            uint8_t bl_rev;    /**< bootloader rev */
-        } adafruit_bno055_rev_info_t;
-
-        /** Vector Mappings **/
-        typedef enum {
-            VECTOR_ACCELEROMETER = BNO055_ACCEL_DATA_X_LSB_ADDR,
-            VECTOR_MAGNETOMETER = BNO055_MAG_DATA_X_LSB_ADDR,
-            VECTOR_GYROSCOPE = BNO055_GYRO_DATA_X_LSB_ADDR,
-            VECTOR_EULER = BNO055_EULER_H_LSB_ADDR,
-            VECTOR_LINEARACCEL = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR,
-            VECTOR_GRAVITY = BNO055_GRAVITY_DATA_X_LSB_ADDR
-        } adafruit_vector_type_t;
-
-        int32_t _sensorID;
-        adafruit_bno055_opmode_t currentMode;
-
-    public:
-        /**
-         * @brief Construct a new IMUSensor object
-         * 
-         */
-        IMUSensor(std::string I2C_FILE);
-
-        /**
-         * @brief Inititlize the IMU
-         * 
-         * @param data Data for initializing the sensor
-         * @return true Initialization Success
-         * @return false Initialization Failure
-         */
-        bool init() override;
-
-        //Data handlers, copied from adafruit implementation (not all of them)
-        void setModeTemp(adafruit_bno055_opmode_t mode);
-        void setModeHard(adafruit_bno055_opmode_t mode);
-        adafruit_bno055_opmode_t getMode();
-
-        imu::Vector<3> getVector(adafruit_vector_type_t vector_type);
-        imu::Quaternion getQuat();
-        int8_t getTemp();
-
-        //Configuration and status getters/setters
-        void setAxisRemap(adafruit_bno055_axis_remap_config_t remapcode);
-        void setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign);        
-        void getSystemStatus(uint8_t *system_status, uint8_t *self_test_result,
-                       uint8_t *system_error);
-        void getCalibration(uint8_t *sys, uint8_t *gyro, uint8_t *accel,
-                      uint8_t *mag);
-
-        /* Functions to deal with raw calibration data */
-        bool getSensorOffsets(uint8_t *calibData);
-        bool getSensorOffsets(adafruit_bno055_offsets_t &offsets_type);
-        void setSensorOffsets(const uint8_t *calibData);
-        void setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type);
-        bool isFullyCalibrated();
-
-        /* Power managments functions */
-        void enterSuspendMode();
-        void enterNormalMode();
-
-};
-\ No newline at end of file
diff --git a/include/state_machine.hpp b/include/state_machine.hpp
deleted file mode 100644
index 874fc08..0000000
--- a/include/state_machine.hpp
+++ /dev/null
@@ -1,26 +0,0 @@
-#ifndef STATE_MACHINE_H
-#define STATE_MACHINE_H
-
-#include <stdint.h>
-
-extern volatile float altitude = 0.0f;
-extern volatile float prev_altitude = 0.0f;
-extern volatile float velocity;
-extern volatile state_t state;
-extern volatile float threshold_altitude;
-extern volatile float threshold_velocity;
-extern volatile uint8_t deployment_percent;
-extern volatile vector3f linear_acceleration;
-extern volatile vector3f acceleration;
-extern volatile quarternion abs_quaternion;
-extern volatile vector3f velocity_vector;
-extern volatile vector3f euler_angles;
-extern volatile vector3f abs_lin_accel;
-extern volatile vector3f prev_abs_lin_accel;
-extern volatile vector3f rot_y_vec;
-extern volatile vector3f vel_at_angle;
-extern volatile vector3f accel_gravity;
-extern volatile CALIB_STATUS calib_status;
-
-#endif
-
diff --git a/include/surfaceFitModel.hpp b/include/surfaceFitModel.hpp
deleted file mode 100644
index 303633f..0000000
--- a/include/surfaceFitModel.hpp
+++ /dev/null
@@ -1,32 +0,0 @@
-#pragma once
-#include <cmath>
-#include "eigen3/Eigen/Dense"
-
-
-using namespace Eigen;
-
-#define X_DEGREE 4 // Highest x-degree of current Surface Fit Model
-#define Y_DEGREE 3 // Highest y-degree of current Surface Fit Model
-
-class SurfaceFitModel {
-
-    private:
-        MatrixXd p; // Polynomial values
-
-    public:
-
-        /**
-         * @brief Construct a new Surface Fit Model object
-         * 
-         */
-        SurfaceFitModel();
-
-        /**
-         * @brief 
-         * 
-         * @param x 
-         * @param y 
-         * @return double 
-         */
-        double getFit(double x, double y);
-};
diff --git a/include/unused/actuationPlan.hpp b/include/unused/actuationPlan.hpp
deleted file mode 100644
index 8d1ef26..0000000
--- a/include/unused/actuationPlan.hpp
+++ /dev/null
@@ -1,36 +0,0 @@
-#pragma once
-#include <algorithm>
-#include <ctime>
-#include "surfaceFitModel.hpp"
-#include "rocketUtils.hpp"
-#include "sensorIMU.hpp"
-#include "sensorAltimeter.hpp"
-
-class ActuationPlan {
-
-    private:
-        SurfaceFitModel sFitModel;
-        
-    public:
-
-        /**
-         * @brief Construct a new Actuation Plan object
-         * 
-         */
-        ActuationPlan();
-
-        /**
-         * @brief Construct a new Actuation Plan object
-         * 
-         * @param sFitModel 
-         */
-        ActuationPlan(SurfaceFitModel sFitModel);
-
-        /**
-         * @brief Run the Fin Actuation Plan.
-         *  Adjusts the fin angle values depending on the current vehicle state during the launch
-         * 
-         * @param rocket Provides current rocket status and hold updated fin angle value.
-         */
-        void runPlan(Vehicle& rocket);
-};
-\ No newline at end of file
diff --git a/include/unused/actuator.hpp b/include/unused/actuator.hpp
deleted file mode 100644
index e7c4120..0000000
--- a/include/unused/actuator.hpp
+++ /dev/null
@@ -1,30 +0,0 @@
-#pragma once
-
-
-class Actuator {
-
-    private:
-
-
-    public:
-
-        /**
-         * @brief Initialize the actuator. 
-         * 
-         * @param data Data for initializing the actuator
-         * 
-         * @return true Initialization Success
-         * @return false Initialization Failure
-         */
-        virtual bool init(void* data) = 0;
-
-        /**
-         * @brief Pass data to the actuator. 
-         * 
-         * @param data Data to sent to the actuator
-         * 
-         * @return true Actuator write Success
-         * @return false Actuator write Failure
-         */
-        virtual bool writeData(void* data) = 0;
-};
-\ No newline at end of file
diff --git a/include/unused/ads.hpp b/include/unused/ads.hpp
deleted file mode 100644
index b16c4fb..0000000
--- a/include/unused/ads.hpp
+++ /dev/null
@@ -1,66 +0,0 @@
-#pragma once
-#include <iostream>
-#include <exception>
-#include <stdexcept>
-#include <chrono>
-#include <thread>
-#include "kalmanfilter.hpp"
-#include "logger.hpp"
-#include "actuationPlan.hpp"
-#include "rocketUtils.hpp"
-#include "sensorIMU.hpp"
-#include "sensorAltimeter.hpp"
-#include "motor.hpp"
-#include "eigen3/Eigen/Dense"
-
-using namespace Eigen;
-
-class ADS {
-
-    private:
-
-        KalmanFilter kf;      
-        ActuationPlan plan;
-
-        IMUSensor imu;
-        AltimeterSensor altimeter;
-        Motor motor;
-        Vehicle rocket;
-
-        /**
-         * @brief Logs a summary of all pertinent current rocket data
-         *  (e.g. Altitude, Velocity, Acceleration)
-         */
-        virtual void logSummary();
-        
-        /**
-         * @brief Performs a routine to calculate the average altitude
-         *      while the vehicle is waiting on the pad.
-         */
-        virtual void updateOnPadAltitude();
-
-        /**
-         * @brief Update the vehicle with the current sensor (IMU & Altimeter) readings
-         */
-        virtual void updateSensorData();
-
-        /**
-         * @brief Update the rocket state based on its current telemetry readings.
-         *     Also Log pertinent telemetry and rocket state data
-         */
-        virtual void updateRocketState();
-
-    public:
-
-        /**
-         * @brief Construct a new ADS object
-         * 
-         * @param plan The Actuation Plan for the Rocket
-         */
-        ADS(ActuationPlan plan);
-
-        /**
-         * @brief Run the full active drag system from launch to landing.
-         */
-        virtual void run();
-};
-\ No newline at end of file
diff --git a/include/unused/logger.hpp b/include/unused/logger.hpp
deleted file mode 100644
index 90fa05a..0000000
--- a/include/unused/logger.hpp
+++ /dev/null
@@ -1,83 +0,0 @@
-#pragma once
-#include <iostream>
-#include <fstream>
-#include <ctime>
-
-class Logger {
-
-    private:
-        std::fstream file;
-        std::string filename;
-        time_t t;
-        tm* now;
-        bool file_open;
-
-        std::string infoTag = "-> [INFO]: ";
-        std::string errorTag = "-> [ERROR]: ";
-        std::string months[12] = {"Jan", "Feb", "Mar", "Apr", "May", "Jun", "Jul", "Aug", "Sept", "Oct", "Nov", "Dec"};
-        std::string days[7] = {"Sun", "Mon", "Tues", "Wed", "Thur", "Frid", "Sat"};
-
-        /**
-         * @brief Create formatted current-date tag 
-         * 
-         * @return string 
-         */
-        std::string getDate();
-
-        /**
-         * @brief Create formatted current-time tag 
-         * 
-         * @return string 
-         */
-        std::string getTime();
-    
-
-    public:
-        
-        /**
-         * @brief 
-         * 
-         */
-        static Logger& Get();
-
-        /**
-         * @brief Open a Log File for writing
-         * 
-         * @param _filename Name of file to open
-         * @return true Successful Open
-         * @return false Unsuccessful Open
-         */
-        bool openLog(std::string _filename);
-
-        /**
-         * @brief Close the Log File
-         * 
-         */
-        void closeLog();
-
-        /**
-         * @brief Write data to Log file
-         * 
-         * @param data Data to log
-         * @return true Data Successfully Logged
-         * @return false Data Logging Unsuccessful
-         */
-        bool log(std::string data);
-
-        /**
-         * @brief Write error data to Log file
-         * 
-         * @param data Error Data to log
-         * @return true Data Successfully Logged
-         * @return false Data Logging Unsuccessful
-         */
-        bool logErr(std::string data);
-
-        /**
-         * @brief Print Log Data to Terminal
-         * 
-         * @return true Successful Print
-         * @return false Unsuccessful Print
-         */
-        bool printLog();
-};
-\ No newline at end of file
diff --git a/include/unused/motor.hpp b/include/unused/motor.hpp
deleted file mode 100644
index cb95b6a..0000000
--- a/include/unused/motor.hpp
+++ /dev/null
@@ -1,36 +0,0 @@
-#pragma once
-#include "actuator.hpp"
-#include "logger.hpp"
-#include "rocketUtils.hpp"
-
-class Motor : public Actuator {
-
-    private:
-
-
-    public:
-
-        /**
-         * @brief Construct a new Motor object
-         * 
-         */
-        Motor();
-
-        /**
-         * @brief Initialize the motor. 
-         * 
-         * @param data Data for initializing the motor
-         * @return true Initialization Success
-         * @return false Initialization Failure
-         */
-        virtual bool init(void* data) override;
-
-        /**
-         * @brief Write data to the motor.
-         * 
-         * @param data Data to be writen to the motor
-         * @return true Motor write Success
-         * @return false Motor write Failure
-         */
-        virtual bool writeData(void* data) override;
-};
-\ No newline at end of file
diff --git a/include/unused/rocketUtils.hpp b/include/unused/rocketUtils.hpp
deleted file mode 100644
index 65b9cea..0000000
--- a/include/unused/rocketUtils.hpp
+++ /dev/null
@@ -1,121 +0,0 @@
-#pragma once
-#include <iostream>
-#include <vector>
-#include <ctime>
-#include <vector>
-#include <iomanip>
-#include <sstream>
-#include <fstream>
-
-// Deployment angle limits
-#define MAX_ANGLE 100
-#define MIN_ANGLE 120
-
-// Altimeter initialization count limit
-#define COUNT_LIMIT 50
-
-// Constants
-#define G_0 9.8066
-
-// Threshold limits
-#define BOOST_ACCEL_THRESH 3
-#define BOOST_HEIGHT_THRESH 20
-#define GLIDE_ACCEL_THRESH 0.5
-
-#define ALTI_DEPL_THRESHOLD 0.5
-#define RATE_ALTI_DEPL 1/50
-#define FSM_DONE_SURFACE_ALTITUDE 200
-#define APOGEE_FSM_CHANGE 3
-
-#define INIT_DEPLOYMENT 0 
-
-#define TIME_BO 8
-#define TIME_APO 25
-#define TIME_END 120
-
-#define PAD_PRESSURE 102250
-
-#define DUTY_MAX 14.5
-#define DUTY_MIN 3
-
-#define LAUNCH_DATE "4-15-2023"
-#define LOG_FILENAME "DataLog_" LAUNCH_DATE ".txt"
-
-#define LED_GAP_TIME 0.5
-#define LED_ONE_PATH "/sys/class/leds/beaglebone:green:usr1"
-#define LED_BRIGHTNESS_FILE "brightness"
-#define LED_FILENAME LED_ONE_PATH LED_BRIGHTNESS_FILE
-
-#define TEST_MODE false
-
-
-enum VehicleState {ON_PAD, BOOST, GLIDE, APOGEE, DONE};
-extern std::string state_for_log[5];
-
-struct Vehicle {
-
-	int status;
-
-	std::vector<double> acceleration; 
-    std::vector<double> linear_acceleration;
-
-    double apogee_altitude;
-	double previous_altitude;
-	double current_altitude;
-	double filtered_altitude;
-
-	double filtered_velocity;
-
-	double deployment_angle;
-
-	bool imuInitFail;
-	bool imuReadFail;
-	bool altiInitFail;
-	bool altiReadFail;
-
-	double ON_PAD_altitude;
-	bool ON_PAD_fail;
-
-	double duty_span;
-
-	double dt;
-
-	int led_brightness;
-
-	time_t start_time;
-	time_t fail_time; // For failure termination
-	time_t liftoff_time;
-	time_t relog_time;
-	time_t deploy_time; // NOT INITIALIZED YET
-	time_t loop_time;
-	time_t led_time;
-};
-
-/**
- * @brief Convert fin deployment percentage to fin rotation angle
- * 
- * @param percentage Fin deployment percentage
- * @return double 
- */
-double deploy_percentage_to_angle(double percentage);
-
-/**
- * @brief Set the decimal precision of the given data, and return it 
- * 		as a formatted string with a prefix containing a relevant description of the data. 
- * 
- * @param prefix Identifying or clarifying information about the loggef data
- * @param data Data to Log
- * @param precision The decimal precision value for the data
- * 
- * @return A string with the formatted data.
- */
-std::string format_data(std::string prefix, double data, int precision);
-
-/**
- * @brief Blink Beaglebone LED 1
- * 
- * @param vehicle Holds settings pertinent to the Beaglebone LED
- * @return true Successful Blink
- * @return false Unsuccessful Blink
- */
-bool led_out(Vehicle *vehicle);
diff --git a/include/unused/sensorAltimeter.hpp b/include/unused/sensorAltimeter.hpp
deleted file mode 100644
index aa3a3cd..0000000
--- a/include/unused/sensorAltimeter.hpp
+++ /dev/null
@@ -1,165 +0,0 @@
-#pragma once
-#include "sensorI2C.hpp"
-#include <string>
-
-/*
-Header file for driver for MPL3115a2 breakout, based heavily on https://github.com/adafruit/Adafruit_MPL3115A2_Library/tree/master
-Based on register declarations found https://cdn-shop.adafruit.com/datasheets/1893_datasheet.pdf
-Designed using subclass for I2C handler for Beaglebone Black
-*/
-
-class AltimeterSensor : public sensorI2C {
-
-
-    private:
-        double internalTemperature = 0;
-        double internalAltitude = 0;
-        
-        //ENUMS COPIED DIRECTLY FROM ADAFRUIT IMPLEMENTATION
-        /** MPL3115A2 registers **/
-        enum {
-            MPL3115A2_REGISTER_STATUS = (0x00),
-
-            MPL3115A2_REGISTER_PRESSURE_MSB = (0x01),
-            MPL3115A2_REGISTER_PRESSURE_CSB = (0x02),
-            MPL3115A2_REGISTER_PRESSURE_LSB = (0x03),
-
-            MPL3115A2_REGISTER_TEMP_MSB = (0x04),
-            MPL3115A2_REGISTER_TEMP_LSB = (0x05),
-
-            MPL3115A2_REGISTER_DR_STATUS = (0x06),
-
-            MPL3115A2_OUT_P_DELTA_MSB = (0x07),
-            MPL3115A2_OUT_P_DELTA_CSB = (0x08),
-            MPL3115A2_OUT_P_DELTA_LSB = (0x09),
-
-            MPL3115A2_OUT_T_DELTA_MSB = (0x0A),
-            MPL3115A2_OUT_T_DELTA_LSB = (0x0B),
-
-            MPL3115A2_WHOAMI = (0x0C),
-            //This is hard-coded in the device from the factory
-            MPL3115A2_WHOAMI_EXPECTED = (0xC4),
-
-            MPL3115A2_BAR_IN_MSB = (0x14),
-            MPL3115A2_BAR_IN_LSB = (0x15),
-
-            MPL3115A2_OFF_H = (0x2D),
-        };
-
-        /** MPL3115A2 status register BITS **/
-        enum {
-            MPL3115A2_REGISTER_STATUS_TDR = 0x02,
-            MPL3115A2_REGISTER_STATUS_PDR = 0x04,
-            MPL3115A2_REGISTER_STATUS_PTDR = 0x08,
-        };
-
-        /** MPL3115A2 PT DATA register BITS **/
-        enum {
-            MPL3115A2_PT_DATA_CFG = 0x13,
-            MPL3115A2_PT_DATA_CFG_TDEFE = 0x01,
-            MPL3115A2_PT_DATA_CFG_PDEFE = 0x02,
-            MPL3115A2_PT_DATA_CFG_DREM = 0x04,
-        };
-
-        /** MPL3115A2 control registers **/
-        enum {
-            MPL3115A2_CTRL_REG1 = (0x26),
-            MPL3115A2_CTRL_REG2 = (0x27),
-            MPL3115A2_CTRL_REG3 = (0x28),
-            MPL3115A2_CTRL_REG4 = (0x29),
-            MPL3115A2_CTRL_REG5 = (0x2A),
-        };
-
-        /** MPL3115A2 control register BITS **/
-        enum {
-            MPL3115A2_CTRL_REG1_SBYB = 0x01,
-            MPL3115A2_CTRL_REG1_OST = 0x02,
-            MPL3115A2_CTRL_REG1_RST = 0x04,
-            MPL3115A2_CTRL_REG1_RAW = 0x40,
-            MPL3115A2_CTRL_REG1_ALT = 0x80,
-            MPL3115A2_CTRL_REG1_BAR = 0x00,
-        };
-
-        /** MPL3115A2 oversample values **/
-        enum {
-            MPL3115A2_CTRL_REG1_OS1 = 0x00,
-            MPL3115A2_CTRL_REG1_OS2 = 0x08,
-            MPL3115A2_CTRL_REG1_OS4 = 0x10,
-            MPL3115A2_CTRL_REG1_OS8 = 0x18,
-            MPL3115A2_CTRL_REG1_OS16 = 0x20,
-            MPL3115A2_CTRL_REG1_OS32 = 0x28,
-            MPL3115A2_CTRL_REG1_OS64 = 0x30,
-            MPL3115A2_CTRL_REG1_OS128 = 0x38,
-        };
-
-        /** MPL3115A2 measurement modes **/
-        typedef enum {
-            MPL3115A2_BAROMETER = 0,
-            MPL3115A2_ALTIMETER,
-        } mpl3115a2_mode_t;
-
-        /** MPL3115A2 measurement types **/
-        typedef enum {
-            MPL3115A2_PRESSURE,
-            MPL3115A2_ALTITUDE,
-            MPL3115A2_TEMPERATURE,
-        } mpl3115a2_meas_t;
-
-        //This never actually gets used, and I can't find anything in the datasheet about it??
-        #define MPL3115A2_REGISTER_STARTCONVERSION (0x12) ///< start conversion
-
-        //Store current operating mode, sent to device during startup procedure
-        //This is why an enum is used rather than raw #define statements
-        mpl3115a2_mode_t currentMode;
-
-        //Struct for storing ctrl register contents, copied from adafruit implementation
-        typedef union {
-            struct {
-                uint8_t SBYB : 1;
-                uint8_t OST : 1;
-                uint8_t RST : 1;
-                uint8_t OS : 3;
-                uint8_t RAW : 1;
-                uint8_t ALT : 1;
-            } bit;
-            uint8_t reg;
-        } CTRL_REG_1_STRUCT;
-        //Create instance of this register config to use during device startup and operation
-        CTRL_REG_1_STRUCT ctrl_reg1;
-
-    public:
-
-        /**
-         * @brief Construct a new Altimeter Sensor object
-         * 
-         */
-        AltimeterSensor(std::string I2C_FILE);
-
-        /**
-         * @brief Initialize the Altimeter
-         * 
-         * @param data Data for initializing the sensor
-         * @return true Initialization Success
-         * @return false Initialization Failure
-         */
-        bool init() override;
-
-        //Data getters and setters
-        // double getPressure();
-        double getAltitude();
-        double getTemperature();
-        double setSeaLevelPressure(double pressure);
-
-        //Data and mode handlers
-        //Use altimeter mode by default as this is what rocket logger wants
-        void setMode(mpl3115a2_mode_t mode = MPL3115A2_ALTIMETER);
-        void requestOneShotReading();
-        bool isNewDataAvailable();
-        void updateCurrentDataBuffer();
-
-
-};
-
-
-
-
diff --git a/include/unused/sensorI2C.hpp b/include/unused/sensorI2C.hpp
deleted file mode 100644
index bd497a5..0000000
--- a/include/unused/sensorI2C.hpp
+++ /dev/null
@@ -1,105 +0,0 @@
-#pragma once
-
-#include <stdio.h>
-#include <fcntl.h>
-#include <unistd.h>
-#include <sys/ioctl.h>
-#include <linux/i2c.h>
-#include <linux/i2c-dev.h>
-#include <cstdint>
-#include <string>
-
-class sensorI2C {
-    
-
-    //Predominantly used for I2C handler functions; implement high-level functions in sensor classes
-    //Implemented as a combination of Jazz' implementation and Derek Malloy's code
-    public:
-        
-        //Initial single byte write, used at beginning of read operation
-        //Returns 0 if successful, -1 if not
-        int initialWrite(unsigned char registerAddress) {
-            unsigned char *convertedAddressBuffer = new unsigned char[1];
-            convertedAddressBuffer[0] = registerAddress;
-            //Expect 1 byte response from 1 byte write
-            if (write(i2c_bus, convertedAddressBuffer, 1) != 1) {
-                fprintf(stderr, "ERROR DOING INITIAL READ TRANSACTION WRITE TO REGISTER %x FOR DEVICE %x\n", registerAddress, deviceAddress);
-                return 0;
-            }
-            return 1;
-        }
-
-        int writeRegister(unsigned char registerAddress, unsigned char value) {
-            //initialWrite() not used here because it's easier to just pack it into one buffer for file writing
-            unsigned char writeBuffer[2];
-            writeBuffer[0] = registerAddress;
-            writeBuffer[1] = value;
-
-            //Expect 2 byte output
-            if (write(i2c_bus, writeBuffer, 2) != 2) {
-                //These error messages are kind of obtuse but I'd rather have too much information than not enough
-                fprintf(stderr, "ERROR WRITING %x TO REGISTER %x ON DEVICE %x\n", value, registerAddress, deviceAddress);
-                return -1;
-            }
-            return 0;
-        }
-
-        //Could probably be uint8_t but Derek Malloy does it with unsigned chars and that's what worked during testing so I don't want to touch it
-        unsigned char readSingleRegister(unsigned char registerAddress) {
-            printf("reg addr: %X\n", registerAddress);
-
-            initialWrite(registerAddress);
-            unsigned char* readBuffer = new unsigned char[1];
-            if (read(i2c_bus, readBuffer, 1) != 1){
-                fprintf(stderr, "FAILED TO READ VALUE FROM REGISTER %x ON DEVICE %x\n", registerAddress, deviceAddress);
-                return -1;
-            }
-            printf("readBuffer: %X\n", readBuffer[0]);
-            return readBuffer[0];
-        }
-
-        unsigned char* readMultipleRegisters(unsigned char startingRegisterAddress, int numberOfRegisters) {            
-            initialWrite(startingRegisterAddress);
-            unsigned char* readBuffer = new unsigned char[numberOfRegisters];
-            if (read(i2c_bus, readBuffer, numberOfRegisters) != numberOfRegisters) {
-                fprintf(stderr, "ERROR TRYING TO READ %d REGISTERS STARTING AT ADDRESS %x ON DEVICE %x\n", 
-                    numberOfRegisters, startingRegisterAddress, deviceAddress);
-            }
-            return readBuffer;
-        }
-
-
-        //Intakes device address and file 
-        //Private because IT'S ASSUMED PROGRAMMER WILL CALL THIS METHOD DURING INIT() ROUTINE
-        int setupI2C(std::string I2C_FILE) {
-            // Open i2c driver file
-            i2c_bus = open("/dev/i2c-2", O_RDWR);
-            if(i2c_bus < 0){
-                fprintf(stderr, "FAILED TO OPEN I2C BUS USING FILE %s\n", "/dev/i2c-2");
-                close(i2c_bus);
-                return -1;
-            }
-
-            // Identify slave device address (MODIFIED FROM INITIAL IMPLEMENTATION, USES INTERNAL deviceAddress INSTEAD OF PARAMETER)
-            if(ioctl(i2c_bus, I2C_SLAVE, deviceAddress) < 0){
-                fprintf(stderr, "FAILED TO CONNECT TO DEVICE AT ADDRESS %x VIA I2C\n", deviceAddress);
-                close(i2c_bus);
-                return -1;
-            }
-            return 0;
-        }
-
-        /**
-         * @brief Initialize the sensor. 
-         * 
-         * @param data Data for initializing the sensor
-         * 
-         * @return true Initialization Success
-         * @return false Initialization Failure
-         */
-        virtual bool init() = 0;
-
-        unsigned char deviceAddress;
-        int i2c_bus;
-        std::string I2C_FILE;
-};
diff --git a/include/unused/sensorIMU.hpp b/include/unused/sensorIMU.hpp
deleted file mode 100644
index caddbe4..0000000
--- a/include/unused/sensorIMU.hpp
+++ /dev/null
@@ -1,312 +0,0 @@
-#pragma once
-#include <vector>
-#include "sensorI2C.hpp"
-#include "logger.hpp"
-#include "rocketUtils.hpp"
-#include "imumath.h"
-#include <string>
-
-struct IMUData {
-    std::vector<double> acceleration[3];
-    std::vector<double> linear_acceleration[3];
-};
-
-//Register addresses and data structs copied from Adafruit implementation
-/** BNO055 Address A **/
-#define BNO055_ADDRESS_A (0x28)
-/** BNO055 Address B **/
-#define BNO055_ADDRESS_B (0x29)
-/** BNO055 ID **/
-#define BNO055_ID (0xA0)
-
-/** Offsets registers **/
-#define NUM_BNO055_OFFSET_REGISTERS (22)
-
-/** A structure to represent offsets **/
-typedef struct {
-    int16_t accel_offset_x; /**< x acceleration offset */
-    int16_t accel_offset_y; /**< y acceleration offset */
-    int16_t accel_offset_z; /**< z acceleration offset */
-
-    int16_t mag_offset_x; /**< x magnetometer offset */
-    int16_t mag_offset_y; /**< y magnetometer offset */
-    int16_t mag_offset_z; /**< z magnetometer offset */
-
-    int16_t gyro_offset_x; /**< x gyroscrope offset */
-    int16_t gyro_offset_y; /**< y gyroscrope offset */
-    int16_t gyro_offset_z; /**< z gyroscrope offset */
-
-    int16_t accel_radius; /**< acceleration radius */
-
-    int16_t mag_radius; /**< magnetometer radius */
-} adafruit_bno055_offsets_t;
-
-/** Operation mode settings **/
-typedef enum {
-    OPERATION_MODE_CONFIG = 0X00,
-    OPERATION_MODE_ACCONLY = 0X01,
-    OPERATION_MODE_MAGONLY = 0X02,
-    OPERATION_MODE_GYRONLY = 0X03,
-    OPERATION_MODE_ACCMAG = 0X04,
-    OPERATION_MODE_ACCGYRO = 0X05,
-    OPERATION_MODE_MAGGYRO = 0X06,
-    OPERATION_MODE_AMG = 0X07,
-    OPERATION_MODE_IMUPLUS = 0X08,
-    OPERATION_MODE_COMPASS = 0X09,
-    OPERATION_MODE_M4G = 0X0A,
-    OPERATION_MODE_NDOF_FMC_OFF = 0X0B,
-    OPERATION_MODE_NDOF = 0X0C
-} adafruit_bno055_opmode_t;
-class IMUSensor : public sensorI2C {
-
-    private:
-        deviceAddress = BNO055_ADDRESS_A;
-
-        adafruit_bno055_opmode_t default_mode = OPERATION_MODE_NDOF;
-        /** BNO055 Registers **/
-        typedef enum {
-            /* Page id register definition */
-            BNO055_PAGE_ID_ADDR = 0X07,
-
-            /* PAGE0 REGISTER DEFINITION START*/
-            BNO055_CHIP_ID_ADDR = 0x00,
-            BNO055_ACCEL_REV_ID_ADDR = 0x01,
-            BNO055_MAG_REV_ID_ADDR = 0x02,
-            BNO055_GYRO_REV_ID_ADDR = 0x03,
-            BNO055_SW_REV_ID_LSB_ADDR = 0x04,
-            BNO055_SW_REV_ID_MSB_ADDR = 0x05,
-            BNO055_BL_REV_ID_ADDR = 0X06,
-
-            /* Accel data register */
-            BNO055_ACCEL_DATA_X_LSB_ADDR = 0X08,
-            BNO055_ACCEL_DATA_X_MSB_ADDR = 0X09,
-            BNO055_ACCEL_DATA_Y_LSB_ADDR = 0X0A,
-            BNO055_ACCEL_DATA_Y_MSB_ADDR = 0X0B,
-            BNO055_ACCEL_DATA_Z_LSB_ADDR = 0X0C,
-            BNO055_ACCEL_DATA_Z_MSB_ADDR = 0X0D,
-
-            /* Mag data register */
-            BNO055_MAG_DATA_X_LSB_ADDR = 0X0E,
-            BNO055_MAG_DATA_X_MSB_ADDR = 0X0F,
-            BNO055_MAG_DATA_Y_LSB_ADDR = 0X10,
-            BNO055_MAG_DATA_Y_MSB_ADDR = 0X11,
-            BNO055_MAG_DATA_Z_LSB_ADDR = 0X12,
-            BNO055_MAG_DATA_Z_MSB_ADDR = 0X13,
-
-            /* Gyro data registers */
-            BNO055_GYRO_DATA_X_LSB_ADDR = 0X14,
-            BNO055_GYRO_DATA_X_MSB_ADDR = 0X15,
-            BNO055_GYRO_DATA_Y_LSB_ADDR = 0X16,
-            BNO055_GYRO_DATA_Y_MSB_ADDR = 0X17,
-            BNO055_GYRO_DATA_Z_LSB_ADDR = 0X18,
-            BNO055_GYRO_DATA_Z_MSB_ADDR = 0X19,
-
-            /* Euler data registers */
-            BNO055_EULER_H_LSB_ADDR = 0X1A,
-            BNO055_EULER_H_MSB_ADDR = 0X1B,
-            BNO055_EULER_R_LSB_ADDR = 0X1C,
-            BNO055_EULER_R_MSB_ADDR = 0X1D,
-            BNO055_EULER_P_LSB_ADDR = 0X1E,
-            BNO055_EULER_P_MSB_ADDR = 0X1F,
-
-            /* Quaternion data registers */
-            BNO055_QUATERNION_DATA_W_LSB_ADDR = 0X20,
-            BNO055_QUATERNION_DATA_W_MSB_ADDR = 0X21,
-            BNO055_QUATERNION_DATA_X_LSB_ADDR = 0X22,
-            BNO055_QUATERNION_DATA_X_MSB_ADDR = 0X23,
-            BNO055_QUATERNION_DATA_Y_LSB_ADDR = 0X24,
-            BNO055_QUATERNION_DATA_Y_MSB_ADDR = 0X25,
-            BNO055_QUATERNION_DATA_Z_LSB_ADDR = 0X26,
-            BNO055_QUATERNION_DATA_Z_MSB_ADDR = 0X27,
-
-            /* Linear acceleration data registers */
-            BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR = 0X28,
-            BNO055_LINEAR_ACCEL_DATA_X_MSB_ADDR = 0X29,
-            BNO055_LINEAR_ACCEL_DATA_Y_LSB_ADDR = 0X2A,
-            BNO055_LINEAR_ACCEL_DATA_Y_MSB_ADDR = 0X2B,
-            BNO055_LINEAR_ACCEL_DATA_Z_LSB_ADDR = 0X2C,
-            BNO055_LINEAR_ACCEL_DATA_Z_MSB_ADDR = 0X2D,
-
-            /* Gravity data registers */
-            BNO055_GRAVITY_DATA_X_LSB_ADDR = 0X2E,
-            BNO055_GRAVITY_DATA_X_MSB_ADDR = 0X2F,
-            BNO055_GRAVITY_DATA_Y_LSB_ADDR = 0X30,
-            BNO055_GRAVITY_DATA_Y_MSB_ADDR = 0X31,
-            BNO055_GRAVITY_DATA_Z_LSB_ADDR = 0X32,
-            BNO055_GRAVITY_DATA_Z_MSB_ADDR = 0X33,
-
-            /* Temperature data register */
-            BNO055_TEMP_ADDR = 0X34,
-
-            /* Status registers */
-            BNO055_CALIB_STAT_ADDR = 0X35,
-            BNO055_SELFTEST_RESULT_ADDR = 0X36,
-            BNO055_INTR_STAT_ADDR = 0X37,
-
-            BNO055_SYS_CLK_STAT_ADDR = 0X38,
-            BNO055_SYS_STAT_ADDR = 0X39,
-            BNO055_SYS_ERR_ADDR = 0X3A,
-
-            /* Unit selection register */
-            BNO055_UNIT_SEL_ADDR = 0X3B,
-
-            /* Mode registers */
-            BNO055_OPR_MODE_ADDR = 0X3D,
-            BNO055_PWR_MODE_ADDR = 0X3E,
-
-            BNO055_SYS_TRIGGER_ADDR = 0X3F,
-            BNO055_TEMP_SOURCE_ADDR = 0X40,
-
-            /* Axis remap registers */
-            BNO055_AXIS_MAP_CONFIG_ADDR = 0X41,
-            BNO055_AXIS_MAP_SIGN_ADDR = 0X42,
-
-            /* SIC registers */
-            BNO055_SIC_MATRIX_0_LSB_ADDR = 0X43,
-            BNO055_SIC_MATRIX_0_MSB_ADDR = 0X44,
-            BNO055_SIC_MATRIX_1_LSB_ADDR = 0X45,
-            BNO055_SIC_MATRIX_1_MSB_ADDR = 0X46,
-            BNO055_SIC_MATRIX_2_LSB_ADDR = 0X47,
-            BNO055_SIC_MATRIX_2_MSB_ADDR = 0X48,
-            BNO055_SIC_MATRIX_3_LSB_ADDR = 0X49,
-            BNO055_SIC_MATRIX_3_MSB_ADDR = 0X4A,
-            BNO055_SIC_MATRIX_4_LSB_ADDR = 0X4B,
-            BNO055_SIC_MATRIX_4_MSB_ADDR = 0X4C,
-            BNO055_SIC_MATRIX_5_LSB_ADDR = 0X4D,
-            BNO055_SIC_MATRIX_5_MSB_ADDR = 0X4E,
-            BNO055_SIC_MATRIX_6_LSB_ADDR = 0X4F,
-            BNO055_SIC_MATRIX_6_MSB_ADDR = 0X50,
-            BNO055_SIC_MATRIX_7_LSB_ADDR = 0X51,
-            BNO055_SIC_MATRIX_7_MSB_ADDR = 0X52,
-            BNO055_SIC_MATRIX_8_LSB_ADDR = 0X53,
-            BNO055_SIC_MATRIX_8_MSB_ADDR = 0X54,
-
-            /* Accelerometer Offset registers */
-            ACCEL_OFFSET_X_LSB_ADDR = 0X55,
-            ACCEL_OFFSET_X_MSB_ADDR = 0X56,
-            ACCEL_OFFSET_Y_LSB_ADDR = 0X57,
-            ACCEL_OFFSET_Y_MSB_ADDR = 0X58,
-            ACCEL_OFFSET_Z_LSB_ADDR = 0X59,
-            ACCEL_OFFSET_Z_MSB_ADDR = 0X5A,
-
-            /* Magnetometer Offset registers */
-            MAG_OFFSET_X_LSB_ADDR = 0X5B,
-            MAG_OFFSET_X_MSB_ADDR = 0X5C,
-            MAG_OFFSET_Y_LSB_ADDR = 0X5D,
-            MAG_OFFSET_Y_MSB_ADDR = 0X5E,
-            MAG_OFFSET_Z_LSB_ADDR = 0X5F,
-            MAG_OFFSET_Z_MSB_ADDR = 0X60,
-
-            /* Gyroscope Offset register s*/
-            GYRO_OFFSET_X_LSB_ADDR = 0X61,
-            GYRO_OFFSET_X_MSB_ADDR = 0X62,
-            GYRO_OFFSET_Y_LSB_ADDR = 0X63,
-            GYRO_OFFSET_Y_MSB_ADDR = 0X64,
-            GYRO_OFFSET_Z_LSB_ADDR = 0X65,
-            GYRO_OFFSET_Z_MSB_ADDR = 0X66,
-
-            /* Radius registers */
-            ACCEL_RADIUS_LSB_ADDR = 0X67,
-            ACCEL_RADIUS_MSB_ADDR = 0X68,
-            MAG_RADIUS_LSB_ADDR = 0X69,
-            MAG_RADIUS_MSB_ADDR = 0X6A
-        } adafruit_bno055_reg_t;
-
-        /** BNO055 power settings */
-        typedef enum {
-            POWER_MODE_NORMAL = 0X00,
-            POWER_MODE_LOWPOWER = 0X01,
-            POWER_MODE_SUSPEND = 0X02
-        } adafruit_bno055_powermode_t;
-
-        /** Remap settings **/
-        typedef enum {
-            REMAP_CONFIG_P0 = 0x21,
-            REMAP_CONFIG_P1 = 0x24, // default
-            REMAP_CONFIG_P2 = 0x24,
-            REMAP_CONFIG_P3 = 0x21,
-            REMAP_CONFIG_P4 = 0x24,
-            REMAP_CONFIG_P5 = 0x21,
-            REMAP_CONFIG_P6 = 0x21,
-            REMAP_CONFIG_P7 = 0x24
-        } adafruit_bno055_axis_remap_config_t;
-
-        /** Remap Signs **/
-        typedef enum {
-            REMAP_SIGN_P0 = 0x04,
-            REMAP_SIGN_P1 = 0x00, // default
-            REMAP_SIGN_P2 = 0x06,
-            REMAP_SIGN_P3 = 0x02,
-            REMAP_SIGN_P4 = 0x03,
-            REMAP_SIGN_P5 = 0x01,
-            REMAP_SIGN_P6 = 0x07,
-            REMAP_SIGN_P7 = 0x05
-        } adafruit_bno055_axis_remap_sign_t;
-
-        /** A structure to represent revisions **/
-        typedef struct {
-            uint8_t accel_rev; /**< acceleration rev */
-            uint8_t mag_rev;   /**< magnetometer rev */
-            uint8_t gyro_rev;  /**< gyroscrope rev */
-            uint16_t sw_rev;   /**< SW rev */
-            uint8_t bl_rev;    /**< bootloader rev */
-        } adafruit_bno055_rev_info_t;
-
-        /** Vector Mappings **/
-        typedef enum {
-            VECTOR_ACCELEROMETER = BNO055_ACCEL_DATA_X_LSB_ADDR,
-            VECTOR_MAGNETOMETER = BNO055_MAG_DATA_X_LSB_ADDR,
-            VECTOR_GYROSCOPE = BNO055_GYRO_DATA_X_LSB_ADDR,
-            VECTOR_EULER = BNO055_EULER_H_LSB_ADDR,
-            VECTOR_LINEARACCEL = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR,
-            VECTOR_GRAVITY = BNO055_GRAVITY_DATA_X_LSB_ADDR
-        } adafruit_vector_type_t;
-
-        int32_t _sensorID;
-        adafruit_bno055_opmode_t currentMode;
-
-    public:
-        /**
-         * @brief Construct a new IMUSensor object
-         * 
-         */
-        IMUSensor(std::string I2C_FILE);
-
-        /**
-         * @brief Inititlize the IMU
-         * 
-         * @param data Data for initializing the sensor
-         * @return true Initialization Success
-         * @return false Initialization Failure
-         */
-        bool init() override;
-
-        //Data handlers, copied from adafruit implementation (not all of them)
-        void setModeTemp(adafruit_bno055_opmode_t mode);
-        void setModeHard(adafruit_bno055_opmode_t mode);
-        adafruit_bno055_opmode_t getMode();
-
-        imu::Vector<3> getVector(adafruit_vector_type_t vector_type);
-        imu::Quaternion getQuat();
-        int8_t getTemp();
-
-        //Configuration and status getters/setters
-        void setAxisRemap(adafruit_bno055_axis_remap_config_t remapcode);
-        void setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign);        
-        void getSystemStatus(uint8_t *system_status, uint8_t *self_test_result,
-                       uint8_t *system_error);
-        void getCalibration(uint8_t *sys, uint8_t *gyro, uint8_t *accel,
-                      uint8_t *mag);
-
-        /* Functions to deal with raw calibration data */
-        bool getSensorOffsets(uint8_t *calibData);
-        bool getSensorOffsets(adafruit_bno055_offsets_t &offsets_type);
-        void setSensorOffsets(const uint8_t *calibData);
-        void setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type);
-        bool isFullyCalibrated();
-
-        /* Power managments functions */
-        void enterSuspendMode();
-        void enterNormalMode();
-
-};
-\ No newline at end of file
diff --git a/include/unused/surfaceFitModel.hpp b/include/unused/surfaceFitModel.hpp
deleted file mode 100644
index 303633f..0000000
--- a/include/unused/surfaceFitModel.hpp
+++ /dev/null
@@ -1,32 +0,0 @@
-#pragma once
-#include <cmath>
-#include "eigen3/Eigen/Dense"
-
-
-using namespace Eigen;
-
-#define X_DEGREE 4 // Highest x-degree of current Surface Fit Model
-#define Y_DEGREE 3 // Highest y-degree of current Surface Fit Model
-
-class SurfaceFitModel {
-
-    private:
-        MatrixXd p; // Polynomial values
-
-    public:
-
-        /**
-         * @brief Construct a new Surface Fit Model object
-         * 
-         */
-        SurfaceFitModel();
-
-        /**
-         * @brief 
-         * 
-         * @param x 
-         * @param y 
-         * @return double 
-         */
-        double getFit(double x, double y);
-};
diff --git a/src/AltEst/algebra.cpp b/src/AltEst/algebra.cpp
deleted file mode 100644
index 653c3b9..0000000
--- a/src/AltEst/algebra.cpp
+++ /dev/null
@@ -1,292 +0,0 @@
-/*
-  algebra.cpp: This file contains a number of utilities useful for handling
-  3D vectors
-
-  This work is an adaptation from vvector.h, written by Linas Vepstras. The
-  original code can be found at:
-
-  https://github.com/markkilgard/glut/blob/master/lib/gle/vvector.h
-
-  HISTORY:
-  Written by Linas Vepstas, August 1991
-  Added 2D code, March 1993
-  Added Outer products, C++ proofed, Linas Vepstas October 1993
-  Adapted for altitude estimation tasks by Juan Gallostra June 2018
-*/
-
-//#include <cmath>
-#include <stdio.h>
-
-#include "algebra.h"
-
-// Copy 3D vector
-void copyVector(float b[3],float a[3])
-{
-   b[0] = a[0];
-   b[1] = a[1];
-   b[2] = a[2];
-}
-
-
-// Vector difference
-void subtractVectors(float v21[3], float v2[3], float v1[3])
-{
-   v21[0] = v2[0] - v1[0];
-   v21[1] = v2[1] - v1[1];
-   v21[2] = v2[2] - v1[2];
-}
-
-// Vector sum
-void sumVectors(float v21[3], float v2[3], float v1[3])
-{
-   v21[0] = v2[0] + v1[0];
-   v21[1] = v2[1] + v1[1];
-   v21[2] = v2[2] + v1[2];
-}
-
-// scalar times vector
-void scaleVector(float c[3],float a, float b[3])
-{
-   (c)[0] = a*b[0];
-   (c)[1] = a*b[1];
-   (c)[2] = a*b[2];
-}
-
-// accumulate scaled vector
-void accumulateScaledVector(float c[3], float a, float b[3])
-{
-   (c)[0] += a*b[0];
-   (c)[1] += a*b[1];
-   (c)[2] += a*b[2];
-}
-
-// Vector dot product
-void dotProductVectors(float * c, float a[3], float b[3])
-{
-   *c = a[0]*b[0] + a[1]*b[1] + a[2]*b[2];
-}
-
-// Vector length
-void vectorLength(float * len, float a[3])
-{
-   float tmp;
-   tmp = a[0]*a[0] + a[1]*a[1]+a[2]*a[2];
-   *len = sqrt(tmp);
-}
-
-// Normalize vector
-void normalizeVector(float a[3])
-{
-   float len;
-   vectorLength(& len,a);
-   if (len != 0.0) {
-      len = 1.0 / len;
-      a[0] *= len;
-      a[1] *= len;
-      a[2] *= len;
-   }
-}
-
-// 3D Vector cross product yeilding vector
-void crossProductVectors(float c[3], float a[3], float b[3])
-{
-   c[0] = a[1] * b[2] - a[2] * b[1];
-   c[1] = a[2] * b[0] - a[0] * b[2];
-   c[2] = a[0] * b[1] - a[1] * b[0];
-}
-
-// initialize matrix
-void identityMatrix3x3(float m[3][3])
-{
-   m[0][0] = 1.0;
-   m[0][1] = 0.0;
-   m[0][2] = 0.0;
-
-   m[1][0] = 0.0;
-   m[1][1] = 1.0;
-   m[1][2] = 0.0;
-
-   m[2][0] = 0.0;
-   m[2][1] = 0.0;
-   m[2][2] = 1.0;
-}
-
-// matrix copy
-void copyMatrix3x3(float b[3][3], float a[3][3])
-{
-   b[0][0] = a[0][0];
-   b[0][1] = a[0][1];
-   b[0][2] = a[0][2];
-
-   b[1][0] = a[1][0];
-   b[1][1] = a[1][1];
-   b[1][2] = a[1][2];
-
-   b[2][0] = a[2][0];
-   b[2][1] = a[2][1];
-   b[2][2] = a[2][2];
-}
-
-// matrix transpose
-void transposeMatrix3x3(float b[3][3], float a[3][3])
-{
-   b[0][0] = a[0][0];
-   b[0][1] = a[1][0];
-   b[0][2] = a[2][0];
-
-   b[1][0] = a[0][1];
-   b[1][1] = a[1][1];
-   b[1][2] = a[2][1];
-
-   b[2][0] = a[0][2];
-   b[2][1] = a[1][2];
-   b[2][2] = a[2][2];
-}
-
-// multiply matrix by scalar
-void scaleMatrix3x3(float b[3][3], float s, float a[3][3])
-{
-   b[0][0] = (s) * a[0][0];
-   b[0][1] = (s) * a[0][1];
-   b[0][2] = (s) * a[0][2];
-
-   b[1][0] = (s) * a[1][0];
-   b[1][1] = (s) * a[1][1];
-   b[1][2] = (s) * a[1][2];
-
-   b[2][0] = (s) * a[2][0];
-   b[2][1] = (s) * a[2][1];
-   b[2][2] = (s) * a[2][2];
-}
-
-// multiply matrix by scalar and add result to another matrix
-void scaleAndAccumulateMatrix3x3(float b[3][3], float s, float a[3][3])
-{
-   b[0][0] += s * a[0][0];
-   b[0][1] += s * a[0][1];
-   b[0][2] += s * a[0][2];
-
-   b[1][0] += s * a[1][0];
-   b[1][1] += s * a[1][1];
-   b[1][2] += s * a[1][2];
-
-   b[2][0] += s * a[2][0];
-   b[2][1] += s * a[2][1];
-   b[2][2] += s * a[2][2];
-}
-
-// matrix product
-// c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y]
-void matrixProduct3x3(float c[3][3], float a[3][3], float b[3][3])
-{
-   c[0][0] = a[0][0]*b[0][0]+a[0][1]*b[1][0]+a[0][2]*b[2][0];
-   c[0][1] = a[0][0]*b[0][1]+a[0][1]*b[1][1]+a[0][2]*b[2][1];
-   c[0][2] = a[0][0]*b[0][2]+a[0][1]*b[1][2]+a[0][2]*b[2][2];
-
-   c[1][0] = a[1][0]*b[0][0]+a[1][1]*b[1][0]+a[1][2]*b[2][0];
-   c[1][1] = a[1][0]*b[0][1]+a[1][1]*b[1][1]+a[1][2]*b[2][1];
-   c[1][2] = a[1][0]*b[0][2]+a[1][1]*b[1][2]+a[1][2]*b[2][2];
-
-   c[2][0] = a[2][0]*b[0][0]+a[2][1]*b[1][0]+a[2][2]*b[2][0];
-   c[2][1] = a[2][0]*b[0][1]+a[2][1]*b[1][1]+a[2][2]*b[2][1];
-   c[2][2] = a[2][0]*b[0][2]+a[2][1]*b[1][2]+a[2][2]*b[2][2];
-}
-
-// matrix times vector
-void matrixDotVector3x3(float p[3], float m[3][3], float v[3])
-{
-   p[0] = m[0][0]*v[0] + m[0][1]*v[1] + m[0][2]*v[2];
-   p[1] = m[1][0]*v[0] + m[1][1]*v[1] + m[1][2]*v[2];
-   p[2] = m[2][0]*v[0] + m[2][1]*v[1] + m[2][2]*v[2];
-}
-
-// determinant of matrix
-// Computes determinant of matrix m, returning d
-void determinant3x3(float * d, float m[3][3])
-{
-   *d = m[0][0] * (m[1][1]*m[2][2] - m[1][2] * m[2][1]);
-   *d -= m[0][1] * (m[1][0]*m[2][2] - m[1][2] * m[2][0]);
-   *d += m[0][2] * (m[1][0]*m[2][1] - m[1][1] * m[2][0]);
-}
-
-// adjoint of matrix
-// Computes adjoint of matrix m, returning a
-// (Note that adjoint is just the transpose of the cofactor matrix)
-void adjoint3x3(float a[3][3], float m[3][3])
-{
-   a[0][0] = m[1][1]*m[2][2] - m[1][2]*m[2][1];
-   a[1][0] = - (m[1][0]*m[2][2] - m[2][0]*m[1][2]);
-   a[2][0] = m[1][0]*m[2][1] - m[1][1]*m[2][0];
-   a[0][1] = - (m[0][1]*m[2][2] - m[0][2]*m[2][1]);
-   a[1][1] = m[0][0]*m[2][2] - m[0][2]*m[2][0];
-   a[2][1] = - (m[0][0]*m[2][1] - m[0][1]*m[2][0]);
-   a[0][2] = m[0][1]*m[1][2] - m[0][2]*m[1][1];
-   a[1][2] = - (m[0][0]*m[1][2] - m[0][2]*m[1][0]);
-   a[2][2] = m[0][0]*m[1][1] - m[0][1]*m[1][0];
-}
-
-// compute adjoint of matrix and scale
-// Computes adjoint of matrix m, scales it by s, returning a
-void scaleAdjoint3x3(float a[3][3], float s, float m[3][3])
-{
-   a[0][0] = (s) * (m[1][1] * m[2][2] - m[1][2] * m[2][1]);
-   a[1][0] = (s) * (m[1][2] * m[2][0] - m[1][0] * m[2][2]);
-   a[2][0] = (s) * (m[1][0] * m[2][1] - m[1][1] * m[2][0]);
-
-   a[0][1] = (s) * (m[0][2] * m[2][1] - m[0][1] * m[2][2]);
-   a[1][1] = (s) * (m[0][0] * m[2][2] - m[0][2] * m[2][0]);
-   a[2][1] = (s) * (m[0][1] * m[2][0] - m[0][0] * m[2][1]);
-
-   a[0][2] = (s) * (m[0][1] * m[1][2] - m[0][2] * m[1][1]);
-   a[1][2] = (s) * (m[0][2] * m[1][0] - m[0][0] * m[1][2]);
-   a[2][2] = (s) * (m[0][0] * m[1][1] - m[0][1] * m[1][0]);
-}
-
-// inverse of matrix
-// Compute inverse of matrix a, returning determinant m and
-// inverse b
-void invert3x3(float b[3][3], float a[3][3])
-{
-   float tmp;
-   determinant3x3(& tmp, a);
-   tmp = 1.0 / (tmp);
-   scaleAdjoint3x3(b, tmp, a);
-}
-
-// skew matrix from vector
-void skew(float a[3][3], float v[3])
-{
-  a[0][1] = -v[2];
-  a[0][2] = v[1];
-  a[1][2] = -v[0];
-  a[1][0] = v[2];
-  a[2][0] = -v[1];
-  a[2][1] = v[0];
-  // set diagonal to 0
-  a[0][0] = 0.0;
-  a[1][1] = 0.0;
-  a[2][2] = 0.0;
-}
-
-void printMatrix3X3(float mmm[3][3])
-{
-   int i,j;
-   printf ("matrix mmm is \n");
-   if (mmm == NULL) {
-      printf (" Null \n");
-   } else {
-      for (i=0; i<3; i++) {
-         for (j=0; j<3; j++) {
-            printf ("%f ", mmm[i][j]);
-         }
-         printf (" \n");
-      }
-   }
-}
-
-void vecPrint(float a[3])
-{
-   float len;
-   vectorLength(& len, a);
-   printf(" a is %f %f %f length of a is %f \n", a[0], a[1], a[2], len);
-}
diff --git a/src/AltEst/algebra.h b/src/AltEst/algebra.h
deleted file mode 100644
index 382103e..0000000
--- a/src/AltEst/algebra.h
+++ /dev/null
@@ -1,96 +0,0 @@
-/*
-  algebra.h: This file contains a number of utilities useful for handling
-  3D vectors
-
-  This work is an adaptation from vvector.h, written by Linas Vepstras. The
-  original code can be found at:
-
-  https://github.com/markkilgard/glut/blob/master/lib/gle/vvector.h
-
-  HISTORY:
-  Written by Linas Vepstas, August 1991
-  Added 2D code, March 1993
-  Added Outer products, C++ proofed, Linas Vepstas October 1993
-  Adapted for altitude estimation tasks by Juan Gallostra June 2018
-  Separated .h, .cpp by Simon D. Levy July 2018
-*/
-
-#pragma once
-
-//#include <cmath>
-#include <math.h>
-
-// Copy 3D vector
-void copyVector(float b[3],float a[3]);
-
-
-// Vector difference
-void subtractVectors(float v21[3], float v2[3], float v1[3]);
-
-// Vector sum
-void sumVectors(float v21[3], float v2[3], float v1[3]);
-
-// scalar times vector
-void scaleVector(float c[3],float a, float b[3]);
-
-// accumulate scaled vector
-void accumulateScaledVector(float c[3], float a, float b[3]);
-
-// Vector dot product
-void dotProductVectors(float * c, float a[3], float b[3]);
-
-// Vector length
-void vectorLength(float * len, float a[3]);
-
-// Normalize vector
-void normalizeVector(float a[3]);
-
-// 3D Vector cross product yeilding vector
-void crossProductVectors(float c[3], float a[3], float b[3]);
-
-// initialize matrix
-void identityMatrix3x3(float m[3][3]);
-
-// matrix copy
-void copyMatrix3x3(float b[3][3], float a[3][3]);
-
-// matrix transpose
-void transposeMatrix3x3(float b[3][3], float a[3][3]);
-
-// multiply matrix by scalar
-void scaleMatrix3x3(float b[3][3], float s, float a[3][3]);
-
-// multiply matrix by scalar and add result to another matrix
-void scaleAndAccumulateMatrix3x3(float b[3][3], float s, float a[3][3]);
-
-// matrix product
-// c[x][y] = a[x][0]*b[0][y]+a[x][1]*b[1][y]+a[x][2]*b[2][y]+a[x][3]*b[3][y]
-void matrixProduct3x3(float c[3][3], float a[3][3], float b[3][3]);
-
-// matrix times vector
-void matrixDotVector3x3(float p[3], float m[3][3], float v[3]);
-
-// determinant of matrix
-// Computes determinant of matrix m, returning d
-void determinant3x3(float * d, float m[3][3]);
-
-// adjoint of matrix
-// Computes adjoint of matrix m, returning a
-// (Note that adjoint is just the transpose of the cofactor matrix);
-void adjoint3x3(float a[3][3], float m[3][3]);
-
-// compute adjoint of matrix and scale
-// Computes adjoint of matrix m, scales it by s, returning a
-void scaleAdjoint3x3(float a[3][3], float s, float m[3][3]);
-
-// inverse of matrix
-// Compute inverse of matrix a, returning determinant m and
-// inverse b
-void invert3x3(float b[3][3], float a[3][3]);
-
-// skew matrix from vector
-void skew(float a[3][3], float v[3]);
-
-void printMatrix3X3(float mmm[3][3]);
-
-void vecPrint(float a[3]);
diff --git a/src/AltEst/altitude.cpp b/src/AltEst/altitude.cpp
deleted file mode 100644
index 8838b36..0000000
--- a/src/AltEst/altitude.cpp
+++ /dev/null
@@ -1,58 +0,0 @@
-/*
-    altitude.cpp: Altitude estimation via barometer/accelerometer fusion
-*/
-
-#include "filters.h"
-#include "algebra.h"
-#include "altitude.h"
-
-AltitudeEstimator::AltitudeEstimator(float sigmaAccel, float sigmaGyro, float sigmaBaro,
-                                     float ca, float accelThreshold)
-:kalman(ca, sigmaGyro, sigmaAccel), complementary(sigmaAccel, sigmaBaro, accelThreshold)
-{
-      this->sigmaAccel = sigmaAccel;
-      this->sigmaGyro = sigmaGyro;
-      this->sigmaBaro = sigmaBaro;
-      this->ca = ca;
-      this->accelThreshold = accelThreshold;
-}
-
-void AltitudeEstimator::estimate(float accel[3], float gyro[3], float baroHeight, uint32_t timestamp)
-{
-        float deltat = (float)(timestamp-previousTime)/1000000.0f;
-        float verticalAccel = kalman.estimate(pastGyro,
-                                              pastAccel,
-                                              deltat);
-        complementary.estimate(& estimatedVelocity,
-                               & estimatedAltitude,
-                               baroHeight,
-                               pastAltitude,
-                               pastVerticalVelocity,
-                               pastVerticalAccel,
-                               deltat);
-        // update values for next iteration
-        copyVector(pastGyro, gyro);
-        copyVector(pastAccel, accel);
-        pastAltitude = estimatedAltitude;
-        pastVerticalVelocity = estimatedVelocity;
-        pastVerticalAccel = verticalAccel;
-        previousTime = timestamp;
-}
-
-float AltitudeEstimator::getAltitude()
-{
-        // return the last estimated altitude
-        return estimatedAltitude;
-}
-
-float AltitudeEstimator::getVerticalVelocity()
-{
-        // return the last estimated vertical velocity
-        return estimatedVelocity;
-}
-
-float AltitudeEstimator::getVerticalAcceleration()
-{
-        // return the last estimated vertical acceleration
-        return pastVerticalAccel;
-}
diff --git a/src/AltEst/altitude.h b/src/AltEst/altitude.h
deleted file mode 100644
index 1ca6cb0..0000000
--- a/src/AltEst/altitude.h
+++ /dev/null
@@ -1,55 +0,0 @@
-/*
-    altitude.h: Altitude estimation via barometer/accelerometer fusion
-*/
-
-# pragma once
-
-#include "filters.h"
-#include "algebra.h"
-#include "pico/time.h"
-#include "pico/types.h"
-
-class AltitudeEstimator {
-
-  private:
-    // required parameters for the filters used for the estimations
-    // sensor's standard deviations
-    float sigmaAccel;
-    float sigmaGyro;
-    float sigmaBaro;
-    // Acceleration markov chain model state transition constant
-    float ca;
-    // Zero-velocity update acceleration threshold
-    float accelThreshold;
-    // gravity
-    float g = 9.81;
-    // For computing the sampling period
-    absolute_time_t prevTime = get_absolute_time();
-    uint32_t previousTime = to_us_since_boot(prevTime);
-    // required filters for altitude and vertical velocity estimation
-    KalmanFilter kalman;
-    ComplementaryFilter complementary;
-    // Estimated past vertical acceleration
-    float pastVerticalAccel = 0;
-    float pastVerticalVelocity = 0;
-    float pastAltitude = 0;
-    float pastGyro[3] = {0, 0, 0};
-    float pastAccel[3] = {0, 0, 0};
-    // estimated altitude and vertical velocity
-    float estimatedAltitude = 0;
-    float estimatedVelocity = 0;
-
-  public:
-
-    AltitudeEstimator(float sigmaAccel, float sigmaGyro, float sigmaBaro,
-                      float ca, float accelThreshold);
-
-    void estimate(float accel[3], float gyro[3], float baroHeight, uint32_t timestamp);
-
-    float getAltitude();
-
-    float getVerticalVelocity();
-
-    float getVerticalAcceleration();
-
-}; // class AltitudeEstimator
diff --git a/src/AltEst/filters.cpp b/src/AltEst/filters.cpp
deleted file mode 100644
index 7902065..0000000
--- a/src/AltEst/filters.cpp
+++ /dev/null
@@ -1,202 +0,0 @@
-/*
-   filters.cpp: Filter class implementations
- */
-
-//#include <cmath>
-#include <stdlib.h> // XXX eventually use fabs() instead of abs() ?
-
-#include "filters.h"
-
-void KalmanFilter::getPredictionCovariance(float covariance[3][3], float previousState[3], float deltat)
-{
-    // required matrices for the operations
-    float sigma[3][3];
-    float identity[3][3];
-    identityMatrix3x3(identity);
-    float skewMatrix[3][3];
-    skew(skewMatrix, previousState);
-    float tmp[3][3];
-    // Compute the prediction covariance matrix
-    scaleMatrix3x3(sigma, pow(sigmaGyro, 2), identity);
-    matrixProduct3x3(tmp, skewMatrix, sigma);
-    matrixProduct3x3(covariance, tmp, skewMatrix);
-    scaleMatrix3x3(covariance, -pow(deltat, 2), covariance);
-}
-
-void KalmanFilter::getMeasurementCovariance(float covariance[3][3])
-{
-    // required matrices for the operations
-    float sigma[3][3];
-    float identity[3][3];
-    identityMatrix3x3(identity);
-    float norm;
-    // Compute measurement covariance
-    scaleMatrix3x3(sigma, pow(sigmaAccel, 2), identity);
-    vectorLength(& norm, previousAccelSensor);
-    scaleAndAccumulateMatrix3x3(sigma, (1.0/3.0)*pow(ca, 2)*norm, identity);
-    copyMatrix3x3(covariance, sigma);
-}
-
-void KalmanFilter::predictState(float predictedState[3], float gyro[3], float deltat)
-{
-    // helper matrices
-    float identity[3][3];
-    identityMatrix3x3(identity);
-    float skewFromGyro[3][3];
-    skew(skewFromGyro, gyro);
-    // Predict state
-    scaleAndAccumulateMatrix3x3(identity, -deltat, skewFromGyro);
-    matrixDotVector3x3(predictedState, identity, currentState);
-    normalizeVector(predictedState);
-}
-
-void KalmanFilter::predictErrorCovariance(float covariance[3][3], float gyro[3], float deltat)
-{
-    // required matrices
-    float Q[3][3];
-    float identity[3][3];
-    identityMatrix3x3(identity);
-    float skewFromGyro[3][3];
-    skew(skewFromGyro, gyro);
-    float tmp[3][3];
-    float tmpTransposed[3][3];
-    float tmp2[3][3];
-    // predict error covariance
-    getPredictionCovariance(Q, currentState, deltat);
-    scaleAndAccumulateMatrix3x3(identity, -deltat, skewFromGyro);
-    copyMatrix3x3(tmp, identity);
-    transposeMatrix3x3(tmpTransposed, tmp);
-    matrixProduct3x3(tmp2, tmp, currErrorCovariance);
-    matrixProduct3x3(covariance, tmp2, tmpTransposed);
-    scaleAndAccumulateMatrix3x3(covariance, 1.0, Q);
-}
-
-void KalmanFilter::updateGain(float gain[3][3], float errorCovariance[3][3])
-{
-    // required matrices
-    float R[3][3];
-    float HTransposed[3][3];
-    transposeMatrix3x3(HTransposed, H);
-    float tmp[3][3];
-    float tmp2[3][3];
-    float tmp2Inverse[3][3];
-    // update kalman gain
-    // P.dot(H.T).dot(inv(H.dot(P).dot(H.T) + R))
-    getMeasurementCovariance(R);
-    matrixProduct3x3(tmp, errorCovariance, HTransposed);
-    matrixProduct3x3(tmp2, H, tmp);
-    scaleAndAccumulateMatrix3x3(tmp2, 1.0, R);
-    invert3x3(tmp2Inverse, tmp2);
-    matrixProduct3x3(gain, tmp, tmp2Inverse);
-}
-
-void KalmanFilter::updateState(float updatedState[3], float predictedState[3], float gain[3][3], float accel[3])
-{
-    // required matrices
-    float tmp[3];
-    float tmp2[3];
-    float measurement[3];
-    scaleVector(tmp, ca, previousAccelSensor);
-    subtractVectors(measurement, accel, tmp);
-    // update state with measurement
-    // predicted_state + K.dot(measurement - H.dot(predicted_state))
-    matrixDotVector3x3(tmp, H, predictedState);
-    subtractVectors(tmp, measurement, tmp);
-    matrixDotVector3x3(tmp2, gain, tmp);
-    sumVectors(updatedState, predictedState, tmp2);
-    normalizeVector(updatedState);
-}
-
-void KalmanFilter::updateErrorCovariance(float covariance[3][3], float errorCovariance[3][3], float gain[3][3])
-{
-    // required matrices
-    float identity[3][3];
-    identityMatrix3x3(identity);
-    float tmp[3][3];
-    float tmp2[3][3];
-    // update error covariance with measurement
-    matrixProduct3x3(tmp, gain, H);
-    matrixProduct3x3(tmp2, tmp, errorCovariance);
-    scaleAndAccumulateMatrix3x3(identity, -1.0, tmp2);
-    copyMatrix3x3(covariance, tmp2);
-}
-
-
-KalmanFilter::KalmanFilter(float ca, float sigmaGyro, float sigmaAccel)
-{
-    this->ca = ca;
-    this->sigmaGyro = sigmaGyro;
-    this->sigmaAccel = sigmaAccel;
-}
-
-float KalmanFilter::estimate(float gyro[3], float accel[3], float deltat)
-{
-    float predictedState[3];
-    float updatedState[3];
-    float errorCovariance[3][3];
-    float updatedErrorCovariance[3][3];
-    float gain[3][3];
-    float accelSensor[3];
-    float tmp[3];
-    float accelEarth;
-    scaleVector(accel, 9.81, accel); // Scale accel readings since they are measured in gs
-    // perform estimation
-    // predictions
-    predictState(predictedState, gyro, deltat);
-    predictErrorCovariance(errorCovariance, gyro, deltat);
-    // updates
-    updateGain(gain, errorCovariance);
-    updateState(updatedState, predictedState, gain, accel);
-    updateErrorCovariance(updatedErrorCovariance, errorCovariance, gain);
-    // Store required values for next iteration
-    copyVector(currentState, updatedState);
-    copyMatrix3x3(currErrorCovariance, updatedErrorCovariance);
-    // return vertical acceleration estimate
-    scaleVector(tmp, 9.81, updatedState);
-    subtractVectors(accelSensor, accel, tmp);
-    copyVector(previousAccelSensor, accelSensor);
-    dotProductVectors(& accelEarth, accelSensor, updatedState);
-    return accelEarth;
-}
-
-
-float ComplementaryFilter::ApplyZUPT(float accel, float vel)
-{
-    // first update ZUPT array with latest estimation
-    ZUPT[ZUPTIdx] = accel;
-    // and move index to next slot
-    uint8_t nextIndex = (ZUPTIdx + 1) % ZUPT_SIZE;
-    ZUPTIdx = nextIndex;
-    // Apply Zero-velocity update
-    for (uint8_t k = 0; k < ZUPT_SIZE; ++k) {
-        if (abs(ZUPT[k]) > accelThreshold) return vel;
-    }
-    return 0.0;
-}
-
-
-ComplementaryFilter::ComplementaryFilter(float sigmaAccel, float sigmaBaro, float accelThreshold)
-{
-    // Compute the filter gain
-    gain[0] = sqrt(2 * sigmaAccel / sigmaBaro);
-    gain[1] = sigmaAccel / sigmaBaro;
-    // If acceleration is below the threshold the ZUPT counter
-    // will be increased
-    this->accelThreshold = accelThreshold;
-    // initialize zero-velocity update
-    ZUPTIdx = 0;
-    for (uint8_t k = 0; k < ZUPT_SIZE; ++k) {
-        ZUPT[k] = 0;
-    }
-}
-
-void ComplementaryFilter::estimate(float * velocity, float * altitude, float baroAltitude,
-        float pastAltitude, float pastVelocity, float accel, float deltat)
-{
-    // Apply complementary filter
-    *altitude = pastAltitude + deltat*(pastVelocity + (gain[0] + gain[1]*deltat/2)*(baroAltitude-pastAltitude))+
-        accel*pow(deltat, 2)/2;
-    *velocity = pastVelocity + deltat*(gain[1]*(baroAltitude-pastAltitude) + accel);
-    // Compute zero-velocity update
-    *velocity = ApplyZUPT(accel, *velocity);
-}
diff --git a/src/AltEst/filters.h b/src/AltEst/filters.h
deleted file mode 100644
index 2e316a3..0000000
--- a/src/AltEst/filters.h
+++ /dev/null
@@ -1,65 +0,0 @@
-/*
-   filters.h: Filter class declarations
- */
-
-#pragma once
-
-//#include <cmath>
-#include <math.h>
-#include <stdint.h>
-
-#include "algebra.h"
-
-class KalmanFilter {
-  private:
-    float currentState[3] = {0, 0, 1};
-    float currErrorCovariance[3][3] = {{100, 0, 0},{0, 100, 0},{0, 0, 100}};
-    float H[3][3] = {{9.81, 0, 0}, {0, 9.81, 0}, {0, 0, 9.81}};
-    float previousAccelSensor[3] = {0, 0, 0};
-    float ca;
-    float sigmaGyro;
-    float sigmaAccel;
-
-    void getPredictionCovariance(float covariance[3][3], float previousState[3], float deltat);
-
-    void getMeasurementCovariance(float covariance[3][3]);
-
-    void predictState(float predictedState[3], float gyro[3], float deltat);
-
-    void predictErrorCovariance(float covariance[3][3], float gyro[3], float deltat);
-
-    void updateGain(float gain[3][3], float errorCovariance[3][3]);
-
-    void updateState(float updatedState[3], float predictedState[3], float gain[3][3], float accel[3]);
-
-    void updateErrorCovariance(float covariance[3][3], float errorCovariance[3][3], float gain[3][3]);
-
-  public:
-
-    KalmanFilter(float ca, float sigmaGyro, float sigmaAccel);
-
-    float estimate(float gyro[3], float accel[3], float deltat);
-
-}; // Class KalmanFilter
-
-class ComplementaryFilter {
-
-  private:
-
-    // filter gain
-    float gain[2];
-    // Zero-velocity update
-    float accelThreshold;
-    static const uint8_t ZUPT_SIZE = 12;
-    uint8_t ZUPTIdx;
-    float   ZUPT[ZUPT_SIZE];
-
-    float ApplyZUPT(float accel, float vel);
-
-  public:
-
-    ComplementaryFilter(float sigmaAccel, float sigmaBaro, float accelThreshold);
-
-    void estimate(float * velocity, float * altitude, float baroAltitude,
-                  float pastAltitude, float pastVelocity, float accel, float deltat);
-}; // Class ComplementaryFilter
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 59d21de..e372541 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,54 +1,17 @@
 add_executable(ads
-        active_drag_system.cpp
-        spi_flash.c
-        bno055.cpp
-        SimpleKalmanFilter.cpp
-        # kalmanfilter.cpp
-        pwm.cpp
-        AltEst/algebra.cpp
-        AltEst/altitude.cpp
-        AltEst/filters.cpp
-        )
-
-add_executable(read_flash
-        read_flash.c
-        spi_flash.c
-        )
-
-add_executable(servo_test
-        servo_test.cpp
-        pwm.cpp
-        )
-
-add_executable(alt_test
-        altimeter.cpp
-        )
-
-# pull in common dependencies
-target_link_libraries(ads pico_stdlib pico_multicore pico_sync hardware_i2c hardware_spi hardware_pwm pico_cyw43_arch_none ${Eigen_LIBRARIES})
+               active_drag_system.cpp
+               spi_flash.c
+               imu.cpp
+               pwm.cpp
+               altimeter.cpp
+               kalman_filter.cpp
+)
+
+target_link_libraries(ads pico_stdlib pico_multicore pico_sync hardware_i2c hardware_spi hardware_pwm hardware_adc pico_cyw43_arch_none ${Eigen_LIBRARIES})
 target_include_directories(ads PUBLIC ../include)
 
-target_link_libraries(read_flash pico_stdlib hardware_spi)
-target_include_directories(read_flash PUBLIC ../include)
-
-target_link_libraries(servo_test pico_stdlib hardware_pwm hardware_i2c)
-target_include_directories(servo_test PUBLIC ../include)
-
-target_link_libraries(alt_test pico_stdlib hardware_i2c hardware_gpio)
-
-pico_enable_stdio_usb(ads 1)
+pico_enable_stdio_usb(ads 0)
 pico_enable_stdio_uart(ads 0)
 
-pico_enable_stdio_usb(read_flash 1)
-pico_enable_stdio_uart(read_flash 0)
-
-pico_enable_stdio_usb(servo_test 1)
-pico_enable_stdio_uart(servo_test 0)
-
-pico_enable_stdio_usb(alt_test 1)
-pico_enable_stdio_uart(alt_test 0)
-# create map/bin/hex file etc.
 pico_add_extra_outputs(ads)
-pico_add_extra_outputs(read_flash)
-pico_add_extra_outputs(servo_test)
-pico_add_extra_outputs(alt_test)
+
diff --git a/src/SimpleKalmanFilter.cpp b/src/SimpleKalmanFilter.cpp
deleted file mode 100644
index 88ba5d4..0000000
--- a/src/SimpleKalmanFilter.cpp
+++ /dev/null
@@ -1,48 +0,0 @@
-/*
- * SimpleKalmanFilter - a Kalman Filter implementation for single variable models.
- * Created by Denys Sene, January, 1, 2017.
- * Released under MIT License - see LICENSE file for details.
- */
-
-#include "SimpleKalmanFilter.h"
-#include <math.h>
-
-SimpleKalmanFilter::SimpleKalmanFilter(float mea_e, float est_e, float q)
-{
-  _err_measure=mea_e;
-  _err_estimate=est_e;
-  _q = q;
-}
-
-float SimpleKalmanFilter::updateEstimate(float mea)
-{
-  _kalman_gain = _err_estimate/(_err_estimate + _err_measure);
-  _current_estimate = _last_estimate + _kalman_gain * (mea - _last_estimate);
-  _err_estimate =  (1.0f - _kalman_gain)*_err_estimate + fabsf(_last_estimate-_current_estimate)*_q;
-  _last_estimate=_current_estimate;
-
-  return _current_estimate;
-}
-
-void SimpleKalmanFilter::setMeasurementError(float mea_e)
-{
-  _err_measure=mea_e;
-}
-
-void SimpleKalmanFilter::setEstimateError(float est_e)
-{
-  _err_estimate=est_e;
-}
-
-void SimpleKalmanFilter::setProcessNoise(float q)
-{
-  _q=q;
-}
-
-float SimpleKalmanFilter::getKalmanGain() {
-  return _kalman_gain;
-}
-
-float SimpleKalmanFilter::getEstimateError() {
-  return _err_estimate;
-}
diff --git a/src/active_drag_system.cpp b/src/active_drag_system.cpp
index 5d248e2..6f5d57e 100644
--- a/src/active_drag_system.cpp
+++ b/src/active_drag_system.cpp
@@ -1,37 +1,45 @@
-#include <algorithm>
-#include <stdio.h>
+#include "cyw43_configport.h"
 #include "pico/multicore.h"
 #include "pico/platform.h"
 #include "pico/sem.h"
-#include "spi_flash.h"
 #include "boards/pico_w.h"
 #include "hardware/gpio.h"
 #include "hardware/i2c.h"
+#include "hardware/adc.h"
 #include "pico/stdlib.h"
 #include "pico/time.h"
 #include "pico/types.h"
 #include "pico/cyw43_arch.h"
+#include <inttypes.h>
 #include <math.h>
 
-#include "bno055.hpp"
-#include "AltEst/altitude.h"
 #include "pwm.hpp"
-#include "SimpleKalmanFilter.h"
+#include "imu.hpp"
+#include "altimeter.hpp"
+#include "kalman_filter.hpp"
+#include "spi_flash.h"
+
+#define MPL3115A2_ADDR 0x60
+
+#define BNO055_ADDR 0x28
+#define BNO055_ID 0xA0
 
-#define ALT_ADDR 0x60
 #define MAX_SCL 400000
 #define DATA_RATE_HZ 100
 #define LOOP_PERIOD (1.0f / DATA_RATE_HZ)
 #define INT1_PIN 6 // INT1 PIN on MPL3115A2 connected to GPIO PIN 9 (GP6)
 #define MOSFET_PIN 26 // MOSFET PIN connected to GPIO PIN 31 (GP26)
 
-#define GRAVITY -9.81
-#define LOG_RATE_HZ 4
+#define LOG_RATE_HZ 8
+#define HEART_RATE_HZ 5
 
 #define MOTOR_BURN_TIME 3900 // Burn time in milliseconds for M2500T
 
+#define PAD_SECONDS 8
+#define PAD_BUFFER_SIZE (PACKET_SIZE * LOG_RATE_HZ * PAD_SECONDS)
+
 typedef enum {
-    PAD,
+    PAD = 0,
     BOOST,
     COAST,
     APOGEE,
@@ -39,76 +47,67 @@ typedef enum {
     END
 } state_t;
 
-BNO055 bno055;
 PWM pwm;
-static AltitudeEstimator vKF = AltitudeEstimator(0.0005, // sigma Accel
-                                                      0.0005, // sigma Gyro
-                                                      0.018,   // sigma Baro
-                                                      0.5, // ca
-                                                      0.1);// accelThreshold
+kalman_filter *kf;
+VectorXf control(1);
+VectorXf measurement(1);
+VectorXf res(2);
 
 void pad_callback(uint gpio, uint32_t event_mask);
 int64_t boost_callback(alarm_id_t id, void* user_data);
 int64_t apogee_callback(alarm_id_t id, void* user_data);
 int64_t coast_callback(alarm_id_t id, void* user_data);
 void recovery_callback(uint gpio, uint32_t event_mask);
-void init_altimeter();
 float get_deploy_percent(float velocity, float altitude);
 
 bool timer_callback(repeating_timer_t *rt);
-bool test_timer_callback(repeating_timer_t *rt);
-
-float get_altitude();
-float get_velocity();
 
-void snapshot();
-bool logging_callback(repeating_timer_t *rt);
+void populate_entry();
+bool logging_buffer_callback(repeating_timer_t *rt);
+bool logging_flash_callback(repeating_timer_t *rt);
+bool heartbeat_callback(repeating_timer_t *rt);
 void logging_core();
 
 semaphore_t sem;
 
 volatile float altitude = 0.0f;
-volatile float prev_altitude = 0.0f;
 volatile float velocity = 0.0f;
 volatile state_t state = PAD;
 volatile float threshold_altitude = 30.0f;
 volatile float threshold_velocity = 30.0f;
 volatile uint8_t deployment_percent = 0;
+volatile uint8_t led_counter;
+volatile uint32_t pad_buffer_offset = 0;
 
-volatile vector3f linear_acceleration;
-volatile vector3f acceleration;
-volatile quarternion abs_quaternion;
-volatile vector3f velocity_vector;
+Eigen::Vector3f linear_acceleration;
+Eigen::Vector4f quaternion;
+Eigen::Vector3f euler_angles;
 
-volatile vector3f euler_angles;
-volatile vector3f abs_lin_accel;
-volatile vector3f prev_abs_lin_accel;
-volatile vector3f rot_y_vec;
-volatile vector3f vel_at_angle;
-
-volatile vector3f accel_gravity;
-
-volatile CALIB_STATUS calib_status;
-uint8_t accel[6];
-uint8_t quat[8];
+volatile calibration_status_t calib_status;
 
 repeating_timer_t data_timer;
 repeating_timer_t log_timer;
+repeating_timer_t heartbeat_timer;
 
 float ground_altitude = 0.0f;
 
-SimpleKalmanFilter altitudeKF(2, 2, 0.01);
-SimpleKalmanFilter velocityKF(1, 1, 0.01);
+altimeter altimeter(i2c_default, MPL3115A2_ADDR);
+imu imu(i2c_default, BNO055_ADDR, BNO055_ID, NDOF);
+
+uint8_t *altimeter_buffer;
+uint8_t *acceleration_buffer;
+uint8_t *quaternion_buffer;
+
+uint8_t entry_buffer[PACKET_SIZE];
+
+uint8_t *pad_buffer;
 
-/**
- * @brief Main function
- * 
- * @return int error code
- */
 int main() {
-    // stdio_init_all();
+    adc_init();
+    adc_set_temp_sensor_enabled(true);
 
     cyw43_arch_init();
+
     i2c_init(i2c_default, MAX_SCL);
     gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C);
     gpio_set_function(PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C);
@@ -131,34 +130,39 @@ int main() {
     
     alarm_pool_init_default();
 
+    altimeter.initialize(30.0f, INT1_PIN, &pad_callback);
 
-    // Initialize altimeter
-    init_altimeter();
+    imu.initialize();
+    imu.linear_acceleration(linear_acceleration);
+    imu.quaternion(quaternion);
+    imu.quaternion_euler(euler_angles, quaternion);
 
-    // Initialize BNO055
-    bno055.init();
-
-    // Initialize PWM
     pwm.init();
 
     // Initialize MOSFET
     gpio_init(MOSFET_PIN);
     gpio_set_dir(MOSFET_PIN, GPIO_OUT);
 
-    // Initialize Kalman Filter
-    float measurement = get_altitude();
-    float v_measurement = get_velocity();
-    altitudeKF.updateEstimate(measurement);
-    velocityKF.updateEstimate(measurement);
+    cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 1);
 
-    ground_altitude = altitude;
+    pad_buffer = (uint8_t*)malloc(PAD_BUFFER_SIZE);
+
+    // Initialize Kalman Filter
+    kf = new kalman_filter(2, 1, 1, 0.01);
+    VectorXf state_vec(2);
+    MatrixXf state_cov(2, 2);
+    state_vec << altimeter.get_altitude_converted(), linear_acceleration.z();
+    state_cov << 0.018, 0.0, 0.0, 0.0005;
+    kf->state_initialize(state_vec, state_cov);
+    ground_altitude = altimeter.get_altitude_converted();
+
+    altimeter.expose_buffer(&altimeter_buffer);
+    imu.expose_acceleration_buffer(&acceleration_buffer);
+    imu.expose_quaternion_buffer(&quaternion_buffer);
 
     sem_init(&sem, 1, 1);
 
-    if (!add_repeating_timer_us(-1000000 / DATA_RATE_HZ,  &timer_callback, NULL, &data_timer)) {
-        // printf("Failed to add timer!\n");
-        return -1;
-    }
+    add_repeating_timer_us(-1000000 / DATA_RATE_HZ,  &timer_callback, NULL, &data_timer);
 
     multicore_launch_core1(logging_core);
 
@@ -167,218 +171,22 @@ int main() {
     }
 }
 
-void logging_core() {
-    add_repeating_timer_us(-1000000 / LOG_RATE_HZ,  &logging_callback, NULL, &log_timer);
-
-    while (1) {
-        tight_loop_contents();
-    }
-}
-
-/**
- * @brief Initializes the altimeter
- * 
- * @param altitude passes the altitude variable to the function
- * @param threshold_altitude passes the threshold altitude variable to the function
- */
-void init_altimeter() {
-
-    uint8_t config[2] = {0};
-
-    // Select control register(0x26)
-    // Active mode, OSR = 16, altimeter mode(0xB8)
-    config[0] = 0x26;
-    config[1] = 0x89;
-    i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
-
-    // Select data configuration register(0x13)
-    // Data ready event enabled for altitude, pressure, temperature(0x07)
-    // config[0] = 0x13;
-    // config[1] = 0x07;
-    // i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
-
-    // Below configures the interrupt for the first transition from PAD to BOOST
-    // Initial Reading
-
-    sleep_ms(1000);
-
-    while (altitude == 0.0f) {
-        altitude = get_altitude();
-    }
-
-    threshold_altitude += altitude; // 30 meters above ground
-
-    // printf("threshold_altitude: %4.2f", threshold_altitude);
-
-    // Select control register 3 (0x28)
-    // Set bot interrupt pins to active low and enable internal pullups
-    config[0] = 0x28;
-    config[1] = 0x01;
-    i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
-
-    // Select pressure target MSB register(0x16)
-    // Set altitude target to 30 meters above ground altitude
-    config[0] = 0x16;
-    config[1] = (uint8_t) (((int16_t)(threshold_altitude)) >> 8);
-    // printf("threshold_alt upper half: %X\n", config[1]);
-    i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
-
-    // Select pressure target LSB register(0x17)
-    // Set altitude target to 30 meters above ground altitude
-    config[0] = 0x17;
-    config[1] = (uint8_t) (((int16_t)(threshold_altitude)));
-    // printf("threshold_alt lower half: %X\n", config[1]);
-    i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
-
-    // Select interrupt enable register (0x29)
-    // Set interrupt enabled for altitude threshold(0x08)
-    config[0] = 0x29;
-    config[1] = 0x08;
-    i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
-
-    // Select interrupt configuration register (0x2A)
-    // Set interrupt enabled for altitude threshold to route to INT1 pin(0x08)
-    config[0] = 0x2A;
-    config[1] = 0x08;
-    i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
-
-    gpio_set_irq_enabled_with_callback(INT1_PIN, GPIO_IRQ_LEVEL_LOW, true, &pad_callback);
-    // End of configuration of interrupt for first transition from PAD to BOOST
-}
-
-void snapshot() {
-    if (state != END) {
-        uint8_t entry[PACKET_SIZE];
-        absolute_time_t now = get_absolute_time();
-        uint64_t now_us = to_us_since_boot(now);
-        uint32_t alt_bits = *((uint32_t *)&altitude);
-        uint32_t vel_bits = *((uint32_t *)&velocity);
-        uint32_t acc_bits = *((uint32_t *)&abs_lin_accel.z);
-        entry[0] = now_us >> 56;
-        entry[1] = now_us >> 48;
-        entry[2] = now_us >> 40;
-        entry[3] = now_us >> 32;
-        entry[4] = now_us >> 24;
-        entry[5] = now_us >> 16;
-        entry[6] = now_us >> 8;
-        entry[7] = now_us;
-
-        switch (state) {
-            case PAD:
-                entry[8] = 'P';
-                break;
-            case BOOST:
-                entry[8] = 'B';
-                break;
-            case COAST:
-                entry[8] = 'C';
-                break;
-            case APOGEE:
-                entry[8] = 'A';
-                break;
-            case RECOVERY:
-                entry[8] = 'R';
-                break;
-            case END:
-                entry[8] = 'E';
-                break;
-        }
-
-        entry[9] = deployment_percent;
-        entry[10] = alt_bits >> 24;
-        entry[11] = alt_bits >> 16;
-        entry[12] = alt_bits >> 8;
-        entry[13] = alt_bits;
-        entry[14] = vel_bits >> 24;
-        entry[15] = vel_bits >> 16;
-        entry[16] = vel_bits >> 8;
-        entry[17] = vel_bits;
-
-        entry[18] = quat[0];
-        entry[19] = quat[1];
-        entry[20] = quat[2];
-        entry[21] = quat[3];
-        entry[22] = quat[4];
-        entry[23] = quat[5];
-        entry[24] = quat[6];
-        entry[25] = quat[7];
-
-        entry[26] = acc_bits >> 24;
-        entry[27] = acc_bits >> 16;
-        entry[28] = acc_bits >> 8;
-        entry[29] = acc_bits;
-        entry[30] = accel[4];
-        entry[31] = accel[5];
-        write_entry(entry);
-    }
-}
-
-bool logging_callback(repeating_timer_t *rt) {
-    static bool LED_STATUS = 0;
-    sem_acquire_blocking(&sem);
-    LED_STATUS = !LED_STATUS;
-    cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, LED_STATUS);
-    snapshot();
-    sem_release(&sem);
-    return true;
-}
+// PRIMARY THREAD RELATED FUNCTIONS AND CALLBACKS
+//===============================================================================
 
 bool timer_callback(repeating_timer_t *rt) {
-    absolute_time_t last = get_absolute_time();
     sem_acquire_blocking(&sem);
-    float measurement = get_altitude();
-    altitude = altitudeKF.updateEstimate(measurement);
-    // float in_velocity = (altitude - prev_altitude) * DATA_RATE_HZ;
-    // velocity = velocityKF.updateEstimate(in_velocity);
-    float acceldata[3];
-    float gyrodata[3];
-
-
-    // printf("Velocity_Delta: %4.2f\tVelocity_Prev: %4.2f\n", velocity, ((altitude - prev_altitude) * DATA_RATE_HZ));
+    imu.linear_acceleration(linear_acceleration);
+    imu.quaternion(quaternion);
 
-    bno055.read_lin_accel();
-    // printf("Linear Acceleration:\n" "x: %f\n" "y: %f\n" "z: %f\n",
-    //  linear_acceleration.x, linear_acceleration.y, linear_acceleration.z);
+    control(0) = linear_acceleration.z();
+    measurement(0) = altimeter.get_altitude_converted();
+    res = kf->run(control, measurement, 0.01f);
+    altitude = res(0);
+    velocity = res(1);
 
-    bno055.read_abs_quaternion();
-    // printf("Absolute Quaternion:\n" "w: %f\n" "x: %f\n" "y: %f\n" "z: %f\n",
-    //  abs_quaternion.w, abs_quaternion.x, abs_quaternion.y, abs_quaternion.z);
 
-    bno055.read_euler_angles();
-    // printf("Euler Angles:\n" "Roll: %f\n" "Pitch: %f\n" "Yaw: %f\n",
-    //  euler_angles.x, euler_angles.y, euler_angles.z);
-
-    // Linear Acceleration and Absolute Quaternion are used to calculate Absolute Linear Acceleration
-    // They must be read before calling this function
-    bno055.calculate_abs_linear_acceleration();
-    // printf("Absolute Linear Acceleration:\n" "x: %f\n" "y: %f\n" "z: %f\n",
-    // abs_lin_accel.x, abs_lin_accel.y, abs_lin_accel.z);
-    
-    acceldata[0] = abs_lin_accel.x;
-    acceldata[1] = abs_lin_accel.y;
-    acceldata[2] = abs_lin_accel.z - 0.4f;
-    gyrodata[0] = 0;
-    gyrodata[1] = 0;
-    gyrodata[2] = 0;
-
-    vKF.estimate(acceldata, gyrodata, altitude, to_us_since_boot(last));
-    velocity = vKF.getVerticalVelocity();
-    // printf("Measurement: %4.2f, Altitude: %4.2f, Velocity: %4.2f\n", measurement, altitude, velocity);
-    // This is wrong but i'm going home.
-    // velocity_vector.x = (prev_abs_lin_accel.x - abs_lin_accel.x) / 0.01f);
-    // velocity_vector.y = (prev_abs_lin_accel.y - abs_lin_accel.y) / 0.01f);
-    // velocity_vector.z = (prev_abs_lin_accel.z - abs_lin_accel.z) / 0.01f);
-    // printf("Velocity Vector:\n" "x: %f\n" "y: %f\n" "z: %f\n",
-    // velocity_vector.x, velocity_vector.y, velocity_vector.z);
-
-    prev_abs_lin_accel.x = abs_lin_accel.x;
-    prev_abs_lin_accel.y = abs_lin_accel.y;
-    prev_abs_lin_accel.z = abs_lin_accel.z;
-
-    bno055.accel_to_gravity();
-    float agl = altitude - ground_altitude;
-
-    deployment_percent = (uint8_t)(std::min(std::max(30.0f, get_deploy_percent(velocity, agl)), 100.0f));
+    deployment_percent = (uint8_t)(std::min(std::max(30.0f, get_deploy_percent(velocity, (altitude - ground_altitude))), 100.0f));
 
     switch(state) {
         case PAD:
@@ -411,69 +219,11 @@ bool timer_callback(repeating_timer_t *rt) {
             deployment_percent = 0;
             break;
     }
-    prev_altitude = altitude;
     sem_release(&sem);
     return true;
 }
 
 /**
- * @brief Test function for timer callback outputs data in ROS2 format
- * 
- * @param rt 
- * @return true
- * @return false 
- */
-// bool test_timer_callback(repeating_timer_t *rt) {
-//     static float prev_altitude = altitude;
-//     absolute_time_t last = get_absolute_time();
-//     altitude = get_altitude();
-//     velocity = ((altitude - prev_altitude) / 0.01f);
-//     prev_altitude = altitude;
-// 
-//     bno055.read_lin_accel();
-//     bno055.read_abs_quaternion();
-// 
-//     absolute_time_t now = get_absolute_time();
-//     int64_t time_delta = absolute_time_diff_us(last, now);
-// 
-//     std::cout << altitude << " " << abs_quaternion.w << " "
-//           << abs_quaternion.x << " "
-//           << abs_quaternion.y << " "
-//           << abs_quaternion.z << " "
-//           << linear_acceleration.x << " "
-//           << linear_acceleration.y << " "
-//           << linear_acceleration.z << std::endl;
-// 
-//     /* switch (state) {
-//         case PAD:
-//             printf("P\n");
-//             break;
-//         case BOOST:
-//             printf("B\n");
-//             break;
-//         case COAST:
-//             printf("C\n");
-//             break;
-//         case APOGEE:
-//             printf("A\n");
-//             break;
-//         case RECOVERY:
-//             printf("R\n");
-//             break;
-//         case END:
-//             printf("E\n");
-//             break;
-//     }*/
-// 
-//     // absolute_time_t now = get_absolute_time();
-//     // int64_t time_delta = absolute_time_diff_us(last, now);
-//     // printf("Time Delta: %" PRIi64"\n", time_delta);
-//     // std::flush(std::cout);
-//     prev_altitude = altitude;
-//     return true;
-// }
-
-/**
  * @brief Call back function for when rocket is on the pad
  * 
  * @param gpio pin number of interrupt
@@ -485,29 +235,12 @@ bool timer_callback(repeating_timer_t *rt) {
  * @link https://www.raspberrypi.com/documentation/pico-sdk/hardware/gpio.html#ga6347e27da3ab34f1ea65b5ae16ab724f
  */
 void pad_callback(uint gpio, uint32_t event_mask) {
-
-    /// @link https://www.raspberrypi.com/documentation/pico-sdk/hardware.html#ga6165f07f4b619dd08ea6dc97d069e78a
-    /// Each pin only supports one call back, so by calling this we overwrite the previous one
-    gpio_set_irq_enabled_with_callback(INT1_PIN, GPIO_IRQ_LEVEL_LOW, false, &pad_callback);
-    uint8_t config[2] = {0};
-    // Select interrupt enable register (0x29)
-    // Set interrupt enabled for altitude threshold(0x08)
-    config[0] = 0x29;
-    config[1] = 0x00;
-    i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
-
-    // Select interrupt configuration register (0x2A)
-    // Set interrupt enabled for altitude threshold to route to INT1 pin(0x08)
-    config[0] = 0x2A;
-    config[1] = 0x00;
-    i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
-
     sem_acquire_blocking(&sem);
+    altimeter.unset_threshold_altitude(INT1_PIN);
     state = BOOST;
-    // start motor burn timer with this function as callback
-    add_alarm_in_ms(MOTOR_BURN_TIME, &boost_callback, NULL, false);
-    snapshot();
     sem_release(&sem);
+    // start motor burn timer with boost transition function as callback
+    add_alarm_in_ms(MOTOR_BURN_TIME, &boost_callback, NULL, false);
 }
 
 int64_t boost_callback(alarm_id_t id, void* user_data) {
@@ -516,7 +249,8 @@ int64_t boost_callback(alarm_id_t id, void* user_data) {
     // transition to APOGEE
     sem_acquire_blocking(&sem);
     state = COAST;
-    snapshot();
+    populate_entry();
+    write_entry(entry_buffer);
     sem_release(&sem);
     add_alarm_in_ms(1000, &coast_callback, NULL, false);
     return 0;
@@ -527,7 +261,8 @@ int64_t coast_callback(alarm_id_t id, void* user_data) {
     if (velocity <= 0.0f) {
         sem_acquire_blocking(&sem);
         state = APOGEE;
-        snapshot();
+        populate_entry();
+        write_entry(entry_buffer);
         sem_release(&sem);
         add_alarm_in_ms(1, &apogee_callback, NULL, false);
     } else {
@@ -537,43 +272,14 @@ int64_t coast_callback(alarm_id_t id, void* user_data) {
 }
 
 int64_t apogee_callback(alarm_id_t id, void* user_data) {
+    state = RECOVERY;
     // Set altimeter interrupt to occur for when rocket touches back to the ground
-
-    uint8_t config[2] = {0};
-    // Select pressure target MSB register(0x16)
-    // Set altitude target to 10 meters above ground altitude
-    float recov_altitude = ground_altitude + 10.0f;
-    // Select pressure target MSB register(0x16)
-    // Set altitude target to 30 meters above ground altitude
-    config[0] = 0x16;
-    config[1] = (uint8_t) (((int16_t)(recov_altitude)) >> 8);
-    // printf("threshold_alt upper half: %X\n", config[1]);
-    i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
-
-    // Select pressure target LSB register(0x17)
-    // Set altitude target to 30 meters above ground altitude
-    config[0] = 0x17;
-    config[1] = (uint8_t) (((int16_t)(recov_altitude)));
-    // printf("threshold_alt lower half: %X\n", config[1]);
-    i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
-
-    // Select interrupt enable register (0x29)
-    // Set interrupt enabled for altitude threshold(0x08)
-    config[0] = 0x29;
-    config[1] = 0x08;
-    i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
-
-    // Select interrupt configuration register (0x2A)
-    // Set interrupt enabled for altitude threshold to route to INT1 pin(0x08)
-    config[0] = 0x2A;
-    config[1] = 0x08;
-    i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
+    altimeter.set_threshold_altitude((ground_altitude + 10.0f), INT1_PIN, &recovery_callback);
 
     sem_acquire_blocking(&sem);
-    state = RECOVERY;
-    snapshot();
+    populate_entry();
+    write_entry(entry_buffer);
     sem_release(&sem);
-    gpio_set_irq_enabled_with_callback(INT1_PIN, GPIO_IRQ_LEVEL_LOW, true, &recovery_callback);
     return 0;
 }
 
@@ -581,22 +287,11 @@ void recovery_callback(uint gpio, uint32_t event_mask) {
     // Essentially just a signal to stop logging data
     sem_acquire_blocking(&sem);
     state = END;
-    snapshot();
+    populate_entry();
+    write_entry(entry_buffer);
     sem_acquire_blocking(&sem);
 }
 
-float get_altitude() {
-    uint8_t reg = 0x01;
-    uint8_t data[5];
-    i2c_write_blocking(i2c_default, ALT_ADDR, &reg, 1, true);
-    i2c_read_blocking(i2c_default, ALT_ADDR, data, 5, false);
-    // Exactly how MPL3115A2 datasheet says to retrieve altitude
-    float altitude = (float) ((int16_t) ((data[0] << 8) | data[1])) + (float) (data[2] >> 4) * 0.0625;
-    // uint32_t temp_alt = (data[1] << 24) | (data[2] << 16) | (data[3] << 8);
-    // float altitude = temp_alt / 65536.0f;
-    return altitude;
-}
-
 /**
  * @brief Calculates the fitted Coeficient of Drag using the Surface Fit Model for the current rocket design.
  * @param velocity Velocity
@@ -637,13 +332,105 @@ float get_deploy_percent(float velocity, float altitude) {
 
 }
 
-float get_velocity() {
-    uint8_t reg = 0x07;
-    uint8_t data[5];
-    i2c_write_blocking(i2c_default, ALT_ADDR, &reg, 1, true);
-    i2c_read_blocking(i2c_default, ALT_ADDR, data, 5, false);
-    float delta = (float) ((int16_t) ((data[0] << 8) | data[1])) + (float) (data[2] >> 4) * 0.0625;
-    float vel = delta * DATA_RATE_HZ;
-    return vel;
+// LOGGING THREAD RELATED FUNCTIONS AND CALLBACKS
+//===============================================================================
+
+void logging_core() {
+    add_repeating_timer_us(-1000000 / LOG_RATE_HZ,  &logging_buffer_callback, NULL, &log_timer);
+    add_repeating_timer_us(-1000000 / HEART_RATE_HZ,  &heartbeat_callback, NULL, &heartbeat_timer);
+
+    while (1) {
+        tight_loop_contents();
+    }
+}
+
+void populate_entry() {
+    absolute_time_t now = get_absolute_time();
+    uint64_t now_us = to_us_since_boot(now);
+    uint32_t vel_bits = *((uint32_t *)&velocity);
+    entry_buffer[0] = now_us >> 56;
+    entry_buffer[1] = now_us >> 48;
+    entry_buffer[2] = now_us >> 40;
+    entry_buffer[3] = now_us >> 32;
+    entry_buffer[4] = now_us >> 24;
+    entry_buffer[5] = now_us >> 16;
+    entry_buffer[6] = now_us >> 8;
+    entry_buffer[7] = now_us;
+
+    adc_select_input(4);
+    uint16_t temperature = adc_read();
+    entry_buffer[8] = ((*(uint8_t *)(&state)) << 4) | (temperature >> 8);
+    entry_buffer[9] = temperature;
+
+    entry_buffer[10] = deployment_percent;
+    entry_buffer[11] = altimeter_buffer[0];
+    entry_buffer[12] = altimeter_buffer[1];
+    entry_buffer[13] = altimeter_buffer[2];
+    entry_buffer[14] = vel_bits >> 24;
+    entry_buffer[15] = vel_bits >> 16;
+    entry_buffer[16] = vel_bits >> 8;
+    entry_buffer[17] = vel_bits;
+    entry_buffer[18] = acceleration_buffer[0];
+    entry_buffer[19] = acceleration_buffer[1];
+    entry_buffer[20] = acceleration_buffer[2];
+    entry_buffer[21] = acceleration_buffer[3];
+    entry_buffer[22] = acceleration_buffer[4];
+    entry_buffer[23] = acceleration_buffer[5];
+    entry_buffer[24] = quaternion_buffer[0];
+    entry_buffer[25] = quaternion_buffer[1];
+    entry_buffer[26] = quaternion_buffer[2];
+    entry_buffer[27] = quaternion_buffer[3];
+    entry_buffer[28] = quaternion_buffer[4];
+    entry_buffer[29] = quaternion_buffer[5];
+    entry_buffer[30] = quaternion_buffer[6];
+    entry_buffer[31] = quaternion_buffer[7];
+}
+
+bool logging_buffer_callback(repeating_timer_t *rt) {
+    sem_acquire_blocking(&sem);
+    populate_entry();
+    sem_release(&sem);
+    for (uint32_t i = 0; i < PACKET_SIZE; i++) {
+        pad_buffer[i + pad_buffer_offset] = entry_buffer[i];
+    }
+    pad_buffer_offset += PACKET_SIZE;
+    pad_buffer_offset %= PAD_BUFFER_SIZE;
+
+    if (state != PAD) {
+        uint32_t idx = ((pad_buffer_offset + PACKET_SIZE) % PAD_BUFFER_SIZE);
+        sem_acquire_blocking(&sem);
+        do {
+            write_entry(pad_buffer + idx);
+            idx += PACKET_SIZE;
+            idx %= PAD_BUFFER_SIZE;
+        } while (idx != pad_buffer_offset);
+        sem_release(&sem);
+        cancel_repeating_timer(&log_timer);
+        free(pad_buffer);
+        add_repeating_timer_us(-1000000 / LOG_RATE_HZ,  &logging_flash_callback, NULL, &log_timer);
+    }
+    return true;
+}
+
+bool logging_flash_callback(repeating_timer_t *rt) {
+    sem_acquire_blocking(&sem);
+    populate_entry();
+    write_entry(entry_buffer);
+    sem_release(&sem);
+    if (state == END) {
+        cancel_repeating_timer(&log_timer);
+    }
+    return true;
+}
+
+bool heartbeat_callback(repeating_timer_t *rt) {
+    const bool sequence[] = {true, false, true, false, false};
+    const uint8_t sequence_length = 5;
+
+    bool led_status = sequence[led_counter];
+    cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, led_status);
+    led_counter++;
+    led_counter %= sequence_length;
+    return true;
 }
 
diff --git a/src/actuationPlan.cpp b/src/actuationPlan.cpp
deleted file mode 100644
index a987478..0000000
--- a/src/actuationPlan.cpp
+++ /dev/null
@@ -1,60 +0,0 @@
-#include "../include/actuationPlan.hpp"
-
-ActuationPlan::ActuationPlan() {}
-
-ActuationPlan::ActuationPlan(SurfaceFitModel sFitModel) : sFitModel(sFitModel) {
-
-}
-
-
-void ActuationPlan::runPlan(Vehicle& rocket) {
-
-
-    if (rocket.imuReadFail || rocket.altiReadFail) {
-        rocket.deployment_angle = deploy_percentage_to_angle(0); // No fin deployment
-    }
-
-    rocket.fail_time = time(nullptr);
-
-    // 2024 Mission---------------------------------------------------------------------
-    if (rocket.status == GLIDE) {
-
-        // Fin deployment based on current drag coefficient value
-        try {
-            double cd = sFitModel.getFit(rocket.filtered_velocity, rocket.filtered_altitude);
-            cd = std::min(std::max(0.0, cd), 100.0);
-            rocket.deployment_angle = deploy_percentage_to_angle(cd);
-        }
-
-        // Full deployment during coasting
-        catch (...) {
-            rocket.deployment_angle = deploy_percentage_to_angle(0);
-            
-            if ((time(nullptr) - rocket.deploy_time) > 2 && (time(nullptr) - rocket.deploy_time) < 7) {
-                rocket.deployment_angle = deploy_percentage_to_angle(100);
-            }
-        }        
-    }
-
-    else if (rocket.status == APOGEE) {
-
-        rocket.deployment_angle = deploy_percentage_to_angle(50);
-    }
-
-    else {
-
-        rocket.deploy_time = time(nullptr);
-    }
-    // End 2024 Mission------------------------------------------------------------------
-}
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/ads.cpp b/src/ads.cpp
deleted file mode 100644
index 5484970..0000000
--- a/src/ads.cpp
+++ /dev/null
@@ -1,286 +0,0 @@
-#include "../include/ads.hpp"
-
-
-// Private----------------------------------------------------------------------
-void ADS::logSummary() {
-    
-    std::string output_string = "" + state_for_log[rocket.status];
-
-    if (!rocket.altiInitFail && !rocket.altiReadFail) {
-
-        output_string += format_data(" ", rocket.filtered_altitude, 3);     
-    }
-
-    output_string += format_data(" ", rocket.deployment_angle, 2);    
-
-    if (!rocket.imuInitFail && !rocket.imuReadFail) {
-
-        output_string += format_data(" ", rocket.acceleration[2], 2);    
-        output_string += format_data(" ", rocket.filtered_velocity, 2);    
-    }
-
-    Logger::Get().log(output_string);
-}
-
-
-void ADS::updateOnPadAltitude() {
-
-    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-
-    double avg_alt = 0;
-    double alt_read_count = 0;
-
-    while (alt_read_count < COUNT_LIMIT) {
-        
-        altimeter.getData(&rocket.current_altitude);
-        alt_read_count++;
-        avg_alt = (avg_alt * (alt_read_count - 1) + rocket.current_altitude) / alt_read_count;
-    }
- 
-    Logger::Get().log(format_data("pad altitude initialization complete - ", avg_alt, 3));
-    rocket.ON_PAD_altitude = avg_alt;
-}
-
-
-void ADS::updateSensorData() {
-
-    if (!rocket.imuInitFail) {
-        
-        try {
-            imu.getData((void*)&rocket);
-        }
-
-        catch (...) {
-            std::exception_ptr e = std::current_exception();
-            Logger::Get().logErr(e.__cxa_exception_type()->name());
-            rocket.imuReadFail = true;
-        }
-    }  
-
-    rocket.previous_altitude = rocket.current_altitude; // Why was this placed here????
-    
-    if (!rocket.altiInitFail) {
-        
-        try {
-            altimeter.getData((void*)&rocket.current_altitude);
-            if (rocket.ON_PAD_fail) {
-                rocket.ON_PAD_altitude = rocket.current_altitude;
-                rocket.ON_PAD_fail = false;
-            }
-
-            rocket.altiReadFail = false;
-        }
-
-        catch (...) {
-            std::exception_ptr e = std::current_exception();
-            Logger::Get().logErr(e.__cxa_exception_type()->name());
-            rocket.altiReadFail = true;
-        }
-    }
-}
-
-
-void ADS::updateRocketState() {
-
-    // Filter sensor data
-    VectorXf control_input(1);
-    VectorXf measurement(1);
-    control_input << rocket.acceleration[2];
-    measurement << rocket.current_altitude;
-    VectorXf filtered = kf.run(control_input, measurement, rocket.dt);
-    rocket.filtered_altitude = filtered(0);
-    rocket.filtered_velocity = filtered(1);
-
-    if (rocket.apogee_altitude < rocket.filtered_altitude) {
-        rocket.apogee_altitude = rocket.filtered_altitude;
-    }
-
-    // (VEHICLE ON PAD)
-    if (rocket.status == ON_PAD) {
-
-        // If launch detected
-        if (rocket.acceleration[2] >= BOOST_ACCEL_THRESH * G_0 
-            && rocket.filtered_altitude >= BOOST_HEIGHT_THRESH + rocket.ON_PAD_altitude) {
-            Logger::Get().log(format_data("LOM at -- ", (double)(rocket.liftoff_time - rocket.start_time), 3));
-        }
-
-        if (TEST_MODE && time(nullptr) - rocket.start_time >= 15) {
-            Logger::Get().log(format_data("TEST LOM at -- ", (double)(rocket.liftoff_time - rocket.start_time), 3));
-        }
-
-        if (time(nullptr) - rocket.relog_time > 2*60*60 
-            && rocket.status == ON_PAD) {
-            std::cout << "OverWR Success" << std::endl;
-        }
-    }
-
-    // (VEHICLE BOOSTING)
-    else if (rocket.status == BOOST) {
-
-        if (rocket.acceleration[2] <= GLIDE_ACCEL_THRESH * G_0 
-            || time(nullptr) - rocket.liftoff_time >= TIME_BO) {
-            rocket.status = GLIDE;
-        }
-        
-    }
-
-    // (VEHICLE IN GLIDE)
-    else if (rocket.status == GLIDE) {
-
-        if (rocket.filtered_altitude < rocket.apogee_altitude - APOGEE_FSM_CHANGE 
-            || time(nullptr) - rocket.liftoff_time >= TIME_BO + TIME_APO) {
-            rocket.status = APOGEE;
-            Logger::Get().log(format_data("APO: ", (double)(rocket.apogee_altitude), 2));
-        }
-    }
-
-    // (VEHICLE AT APOGEE)
-    else if (rocket.status == APOGEE) {
-
-        if (rocket.filtered_altitude <= FSM_DONE_SURFACE_ALTITUDE + rocket.ON_PAD_altitude) {
-            rocket.status = DONE;
-            return;
-        }
-    }
-}
-
-
-// Public----------------------------------------------------------------------
-ADS::ADS(ActuationPlan plan) : plan(plan) {
-
-    rocket.status = ON_PAD;
-
-    rocket.apogee_altitude = 0;
-    rocket.previous_altitude = 0;
-    rocket.current_altitude = 0;
-    rocket.filtered_altitude = 0;
-
-    rocket.filtered_velocity = 0;
-
-    rocket.duty_span = DUTY_MAX - DUTY_MIN;
-    rocket.deployment_angle = deploy_percentage_to_angle(INIT_DEPLOYMENT); 
-
-    rocket.dt = 0.1;
-
-    rocket.imuInitFail = false;
-    rocket.imuReadFail = false;
-    rocket.altiInitFail = false;
-    rocket.altiReadFail = false;
-
-    rocket.ON_PAD_altitude = 0;
-    rocket.ON_PAD_fail = false;
-
-    rocket.start_time = time(nullptr);
-    rocket.fail_time = rocket.start_time;
-    rocket.relog_time = rocket.start_time;
-    rocket.led_time = rocket.start_time;
-
-    imu = IMUSensor();
-    altimeter = AltimeterSensor();
-    motor = Motor();
-    kf = KalmanFilter(2, 1, 1, rocket.dt);
-
-    Logger::Get().openLog(LOG_FILENAME);
-    
-    motor.init(&rocket);
-
-    imu.init(nullptr);
-    altimeter.init(nullptr);
-
-    if (TEST_MODE) {
-
-        Logger::Get().log("TEST Record Start --");
-    }
-}
-
-
-
-void ADS::run() {
-
-    if (!rocket.altiInitFail) {
-        try {
-            updateOnPadAltitude();
-        }
-
-        catch (...) {
-            std::exception_ptr e = std::current_exception();
-            Logger::Get().logErr(e.__cxa_exception_type()->name());
-            rocket.ON_PAD_fail = true;
-        }
-    }
-
-    rocket.loop_time = time(nullptr);
-    while (rocket.status != DONE) {
-
-        updateSensorData();
-
-        if (!rocket.imuInitFail && !rocket.altiInitFail) {
-
-            updateRocketState();
-
-            // Run the Actuation Plan----------------------------------
-            plan.runPlan(rocket);
-
-            if (rocket.imuReadFail || rocket.altiReadFail) {
-
-                if (rocket.imuReadFail) {
-                    imu.init(nullptr); // Restart
-                    Logger::Get().log("Altimeter reset attempt");
-                }
-
-                if (rocket.altiReadFail) {
-                    altimeter.init(nullptr); // Restart
-                    Logger::Get().log("IMU reset attempt");
-                }
-            }
-        }
-
-        // Altimeter or IMU setup failed. Attempt to reinitialize
-        else {
-
-            if (time(nullptr) - rocket.fail_time >= TIME_END) {
-                rocket.status = DONE;
-            }
-
-            if (rocket.altiInitFail || rocket.altiReadFail) {
-                imu.init(nullptr); // Restart
-                Logger::Get().log("Altimeter reset attempt");
-            }
-
-            if (rocket.imuInitFail || rocket.imuReadFail) {
-                altimeter.init(nullptr); // Restart
-                Logger::Get().log("IMU reset attempt");
-            }
-
-            rocket.deployment_angle = deploy_percentage_to_angle(INIT_DEPLOYMENT);
-        }
-
-        // Actuate Servos
-        motor.writeData(&rocket);
-
-        logSummary();
-
-        // Blink Beaglebone LED 1
-        if (time(nullptr) - rocket.led_time > LED_GAP_TIME) {
-            led_out(&rocket);
-        }
-
-        std::this_thread::sleep_for(std::chrono::milliseconds(1));
-        rocket.dt = time(nullptr) - rocket.loop_time;
-        rocket.loop_time = time(nullptr);
-    }
-
-    Logger::Get().closeLog();
-    std::cout << "Done" << std::endl;
-}
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/altimeter.cpp b/src/altimeter.cpp
index a5c8849..9c47ceb 100644
--- a/src/altimeter.cpp
+++ b/src/altimeter.cpp
@@ -1,50 +1,145 @@
-#include <stdio.h>
-
+#include "altimeter.hpp"
 #include "hardware/gpio.h"
-#include "boards/pico_w.h"
-#include "hardware/i2c.h"
-#include "pico/stdio.h"
-#include "pico/time.h"
 
-#define ALT_ADDR 0x60
-#define MAX_SCL 400000
-#define DATA_RATE_HZ 15
+altimeter::altimeter(i2c_inst_t* inst, uint8_t addr) {
+    this->inst = inst;
+    this->addr = addr;
+}
 
-float altitude = 0.0f;
-float get_altitude();
+void altimeter::initialize() {
+    // Select control register(0x26)
+    // Active mode, OSR = 16, altimeter mode(0xB8)
+    this->buffer[0] = 0x26;
+    this->buffer[1] = 0x89;
+    i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
+}
 
-int main() {
-    stdio_init_all();
+void altimeter::initialize(float threshold_altitude, uint8_t interrupt_pin, gpio_irq_callback_t callback) {
+    this->initialize();
 
-    i2c_init(i2c_default, MAX_SCL);
-    gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C);
-    gpio_set_function(PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C);
-    gpio_pull_up(PICO_DEFAULT_I2C_SDA_PIN);
-    gpio_pull_up(PICO_DEFAULT_I2C_SCL_PIN);
+    // Below configures the interrupt for the first transition from PAD to BOOST
+    // Initial Reading
 
-    uint8_t config[2] = {0};
+    sleep_ms(1000);
 
-    // Select control register(0x26)
-    // Active mode, OSR = 16, altimeter mode(0xB8)
-    config[0] = 0x26;
-    config[1] = 0xB9;
-    i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
-    sleep_ms(1500);
-
-    while (1) {
-        sleep_ms(1000);
-        altitude = get_altitude();
-        printf("Altitude: %4.2f\n", altitude);
+    float altitude = 0.0f;
+
+    while (altitude == 0.0f) {
+        altitude = this->get_altitude_converted();
     }
+
+    threshold_altitude += altitude; // 30 meters above ground
+
+    // Select control register 3 (0x28)
+    // Set bot interrupt pins to active low and enable internal pullups
+    this->buffer[0] = 0x28;
+    this->buffer[1] = 0x01;
+    i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
+
+    // Select pressure target MSB register(0x16)
+    // Set altitude target to 30 meters above ground altitude
+    this->buffer[0] = 0x16;
+    this->buffer[1] = (uint8_t) (((int16_t)(threshold_altitude)) >> 8);
+    i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
+
+    // Select pressure target LSB register(0x17)
+    // Set altitude target to 30 meters above ground altitude
+    this->buffer[0] = 0x17;
+    this->buffer[1] = (uint8_t) (((int16_t)(threshold_altitude)));
+    i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
+
+    // Select interrupt enable register (0x29)
+    // Set interrupt enabled for altitude threshold(0x08)
+    this->buffer[0] = 0x29;
+    this->buffer[1] = 0x08;
+    i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
+
+    // Select interrupt this->bufferuration register (0x2A)
+    // Set interrupt enabled for altitude threshold to route to INT1 pin(0x08)
+    this->buffer[0] = 0x2A;
+    this->buffer[1] = 0x08;
+    i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
+
+    gpio_set_irq_enabled_with_callback(interrupt_pin, GPIO_IRQ_LEVEL_LOW, true, callback);
+    // End of configuration of interrupt for first transition from PAD to BOOST
 }
 
-float get_altitude() {
+void altimeter::set_threshold_altitude(float threshold_altitude, uint8_t interrupt_pin, gpio_irq_callback_t callback) {
+    float altitude = 0.0f;
+
+    while (altitude == 0.0f) {
+        altitude = get_altitude_converted();
+    }
+
+    threshold_altitude += altitude;
+
+    // Select control register 3 (0x28)
+    // Set bot interrupt pins to active low and enable internal pullups
+    this->buffer[0] = 0x28;
+    this->buffer[1] = 0x01;
+    i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
+
+    // Select pressure target MSB register(0x16)
+    // Set altitude target to 30 meters above ground altitude
+    this->buffer[0] = 0x16;
+    this->buffer[1] = (uint8_t) (((int16_t)(threshold_altitude)) >> 8);
+    i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
+
+    // Select pressure target LSB register(0x17)
+    // Set altitude target to provided threshold altitude
+    this->buffer[0] = 0x17;
+    this->buffer[1] = (uint8_t) (((int16_t)(threshold_altitude)));
+    i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
+
+    // Select interrupt enable register (0x29)
+    // Set interrupt enabled for altitude threshold(0x08)
+    this->buffer[0] = 0x29;
+    this->buffer[1] = 0x08;
+    i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
+
+    // Select interrupt this->bufferuration register (0x2A)
+    // Set interrupt enabled for altitude threshold to route to INT1 pin(0x08)
+    this->buffer[0] = 0x2A;
+    this->buffer[1] = 0x08;
+    i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
+
+    gpio_set_irq_enabled_with_callback(interrupt_pin, GPIO_IRQ_LEVEL_LOW, true, callback);
+    // End of configuration of interrupt for first transition from PAD to BOOST
+}
+
+void altimeter::unset_threshold_altitude(uint8_t interrupt_pin) {
+    gpio_set_irq_enabled_with_callback(interrupt_pin, GPIO_IRQ_LEVEL_LOW, false, NULL);
+
+    // Select interrupt enable register (0x29)
+    // Set interrupt enabled for altitude threshold(0x08)
+    this->buffer[0] = 0x29;
+    this->buffer[1] = 0x00;
+    i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
+
+    // Select interrupt configuration register (0x2A)
+    // Set interrupt enabled for altitude threshold to route to INT1 pin(0x08)
+    this->buffer[0] = 0x2A;
+    this->buffer[1] = 0x00;
+    i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
+}
+
+float altimeter::get_altitude_converted() {
     uint8_t reg = 0x01;
-    uint8_t data[5];
-    i2c_write_blocking(i2c_default, ALT_ADDR, &reg, 1, true);
-    i2c_read_blocking(i2c_default, ALT_ADDR, data, 5, false);
+    i2c_write_blocking(this->inst, this->addr, &reg, 1, true);
+    i2c_read_blocking(this->inst, this->addr, this->altitude_buffer, 4, false);
     // Exactly how MPL3115A2 datasheet says to retrieve altitude
-    float altitude = (float) ((int16_t) ((data[0] << 8) | data[1])) + (float) (data[2] >> 4) * 0.0625;
+    float altitude = (float) ((int16_t) ((this->altitude_buffer[0] << 8) | this->altitude_buffer[1])) + (float) (this->altitude_buffer[2] >> 4) * 0.0625;
     return altitude;
 }
 
+void altimeter::get_altitude_raw(uint8_t* buffer) {
+    uint8_t reg = 0x01;
+    i2c_write_blocking(this->inst, this->addr, &reg, 1, true);
+    i2c_read_blocking(this->inst, this->addr, buffer, 3, false);
+}
+
+uint32_t altimeter::expose_buffer(uint8_t** buffer) {
+    *buffer = this->altitude_buffer;
+    return sizeof(this->altitude_buffer);
+}
+
diff --git a/src/bno055.cpp b/src/bno055.cpp
deleted file mode 100644
index fb51986..0000000
--- a/src/bno055.cpp
+++ /dev/null
@@ -1,201 +0,0 @@
-#include "bno055.hpp"
-
-/// @link [Pico BNO055 Example](https://learnembeddedsystems.co.uk/bno005-i2c-example-code)
-
-BNO055::BNO055() {
-    bno055_address = BNO055_ADDRESS_A;
-    _sensorID = BNO055_ID;
-    default_mode = OPERATION_MODE_NDOF;
-}
-
-void BNO055::reset_bno055() {
-    uint8_t data[2];
-    data[0] = BNO055_SYS_TRIGGER_ADDR;
-    data[1] = 0x20; // Reset system
-    i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
-    sleep_ms(1000); // Wait 650ms for the sensor to reset
-}
-
-void BNO055::init() {
-    sleep_ms(1000); // Wait 650ms for the sensor to reset
-    uint8_t chip_id_addr = BNO055_CHIP_ID_ADDR;
-    uint8_t id[1];
-    i2c_write_blocking(i2c_default, bno055_address, &chip_id_addr, 1, false);
-    i2c_read_blocking(i2c_default, bno055_address, id, 1, false);
-    if (!id[0] == _sensorID) {
-        printf("BNO055 not detected\n");
-    }
-
-    // Use internal oscillator
-    uint8_t data[2];
-    data[0] = BNO055_SYS_TRIGGER_ADDR;
-    data[1] = 0x40; // Set to use internal oscillator
-    i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
-
-    // Reset all interrupt status bits
-    data[0] = BNO055_SYS_TRIGGER_ADDR;
-    data[1] = 0x01; // Reset interrupt status
-    // 0x05 = Reset system
-    i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
-
-    // Set to normal power mode
-    data[0] = BNO055_PWR_MODE_ADDR;
-    data[1] = 0x00; // Normal power mode
-    i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
-    sleep_ms(50); // Wait 50ms for the sensor to switch to normal power mode
-
-    // Page 25 of the datasheet
-    // Default Axis Config
-    data[0] = BNO055_AXIS_MAP_CONFIG_ADDR;
-    data[1] = 0x24; // P1=Z, P2=Y, P3=X
-    i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
-
-    // Default Axis Sign
-    data[0] = BNO055_AXIS_MAP_SIGN_ADDR;
-    data[1] = 0x00; // P1=Positive, P2=Positive, P3=Positive
-    i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
-
-    // Set units to m/s^2
-    data[0] = BNO055_UNIT_SEL_ADDR;
-    data[1] = 0x00; // Windows, Celsius, Degrees, DPS, m/s^2
-    i2c_write_blocking(i2c_default, bno055_address, data, 2, true);
-    sleep_ms(30);
-
-    //The default operation mode after power-on is CONFIGMODE
-    // Set mode to NDOF
-    // Takes 7ms to switch from CONFIG mode; see page 21 on datasheet (3.3)
-    data[0] = BNO055_OPR_MODE_ADDR;
-    data[1] = default_mode; // NDOF
-    i2c_write_blocking(i2c_default, bno055_address, data, 2, false);
-    sleep_ms(100);
-
-    
-}
-
-void BNO055::read_calib_status() {
-    uint8_t calib_stat_reg = BNO055_CALIB_STAT_ADDR;
-    uint8_t calib_stat[1];
-    i2c_write_blocking(i2c_default, bno055_address, &calib_stat_reg, 1, true);
-    i2c_read_blocking(i2c_default, bno055_address, calib_stat, 1, false);
-    calib_status.mag = ((calib_stat[0] & 0b00000011) >> 0);
-    calib_status.accel = ((calib_stat[0] & 0b00001100) >> 2);
-    calib_status.gyro = ((calib_stat[0] & 0b00110000) >> 4);
-    calib_status.sys = ((calib_stat[0] & 0b11000000) >> 6);
-}
-
-void BNO055::read_lin_accel() {
-    uint8_t lin_accel_reg = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR;
-    i2c_write_blocking(i2c_default, bno055_address, &lin_accel_reg, 1, true);
-    i2c_read_blocking(i2c_default, bno055_address, accel, 6, false);
-    int16_t x, y, z;
-    x = y = z = 0;
-    x = ((int16_t)accel[0]) | (((int16_t)accel[1]) << 8);
-    y = ((int16_t)accel[2]) | (((int16_t)accel[3]) << 8);
-    z = ((int16_t)accel[4]) | (((int16_t)accel[5]) << 8);
-    linear_acceleration.x = ((float)x) / 100.0;
-    linear_acceleration.y = ((float)y) / 100.0;
-    linear_acceleration.z = ((float)z) / 100.0;
-}
-
-void BNO055::clamp_close_zero(volatile float &val) {
-    if (val < 0.01 && val > -0.01) {
-        val = 0.0;
-    }
-}
-
-void BNO055::accel_to_gravity() {
-    accel_gravity.x = abs_lin_accel.x / 9.81;
-    accel_gravity.y = abs_lin_accel.y / 9.81;
-    accel_gravity.z = abs_lin_accel.z / 9.81;
-}
-
-void BNO055::read_abs_quaternion() {
-    uint8_t quat_reg = BNO055_QUATERNION_DATA_W_LSB_ADDR;
-    i2c_write_blocking(i2c_default, bno055_address, &quat_reg, 1, true);
-    i2c_read_blocking(i2c_default, bno055_address, quat, 8, false);
-    int16_t w, x, y, z;
-    w = x = y = z = 0;
-    w = ((int16_t)quat[0]) | (((int16_t)quat[1]) << 8);
-    x = ((int16_t)quat[2]) | (((int16_t)quat[3]) << 8);
-    y = ((int16_t)quat[4]) | (((int16_t)quat[5]) << 8);
-    z = ((int16_t)quat[6]) | (((int16_t)quat[7]) << 8);
-    abs_quaternion.w = ((float)w) / 16384.0; // 2^14 LSB
-    abs_quaternion.x = ((float)x) / 16384.0;
-    abs_quaternion.y = ((float)y) / 16384.0;
-    abs_quaternion.z = ((float)z) / 16384.0;
-}
-
-void BNO055::read_euler_angles() {
-    uint8_t euler[6];
-    uint8_t euler_reg = BNO055_EULER_H_LSB_ADDR;
-    i2c_write_blocking(i2c_default, bno055_address, &euler_reg, 1, true);
-    i2c_read_blocking(i2c_default, bno055_address, euler, 6, false);
-    /// @note heading = yaw
-    int16_t heading, roll, pitch;
-    heading = roll = pitch = 0;
-    heading = ((int16_t)euler[0]) | (((int16_t)euler[1]) << 8);
-    roll = ((int16_t)euler[2]) | (((int16_t)euler[3]) << 8);
-    pitch = ((int16_t)euler[4]) | (((int16_t)euler[5]) << 8);
-    euler_angles.x = ((float)roll) / 16.0;
-    euler_angles.y = ((float)pitch) / 16.0;
-    euler_angles.z = ((float)heading) / 16.0;
-}
-
-void BNO055::read_accel() {
-    uint8_t accel[6];
-    uint8_t accel_reg = BNO055_ACCEL_DATA_X_LSB_ADDR;
-    i2c_write_blocking(i2c_default, bno055_address, &accel_reg, 1, true);
-    i2c_read_blocking(i2c_default, bno055_address, accel, 6, false);
-    int16_t x, y, z;
-    x = y = z = 0;
-    x = ((int16_t)accel[0]) | (((int16_t)accel[1]) << 8);
-    y = ((int16_t)accel[2]) | (((int16_t)accel[3]) << 8);
-    z = ((int16_t)accel[4]) | (((int16_t)accel[5]) << 8);
-    acceleration.x = ((float)x) / 100.0;
-    acceleration.y = ((float)y) / 100.0;
-    acceleration.z = ((float)z) / 100.0;
-}
-
-void BNO055::quaternion_to_euler() {
-    Eigen::Quaternion<float> q;
-    q.w() = abs_quaternion.w;
-    q.x() = abs_quaternion.x;
-    q.y() = abs_quaternion.y;
-    q.z() = abs_quaternion.z;
-    q.normalize();
-    Eigen::Matrix3f m = q.toRotationMatrix();
-    euler_angles.x = atan2f(m(2,1), m(2,2));
-    euler_angles.y = asinf(-m(2,0));
-    euler_angles.z = atan2f(m(1,0), m(0,0));
-}
-
-void BNO055::calculate_abs_linear_acceleration() {
-    Eigen::Quaternion<float> q;
-    q.w() = abs_quaternion.w;
-    q.x() = abs_quaternion.x;
-    q.y() = abs_quaternion.y;
-    q.z() = abs_quaternion.z;
-    // q.normalize();
-    Eigen::Matrix3f rotation_matrix = q.toRotationMatrix();
-    Eigen::Vector3f lin_accel;
-    lin_accel.x() = linear_acceleration.x;
-    lin_accel.y() = linear_acceleration.y;
-    lin_accel.z() = linear_acceleration.z;
-    abs_lin_accel.x = lin_accel.x() * rotation_matrix(0, 0) + lin_accel.y() * rotation_matrix(0, 1) + lin_accel.z() * rotation_matrix(0, 2);
-    abs_lin_accel.y = lin_accel.x() * rotation_matrix(1, 0) + lin_accel.y() * rotation_matrix(1, 1) + lin_accel.z() * rotation_matrix(1, 2);
-    abs_lin_accel.z = -1.0f * (lin_accel.x() * rotation_matrix(2, 0) + lin_accel.y() * rotation_matrix(2, 1) + lin_accel.z() * rotation_matrix(2, 2));
-}
-
-void BNO055::get_rotation_vector() {
-    Eigen::Quaternion<float> q;
-    q.w() = abs_quaternion.w;
-    q.x() = abs_quaternion.x;
-    q.y() = abs_quaternion.y;
-    q.z() = abs_quaternion.z;
-    q.normalize();
-    Eigen::Matrix3f rotation_matrix = q.toRotationMatrix();
-    rot_y_vec.x = rotation_matrix(1, 0);
-    rot_y_vec.y = rotation_matrix(1, 1);
-    rot_y_vec.z = rotation_matrix(1, 2);
-}
-
diff --git a/src/imu.cpp b/src/imu.cpp
new file mode 100644
index 0000000..4a81042
--- /dev/null
+++ b/src/imu.cpp
@@ -0,0 +1,160 @@
+#include "imu.hpp"
+
+imu::imu(i2c_inst_t* inst, uint8_t addr, uint8_t id, imu_opmode_t mode) {
+    this->inst = inst;
+    this->addr = addr;
+    this->id = id;
+    this->mode = mode;
+}
+
+void imu::reset() {
+    this->buffer[0] = SYS_TRIGGER;
+    this->buffer[1] = 0x20; // Reset system
+    i2c_write_blocking(this->inst, this->addr, buffer, 2, true);
+    sleep_ms(1000); // Wait 650ms for the sensor to reset
+}
+
+void imu::initialize() {
+    sleep_ms(1000); // Wait 650ms for the sensor to reset
+
+    uint8_t chip_id_addr = CHIP_ID;
+    uint8_t read_id = 0x00;
+    while (read_id != this->id) {
+        i2c_write_blocking(this->inst, this->addr, &chip_id_addr, 1, false);
+        i2c_read_blocking(this->inst, this->addr, &read_id, 1, false);
+        sleep_ms(100);
+    }
+
+    // Use internal oscillator
+    this->buffer[0] = SYS_TRIGGER;
+    this->buffer[1] = 0x40; // Set to use internal oscillator
+    i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
+    sleep_ms(50);
+
+    // Reset all interrupt status bits
+    this->buffer[0] = SYS_TRIGGER;
+    this->buffer[1] = 0x01; // Reset interrupt status
+    i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
+    sleep_ms(50);
+
+    // Set to normal power mode
+    this->buffer[0] = POWER_MODE;
+    this->buffer[1] = 0x00; // Normal power mode
+    i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
+    sleep_ms(50);
+
+    // Default Axis Config
+    this->buffer[0] = AXIS_MAP_CONFIG;
+    this->buffer[1] = 0x24; // P1=Z, P2=Y, P3=X
+    i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
+    sleep_ms(50);
+
+    // Default Axis Sign
+    this->buffer[0] = AXIS_MAP_SIGN;
+    this->buffer[1] = 0x00; // P1=Positive, P2=Positive, P3=Positive
+    i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
+    sleep_ms(50);
+
+    // Set units to m/s^2
+    this->buffer[0] = UNIT_SELECTION;
+    this->buffer[1] = 0x00; // Windows, Celsius, Degrees, DPS, m/s^2
+    i2c_write_blocking(this->inst, this->addr, this->buffer, 2, true);
+    sleep_ms(200);
+    
+    uint8_t sensor_offsets[19];
+    sensor_offsets[0] = ACCELERATION_OFFSET_X_LSB;
+    sensor_offsets[1] = 0x00;
+    sensor_offsets[2] = 0x00;
+    sensor_offsets[3] = 0x00;
+    sensor_offsets[4] = 0x00;
+    sensor_offsets[5] = 0x00;
+    sensor_offsets[6] = 0x00;
+    sensor_offsets[7] = 0x00;
+    sensor_offsets[8] = 0x00;
+    sensor_offsets[9] = 0x00;
+    sensor_offsets[10] = 0x00;
+    sensor_offsets[11] = 0x00;
+    sensor_offsets[12] = 0x00;
+    sensor_offsets[13] = 0x00;
+    sensor_offsets[14] = 0x00;
+    sensor_offsets[15] = 0x00; 
+    sensor_offsets[16] = 0x00;
+    sensor_offsets[17] = 0x00;
+    sensor_offsets[18] = 0x00;
+    i2c_write_blocking(this->inst, this->addr, sensor_offsets, 19, true);
+    sleep_ms(200);
+    
+    // The default operation mode after power-on is CONFIG
+    // Set to desired mode
+    this->buffer[0] = OPERATION_MODE;
+    this->buffer[1] = this->mode; // NDOF
+    i2c_write_blocking(this->inst, this->addr, this->buffer, 2, false);
+    sleep_ms(100);
+}
+
+void imu::calibration_status(calibration_status_t* status) {
+    read_register(CALIBRATION_STATUS, 1, this->buffer);
+    status->mag = ((this->buffer[0] & 0b00000011) >> 0);
+    status->accel = ((this->buffer[0] & 0b00001100) >> 2);
+    status->gyro = ((this->buffer[0] & 0b00110000) >> 4);
+    status->sys = ((this->buffer[0] & 0b11000000) >> 6);
+}
+
+void imu::linear_acceleration(Eigen::Vector3f& vec) {
+    read_register(LINEAR_ACCELERATION_X_LSB, 6, this->accel_buffer);
+    int16_t x, y, z;
+    x = y = z = 0;
+    x = ((int16_t)this->accel_buffer[0]) | (((int16_t)this->accel_buffer[1]) << 8);
+    y = ((int16_t)this->accel_buffer[2]) | (((int16_t)this->accel_buffer[3]) << 8);
+    z = ((int16_t)this->accel_buffer[4]) | (((int16_t)this->accel_buffer[5]) << 8);
+    vec(0) = ((float)x) / 100.0;
+    vec(1) = ((float)y) / 100.0;
+    vec(2) = ((float)z) / 100.0;
+}
+
+void imu::quaternion(Eigen::Vector4f& vec) {
+    read_register(QUATERNION_W_LSB, 8, this->quat_buffer);
+    int16_t w, x, y, z;
+    w = x = y = z = 0;
+    w = ((int16_t)this->quat_buffer[0]) | (((int16_t)this->quat_buffer[1]) << 8);
+    x = ((int16_t)this->quat_buffer[2]) | (((int16_t)this->quat_buffer[3]) << 8);
+    y = ((int16_t)this->quat_buffer[4]) | (((int16_t)this->quat_buffer[5]) << 8);
+    z = ((int16_t)this->quat_buffer[6]) | (((int16_t)this->quat_buffer[7]) << 8);
+    vec(0) = ((float)w) / 16384.0;
+    vec(1) = ((float)x) / 16384.0;
+    vec(2) = ((float)y) / 16384.0;
+    vec(3) = ((float)z) / 16384.0;
+}
+
+void imu::quaternion_euler(Eigen::Vector3f& angles, Eigen::Vector4f& quat) {
+    // roll (x-axis rotation)
+    float sinr_cosp = 2 * (quat(0) * quat(1) + quat(2) * quat(3));
+    float cosr_cosp = 1 - 2 * (quat(1) * quat(1) + quat(2) * quat(2));
+    angles(0) = Eigen::numext::atan2(sinr_cosp, cosr_cosp);
+
+    // pitch (y-axis rotation)
+    float sinp = Eigen::numext::sqrt(1 + 2 * (quat(0) * quat(2) - quat(1) * quat(3)));
+    float cosp = Eigen::numext::sqrt(1 - 2 * (quat(0) * quat(2) - quat(1) * quat(3)));
+    angles(1) = 2 * Eigen::numext::atan2(sinp, cosp) - M_PI / 2;
+
+    // yaw (z-axis rotation)
+    float siny_cosp = 2 * (quat(0) * quat(3) + quat(1) * quat(2));
+    float cosy_cosp = 1 - 2 * (quat(2) * quat(2) + quat(3) * quat(3));
+    angles(2) = Eigen::numext::atan2(siny_cosp, cosy_cosp);
+}
+
+uint32_t imu::expose_acceleration_buffer(uint8_t** buffer) {
+    *buffer = this->accel_buffer;
+    return sizeof(this->accel_buffer);
+}
+
+uint32_t imu::expose_quaternion_buffer(uint8_t** buffer) {
+    *buffer = this->quat_buffer;
+    return sizeof(this->quat_buffer);
+}
+
+void imu::read_register(uint8_t reg, size_t len, uint8_t* buffer) {
+    i2c_write_blocking(this->inst, this->addr, &reg, 1, true);
+    i2c_read_blocking(this->inst, this->addr, buffer, len, false);
+}
+
diff --git a/src/kalman_filter.cpp b/src/kalman_filter.cpp
new file mode 100644
index 0000000..d4aff7a
--- /dev/null
+++ b/src/kalman_filter.cpp
@@ -0,0 +1,76 @@
+#include "kalman_filter.hpp"
+
+void kalman_filter::matrix_initialize() {
+    state_vector.setZero(n);
+    state_covariance.setZero(n, n); 
+    state_transition_M = MatrixXf::Zero(n, n);  
+    control_input_M = MatrixXf::Zero(n, p);
+    I = MatrixXf::Identity(n, n);
+    measurement_M.setIdentity(m, n); // Setup Measurement Matrix
+    process_noise_covariance = MatrixXf::Zero(n, n); 
+    measurement_covariance = MatrixXf::Zero(m, m); 
+
+    // Setup State Transition Matrix
+    state_transition_M << 1.0, dt, 0.0, 1.0;
+
+    // Setup Control Input Matrix
+    control_input_M << 0.5 * dt * dt, dt; // (Linear Displacement Eq.)
+
+    // Setup Process-Noise Covariance
+    process_noise_covariance(0,0) = 0.01;
+    process_noise_covariance(1,1) = 0.1;
+
+    // Setup Measurement Covariance
+    measurement_covariance << 1e-12;
+}
+
+void kalman_filter::matrix_update() {
+    state_transition_M(0, 1) = dt;
+    control_input_M(0, 0) = 0.5f * dt * dt;
+    control_input_M(1, 0) = dt;
+}
+
+void kalman_filter::predict(VectorXf control_vec) {
+    state_vector = (state_transition_M * state_vector) + (control_input_M * control_vec);
+    state_covariance = (state_transition_M * (state_covariance * state_transition_M.transpose())) + process_noise_covariance;
+}
+
+void kalman_filter::update(VectorXf measurement) {
+    // Innovation
+    VectorXf y = measurement - (measurement_M * state_vector);
+
+    // Residual/Innovation Covariance
+    MatrixXf S = (measurement_M * (state_covariance * measurement_M.transpose())) + measurement_covariance;
+
+    // Kalman Gain
+    MatrixXf K = (state_covariance * measurement_M.transpose()) * S.inverse();
+
+    // Update
+    state_vector = state_vector + (K * y);
+    state_covariance = (I - (K * measurement_M)) * state_covariance;
+}
+
+kalman_filter::kalman_filter(int state_dim, int control_dim, int measurement_dim, float dt) : n(state_dim), p(control_dim), m(measurement_dim), dt(dt) {
+    matrix_initialize();
+}
+
+bool kalman_filter::state_initialize(VectorXf state_vec, MatrixXf state_cov) {
+    bool result { false };
+    if (state_vec.size() == n && state_cov.rows() == n) {
+        state_vector = state_vec;
+        state_covariance = state_cov;
+        result = true;
+    }
+    return result;
+}
+
+VectorXf kalman_filter::run(VectorXf control, VectorXf measurement, float _dt) {
+    if (control.size() == p && measurement.size() == m) {
+        dt = _dt;
+        matrix_update();
+        predict(control);
+        update(measurement);
+    }
+    return state_vector;
+}
+
diff --git a/src/kalmanfilter.cpp b/src/kalmanfilter.cpp
deleted file mode 100644
index 8174513..0000000
--- a/src/kalmanfilter.cpp
+++ /dev/null
@@ -1,107 +0,0 @@
-#include <climits>
-#include "../include/kalmanfilter.hpp"
-
-// Private----------------------------------------------------------------------
-void KalmanFilter::matrixInit() {
-
-    state_vector.setZero(n);
-    state_covariance.setZero(n, n); 
-    state_transition_M = MatrixXf::Zero(n, n);  
-    control_input_M = MatrixXf::Zero(n, p);
-    I = MatrixXf::Identity(n, n);
-    measurement_M.setIdentity(m, n); // Setup Measurement Matrix
-    process_noise_covariance = MatrixXf::Zero(n, n); 
-    measurement_covariance = MatrixXf::Zero(m, m); 
-
-    // Setup State Transition Matrix
-    state_transition_M << 1.0, dt, 
-        0.0, 1.0;
-    
-    // Setup Control Input Matrix
-    control_input_M << 0.5 * std::pow(dt, 2), // (Linear Displacement Eq.)
-        dt; 
-    
-    // Setup Process-Noise Covariance
-    process_noise_covariance(0,0) = 0.01;
-    process_noise_covariance(1,1) = 0.1;
-
-    // Setup Measurement Covariance
-    measurement_covariance << 1e-12;
-}
-
-
-void KalmanFilter::updateMatrices() {
-
-    state_transition_M(0, 1) = dt;
-    control_input_M(0, 0) = 0.5 * std::pow(dt, 2);
-    control_input_M(1, 0) = dt;
-}
-
-
-void KalmanFilter::prediction(VectorXf control_vec) {
-
-    state_vector = (state_transition_M * state_vector) + (control_input_M * control_vec);
-    state_covariance = (state_transition_M * (state_covariance * state_transition_M.transpose())) + process_noise_covariance;
-}
-
-void KalmanFilter::update(VectorXf measurement) {
-
-    // Innovation
-    VectorXf y = measurement - (measurement_M * state_vector);
-
-    // Residual/Innovation Covariance
-    MatrixXf S = (measurement_M * (state_covariance * measurement_M.transpose())) + measurement_covariance;
-
-    // Kalman Gain
-    MatrixXf K = (state_covariance * measurement_M.transpose()) * S.inverse();
-
-    // Update
-    state_vector = state_vector + (K * y);
-    state_covariance = (I - (K * measurement_M)) * state_covariance;
-}
-
-
-
-// Public----------------------------------------------------------------------
-KalmanFilter::KalmanFilter() {
-    
-}
-
-
-KalmanFilter::KalmanFilter(int state_dim, int control_dim, int measurement_dim, double dt) 
-    : n(state_dim), p(control_dim), m(measurement_dim), dt(dt) {
-
-    matrixInit();
-}
-
-bool KalmanFilter::setInitialState(VectorXf state_vec, MatrixXf state_cov) {
-
-    if (state_vec.size() != n || state_cov.rows() != n) {
-        std::cout << "Error: Max State & Covariance Dimension should be " << n << std::endl;
-        return false;
-    }
-
-    state_vector = state_vec;
-    state_covariance = state_cov;
-    return true;
-}
-
-
-
-
-VectorXf KalmanFilter::run(VectorXf control, VectorXf measurement, double _dt) {
-    
-     if (control.size() != p || measurement.size() != m) {
-        std::cout << "Error: Control Vector Size should be "<< p 
-            << " Measurement Vector Size should be " << m << std::endl;
-        return state_vector;
-    }
-
-    dt = _dt;
-    updateMatrices();
-
-    prediction(control);
-    update(measurement);
-
-    return state_vector;
-}
-\ No newline at end of file
diff --git a/src/logger.cpp b/src/logger.cpp
deleted file mode 100644
index a857be8..0000000
--- a/src/logger.cpp
+++ /dev/null
@@ -1,132 +0,0 @@
-#include "../include/logger.hpp"
-
-// Private----------------------------------------------------------------------
-std::string Logger::getDate() {
-
-    t = time(nullptr);
-    now = localtime(&t);
-    return "(" + days[now->tm_wday] + " " + months[now->tm_mon] + " " 
-        + std::to_string(now->tm_mday) + " " + std::to_string(now->tm_year + 1900) + ")";
-}
-
-std::string Logger::getTime() {
-
-    t = time(nullptr);
-    now = localtime(&t);
-    std::string hour = std::to_string(now->tm_hour);
-    std::string min = std::to_string(now->tm_min);
-    std::string sec = std::to_string(now->tm_sec);
-    //string hour = "0" + to_string(now->tm_hour);
-
-    if (now->tm_hour < 10) {
-        hour = "0" + std::to_string(now->tm_hour);
-    }
-
-    if (now->tm_min < 10) {
-        min = "0" + std::to_string(now->tm_min);
-    }
-
-    if (now->tm_sec < 10) {
-        sec = "0" + std::to_string(now->tm_sec);
-    }
-
-    return hour + ":" + min + 
-        ":" + sec;
-}
-
-
-// Public----------------------------------------------------------------------
-Logger& Logger::Get() {
-
-    static Logger loggerSingleton;
-    return loggerSingleton;
-}
-
-//Logger Logger::loggerSingleton;
-
-
-bool Logger::openLog(std::string _filename) {
-
-    filename = _filename;
-    
-    if (file_open) {
-        return false;
-    }
-    
-    file.open(filename, std::ios::in | std::ios::out | std::ios::app);
-
-    if (!file) {
-        return false;
-    }
-
-    file_open = true;
-    std::string date = getDate();
-    std::string timestamp = getTime();
-    file << timestamp << infoTag << "Log Start---- " << date << std::endl;
-
-    return true;
-}
-
-
-void Logger::closeLog() {
-
-    std::string timestamp = getTime();
-    file << timestamp << infoTag << "Log End----\n\n";
-
-    file.close();
-    file_open = false;
-}
-
-
-bool Logger::log(std::string data) {
-
-    if (!file) {
-        return false;
-    }
-
-    if (!file_open) {
-        return false;
-    }
-    std::string timestamp = getTime();
-    file << timestamp << infoTag << data << std::endl;
-    return true;
-}
-
-bool Logger::logErr(std::string data) {
-
-    if (!file) {
-        return false;
-    }
-
-    if (!file_open) {
-        return false;
-    }
-
-    std::string timestamp = getTime();
-    file << timestamp << errorTag << data << std::endl;
-    return true;
-}
-
-
-bool Logger::printLog() {
-
-    if (file.is_open()) {
-        std::cout << "Log still open. Please close Log." << std::endl;
-        return false;
-    }
-
-    file.open(filename, std::ios::in);
-
-    if (!file.is_open()) {
-        return false;
-    }
-
-    std::string line;
-    while(getline(file, line)) {
-        std::cout << line << std::endl;
-    }
-
-    file.close();
-
-    return true;
-}
diff --git a/src/motor.cpp b/src/motor.cpp
deleted file mode 100644
index 84785a9..0000000
--- a/src/motor.cpp
+++ /dev/null
@@ -1,46 +0,0 @@
-#include "../include/motor.hpp"
-
-
-
-Motor::Motor() {
-
-
-}
-
-bool Motor::init(void* data) {
-
-    Vehicle *vehicle = (Vehicle *) data;
-    double duty = 100 - ((MIN_ANGLE / 180) * vehicle->duty_span + DUTY_MIN);
-
-    // Initialize stuff
-    // .....
-    // .....
-
-
-    data = (void*) vehicle; // Is this necessary?
-    return true;
-}
-
-
-bool Motor::writeData(void* data) {
-
-    Vehicle *vehicle = (Vehicle *) data;
-    double duty = 100 - ((vehicle->deployment_angle / 180) * vehicle->duty_span + DUTY_MIN);
-
-    // Send the Data somewhere
-    // ..... Pin
-    // ..... Duty
-    // ..... PWM frequency Hz
-    // ..... Polarity
-    
-
-    if (1 == 2) {
-        Logger::Get().logErr("Some type of Error");
-        return false;
-    }
-
-    data = (void*) vehicle; // Is this necessary?
-    return true;
-}
-
-
diff --git a/src/rocketUtils.cpp b/src/rocketUtils.cpp
deleted file mode 100644
index 45fcfc3..0000000
--- a/src/rocketUtils.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
-#include "../include/rocketUtils.hpp"
-
-double deploy_percentage_to_angle(double percentage) {
-
-    return (MAX_ANGLE - MIN_ANGLE) / 100.0 * percentage + MIN_ANGLE;
-}
-
-
-std::string format_data(std::string prefix, double data, int precision) {
-
-    std::stringstream stream;
-    stream << std::fixed << std::setprecision(precision) << data;
-    std::string s = stream.str();
-    return prefix + s;
-}
-
-bool led_out(Vehicle *vehicle) {
-
-    std::ofstream file;
-    file.open(LED_FILENAME);
-    if (!file.is_open()) {
-        return false;
-    }
-
-    file << std::to_string(vehicle->led_brightness);
-    file.close();
-
-    vehicle->led_time = time(nullptr);
-    vehicle->led_brightness = (vehicle->led_brightness + 1) % 2;
-
-    return true;
-}
-
-std::string state_for_log[5] = {"ON_PAD", "BOOST", "GLIDE", "APOGEE", "DONE"};
-	
-\ No newline at end of file
diff --git a/src/sensorAltimeter.cpp b/src/sensorAltimeter.cpp
deleted file mode 100644
index 8ec065d..0000000
--- a/src/sensorAltimeter.cpp
+++ /dev/null
@@ -1,115 +0,0 @@
-#include "sensorAltimeter.hpp"
-
-AltimeterSensor::AltimeterSensor(std::string I2C_FILE_in) {
-    I2C_FILE = I2C_FILE_in;
-    deviceAddress = 0x60;
-}
-
-//Startup routine copied from Adafruit library, as is most of the data getting methods
-//Adaptation is largely editing for readability and porting from Adafruit_I2C to BBB I2C (sensorI2C.hpp implementation)
-bool AltimeterSensor::init() {
-
-    // Vehicle *vehicle = (Vehicle *) data;
-    // // Do Stuff 
-    // data = (void*) vehicle;
-
-    //Pass file string from parent to setup function, actual I2C bus gets stored internally.
-    setupI2C(I2C_FILE);
-
-    // Check a register with a hard-coded value to see if comms are working
-    uint8_t whoami = readSingleRegister(MPL3115A2_WHOAMI);
-    if (whoami != 0xC4) {
-        fprintf(stderr, "MPL INITIALIZATION DID NOT PASS WHOAMI DEVICE CHECK!, got: %X, expected: 0xC4\n", whoami);
-        return false;
-    }
-
-    //Send device dedicated reset byte to CTRL1 Register
-    writeRegister(MPL3115A2_CTRL_REG1, MPL3115A2_CTRL_REG1_RST);
-    //Wait for reset to wipe its way through device and reset appropriate bit of CTRL1 Register
-    while (readSingleRegister(MPL3115A2_CTRL_REG1) & MPL3115A2_CTRL_REG1_RST);
-
-     //Set oversampling (?) and altitude mode by default
-    currentMode = MPL3115A2_ALTIMETER;
-    ctrl_reg1.reg = MPL3115A2_CTRL_REG1_OS128 | MPL3115A2_CTRL_REG1_ALT;
-    writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg);
-
-    //Configure data return types, I don't really understand this chunk but Adafruit does it this way so we will too I guess
-    writeRegister(MPL3115A2_PT_DATA_CFG, MPL3115A2_PT_DATA_CFG_TDEFE |
-                                    MPL3115A2_PT_DATA_CFG_PDEFE |
-                                    MPL3115A2_PT_DATA_CFG_DREM);
-
-    return true;
-}
-
-//EXPECTED THAT USER WILL NEVER SET MODE TO PRESSURE AFTER INITIAL CONFIGURATION
-void AltimeterSensor::setMode(mpl3115a2_mode_t mode) {
-    ctrl_reg1.reg = readSingleRegister(MPL3115A2_CTRL_REG1);
-    ctrl_reg1.bit.ALT = mode;
-    writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg);
-    currentMode = mode;
-}
-
-double AltimeterSensor::getAltitude() {
-    //Request new data reading
-    requestOneShotReading();
-    //If new data is available, read it and store it to internal fields
-    if (isNewDataAvailable()) {
-        //Logger flag here for new data?
-        updateCurrentDataBuffer();
-    }
-    //Return internal field, whether updated or not
-    return internalAltitude;
-}
-
-double AltimeterSensor::getTemperature() {
-    //Request new data reading
-    requestOneShotReading();
-    //If new data is available, read it and store it to internal fields
-    if (isNewDataAvailable()) {
-        //Logger flag here for new data?
-        updateCurrentDataBuffer();
-    }
-    //Return internal field, whether updated or not
-    return internalTemperature;
-}
-
-void AltimeterSensor::requestOneShotReading() {
-    //Request current status of oneshot reading
-    ctrl_reg1.reg = readSingleRegister(MPL3115A2_CTRL_REG1);
-    //If oneshot is complete, proc a new one; if it isn't, do nothing.
-    //THIS PRODUCES DUPLICATE DATA IF READING REQUESTS FROM BB DON'T LINE UP WITH READING COMPLETION ON SENSOR.
-    if (!ctrl_reg1.bit.OST) {
-        // initiate one-shot measurement
-        ctrl_reg1.bit.OST = 1;
-        writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg);
-    }
-}
-
-bool AltimeterSensor::isNewDataAvailable() {
-    //Returns PTDR bit of status register, 1 if new data for Temp OR Alt/Pres is available
-    //There *are* registers available for exclusively temperature *or* pressure/altitude, but 
-    //for simplicity's sake we'll use the combined one for now.
-    return ((readSingleRegister(MPL3115A2_REGISTER_STATUS) & MPL3115A2_REGISTER_STATUS_PTDR) != 0);
-}
-
-//Adafruit returns specific field based on input parameter, this method updates all internal fields at once instead
-void AltimeterSensor::updateCurrentDataBuffer() {
-    uint8_t buffer[5] = {MPL3115A2_REGISTER_PRESSURE_MSB, 0, 0, 0, 0};
-    readMultipleRegisters(MPL3115A2_REGISTER_PRESSURE_MSB, 5);
-
-    //Pressure is no longer used, assumed rocket is only logging altitude
-    // uint32_t pressure;
-    // pressure = uint32_t(buffer[0]) << 16 | uint32_t(buffer[1]) << 8 |
-    //         uint32_t(buffer[2]);
-    // return double(pressure) / 6400.0;
-
-    //Altitude Conversion
-    int32_t alt;
-    alt = uint32_t(buffer[0]) << 24 | uint32_t(buffer[1]) << 16 |
-        uint32_t(buffer[2]) << 8;
-    internalAltitude = double(alt) / 65536.0;
-
-    int16_t t;
-    t = uint16_t(buffer[3]) << 8 | uint16_t(buffer[4]);
-    internalTemperature = double(t) / 256.0;
-}
diff --git a/src/sensorIMU.cpp b/src/sensorIMU.cpp
deleted file mode 100644
index 941ea35..0000000
--- a/src/sensorIMU.cpp
+++ /dev/null
@@ -1,385 +0,0 @@
-#include "../include/sensorIMU.hpp"
-
-IMUSensor::IMUSensor(std::string I2C_FILE) {
-    this -> I2C_FILE = I2C_FILE;
-}
-
-bool IMUSensor::init(void* data) {
-
-    //I2C_File passed on object creation, stored in sensorI2C parent
-    setupI2C(I2C_FILE);
-    
-    //In the adafruit code there's a big step of waiting for timeout and connection stuff for up to a full second
-    //I don't do that here because the BBB takes like 17 years to boot so we'll just hope it goes faster than that
-
-    //Sanity check for factory device ID 
-    uint8_t id = readSingleRegister(BNO055_CHIP_ID_ADDR);
-    if (id != BNO055_ID) {
-        fprintf(stderr, "DEVICE ID DID NOT PASS SANITY CHECK FOR BNO IMU!");
-        return false;
-    }
-
-    //Set default operating mode of IMU into config from startup (will be set properly after config phase)
-    setModeHard(OPERATION_MODE_CONFIG);
-
-    //Writes 1 to the system reset bit in the trigger register
-    writeRegister(BNO055_SYS_TRIGGER_ADDR, 0x20);
-    //Wait for reset to complete by doing sanity check again
-    while (readSingleRegister(BNO055_CHIP_ID_ADDR) != BNO055_ID);
-
-    //Set power mode for sensor
-    writeRegister(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL);
-
-    //Sensor chip uses two "pages" to multiplex register values
-    //Page 0 contains the sensor data (not configuration), which is what we want
-    writeRegister(BNO055_PAGE_ID_ADDR, 0);
-
-    //Genuinely no idea why Adafruit does this, ensuring all triggers are off before mode config I guess
-    writeRegister(BNO055_SYS_TRIGGER_ADDR, 0x0);
-
-    setModeTemp(default_mode);
-
-    return true;
-}
-
-//Sets mode so it can be undone for temporary changes, like operation setting
-void IMUSensor::setModeTemp(adafruit_bno055_opmode_t mode) {
-    currentMode = mode;
-    writeRegister(BNO055_OPR_MODE_ADDR, currentMode);
-}
-
-//Sets mode *AND* internal state variable
-void IMUSensor::setModeTemp(adafruit_bno055_opmode_t mode) {
-    writeRegister(BNO055_OPR_MODE_ADDR, currentMode);
-}
-
-adafruit_bno055_opmode_t IMUSensor::getMode() {
-    return (adafruit_bno055_opmode_t)readSingleRegister(BNO055_OPR_MODE_ADDR);
-}
-
-imu::Vector<3> IMUSensor::getVector(adafruit_vector_type_t vector_type) {
-    imu::Vector<3> xyz;
-    uint8_t buffer[6] = readMultipleRegisters((adafruit_bno055_reg_t)vector_type, 6);
-
-    int16_t x, y, z;
-    x = y = z = 0;
-
-    /* Read vector data (6 bytes) */
-    x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8);
-    y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8);
-    z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8);
-
-    /*!
-    * Convert the value to an appropriate range (section 3.6.4)
-    * and assign the value to the Vector type
-    */
-    switch (vector_type) {
-        case VECTOR_MAGNETOMETER:
-            /* 1uT = 16 LSB */
-            xyz[0] = ((double)x) / 16.0;
-            xyz[1] = ((double)y) / 16.0;
-            xyz[2] = ((double)z) / 16.0;
-            break;
-        case VECTOR_GYROSCOPE:
-            /* 1dps = 16 LSB */
-            xyz[0] = ((double)x) / 16.0;
-            xyz[1] = ((double)y) / 16.0;
-            xyz[2] = ((double)z) / 16.0;
-            break;
-        case VECTOR_EULER:
-            /* 1 degree = 16 LSB */
-            xyz[0] = ((double)x) / 16.0;
-            xyz[1] = ((double)y) / 16.0;
-            xyz[2] = ((double)z) / 16.0;
-            break;
-        case VECTOR_ACCELEROMETER:
-            /* 1m/s^2 = 100 LSB */
-            xyz[0] = ((double)x) / 100.0;
-            xyz[1] = ((double)y) / 100.0;
-            xyz[2] = ((double)z) / 100.0;
-            break;
-        case VECTOR_LINEARACCEL:
-            /* 1m/s^2 = 100 LSB */
-            xyz[0] = ((double)x) / 100.0;
-            xyz[1] = ((double)y) / 100.0;
-            xyz[2] = ((double)z) / 100.0;
-            break;
-        case VECTOR_GRAVITY:
-            /* 1m/s^2 = 100 LSB */
-            xyz[0] = ((double)x) / 100.0;
-            xyz[1] = ((double)y) / 100.0;
-            xyz[2] = ((double)z) / 100.0;
-            break;
-    }
-
-    return xyz;
-}
-imu::Quaternion IMUSensor::getQuat() {
-    uint8_t buffer[8] = readMultipleRegisters(BNO055_QUATERNION_DATA_W_LSB_ADDR, 8);
-
-    int16_t x, y, z, w;
-    x = y = z = w = 0;
-
-    //Bit shift data into the right places and store it
-    w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
-    x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
-    y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
-    z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);
-
-    /*!
-    * Assign to Quaternion
-    * See
-    * https://cdn-shop.adafruit.com/datasheets/BST_BNO055_DS000_12.pdf
-    * 3.6.5.5 Orientation (Quaternion)
-    */
-    const double scale = (1.0 / (1 << 14));
-    imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z);
-    return quat;
-}
-
-int8_t IMUSensor::getTemp() {
-    int8_t temp = (int8_t)(readSingleRegister(BNO055_TEMP_ADDR));
-    return temp;
-}
-
-void IMUSensor::setAxisRemap(adafruit_bno055_axis_remap_config_t remapcode) {
-    //Put into proper config for mapping stuff
-    setModeTemp(OPERATION_MODE_CONFIG);
-    writeRegister(BNO055_AXIS_MAP_CONFIG_ADDR, remapcode);
-
-    //Return mode to operating mode
-    setModeTemp(currentMode);
-}
-
-void IMUSensor::setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign) {
-    //See above method, pretty much the exact same
-    setModeTemp(OPERATION_MODE_CONFIG);
-    writeRegister(BNO055_AXIS_MAP_SIGN_ADDR, remapsign);
-    setModeTemp(currentMode);
-}    
-
-//This method is weird; it intakes several existing byte pointers to see what action it should take. Luckily, we shouldn't have to use it.
-void IMUSensor::getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error) {
-    //Make sure IMU is on proper register page to get system status
-    writeRegister(BNO055_PAGE_ID_ADDR, 0);
-    
-    //If system status requested, read the status.
-    if (system_status != 0) *system_status = readSingleRegister(BNO055_SYS_STAT_ADDR);
-    //If self test result requested, pull the self test results.
-    if (self_test_result != 0) *self_test_result = readSingleRegister(BNO055_SELFTEST_RESULT_ADDR);
-    //Finally, if there's an error pull and stash it.
-    if (system_error != 0) *system_error = readSingleRegister(BNO055_SYS_ERR_ADDR);
-}
-
-//Same as above method, byte pointers are fed into it as parameters that get populated by method.
-void IMUSensor::getCalibration(uint8_t *sys, uint8_t *gyro, uint8_t *accel, uint8_t *mag) {
-    uint8_t calData = readSingleRegister(BNO055_CALIB_STAT_ADDR);
-    if (sys != NULL) {
-        *sys = (calData >> 6) & 0x03;
-    }
-    if (gyro != NULL) {
-        *gyro = (calData >> 4) & 0x03;
-    }
-    if (accel != NULL) {
-        *accel = (calData >> 2) & 0x03;
-    }
-    if (mag != NULL) {
-        *mag = calData & 0x03;
-    }
-}
-
-/* Functions to deal with raw calibration data */
-bool IMUSensor::getSensorOffsets(uint8_t *calibData) {
-    if (isFullyCalibrated()) {
-        setModeTemp(OPERATION_MODE_CONFIG);
-
-        calibData = readMultipleRegisters(ACCEL_OFFSET_X_LSB_ADDR, NUM_BNO055_OFFSET_REGISTERS);
-
-        setModeTemp(currentMode);
-        return true;
-    }
-    return false;
-}
-
-//Fully populated offset getter using type of offset, not just calibration data
-bool IMUSensor::getSensorOffsets(adafruit_bno055_offsets_t &offsets_type) {
-    if (isFullyCalibrated()) {
-        setModeTemp(OPERATION_MODE_CONFIG);
-
-        /* Accel offset range depends on the G-range:
-        +/-2g  = +/- 2000 mg
-        +/-4g  = +/- 4000 mg
-        +/-8g  = +/- 8000 mg
-        +/-1§g = +/- 16000 mg */
-        offsets_type.accel_offset_x = (readSingleRegister(ACCEL_OFFSET_X_MSB_ADDR) << 8) |
-                                    (readSingleRegister(ACCEL_OFFSET_X_LSB_ADDR));
-        offsets_type.accel_offset_y = (readSingleRegister(ACCEL_OFFSET_Y_MSB_ADDR) << 8) |
-                                    (readSingleRegister(ACCEL_OFFSET_Y_LSB_ADDR));
-        offsets_type.accel_offset_z = (readSingleRegister(ACCEL_OFFSET_Z_MSB_ADDR) << 8) |
-                                    (readSingleRegister(ACCEL_OFFSET_Z_LSB_ADDR));
-
-        /* Magnetometer offset range = +/- 6400 LSB where 1uT = 16 LSB */
-        offsets_type.mag_offset_x =
-            (readSingleRegister(MAG_OFFSET_X_MSB_ADDR) << 8) | (readSingleRegister(MAG_OFFSET_X_LSB_ADDR));
-        offsets_type.mag_offset_y =
-            (readSingleRegister(MAG_OFFSET_Y_MSB_ADDR) << 8) | (readSingleRegister(MAG_OFFSET_Y_LSB_ADDR));
-        offsets_type.mag_offset_z =
-            (readSingleRegister(MAG_OFFSET_Z_MSB_ADDR) << 8) | (readSingleRegister(MAG_OFFSET_Z_LSB_ADDR));
-
-        /* Gyro offset range depends on the DPS range:
-        2000 dps = +/- 32000 LSB
-        1000 dps = +/- 16000 LSB
-        500 dps = +/- 8000 LSB
-        250 dps = +/- 4000 LSB
-        125 dps = +/- 2000 LSB
-        ... where 1 DPS = 16 LSB */
-        offsets_type.gyro_offset_x =
-            (readSingleRegister(GYRO_OFFSET_X_MSB_ADDR) << 8) | (readSingleRegister(GYRO_OFFSET_X_LSB_ADDR));
-        offsets_type.gyro_offset_y =
-            (readSingleRegister(GYRO_OFFSET_Y_MSB_ADDR) << 8) | (readSingleRegister(GYRO_OFFSET_Y_LSB_ADDR));
-        offsets_type.gyro_offset_z =
-            (readSingleRegister(GYRO_OFFSET_Z_MSB_ADDR) << 8) | (readSingleRegister(GYRO_OFFSET_Z_LSB_ADDR));
-
-        /* Accelerometer radius = +/- 1000 LSB */
-        offsets_type.accel_radius =
-            (readSingleRegister(ACCEL_RADIUS_MSB_ADDR) << 8) | (readSingleRegister(ACCEL_RADIUS_LSB_ADDR));
-
-        /* Magnetometer radius = +/- 960 LSB */
-        offsets_type.mag_radius =
-            (readSingleRegister(MAG_RADIUS_MSB_ADDR) << 8) | (readSingleRegister(MAG_RADIUS_LSB_ADDR));
-
-        setModeTemp(currentMode);
-        return true;
-    }
-    return false;
-}
-
-void IMUSensor::setSensorOffsets(const uint8_t *calibData) {
-    setModeTemp(OPERATION_MODE_CONFIG);
-
-    /* Note: Configuration will take place only when user writes to the last
-        byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.).
-        Therefore the last byte must be written whenever the user wants to
-        changes the configuration. */
-
-    /* A writeLen() would make this much cleaner */
-    writeRegister(ACCEL_OFFSET_X_LSB_ADDR, calibData[0]);
-    writeRegister(ACCEL_OFFSET_X_MSB_ADDR, calibData[1]);
-    writeRegister(ACCEL_OFFSET_Y_LSB_ADDR, calibData[2]);
-    writeRegister(ACCEL_OFFSET_Y_MSB_ADDR, calibData[3]);
-    writeRegister(ACCEL_OFFSET_Z_LSB_ADDR, calibData[4]);
-    writeRegister(ACCEL_OFFSET_Z_MSB_ADDR, calibData[5]);
-
-    writeRegister(MAG_OFFSET_X_LSB_ADDR, calibData[6]);
-    writeRegister(MAG_OFFSET_X_MSB_ADDR, calibData[7]);
-    writeRegister(MAG_OFFSET_Y_LSB_ADDR, calibData[8]);
-    writeRegister(MAG_OFFSET_Y_MSB_ADDR, calibData[9]);
-    writeRegister(MAG_OFFSET_Z_LSB_ADDR, calibData[10]);
-    writeRegister(MAG_OFFSET_Z_MSB_ADDR, calibData[11]);
-
-    writeRegister(GYRO_OFFSET_X_LSB_ADDR, calibData[12]);
-    writeRegister(GYRO_OFFSET_X_MSB_ADDR, calibData[13]);
-    writeRegister(GYRO_OFFSET_Y_LSB_ADDR, calibData[14]);
-    writeRegister(GYRO_OFFSET_Y_MSB_ADDR, calibData[15]);
-    writeRegister(GYRO_OFFSET_Z_LSB_ADDR, calibData[16]);
-    writeRegister(GYRO_OFFSET_Z_MSB_ADDR, calibData[17]);
-
-    writeRegister(ACCEL_RADIUS_LSB_ADDR, calibData[18]);
-    writeRegister(ACCEL_RADIUS_MSB_ADDR, calibData[19]);
-
-    writeRegister(MAG_RADIUS_LSB_ADDR, calibData[20]);
-    writeRegister(MAG_RADIUS_MSB_ADDR, calibData[21]);
-
-    setModeTemp(currentMode);
-}
-
-void IMUSensor::setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type) {
-    setModeTemp(OPERATION_MODE_CONFIG);
-
-    /* Note: Configuration will take place only when user writes to the last
-        byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.).
-        Therefore the last byte must be written whenever the user wants to
-        changes the configuration. */
-
-    writeRegister(ACCEL_OFFSET_X_LSB_ADDR, (offsets_type.accel_offset_x) & 0x0FF);
-    writeRegister(ACCEL_OFFSET_X_MSB_ADDR, (offsets_type.accel_offset_x >> 8) & 0x0FF);
-    writeRegister(ACCEL_OFFSET_Y_LSB_ADDR, (offsets_type.accel_offset_y) & 0x0FF);
-    writeRegister(ACCEL_OFFSET_Y_MSB_ADDR, (offsets_type.accel_offset_y >> 8) & 0x0FF);
-    writeRegister(ACCEL_OFFSET_Z_LSB_ADDR, (offsets_type.accel_offset_z) & 0x0FF);
-    writeRegister(ACCEL_OFFSET_Z_MSB_ADDR, (offsets_type.accel_offset_z >> 8) & 0x0FF);
-
-    writeRegister(MAG_OFFSET_X_LSB_ADDR, (offsets_type.mag_offset_x) & 0x0FF);
-    writeRegister(MAG_OFFSET_X_MSB_ADDR, (offsets_type.mag_offset_x >> 8) & 0x0FF);
-    writeRegister(MAG_OFFSET_Y_LSB_ADDR, (offsets_type.mag_offset_y) & 0x0FF);
-    writeRegister(MAG_OFFSET_Y_MSB_ADDR, (offsets_type.mag_offset_y >> 8) & 0x0FF);
-    writeRegister(MAG_OFFSET_Z_LSB_ADDR, (offsets_type.mag_offset_z) & 0x0FF);
-    writeRegister(MAG_OFFSET_Z_MSB_ADDR, (offsets_type.mag_offset_z >> 8) & 0x0FF);
-
-    writeRegister(GYRO_OFFSET_X_LSB_ADDR, (offsets_type.gyro_offset_x) & 0x0FF);
-    writeRegister(GYRO_OFFSET_X_MSB_ADDR, (offsets_type.gyro_offset_x >> 8) & 0x0FF);
-    writeRegister(GYRO_OFFSET_Y_LSB_ADDR, (offsets_type.gyro_offset_y) & 0x0FF);
-    writeRegister(GYRO_OFFSET_Y_MSB_ADDR, (offsets_type.gyro_offset_y >> 8) & 0x0FF);
-    writeRegister(GYRO_OFFSET_Z_LSB_ADDR, (offsets_type.gyro_offset_z) & 0x0FF);
-    writeRegister(GYRO_OFFSET_Z_MSB_ADDR, (offsets_type.gyro_offset_z >> 8) & 0x0FF);
-
-    writeRegister(ACCEL_RADIUS_LSB_ADDR, (offsets_type.accel_radius) & 0x0FF);
-    writeRegister(ACCEL_RADIUS_MSB_ADDR, (offsets_type.accel_radius >> 8) & 0x0FF);
-
-    writeRegister(MAG_RADIUS_LSB_ADDR, (offsets_type.mag_radius) & 0x0FF);
-    writeRegister(MAG_RADIUS_MSB_ADDR, (offsets_type.mag_radius >> 8) & 0x0FF);
-
-    setModeTemp(currentMode);
-
-}
-
-bool IMUSensor::isFullyCalibrated() {
-    uint8_t system, gyro, accel, mag;
-    getCalibration(&system, &gyro, &accel, &mag);
-
-    switch (currentMode) {
-        case OPERATION_MODE_ACCONLY:
-            return (accel == 3);
-        case OPERATION_MODE_MAGONLY:
-            return (mag == 3);
-        case OPERATION_MODE_GYRONLY:
-        case OPERATION_MODE_M4G: /* No magnetometer calibration required. */
-            return (gyro == 3);
-        case OPERATION_MODE_ACCMAG:
-        case OPERATION_MODE_COMPASS:
-            return (accel == 3 && mag == 3);
-        case OPERATION_MODE_ACCGYRO:
-        case OPERATION_MODE_IMUPLUS:
-            return (accel == 3 && gyro == 3);
-        case OPERATION_MODE_MAGGYRO:
-            return (mag == 3 && gyro == 3);
-        default:
-            return (system == 3 && gyro == 3 && accel == 3 && mag == 3);
-    }   
-}
-
-/* Power managments functions */
-void IMUSensor::enterSuspendMode() {
-    /* Switch to config mode (just in case since this is the default) */
-    setModeTemp(OPERATION_MODE_CONFIG);
-    writeRegister(BNO055_PWR_MODE_ADDR, 0x02);
-    /* Set the requested operating mode (see section 3.3) */
-    setModeTemp(currentMode);
-}
-
-void IMUSensor::enterNormalMode() {
-    /* Switch to config mode (just in case since this is the default) */
-    setModeTemp(OPERATION_MODE_CONFIG);
-    writeRegister(BNO055_PWR_MODE_ADDR, 0x00);
-    /* Set the requested operating mode (see section 3.3) */
-    setModeTemp(modeback);
-}
-
-
-
-
-
-
-
-
-
-
diff --git a/src/surfaceFitModel.cpp b/src/surfaceFitModel.cpp
deleted file mode 100644
index d48da49..0000000
--- a/src/surfaceFitModel.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-#include "../include/surfaceFitModel.hpp"
-
-SurfaceFitModel::SurfaceFitModel() {
-
-    p = MatrixXd::Zero(X_DEGREE + 1, Y_DEGREE + 1);
-
-    p(0, 0) = -781536.384794701;
-    p(1, 0) = 8623.59011973048;
-    p(0, 1) = 643.65918253;
-    p(2, 0) = -34.3646691281487;
-    p(1, 1) = -5.46066535343611;
-    p(0, 2) = -0.177121900557321;
-    p(3, 0) = 0.0573287698655951;
-    p(2, 1) = 0.0150031142038895;
-    p(1, 2) = 0.00101871763126609;
-    p(0, 3) = 1.63862900553892e-05;
-    p(4, 0) = -3.21785828407871e-05;
-    p(3, 1) = -1.3161091180883e-05;
-    p(2, 2) = -1.42505256569339e-06;
-    p(1, 3) = -4.76209793830867e-08;
-}
-
-
-double SurfaceFitModel::getFit(double x, double y) {
-
-    return p(0, 0) + p(1, 0) * x + p(0, 1) * y + p(2, 0) * pow(x, 2) +
-	p(1, 1) * x * y + p(0, 2) * pow(y, 2) + p(3, 0) * pow(x, 3) +
-	p(2, 1) * pow(x, 2) * y + p(1, 2) * x * pow(y, 2) + p(0, 3) * pow(y, 3) +
-	p(4, 0) * pow(x, 4) + p(3, 1) * pow(x, 3) * y + p(2, 2) * pow(x, 2) * pow(y, 2) +
-	p(1, 3) * x * pow(y, 3); 
-}
-
-
-
-
-
-
-
-
-
diff --git a/src/unused/actuationPlan.cpp b/src/unused/actuationPlan.cpp
deleted file mode 100644
index a987478..0000000
--- a/src/unused/actuationPlan.cpp
+++ /dev/null
@@ -1,60 +0,0 @@
-#include "../include/actuationPlan.hpp"
-
-ActuationPlan::ActuationPlan() {}
-
-ActuationPlan::ActuationPlan(SurfaceFitModel sFitModel) : sFitModel(sFitModel) {
-
-}
-
-
-void ActuationPlan::runPlan(Vehicle& rocket) {
-
-
-    if (rocket.imuReadFail || rocket.altiReadFail) {
-        rocket.deployment_angle = deploy_percentage_to_angle(0); // No fin deployment
-    }
-
-    rocket.fail_time = time(nullptr);
-
-    // 2024 Mission---------------------------------------------------------------------
-    if (rocket.status == GLIDE) {
-
-        // Fin deployment based on current drag coefficient value
-        try {
-            double cd = sFitModel.getFit(rocket.filtered_velocity, rocket.filtered_altitude);
-            cd = std::min(std::max(0.0, cd), 100.0);
-            rocket.deployment_angle = deploy_percentage_to_angle(cd);
-        }
-
-        // Full deployment during coasting
-        catch (...) {
-            rocket.deployment_angle = deploy_percentage_to_angle(0);
-            
-            if ((time(nullptr) - rocket.deploy_time) > 2 && (time(nullptr) - rocket.deploy_time) < 7) {
-                rocket.deployment_angle = deploy_percentage_to_angle(100);
-            }
-        }        
-    }
-
-    else if (rocket.status == APOGEE) {
-
-        rocket.deployment_angle = deploy_percentage_to_angle(50);
-    }
-
-    else {
-
-        rocket.deploy_time = time(nullptr);
-    }
-    // End 2024 Mission------------------------------------------------------------------
-}
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/unused/ads.cpp b/src/unused/ads.cpp
deleted file mode 100644
index 5484970..0000000
--- a/src/unused/ads.cpp
+++ /dev/null
@@ -1,286 +0,0 @@
-#include "../include/ads.hpp"
-
-
-// Private----------------------------------------------------------------------
-void ADS::logSummary() {
-    
-    std::string output_string = "" + state_for_log[rocket.status];
-
-    if (!rocket.altiInitFail && !rocket.altiReadFail) {
-
-        output_string += format_data(" ", rocket.filtered_altitude, 3);     
-    }
-
-    output_string += format_data(" ", rocket.deployment_angle, 2);    
-
-    if (!rocket.imuInitFail && !rocket.imuReadFail) {
-
-        output_string += format_data(" ", rocket.acceleration[2], 2);    
-        output_string += format_data(" ", rocket.filtered_velocity, 2);    
-    }
-
-    Logger::Get().log(output_string);
-}
-
-
-void ADS::updateOnPadAltitude() {
-
-    std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-
-    double avg_alt = 0;
-    double alt_read_count = 0;
-
-    while (alt_read_count < COUNT_LIMIT) {
-        
-        altimeter.getData(&rocket.current_altitude);
-        alt_read_count++;
-        avg_alt = (avg_alt * (alt_read_count - 1) + rocket.current_altitude) / alt_read_count;
-    }
- 
-    Logger::Get().log(format_data("pad altitude initialization complete - ", avg_alt, 3));
-    rocket.ON_PAD_altitude = avg_alt;
-}
-
-
-void ADS::updateSensorData() {
-
-    if (!rocket.imuInitFail) {
-        
-        try {
-            imu.getData((void*)&rocket);
-        }
-
-        catch (...) {
-            std::exception_ptr e = std::current_exception();
-            Logger::Get().logErr(e.__cxa_exception_type()->name());
-            rocket.imuReadFail = true;
-        }
-    }  
-
-    rocket.previous_altitude = rocket.current_altitude; // Why was this placed here????
-    
-    if (!rocket.altiInitFail) {
-        
-        try {
-            altimeter.getData((void*)&rocket.current_altitude);
-            if (rocket.ON_PAD_fail) {
-                rocket.ON_PAD_altitude = rocket.current_altitude;
-                rocket.ON_PAD_fail = false;
-            }
-
-            rocket.altiReadFail = false;
-        }
-
-        catch (...) {
-            std::exception_ptr e = std::current_exception();
-            Logger::Get().logErr(e.__cxa_exception_type()->name());
-            rocket.altiReadFail = true;
-        }
-    }
-}
-
-
-void ADS::updateRocketState() {
-
-    // Filter sensor data
-    VectorXf control_input(1);
-    VectorXf measurement(1);
-    control_input << rocket.acceleration[2];
-    measurement << rocket.current_altitude;
-    VectorXf filtered = kf.run(control_input, measurement, rocket.dt);
-    rocket.filtered_altitude = filtered(0);
-    rocket.filtered_velocity = filtered(1);
-
-    if (rocket.apogee_altitude < rocket.filtered_altitude) {
-        rocket.apogee_altitude = rocket.filtered_altitude;
-    }
-
-    // (VEHICLE ON PAD)
-    if (rocket.status == ON_PAD) {
-
-        // If launch detected
-        if (rocket.acceleration[2] >= BOOST_ACCEL_THRESH * G_0 
-            && rocket.filtered_altitude >= BOOST_HEIGHT_THRESH + rocket.ON_PAD_altitude) {
-            Logger::Get().log(format_data("LOM at -- ", (double)(rocket.liftoff_time - rocket.start_time), 3));
-        }
-
-        if (TEST_MODE && time(nullptr) - rocket.start_time >= 15) {
-            Logger::Get().log(format_data("TEST LOM at -- ", (double)(rocket.liftoff_time - rocket.start_time), 3));
-        }
-
-        if (time(nullptr) - rocket.relog_time > 2*60*60 
-            && rocket.status == ON_PAD) {
-            std::cout << "OverWR Success" << std::endl;
-        }
-    }
-
-    // (VEHICLE BOOSTING)
-    else if (rocket.status == BOOST) {
-
-        if (rocket.acceleration[2] <= GLIDE_ACCEL_THRESH * G_0 
-            || time(nullptr) - rocket.liftoff_time >= TIME_BO) {
-            rocket.status = GLIDE;
-        }
-        
-    }
-
-    // (VEHICLE IN GLIDE)
-    else if (rocket.status == GLIDE) {
-
-        if (rocket.filtered_altitude < rocket.apogee_altitude - APOGEE_FSM_CHANGE 
-            || time(nullptr) - rocket.liftoff_time >= TIME_BO + TIME_APO) {
-            rocket.status = APOGEE;
-            Logger::Get().log(format_data("APO: ", (double)(rocket.apogee_altitude), 2));
-        }
-    }
-
-    // (VEHICLE AT APOGEE)
-    else if (rocket.status == APOGEE) {
-
-        if (rocket.filtered_altitude <= FSM_DONE_SURFACE_ALTITUDE + rocket.ON_PAD_altitude) {
-            rocket.status = DONE;
-            return;
-        }
-    }
-}
-
-
-// Public----------------------------------------------------------------------
-ADS::ADS(ActuationPlan plan) : plan(plan) {
-
-    rocket.status = ON_PAD;
-
-    rocket.apogee_altitude = 0;
-    rocket.previous_altitude = 0;
-    rocket.current_altitude = 0;
-    rocket.filtered_altitude = 0;
-
-    rocket.filtered_velocity = 0;
-
-    rocket.duty_span = DUTY_MAX - DUTY_MIN;
-    rocket.deployment_angle = deploy_percentage_to_angle(INIT_DEPLOYMENT); 
-
-    rocket.dt = 0.1;
-
-    rocket.imuInitFail = false;
-    rocket.imuReadFail = false;
-    rocket.altiInitFail = false;
-    rocket.altiReadFail = false;
-
-    rocket.ON_PAD_altitude = 0;
-    rocket.ON_PAD_fail = false;
-
-    rocket.start_time = time(nullptr);
-    rocket.fail_time = rocket.start_time;
-    rocket.relog_time = rocket.start_time;
-    rocket.led_time = rocket.start_time;
-
-    imu = IMUSensor();
-    altimeter = AltimeterSensor();
-    motor = Motor();
-    kf = KalmanFilter(2, 1, 1, rocket.dt);
-
-    Logger::Get().openLog(LOG_FILENAME);
-    
-    motor.init(&rocket);
-
-    imu.init(nullptr);
-    altimeter.init(nullptr);
-
-    if (TEST_MODE) {
-
-        Logger::Get().log("TEST Record Start --");
-    }
-}
-
-
-
-void ADS::run() {
-
-    if (!rocket.altiInitFail) {
-        try {
-            updateOnPadAltitude();
-        }
-
-        catch (...) {
-            std::exception_ptr e = std::current_exception();
-            Logger::Get().logErr(e.__cxa_exception_type()->name());
-            rocket.ON_PAD_fail = true;
-        }
-    }
-
-    rocket.loop_time = time(nullptr);
-    while (rocket.status != DONE) {
-
-        updateSensorData();
-
-        if (!rocket.imuInitFail && !rocket.altiInitFail) {
-
-            updateRocketState();
-
-            // Run the Actuation Plan----------------------------------
-            plan.runPlan(rocket);
-
-            if (rocket.imuReadFail || rocket.altiReadFail) {
-
-                if (rocket.imuReadFail) {
-                    imu.init(nullptr); // Restart
-                    Logger::Get().log("Altimeter reset attempt");
-                }
-
-                if (rocket.altiReadFail) {
-                    altimeter.init(nullptr); // Restart
-                    Logger::Get().log("IMU reset attempt");
-                }
-            }
-        }
-
-        // Altimeter or IMU setup failed. Attempt to reinitialize
-        else {
-
-            if (time(nullptr) - rocket.fail_time >= TIME_END) {
-                rocket.status = DONE;
-            }
-
-            if (rocket.altiInitFail || rocket.altiReadFail) {
-                imu.init(nullptr); // Restart
-                Logger::Get().log("Altimeter reset attempt");
-            }
-
-            if (rocket.imuInitFail || rocket.imuReadFail) {
-                altimeter.init(nullptr); // Restart
-                Logger::Get().log("IMU reset attempt");
-            }
-
-            rocket.deployment_angle = deploy_percentage_to_angle(INIT_DEPLOYMENT);
-        }
-
-        // Actuate Servos
-        motor.writeData(&rocket);
-
-        logSummary();
-
-        // Blink Beaglebone LED 1
-        if (time(nullptr) - rocket.led_time > LED_GAP_TIME) {
-            led_out(&rocket);
-        }
-
-        std::this_thread::sleep_for(std::chrono::milliseconds(1));
-        rocket.dt = time(nullptr) - rocket.loop_time;
-        rocket.loop_time = time(nullptr);
-    }
-
-    Logger::Get().closeLog();
-    std::cout << "Done" << std::endl;
-}
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/unused/logger.cpp b/src/unused/logger.cpp
deleted file mode 100644
index a857be8..0000000
--- a/src/unused/logger.cpp
+++ /dev/null
@@ -1,132 +0,0 @@
-#include "../include/logger.hpp"
-
-// Private----------------------------------------------------------------------
-std::string Logger::getDate() {
-
-    t = time(nullptr);
-    now = localtime(&t);
-    return "(" + days[now->tm_wday] + " " + months[now->tm_mon] + " " 
-        + std::to_string(now->tm_mday) + " " + std::to_string(now->tm_year + 1900) + ")";
-}
-
-std::string Logger::getTime() {
-
-    t = time(nullptr);
-    now = localtime(&t);
-    std::string hour = std::to_string(now->tm_hour);
-    std::string min = std::to_string(now->tm_min);
-    std::string sec = std::to_string(now->tm_sec);
-    //string hour = "0" + to_string(now->tm_hour);
-
-    if (now->tm_hour < 10) {
-        hour = "0" + std::to_string(now->tm_hour);
-    }
-
-    if (now->tm_min < 10) {
-        min = "0" + std::to_string(now->tm_min);
-    }
-
-    if (now->tm_sec < 10) {
-        sec = "0" + std::to_string(now->tm_sec);
-    }
-
-    return hour + ":" + min + 
-        ":" + sec;
-}
-
-
-// Public----------------------------------------------------------------------
-Logger& Logger::Get() {
-
-    static Logger loggerSingleton;
-    return loggerSingleton;
-}
-
-//Logger Logger::loggerSingleton;
-
-
-bool Logger::openLog(std::string _filename) {
-
-    filename = _filename;
-    
-    if (file_open) {
-        return false;
-    }
-    
-    file.open(filename, std::ios::in | std::ios::out | std::ios::app);
-
-    if (!file) {
-        return false;
-    }
-
-    file_open = true;
-    std::string date = getDate();
-    std::string timestamp = getTime();
-    file << timestamp << infoTag << "Log Start---- " << date << std::endl;
-
-    return true;
-}
-
-
-void Logger::closeLog() {
-
-    std::string timestamp = getTime();
-    file << timestamp << infoTag << "Log End----\n\n";
-
-    file.close();
-    file_open = false;
-}
-
-
-bool Logger::log(std::string data) {
-
-    if (!file) {
-        return false;
-    }
-
-    if (!file_open) {
-        return false;
-    }
-    std::string timestamp = getTime();
-    file << timestamp << infoTag << data << std::endl;
-    return true;
-}
-
-bool Logger::logErr(std::string data) {
-
-    if (!file) {
-        return false;
-    }
-
-    if (!file_open) {
-        return false;
-    }
-
-    std::string timestamp = getTime();
-    file << timestamp << errorTag << data << std::endl;
-    return true;
-}
-
-
-bool Logger::printLog() {
-
-    if (file.is_open()) {
-        std::cout << "Log still open. Please close Log." << std::endl;
-        return false;
-    }
-
-    file.open(filename, std::ios::in);
-
-    if (!file.is_open()) {
-        return false;
-    }
-
-    std::string line;
-    while(getline(file, line)) {
-        std::cout << line << std::endl;
-    }
-
-    file.close();
-
-    return true;
-}
diff --git a/src/unused/motor.cpp b/src/unused/motor.cpp
deleted file mode 100644
index 84785a9..0000000
--- a/src/unused/motor.cpp
+++ /dev/null
@@ -1,46 +0,0 @@
-#include "../include/motor.hpp"
-
-
-
-Motor::Motor() {
-
-
-}
-
-bool Motor::init(void* data) {
-
-    Vehicle *vehicle = (Vehicle *) data;
-    double duty = 100 - ((MIN_ANGLE / 180) * vehicle->duty_span + DUTY_MIN);
-
-    // Initialize stuff
-    // .....
-    // .....
-
-
-    data = (void*) vehicle; // Is this necessary?
-    return true;
-}
-
-
-bool Motor::writeData(void* data) {
-
-    Vehicle *vehicle = (Vehicle *) data;
-    double duty = 100 - ((vehicle->deployment_angle / 180) * vehicle->duty_span + DUTY_MIN);
-
-    // Send the Data somewhere
-    // ..... Pin
-    // ..... Duty
-    // ..... PWM frequency Hz
-    // ..... Polarity
-    
-
-    if (1 == 2) {
-        Logger::Get().logErr("Some type of Error");
-        return false;
-    }
-
-    data = (void*) vehicle; // Is this necessary?
-    return true;
-}
-
-
diff --git a/src/unused/rocketUtils.cpp b/src/unused/rocketUtils.cpp
deleted file mode 100644
index 45fcfc3..0000000
--- a/src/unused/rocketUtils.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
-#include "../include/rocketUtils.hpp"
-
-double deploy_percentage_to_angle(double percentage) {
-
-    return (MAX_ANGLE - MIN_ANGLE) / 100.0 * percentage + MIN_ANGLE;
-}
-
-
-std::string format_data(std::string prefix, double data, int precision) {
-
-    std::stringstream stream;
-    stream << std::fixed << std::setprecision(precision) << data;
-    std::string s = stream.str();
-    return prefix + s;
-}
-
-bool led_out(Vehicle *vehicle) {
-
-    std::ofstream file;
-    file.open(LED_FILENAME);
-    if (!file.is_open()) {
-        return false;
-    }
-
-    file << std::to_string(vehicle->led_brightness);
-    file.close();
-
-    vehicle->led_time = time(nullptr);
-    vehicle->led_brightness = (vehicle->led_brightness + 1) % 2;
-
-    return true;
-}
-
-std::string state_for_log[5] = {"ON_PAD", "BOOST", "GLIDE", "APOGEE", "DONE"};
-	
-\ No newline at end of file
diff --git a/src/unused/sensorAltimeter.cpp b/src/unused/sensorAltimeter.cpp
deleted file mode 100644
index 8ec065d..0000000
--- a/src/unused/sensorAltimeter.cpp
+++ /dev/null
@@ -1,115 +0,0 @@
-#include "sensorAltimeter.hpp"
-
-AltimeterSensor::AltimeterSensor(std::string I2C_FILE_in) {
-    I2C_FILE = I2C_FILE_in;
-    deviceAddress = 0x60;
-}
-
-//Startup routine copied from Adafruit library, as is most of the data getting methods
-//Adaptation is largely editing for readability and porting from Adafruit_I2C to BBB I2C (sensorI2C.hpp implementation)
-bool AltimeterSensor::init() {
-
-    // Vehicle *vehicle = (Vehicle *) data;
-    // // Do Stuff 
-    // data = (void*) vehicle;
-
-    //Pass file string from parent to setup function, actual I2C bus gets stored internally.
-    setupI2C(I2C_FILE);
-
-    // Check a register with a hard-coded value to see if comms are working
-    uint8_t whoami = readSingleRegister(MPL3115A2_WHOAMI);
-    if (whoami != 0xC4) {
-        fprintf(stderr, "MPL INITIALIZATION DID NOT PASS WHOAMI DEVICE CHECK!, got: %X, expected: 0xC4\n", whoami);
-        return false;
-    }
-
-    //Send device dedicated reset byte to CTRL1 Register
-    writeRegister(MPL3115A2_CTRL_REG1, MPL3115A2_CTRL_REG1_RST);
-    //Wait for reset to wipe its way through device and reset appropriate bit of CTRL1 Register
-    while (readSingleRegister(MPL3115A2_CTRL_REG1) & MPL3115A2_CTRL_REG1_RST);
-
-     //Set oversampling (?) and altitude mode by default
-    currentMode = MPL3115A2_ALTIMETER;
-    ctrl_reg1.reg = MPL3115A2_CTRL_REG1_OS128 | MPL3115A2_CTRL_REG1_ALT;
-    writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg);
-
-    //Configure data return types, I don't really understand this chunk but Adafruit does it this way so we will too I guess
-    writeRegister(MPL3115A2_PT_DATA_CFG, MPL3115A2_PT_DATA_CFG_TDEFE |
-                                    MPL3115A2_PT_DATA_CFG_PDEFE |
-                                    MPL3115A2_PT_DATA_CFG_DREM);
-
-    return true;
-}
-
-//EXPECTED THAT USER WILL NEVER SET MODE TO PRESSURE AFTER INITIAL CONFIGURATION
-void AltimeterSensor::setMode(mpl3115a2_mode_t mode) {
-    ctrl_reg1.reg = readSingleRegister(MPL3115A2_CTRL_REG1);
-    ctrl_reg1.bit.ALT = mode;
-    writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg);
-    currentMode = mode;
-}
-
-double AltimeterSensor::getAltitude() {
-    //Request new data reading
-    requestOneShotReading();
-    //If new data is available, read it and store it to internal fields
-    if (isNewDataAvailable()) {
-        //Logger flag here for new data?
-        updateCurrentDataBuffer();
-    }
-    //Return internal field, whether updated or not
-    return internalAltitude;
-}
-
-double AltimeterSensor::getTemperature() {
-    //Request new data reading
-    requestOneShotReading();
-    //If new data is available, read it and store it to internal fields
-    if (isNewDataAvailable()) {
-        //Logger flag here for new data?
-        updateCurrentDataBuffer();
-    }
-    //Return internal field, whether updated or not
-    return internalTemperature;
-}
-
-void AltimeterSensor::requestOneShotReading() {
-    //Request current status of oneshot reading
-    ctrl_reg1.reg = readSingleRegister(MPL3115A2_CTRL_REG1);
-    //If oneshot is complete, proc a new one; if it isn't, do nothing.
-    //THIS PRODUCES DUPLICATE DATA IF READING REQUESTS FROM BB DON'T LINE UP WITH READING COMPLETION ON SENSOR.
-    if (!ctrl_reg1.bit.OST) {
-        // initiate one-shot measurement
-        ctrl_reg1.bit.OST = 1;
-        writeRegister(MPL3115A2_CTRL_REG1, ctrl_reg1.reg);
-    }
-}
-
-bool AltimeterSensor::isNewDataAvailable() {
-    //Returns PTDR bit of status register, 1 if new data for Temp OR Alt/Pres is available
-    //There *are* registers available for exclusively temperature *or* pressure/altitude, but 
-    //for simplicity's sake we'll use the combined one for now.
-    return ((readSingleRegister(MPL3115A2_REGISTER_STATUS) & MPL3115A2_REGISTER_STATUS_PTDR) != 0);
-}
-
-//Adafruit returns specific field based on input parameter, this method updates all internal fields at once instead
-void AltimeterSensor::updateCurrentDataBuffer() {
-    uint8_t buffer[5] = {MPL3115A2_REGISTER_PRESSURE_MSB, 0, 0, 0, 0};
-    readMultipleRegisters(MPL3115A2_REGISTER_PRESSURE_MSB, 5);
-
-    //Pressure is no longer used, assumed rocket is only logging altitude
-    // uint32_t pressure;
-    // pressure = uint32_t(buffer[0]) << 16 | uint32_t(buffer[1]) << 8 |
-    //         uint32_t(buffer[2]);
-    // return double(pressure) / 6400.0;
-
-    //Altitude Conversion
-    int32_t alt;
-    alt = uint32_t(buffer[0]) << 24 | uint32_t(buffer[1]) << 16 |
-        uint32_t(buffer[2]) << 8;
-    internalAltitude = double(alt) / 65536.0;
-
-    int16_t t;
-    t = uint16_t(buffer[3]) << 8 | uint16_t(buffer[4]);
-    internalTemperature = double(t) / 256.0;
-}
diff --git a/src/unused/sensorIMU.cpp b/src/unused/sensorIMU.cpp
deleted file mode 100644
index 941ea35..0000000
--- a/src/unused/sensorIMU.cpp
+++ /dev/null
@@ -1,385 +0,0 @@
-#include "../include/sensorIMU.hpp"
-
-IMUSensor::IMUSensor(std::string I2C_FILE) {
-    this -> I2C_FILE = I2C_FILE;
-}
-
-bool IMUSensor::init(void* data) {
-
-    //I2C_File passed on object creation, stored in sensorI2C parent
-    setupI2C(I2C_FILE);
-    
-    //In the adafruit code there's a big step of waiting for timeout and connection stuff for up to a full second
-    //I don't do that here because the BBB takes like 17 years to boot so we'll just hope it goes faster than that
-
-    //Sanity check for factory device ID 
-    uint8_t id = readSingleRegister(BNO055_CHIP_ID_ADDR);
-    if (id != BNO055_ID) {
-        fprintf(stderr, "DEVICE ID DID NOT PASS SANITY CHECK FOR BNO IMU!");
-        return false;
-    }
-
-    //Set default operating mode of IMU into config from startup (will be set properly after config phase)
-    setModeHard(OPERATION_MODE_CONFIG);
-
-    //Writes 1 to the system reset bit in the trigger register
-    writeRegister(BNO055_SYS_TRIGGER_ADDR, 0x20);
-    //Wait for reset to complete by doing sanity check again
-    while (readSingleRegister(BNO055_CHIP_ID_ADDR) != BNO055_ID);
-
-    //Set power mode for sensor
-    writeRegister(BNO055_PWR_MODE_ADDR, POWER_MODE_NORMAL);
-
-    //Sensor chip uses two "pages" to multiplex register values
-    //Page 0 contains the sensor data (not configuration), which is what we want
-    writeRegister(BNO055_PAGE_ID_ADDR, 0);
-
-    //Genuinely no idea why Adafruit does this, ensuring all triggers are off before mode config I guess
-    writeRegister(BNO055_SYS_TRIGGER_ADDR, 0x0);
-
-    setModeTemp(default_mode);
-
-    return true;
-}
-
-//Sets mode so it can be undone for temporary changes, like operation setting
-void IMUSensor::setModeTemp(adafruit_bno055_opmode_t mode) {
-    currentMode = mode;
-    writeRegister(BNO055_OPR_MODE_ADDR, currentMode);
-}
-
-//Sets mode *AND* internal state variable
-void IMUSensor::setModeTemp(adafruit_bno055_opmode_t mode) {
-    writeRegister(BNO055_OPR_MODE_ADDR, currentMode);
-}
-
-adafruit_bno055_opmode_t IMUSensor::getMode() {
-    return (adafruit_bno055_opmode_t)readSingleRegister(BNO055_OPR_MODE_ADDR);
-}
-
-imu::Vector<3> IMUSensor::getVector(adafruit_vector_type_t vector_type) {
-    imu::Vector<3> xyz;
-    uint8_t buffer[6] = readMultipleRegisters((adafruit_bno055_reg_t)vector_type, 6);
-
-    int16_t x, y, z;
-    x = y = z = 0;
-
-    /* Read vector data (6 bytes) */
-    x = ((int16_t)buffer[0]) | (((int16_t)buffer[1]) << 8);
-    y = ((int16_t)buffer[2]) | (((int16_t)buffer[3]) << 8);
-    z = ((int16_t)buffer[4]) | (((int16_t)buffer[5]) << 8);
-
-    /*!
-    * Convert the value to an appropriate range (section 3.6.4)
-    * and assign the value to the Vector type
-    */
-    switch (vector_type) {
-        case VECTOR_MAGNETOMETER:
-            /* 1uT = 16 LSB */
-            xyz[0] = ((double)x) / 16.0;
-            xyz[1] = ((double)y) / 16.0;
-            xyz[2] = ((double)z) / 16.0;
-            break;
-        case VECTOR_GYROSCOPE:
-            /* 1dps = 16 LSB */
-            xyz[0] = ((double)x) / 16.0;
-            xyz[1] = ((double)y) / 16.0;
-            xyz[2] = ((double)z) / 16.0;
-            break;
-        case VECTOR_EULER:
-            /* 1 degree = 16 LSB */
-            xyz[0] = ((double)x) / 16.0;
-            xyz[1] = ((double)y) / 16.0;
-            xyz[2] = ((double)z) / 16.0;
-            break;
-        case VECTOR_ACCELEROMETER:
-            /* 1m/s^2 = 100 LSB */
-            xyz[0] = ((double)x) / 100.0;
-            xyz[1] = ((double)y) / 100.0;
-            xyz[2] = ((double)z) / 100.0;
-            break;
-        case VECTOR_LINEARACCEL:
-            /* 1m/s^2 = 100 LSB */
-            xyz[0] = ((double)x) / 100.0;
-            xyz[1] = ((double)y) / 100.0;
-            xyz[2] = ((double)z) / 100.0;
-            break;
-        case VECTOR_GRAVITY:
-            /* 1m/s^2 = 100 LSB */
-            xyz[0] = ((double)x) / 100.0;
-            xyz[1] = ((double)y) / 100.0;
-            xyz[2] = ((double)z) / 100.0;
-            break;
-    }
-
-    return xyz;
-}
-imu::Quaternion IMUSensor::getQuat() {
-    uint8_t buffer[8] = readMultipleRegisters(BNO055_QUATERNION_DATA_W_LSB_ADDR, 8);
-
-    int16_t x, y, z, w;
-    x = y = z = w = 0;
-
-    //Bit shift data into the right places and store it
-    w = (((uint16_t)buffer[1]) << 8) | ((uint16_t)buffer[0]);
-    x = (((uint16_t)buffer[3]) << 8) | ((uint16_t)buffer[2]);
-    y = (((uint16_t)buffer[5]) << 8) | ((uint16_t)buffer[4]);
-    z = (((uint16_t)buffer[7]) << 8) | ((uint16_t)buffer[6]);
-
-    /*!
-    * Assign to Quaternion
-    * See
-    * https://cdn-shop.adafruit.com/datasheets/BST_BNO055_DS000_12.pdf
-    * 3.6.5.5 Orientation (Quaternion)
-    */
-    const double scale = (1.0 / (1 << 14));
-    imu::Quaternion quat(scale * w, scale * x, scale * y, scale * z);
-    return quat;
-}
-
-int8_t IMUSensor::getTemp() {
-    int8_t temp = (int8_t)(readSingleRegister(BNO055_TEMP_ADDR));
-    return temp;
-}
-
-void IMUSensor::setAxisRemap(adafruit_bno055_axis_remap_config_t remapcode) {
-    //Put into proper config for mapping stuff
-    setModeTemp(OPERATION_MODE_CONFIG);
-    writeRegister(BNO055_AXIS_MAP_CONFIG_ADDR, remapcode);
-
-    //Return mode to operating mode
-    setModeTemp(currentMode);
-}
-
-void IMUSensor::setAxisSign(adafruit_bno055_axis_remap_sign_t remapsign) {
-    //See above method, pretty much the exact same
-    setModeTemp(OPERATION_MODE_CONFIG);
-    writeRegister(BNO055_AXIS_MAP_SIGN_ADDR, remapsign);
-    setModeTemp(currentMode);
-}    
-
-//This method is weird; it intakes several existing byte pointers to see what action it should take. Luckily, we shouldn't have to use it.
-void IMUSensor::getSystemStatus(uint8_t *system_status, uint8_t *self_test_result, uint8_t *system_error) {
-    //Make sure IMU is on proper register page to get system status
-    writeRegister(BNO055_PAGE_ID_ADDR, 0);
-    
-    //If system status requested, read the status.
-    if (system_status != 0) *system_status = readSingleRegister(BNO055_SYS_STAT_ADDR);
-    //If self test result requested, pull the self test results.
-    if (self_test_result != 0) *self_test_result = readSingleRegister(BNO055_SELFTEST_RESULT_ADDR);
-    //Finally, if there's an error pull and stash it.
-    if (system_error != 0) *system_error = readSingleRegister(BNO055_SYS_ERR_ADDR);
-}
-
-//Same as above method, byte pointers are fed into it as parameters that get populated by method.
-void IMUSensor::getCalibration(uint8_t *sys, uint8_t *gyro, uint8_t *accel, uint8_t *mag) {
-    uint8_t calData = readSingleRegister(BNO055_CALIB_STAT_ADDR);
-    if (sys != NULL) {
-        *sys = (calData >> 6) & 0x03;
-    }
-    if (gyro != NULL) {
-        *gyro = (calData >> 4) & 0x03;
-    }
-    if (accel != NULL) {
-        *accel = (calData >> 2) & 0x03;
-    }
-    if (mag != NULL) {
-        *mag = calData & 0x03;
-    }
-}
-
-/* Functions to deal with raw calibration data */
-bool IMUSensor::getSensorOffsets(uint8_t *calibData) {
-    if (isFullyCalibrated()) {
-        setModeTemp(OPERATION_MODE_CONFIG);
-
-        calibData = readMultipleRegisters(ACCEL_OFFSET_X_LSB_ADDR, NUM_BNO055_OFFSET_REGISTERS);
-
-        setModeTemp(currentMode);
-        return true;
-    }
-    return false;
-}
-
-//Fully populated offset getter using type of offset, not just calibration data
-bool IMUSensor::getSensorOffsets(adafruit_bno055_offsets_t &offsets_type) {
-    if (isFullyCalibrated()) {
-        setModeTemp(OPERATION_MODE_CONFIG);
-
-        /* Accel offset range depends on the G-range:
-        +/-2g  = +/- 2000 mg
-        +/-4g  = +/- 4000 mg
-        +/-8g  = +/- 8000 mg
-        +/-1§g = +/- 16000 mg */
-        offsets_type.accel_offset_x = (readSingleRegister(ACCEL_OFFSET_X_MSB_ADDR) << 8) |
-                                    (readSingleRegister(ACCEL_OFFSET_X_LSB_ADDR));
-        offsets_type.accel_offset_y = (readSingleRegister(ACCEL_OFFSET_Y_MSB_ADDR) << 8) |
-                                    (readSingleRegister(ACCEL_OFFSET_Y_LSB_ADDR));
-        offsets_type.accel_offset_z = (readSingleRegister(ACCEL_OFFSET_Z_MSB_ADDR) << 8) |
-                                    (readSingleRegister(ACCEL_OFFSET_Z_LSB_ADDR));
-
-        /* Magnetometer offset range = +/- 6400 LSB where 1uT = 16 LSB */
-        offsets_type.mag_offset_x =
-            (readSingleRegister(MAG_OFFSET_X_MSB_ADDR) << 8) | (readSingleRegister(MAG_OFFSET_X_LSB_ADDR));
-        offsets_type.mag_offset_y =
-            (readSingleRegister(MAG_OFFSET_Y_MSB_ADDR) << 8) | (readSingleRegister(MAG_OFFSET_Y_LSB_ADDR));
-        offsets_type.mag_offset_z =
-            (readSingleRegister(MAG_OFFSET_Z_MSB_ADDR) << 8) | (readSingleRegister(MAG_OFFSET_Z_LSB_ADDR));
-
-        /* Gyro offset range depends on the DPS range:
-        2000 dps = +/- 32000 LSB
-        1000 dps = +/- 16000 LSB
-        500 dps = +/- 8000 LSB
-        250 dps = +/- 4000 LSB
-        125 dps = +/- 2000 LSB
-        ... where 1 DPS = 16 LSB */
-        offsets_type.gyro_offset_x =
-            (readSingleRegister(GYRO_OFFSET_X_MSB_ADDR) << 8) | (readSingleRegister(GYRO_OFFSET_X_LSB_ADDR));
-        offsets_type.gyro_offset_y =
-            (readSingleRegister(GYRO_OFFSET_Y_MSB_ADDR) << 8) | (readSingleRegister(GYRO_OFFSET_Y_LSB_ADDR));
-        offsets_type.gyro_offset_z =
-            (readSingleRegister(GYRO_OFFSET_Z_MSB_ADDR) << 8) | (readSingleRegister(GYRO_OFFSET_Z_LSB_ADDR));
-
-        /* Accelerometer radius = +/- 1000 LSB */
-        offsets_type.accel_radius =
-            (readSingleRegister(ACCEL_RADIUS_MSB_ADDR) << 8) | (readSingleRegister(ACCEL_RADIUS_LSB_ADDR));
-
-        /* Magnetometer radius = +/- 960 LSB */
-        offsets_type.mag_radius =
-            (readSingleRegister(MAG_RADIUS_MSB_ADDR) << 8) | (readSingleRegister(MAG_RADIUS_LSB_ADDR));
-
-        setModeTemp(currentMode);
-        return true;
-    }
-    return false;
-}
-
-void IMUSensor::setSensorOffsets(const uint8_t *calibData) {
-    setModeTemp(OPERATION_MODE_CONFIG);
-
-    /* Note: Configuration will take place only when user writes to the last
-        byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.).
-        Therefore the last byte must be written whenever the user wants to
-        changes the configuration. */
-
-    /* A writeLen() would make this much cleaner */
-    writeRegister(ACCEL_OFFSET_X_LSB_ADDR, calibData[0]);
-    writeRegister(ACCEL_OFFSET_X_MSB_ADDR, calibData[1]);
-    writeRegister(ACCEL_OFFSET_Y_LSB_ADDR, calibData[2]);
-    writeRegister(ACCEL_OFFSET_Y_MSB_ADDR, calibData[3]);
-    writeRegister(ACCEL_OFFSET_Z_LSB_ADDR, calibData[4]);
-    writeRegister(ACCEL_OFFSET_Z_MSB_ADDR, calibData[5]);
-
-    writeRegister(MAG_OFFSET_X_LSB_ADDR, calibData[6]);
-    writeRegister(MAG_OFFSET_X_MSB_ADDR, calibData[7]);
-    writeRegister(MAG_OFFSET_Y_LSB_ADDR, calibData[8]);
-    writeRegister(MAG_OFFSET_Y_MSB_ADDR, calibData[9]);
-    writeRegister(MAG_OFFSET_Z_LSB_ADDR, calibData[10]);
-    writeRegister(MAG_OFFSET_Z_MSB_ADDR, calibData[11]);
-
-    writeRegister(GYRO_OFFSET_X_LSB_ADDR, calibData[12]);
-    writeRegister(GYRO_OFFSET_X_MSB_ADDR, calibData[13]);
-    writeRegister(GYRO_OFFSET_Y_LSB_ADDR, calibData[14]);
-    writeRegister(GYRO_OFFSET_Y_MSB_ADDR, calibData[15]);
-    writeRegister(GYRO_OFFSET_Z_LSB_ADDR, calibData[16]);
-    writeRegister(GYRO_OFFSET_Z_MSB_ADDR, calibData[17]);
-
-    writeRegister(ACCEL_RADIUS_LSB_ADDR, calibData[18]);
-    writeRegister(ACCEL_RADIUS_MSB_ADDR, calibData[19]);
-
-    writeRegister(MAG_RADIUS_LSB_ADDR, calibData[20]);
-    writeRegister(MAG_RADIUS_MSB_ADDR, calibData[21]);
-
-    setModeTemp(currentMode);
-}
-
-void IMUSensor::setSensorOffsets(const adafruit_bno055_offsets_t &offsets_type) {
-    setModeTemp(OPERATION_MODE_CONFIG);
-
-    /* Note: Configuration will take place only when user writes to the last
-        byte of each config data pair (ex. ACCEL_OFFSET_Z_MSB_ADDR, etc.).
-        Therefore the last byte must be written whenever the user wants to
-        changes the configuration. */
-
-    writeRegister(ACCEL_OFFSET_X_LSB_ADDR, (offsets_type.accel_offset_x) & 0x0FF);
-    writeRegister(ACCEL_OFFSET_X_MSB_ADDR, (offsets_type.accel_offset_x >> 8) & 0x0FF);
-    writeRegister(ACCEL_OFFSET_Y_LSB_ADDR, (offsets_type.accel_offset_y) & 0x0FF);
-    writeRegister(ACCEL_OFFSET_Y_MSB_ADDR, (offsets_type.accel_offset_y >> 8) & 0x0FF);
-    writeRegister(ACCEL_OFFSET_Z_LSB_ADDR, (offsets_type.accel_offset_z) & 0x0FF);
-    writeRegister(ACCEL_OFFSET_Z_MSB_ADDR, (offsets_type.accel_offset_z >> 8) & 0x0FF);
-
-    writeRegister(MAG_OFFSET_X_LSB_ADDR, (offsets_type.mag_offset_x) & 0x0FF);
-    writeRegister(MAG_OFFSET_X_MSB_ADDR, (offsets_type.mag_offset_x >> 8) & 0x0FF);
-    writeRegister(MAG_OFFSET_Y_LSB_ADDR, (offsets_type.mag_offset_y) & 0x0FF);
-    writeRegister(MAG_OFFSET_Y_MSB_ADDR, (offsets_type.mag_offset_y >> 8) & 0x0FF);
-    writeRegister(MAG_OFFSET_Z_LSB_ADDR, (offsets_type.mag_offset_z) & 0x0FF);
-    writeRegister(MAG_OFFSET_Z_MSB_ADDR, (offsets_type.mag_offset_z >> 8) & 0x0FF);
-
-    writeRegister(GYRO_OFFSET_X_LSB_ADDR, (offsets_type.gyro_offset_x) & 0x0FF);
-    writeRegister(GYRO_OFFSET_X_MSB_ADDR, (offsets_type.gyro_offset_x >> 8) & 0x0FF);
-    writeRegister(GYRO_OFFSET_Y_LSB_ADDR, (offsets_type.gyro_offset_y) & 0x0FF);
-    writeRegister(GYRO_OFFSET_Y_MSB_ADDR, (offsets_type.gyro_offset_y >> 8) & 0x0FF);
-    writeRegister(GYRO_OFFSET_Z_LSB_ADDR, (offsets_type.gyro_offset_z) & 0x0FF);
-    writeRegister(GYRO_OFFSET_Z_MSB_ADDR, (offsets_type.gyro_offset_z >> 8) & 0x0FF);
-
-    writeRegister(ACCEL_RADIUS_LSB_ADDR, (offsets_type.accel_radius) & 0x0FF);
-    writeRegister(ACCEL_RADIUS_MSB_ADDR, (offsets_type.accel_radius >> 8) & 0x0FF);
-
-    writeRegister(MAG_RADIUS_LSB_ADDR, (offsets_type.mag_radius) & 0x0FF);
-    writeRegister(MAG_RADIUS_MSB_ADDR, (offsets_type.mag_radius >> 8) & 0x0FF);
-
-    setModeTemp(currentMode);
-
-}
-
-bool IMUSensor::isFullyCalibrated() {
-    uint8_t system, gyro, accel, mag;
-    getCalibration(&system, &gyro, &accel, &mag);
-
-    switch (currentMode) {
-        case OPERATION_MODE_ACCONLY:
-            return (accel == 3);
-        case OPERATION_MODE_MAGONLY:
-            return (mag == 3);
-        case OPERATION_MODE_GYRONLY:
-        case OPERATION_MODE_M4G: /* No magnetometer calibration required. */
-            return (gyro == 3);
-        case OPERATION_MODE_ACCMAG:
-        case OPERATION_MODE_COMPASS:
-            return (accel == 3 && mag == 3);
-        case OPERATION_MODE_ACCGYRO:
-        case OPERATION_MODE_IMUPLUS:
-            return (accel == 3 && gyro == 3);
-        case OPERATION_MODE_MAGGYRO:
-            return (mag == 3 && gyro == 3);
-        default:
-            return (system == 3 && gyro == 3 && accel == 3 && mag == 3);
-    }   
-}
-
-/* Power managments functions */
-void IMUSensor::enterSuspendMode() {
-    /* Switch to config mode (just in case since this is the default) */
-    setModeTemp(OPERATION_MODE_CONFIG);
-    writeRegister(BNO055_PWR_MODE_ADDR, 0x02);
-    /* Set the requested operating mode (see section 3.3) */
-    setModeTemp(currentMode);
-}
-
-void IMUSensor::enterNormalMode() {
-    /* Switch to config mode (just in case since this is the default) */
-    setModeTemp(OPERATION_MODE_CONFIG);
-    writeRegister(BNO055_PWR_MODE_ADDR, 0x00);
-    /* Set the requested operating mode (see section 3.3) */
-    setModeTemp(modeback);
-}
-
-
-
-
-
-
-
-
-
-
diff --git a/src/unused/surfaceFitModel.cpp b/src/unused/surfaceFitModel.cpp
deleted file mode 100644
index d48da49..0000000
--- a/src/unused/surfaceFitModel.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-#include "../include/surfaceFitModel.hpp"
-
-SurfaceFitModel::SurfaceFitModel() {
-
-    p = MatrixXd::Zero(X_DEGREE + 1, Y_DEGREE + 1);
-
-    p(0, 0) = -781536.384794701;
-    p(1, 0) = 8623.59011973048;
-    p(0, 1) = 643.65918253;
-    p(2, 0) = -34.3646691281487;
-    p(1, 1) = -5.46066535343611;
-    p(0, 2) = -0.177121900557321;
-    p(3, 0) = 0.0573287698655951;
-    p(2, 1) = 0.0150031142038895;
-    p(1, 2) = 0.00101871763126609;
-    p(0, 3) = 1.63862900553892e-05;
-    p(4, 0) = -3.21785828407871e-05;
-    p(3, 1) = -1.3161091180883e-05;
-    p(2, 2) = -1.42505256569339e-06;
-    p(1, 3) = -4.76209793830867e-08;
-}
-
-
-double SurfaceFitModel::getFit(double x, double y) {
-
-    return p(0, 0) + p(1, 0) * x + p(0, 1) * y + p(2, 0) * pow(x, 2) +
-	p(1, 1) * x * y + p(0, 2) * pow(y, 2) + p(3, 0) * pow(x, 3) +
-	p(2, 1) * pow(x, 2) * y + p(1, 2) * x * pow(y, 2) + p(0, 3) * pow(y, 3) +
-	p(4, 0) * pow(x, 4) + p(3, 1) * pow(x, 3) * y + p(2, 2) * pow(x, 2) * pow(y, 2) +
-	p(1, 3) * x * pow(y, 3); 
-}
-
-
-
-
-
-
-
-
-
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
deleted file mode 100644
index 13d4dc6..0000000
--- a/test/CMakeLists.txt
+++ /dev/null
@@ -1,70 +0,0 @@
-cmake_minimum_required(VERSION 3.16.3)
-
-project(ADSTests)
-
-# Set Variables
-set(TARGET_A TESTkalmanFilter)
-set(SOURCES_A kalmanFilterTest.cpp ../src/kalmanfilter.cpp)
-
-set(TARGET_B TESTsurfaceFitModel)
-set(SOURCES_B surfaceFitModelTest.cpp ../src/surfaceFitModel.cpp)
-
-set(TARGET_C TESTactuationPlan)
-set(SOURCES_C actuationPlanTest.cpp ../src/actuationPlan.cpp ../src/surfaceFitModel.cpp ../src/rocketUtils.cpp ../src/sensorIMU.cpp ../src/sensorAltimeter.cpp)
-
-# set(TARGET_D TESTads)
-# set(SOURCES_D adsTest.cpp ../src/ads.cpp ../src/actuationPlan.cpp ../src/surfaceFitModel.cpp ../src/rocketUtils.cpp ../src/sensorIMU.cpp ../src/sensorAltimeter.cpp ../src/motor.cpp ../src/logger.cpp ../src/kalmanfilter.cpp)
-
-set(TARGET_E TESTmotorPlan)
-set(SOURCES_E motorTest.cpp ../src/motor.cpp ../src/rocketUtils.cpp)
-
-set(TARGET_F TESTsensorAltimeterPlan)
-set(SOURCES_F sensorAltimeterTest.cpp ../src/sensorAltimeter.cpp ../src/rocketUtils.cpp)
-
-set(TARGET_G TESTsensorIMUPlan)
-set(SOURCES_G sensorIMUTest.cpp ../src/sensorIMU.cpp ../src/rocketUtils.cpp)
-
-
-#Set-up Google Test
-set(CMAKE_CXX_STANDARD 14)
-include(FetchContent)
-FetchContent_Declare(
-  googletest
-  URL https://github.com/google/googletest/archive/03597a01ee50ed33e9dfd640b249b4be3799d395.zip
-)
-# For Windows: Prevent overriding the parent project's compiler/linker settings
-set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
-FetchContent_MakeAvailable(googletest)
-
-
-
-
-# Create Executables & Link Dependencies
-add_executable(${TARGET_A} ${SOURCES_A})
-target_link_libraries(${TARGET_A} PUBLIC gtest_main) # Link GoogleTest's main() to Executable
-# Tell CMake the Target is a Unit Test
-add_test(NAME ${TARGET_A} COMMAND ${TARGET_A})
-
-add_executable(${TARGET_B} ${SOURCES_B})
-target_link_libraries(${TARGET_B} PUBLIC gtest_main)
-add_test(NAME ${TARGET_B} COMMAND ${TARGET_B})
-
-add_executable(${TARGET_C} ${SOURCES_C})
-target_link_libraries(${TARGET_C} PUBLIC gtest_main)
-add_test(NAME ${TARGET_C} COMMAND ${TARGET_C})
-
-# add_executable(${TARGET_D} ${SOURCES_D})
-# target_link_libraries(${TARGET_D} PUBLIC gtest_main)
-# add_test(NAME ${TARGET_D} COMMAND ${TARGET_D})
-
-add_executable(${TARGET_E} ${SOURCES_E})
-target_link_libraries(${TARGET_E} PUBLIC gtest_main)
-add_test(NAME ${TARGET_E} COMMAND ${TARGET_E})
-
-add_executable(${TARGET_F} ${SOURCES_F})
-target_link_libraries(${TARGET_F} PUBLIC gtest_main)
-add_test(NAME ${TARGET_F} COMMAND ${TARGET_F})
-
-add_executable(${TARGET_G} ${SOURCES_G})
-target_link_libraries(${TARGET_G} PUBLIC gtest_main)
-add_test(NAME ${TARGET_G} COMMAND ${TARGET_G})
-\ No newline at end of file
diff --git a/test/actuationPlanTest.cpp b/test/actuationPlanTest.cpp
deleted file mode 100644
index 72410b0..0000000
--- a/test/actuationPlanTest.cpp
+++ /dev/null
@@ -1,54 +0,0 @@
-#include <iostream>
-#include <gtest/gtest.h>
-#include "../include/surfaceFitModel.hpp"
-#include "../include/actuationPlan.hpp"
-#include "../include/rocketUtils.hpp"
-
-
-class ActuationPlanTest : public ::testing::Test {
-
-	protected:
-
-		ActuationPlanTest() {
-
-            SurfaceFitModel sfm = SurfaceFitModel();
-			plan = new ActuationPlan(sfm);
-		}
-			
-		//~ActuationPlanTest() {}
-
-		ActuationPlan *plan;
-};
-
-
-/**
- *	@brief Tests running the Actuation Plan
- *
- * **/
-TEST_F(ActuationPlanTest, runPlan) {
-	
-	Vehicle rocket;
-
-    rocket.imuInitFail = false;
-    rocket.imuReadFail = false;
-    rocket.altiInitFail = false;
-    rocket.altiReadFail = false;
-
-    // Test when Vehicle Status: Glide
-    rocket.fail_time = (time_t)(-1);
-    rocket.deploy_time = time(nullptr);
-    rocket.status = GLIDE;
-    rocket.filtered_altitude = 1;
-    rocket.filtered_velocity = 2;
-	plan->runPlan(rocket);
-    EXPECT_NEAR(rocket.deployment_angle, 120.0, 0.01);
-    EXPECT_NE(rocket.fail_time, (time_t)(-1));
-
-    // Test when Vehicle Status: Apogee
-    rocket.deploy_time = time(nullptr);
-    rocket.status = APOGEE;
-    rocket.filtered_altitude = 1;
-    rocket.filtered_velocity = 2;
-	plan->runPlan(rocket);
-    EXPECT_NEAR(rocket.deployment_angle, 110.0, 0.01);
-}
-\ No newline at end of file
diff --git a/test/kalmanFilterTest.cpp b/test/kalmanFilterTest.cpp
deleted file mode 100644
index bb0acda..0000000
--- a/test/kalmanFilterTest.cpp
+++ /dev/null
@@ -1,101 +0,0 @@
-#include <iostream>
-#include <gtest/gtest.h>
-#include <Eigen/Dense>
-#include "../include/kalmanfilter.hpp"
-
-using namespace Eigen;
-
-class KalmanFilterTest : public ::testing::Test {
-
-	protected:
-
-		KalmanFilterTest() {
-
-			kf = new KalmanFilter(2, 1, 1, 1);
-		}
-			
-		//~KalmanFilterTest() {}
-
-		KalmanFilter *kf;
-};
-
-
-/**
- *	@brief Test Setting the initial state x & P
- *
- * **/
-TEST_F(KalmanFilterTest, setInitialState) {
-	
-	VectorXf state_vec(2);
-	MatrixXf state_cov(2, 2);
-	state_vec << 1, 2;
-	state_cov << 1, 3, 4, 9;
-	
-	// Success Case
-	EXPECT_TRUE(kf->setInitialState(state_vec, state_cov));
-
-	// Failure Case
-	VectorXf state_vec2(4);
-	state_vec2 << 1, 2, 3, 4;
-	EXPECT_FALSE(kf->setInitialState(state_vec2, state_cov));
-}
-
-/**
- *	@brief Test a single iteration of the Kalman Filter
- *
- * **/
-TEST_F(KalmanFilterTest, run) {
-	
-	VectorXf control(1);
-	VectorXf measurement(1);
-	control << 1;
-	measurement << 1;
-	VectorXf res(1);
-
-	res = kf->run(control, measurement, 1);
-
-	EXPECT_NEAR(0.5454, res(0), 0.0001);
-	EXPECT_NEAR(1, res(1), 0.0001);
-}
-
-
-/**
- * @brief Test run() when the time step value is changed between function calls.
- * 
- */
-TEST_F(KalmanFilterTest, runChange) {
-	
-	VectorXf control(1);
-	VectorXf measurement(1);
-	control << 1;
-	measurement << 1;
-	VectorXf res(1);
-
-	res = kf->run(control, measurement, 0.1);
-	EXPECT_NEAR(0.09545, res(0), 0.00001);
-	EXPECT_NEAR(0.1, res(1), 0.1);
-
-	res = kf->run(control, measurement, 0.15);
-	EXPECT_NEAR(0.2761, res(0), 0.0001);
-	EXPECT_NEAR(0.3585, res(1), 0.0001);
-}
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/test/loggerTest.cpp b/test/loggerTest.cpp
deleted file mode 100644
index fab2f25..0000000
--- a/test/loggerTest.cpp
+++ /dev/null
@@ -1,57 +0,0 @@
-#include "../include/logger.hpp"
-
-class Two {
-
-    public: 
-        Two() {
-
-        }
-
-        void WritetoLog() {
-            Logger::Get().log("THIS IS A NEEEEEWWWW TEST!!!!!");
-            Logger::Get().logErr("ERRORRRRRRRR");
-            Logger::Get().printLog();
-        }
-
-        void TryPrint() {
-            Logger::Get().printLog();
-        }
-};
-
-class One {
-
-    public: 
-        Two two;
-        One() {
-            two = Two();
-        }
-
-        void OpenLog() {
-            Logger::Get().openLog("output.txt");
-            two.WritetoLog();
-            Logger::Get().closeLog();
-            two.TryPrint();
-        }
-};
-
-
-
-
-int main(int argc, char* argv[]) {
-
-    // Logger::Get().openLog("output.txt");
-    // Logger::Get().log("Testing Info aksdjflkas lksajfdlasjfaowe aslkdjf alskjf asodfj03945430 0349534 5039485 345");
-    // Logger::Get().log("Testing Info Just some stuff");
-    // Logger::Get().log("Testing something words words words");
-    // Logger::Get().logErr("Testing Error lksdj fa 3459374");
-    // Logger::Get().log("Testing blah blah blah blah blah lbal lab lbas labal abala");
-    // Logger::Get().log("Testing Info aksdjflkas l11111111111111111111111111111111111111111485 345");
-    // Logger::Get().logErr("Testing Error Some other type of error");
-    // Logger::Get().logErr("Testing Error Another one");
-    // Logger::Get().closeLog();
-    // //log.printLog();
-
-    One one = One();
-    one.OpenLog();
-
-}
-\ No newline at end of file
diff --git a/test/motorTest.cpp b/test/motorTest.cpp
deleted file mode 100644
index c570826..0000000
--- a/test/motorTest.cpp
+++ /dev/null
@@ -1,50 +0,0 @@
-#include <iostream>
-#include <vector>
-#include <gtest/gtest.h>
-#include "../include/motor.hpp"
-#include "../include/rocketUtils.hpp"
-
-class MotorTest : public ::testing::Test {
-
-	protected:
-
-		MotorTest() {
-
-			m1 = new Motor();
-		}
-			
-		//~IMUSensorTest() {}
-
-		Motor *m1;
-};
-
-
-/**
- *	@brief Test a 
- *
- * **/
-TEST_F(MotorTest, init) {
-
-    Vehicle rocket;
-
-    m1->init((void*)&rocket);
-
-	// ASSERT Statements.....
-}
-
-
-/**
- *	@brief Test a 
- *
- * **/
-TEST_F(MotorTest, writeData) {
-
-    Vehicle rocket;
-
-    m1->writeData((void*)&rocket);
-
-	// ASSERT Statements.....
-}
-
-
-// TODO: FIGURE OUT WHY MAKING 'rocket' a POINTER CAUSES A SEGFAULT
diff --git a/test/sensorAltimeterTest.cpp b/test/sensorAltimeterTest.cpp
deleted file mode 100644
index 7ba9521..0000000
--- a/test/sensorAltimeterTest.cpp
+++ /dev/null
@@ -1,50 +0,0 @@
-#include <iostream>
-#include <vector>
-#include <gtest/gtest.h>
-#include "../include/sensorAltimeter.hpp"
-#include "../include/rocketUtils.hpp"
-
-class AltimeterSensorTest : public ::testing::Test {
-
-	protected:
-
-		AltimeterSensorTest() {
-
-			alti = new AltimeterSensor();
-		}
-			
-		//~IMUSensorTest() {}
-
-		AltimeterSensor *alti;
-};
-
-
-/**
- *	@brief Test a 
- *
- * **/
-TEST_F(AltimeterSensorTest, init) {
-
-    Vehicle rocket;
-
-    alti->init((void*)&rocket);
-
-	// ASSERT Statements.....
-}
-
-
-/**
- *	@brief Test a 
- *
- * **/
-TEST_F(AltimeterSensorTest, getData) {
-
-    Vehicle rocket;
-
-    alti->getData((void*)&rocket.current_altitude);
-
-	// ASSERT Statements.....
-}
-
-
-// TODO: FIGURE OUT WHY MAKING 'rocket' a POINTER CAUSES A SEGFAULT
diff --git a/test/sensorIMUTest.cpp b/test/sensorIMUTest.cpp
deleted file mode 100644
index d16930e..0000000
--- a/test/sensorIMUTest.cpp
+++ /dev/null
@@ -1,50 +0,0 @@
-#include <iostream>
-#include <vector>
-#include <gtest/gtest.h>
-#include "../include/sensorIMU.hpp"
-#include "../include/rocketUtils.hpp"
-
-class IMUSensorTest : public ::testing::Test {
-
-	protected:
-
-		IMUSensorTest() {
-
-			imu = new IMUSensor();
-		}
-			
-		//~IMUSensorTest() {}
-
-		IMUSensor *imu;
-};
-
-
-/**
- *	@brief Test a 
- *
- * **/
-TEST_F(IMUSensorTest, init) {
-
-    Vehicle rocket;
-
-    imu->init((void*)&rocket);
-
-	// ASSERT Statements.....
-}
-
-
-/**
- *	@brief Test a 
- *
- * **/
-TEST_F(IMUSensorTest, getData) {
-
-    Vehicle rocket;
-
-    imu->getData((void*)&rocket);
-
-	// ASSERT Statements.....
-}
-
-
-// TODO: FIGURE OUT WHY MAKING 'rocket' a POINTER CAUSES A SEGFAULT
diff --git a/test/sensor_test.cpp b/test/sensor_test.cpp
deleted file mode 100644
index e4ab5ca..0000000
--- a/test/sensor_test.cpp
+++ /dev/null
@@ -1,10 +0,0 @@
-#include <iostream>
-#include "sensorI2C.hpp"
-
-int main (int argc, char** argv) {
-    AltimeterSensor altimeter = new AltimeterSensor("/dev/i2c-2");
-    altimeter.init();
-    for (int i = 0; i < 1000; i++) {
-        std::cout << "Altitude: " << altimeter.getAltitude() << std::endl;
-    }
-}
diff --git a/test/surfaceFitModelTest.cpp b/test/surfaceFitModelTest.cpp
deleted file mode 100644
index f0521b5..0000000
--- a/test/surfaceFitModelTest.cpp
+++ /dev/null
@@ -1,43 +0,0 @@
-#include <iostream>
-#include <gtest/gtest.h>
-#include "eigen3/Eigen/Dense"
-#include "../include/surfaceFitModel.hpp"
-
-using namespace Eigen;
-
-class SurfaceFitModelTest : public ::testing::Test {
-
-	protected:
-
-		SurfaceFitModelTest() {
-
-			surfFM = new SurfaceFitModel();
-		}
-			
-		//~SurfaceFitModelTest() {}
-
-		SurfaceFitModel *surfFM;
-};
-
-
-/**
- *	@brief Test a 
- *
- * **/
-TEST_F(SurfaceFitModelTest, getFit1) {
-	
-	double res = surfFM->getFit(1, 2);
-
-	EXPECT_NEAR(res, -771671.3793209707, 0.01);
-}
-
-/**
- *	@brief Test a 
- *
- * **/
-TEST_F(SurfaceFitModelTest, getFit2) {
-	
-	double res = surfFM->getFit(33.3, 49);
-
-	EXPECT_NEAR(res, -507325.4658735892, 0.01); 
-}
-\ No newline at end of file
diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt
new file mode 100644
index 0000000..8806fb0
--- /dev/null
+++ b/tools/CMakeLists.txt
@@ -0,0 +1,50 @@
+if (COMPILE_TOOLS)
+    add_executable(read_flash
+                   read_flash.c
+                   ../src/spi_flash.c
+    )
+
+    add_executable(servo_test
+                   servo_test.cpp
+                   ../src/pwm.cpp
+    )
+
+    add_executable(alt_test
+                   alt_test.cpp
+    )
+
+    add_executable(imu_calib
+                   imu_calib.cpp
+    )
+
+    # pull in common dependencies
+    target_link_libraries(read_flash pico_stdlib hardware_spi)
+    target_include_directories(read_flash PUBLIC ../include)
+
+    target_link_libraries(servo_test pico_stdlib hardware_pwm hardware_i2c)
+    target_include_directories(servo_test PUBLIC ../include)
+
+    target_link_libraries(alt_test pico_stdlib hardware_i2c hardware_gpio)
+
+    target_link_libraries(imu_calib pico_stdlib hardware_i2c hardware_gpio)
+    target_include_directories(imu_calib PUBLIC ../include)
+
+    pico_enable_stdio_usb(read_flash 1)
+    pico_enable_stdio_uart(read_flash 0)
+
+    pico_enable_stdio_usb(servo_test 1)
+    pico_enable_stdio_uart(servo_test 0)
+
+    pico_enable_stdio_usb(alt_test 1)
+    pico_enable_stdio_uart(alt_test 0)
+
+    pico_enable_stdio_usb(imu_calib 1)
+    pico_enable_stdio_uart(imu_calib 0)
+
+    # create map/bin/hex file etc.
+    pico_add_extra_outputs(read_flash)
+    pico_add_extra_outputs(servo_test)
+    pico_add_extra_outputs(alt_test)
+    pico_add_extra_outputs(imu_calib)
+endif()
+
diff --git a/tools/alt_test.cpp b/tools/alt_test.cpp
new file mode 100644
index 0000000..a5c8849
--- /dev/null
+++ b/tools/alt_test.cpp
@@ -0,0 +1,50 @@
+#include <stdio.h>
+
+#include "hardware/gpio.h"
+#include "boards/pico_w.h"
+#include "hardware/i2c.h"
+#include "pico/stdio.h"
+#include "pico/time.h"
+
+#define ALT_ADDR 0x60
+#define MAX_SCL 400000
+#define DATA_RATE_HZ 15
+
+float altitude = 0.0f;
+float get_altitude();
+
+int main() {
+    stdio_init_all();
+
+    i2c_init(i2c_default, MAX_SCL);
+    gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C);
+    gpio_set_function(PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C);
+    gpio_pull_up(PICO_DEFAULT_I2C_SDA_PIN);
+    gpio_pull_up(PICO_DEFAULT_I2C_SCL_PIN);
+
+    uint8_t config[2] = {0};
+
+    // Select control register(0x26)
+    // Active mode, OSR = 16, altimeter mode(0xB8)
+    config[0] = 0x26;
+    config[1] = 0xB9;
+    i2c_write_blocking(i2c_default, ALT_ADDR, config, 2, true);
+    sleep_ms(1500);
+
+    while (1) {
+        sleep_ms(1000);
+        altitude = get_altitude();
+        printf("Altitude: %4.2f\n", altitude);
+    }
+}
+
+float get_altitude() {
+    uint8_t reg = 0x01;
+    uint8_t data[5];
+    i2c_write_blocking(i2c_default, ALT_ADDR, &reg, 1, true);
+    i2c_read_blocking(i2c_default, ALT_ADDR, data, 5, false);
+    // Exactly how MPL3115A2 datasheet says to retrieve altitude
+    float altitude = (float) ((int16_t) ((data[0] << 8) | data[1])) + (float) (data[2] >> 4) * 0.0625;
+    return altitude;
+}
+
diff --git a/tools/imu_calib.cpp b/tools/imu_calib.cpp
new file mode 100644
index 0000000..dc45f39
--- /dev/null
+++ b/tools/imu_calib.cpp
@@ -0,0 +1,222 @@
+#include <stdio.h>
+#include <stdint.h>
+#include <inttypes.h>
+#include <Eigen/Geometry>
+
+#include "pico/stdio.h"
+#include "hardware/gpio.h"
+#include "hardware/i2c.h"
+
+#define MAX_SCL 400000
+
+#define BNO055_OPR_MODE_ADDR 0x3D
+#define BNO055_OPR_MODE_CONFIG 0x00
+#define BNO055_SYS_TRIGGER_ADDR 0x3F
+#define BNO055_ADDRESS 0x28
+#define BNO055_CHIP_ID_ADDR 0x00
+#define BNO055_CHIP_ID 0xA0
+#define BNO055_OPR_MODE_NDOF 0x0C
+#define BNO055_CALIB_STAT_ADDR 0x35
+#define ACCEL_OFFSET_X_LSB_ADDR 0x55
+#define BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR 0x28
+#define BNO055_QUATERNION_DATA_W_LSB_ADDR 0x20
+#define UNIT_SELECTION 0x3B
+
+void get_calibration(uint8_t *sys, uint8_t *gyro, uint8_t *accel, uint8_t *mag);
+
+int main() {
+    stdio_init_all();
+
+    getchar();
+
+    i2c_init(i2c_default, MAX_SCL);
+    gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C);
+    gpio_set_function(PICO_DEFAULT_I2C_SCL_PIN, GPIO_FUNC_I2C);
+    gpio_pull_up(PICO_DEFAULT_I2C_SDA_PIN);
+    gpio_pull_up(PICO_DEFAULT_I2C_SCL_PIN);
+
+    uint8_t buf[2] = {BNO055_CHIP_ID_ADDR};
+
+    uint8_t id = 0x00;
+    sleep_ms(1000);
+    i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 1, false);
+    i2c_read_blocking(i2c_default, BNO055_ADDRESS, &id, 1, false);
+    while (id != BNO055_CHIP_ID) {
+        i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 1, false);
+        i2c_read_blocking(i2c_default, BNO055_ADDRESS, &id, 1, false);
+        printf("Id not correct!, seeing: %" PRIu8 "\n", id);
+        sleep_ms(10);
+    }
+
+    buf[0] = BNO055_OPR_MODE_ADDR;
+    buf[1] = BNO055_OPR_MODE_CONFIG;
+    i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 2, false);
+
+    buf[0] = BNO055_SYS_TRIGGER_ADDR;
+    buf[1] = 0x20; // RESET
+    i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 2, false);
+    sleep_ms(30);
+
+    buf[0] = BNO055_CHIP_ID_ADDR;
+    id = 0x00;
+    while (id != BNO055_CHIP_ID) {
+        i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 1, false);
+        i2c_read_blocking(i2c_default, BNO055_ADDRESS, &id, 1, false);
+        printf("Id not correct!, seeing: %" PRIu8 "\n", id);
+        sleep_ms(10);
+    }
+
+    buf[0] = BNO055_SYS_TRIGGER_ADDR;
+    buf[1] = 0x00; // RESET
+    i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 2, false);
+    sleep_ms(30);
+    
+    // Set units to m/s^2
+    buf[0] = UNIT_SELECTION;
+    buf[1] = 0x00; // Windows, Celsius, Degrees, DPS, m/s^2
+    i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 2, false);
+    sleep_ms(50);
+
+    buf[0] = BNO055_OPR_MODE_ADDR;
+    buf[1] = BNO055_OPR_MODE_NDOF;
+    i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 2, false);
+
+    uint8_t gyro = 0x00, accel = 0x00, mag = 0x00;
+
+    printf("Magnetometer: Perform the figure-eight calibration dance.\n");
+    while (mag != 3) {
+        // Calibration Dance Step One: Magnetometer
+        //   Move sensor away from magnetic interference or shields
+        //   Perform the figure-eight until calibrated
+        get_calibration(NULL, NULL, NULL, &mag);
+        printf("Mag Calib Status: %3.0f\n", (100 / 3 * mag));
+        sleep_ms(1000);
+    }
+    printf("... CALIBRATED\n");
+    sleep_ms(1000);
+
+    printf("Accelerometer: Perform the six-step calibration dance.\n");
+    while (accel != 3) {
+        // Calibration Dance Step Two: Accelerometer
+        //   Place sensor board into six stable positions for a few seconds each:
+        //    1) x-axis right, y-axis up,    z-axis away
+        //    2) x-axis up,    y-axis left,  z-axis away
+        //    3) x-axis left,  y-axis down,  z-axis away
+        //    4) x-axis down,  y-axis right, z-axis away
+        //    5) x-axis left,  y-axis right, z-axis up
+        //    6) x-axis right, y-axis left,  z-axis down
+        //   Repeat the steps until calibrated
+        get_calibration(NULL, NULL, &accel, NULL);
+        printf("Accel Calib Status: %3.0f\n", (100 / 3 * accel));
+        sleep_ms(1000);
+    }
+    printf("... CALIBRATED\n");
+    sleep_ms(1000);
+
+    printf("Gyroscope: Perform the hold-in-place calibration dance.\n");
+    while (gyro != 3) {
+        // Calibration Dance Step Three: Gyroscope
+        //  Place sensor in any stable position for a few seconds
+        //  (Accelerometer calibration may also calibrate the gyro)
+        get_calibration(NULL, &gyro, NULL, NULL);
+        printf("Gyro Calib Status: %3.0f\n", (100 / 3 * gyro));
+        sleep_ms(1000);
+    }
+    printf("... CALIBRATED\n");
+    sleep_ms(1000);
+    printf("CALIBRATION COMPLETED\n");
+
+    // Get Sensor Offsets
+    buf[0] = BNO055_OPR_MODE_ADDR;
+    buf[1] = BNO055_OPR_MODE_CONFIG;
+    uint8_t sensor_offsets[22];
+    i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 2, false);
+    sleep_ms(30);
+
+    buf[0] = ACCEL_OFFSET_X_LSB_ADDR;
+    i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 1, false);
+    i2c_read_blocking(i2c_default, BNO055_ADDRESS, sensor_offsets, 18, false);
+    for (uint8_t i = 0; i < 18; i++) {
+        printf("sensor_offsets[%" PRIu8 "] = 0x%" PRIx8 ";\r\n", i + 1, sensor_offsets[i]);
+    }
+    sleep_ms(5000);
+
+    buf[0] = BNO055_OPR_MODE_ADDR;
+    buf[1] = BNO055_OPR_MODE_NDOF;
+    i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 2, false);
+    sleep_ms(5000);
+
+    getchar();
+
+    uint8_t lin_accel[6];
+    uint8_t quat[8];
+    float accel_x, accel_y, accel_z;
+    float abs_lin_accel_x, abs_lin_accel_y, abs_lin_accel_z;
+    float abs_quaternion_w, abs_quaternion_x, abs_quaternion_y, abs_quaternion_z;
+    while (1) {
+        uint8_t lin_accel_reg = BNO055_LINEAR_ACCEL_DATA_X_LSB_ADDR;
+        i2c_write_blocking(i2c_default, BNO055_ADDRESS, &lin_accel_reg, 1, true);
+        i2c_read_blocking(i2c_default, BNO055_ADDRESS, lin_accel, 6, false);
+        int16_t x, y, z;
+        x = y = z = 0;
+        x = ((int16_t)lin_accel[0]) | (((int16_t)lin_accel[1]) << 8);
+        y = ((int16_t)lin_accel[2]) | (((int16_t)lin_accel[3]) << 8);
+        z = ((int16_t)lin_accel[4]) | (((int16_t)lin_accel[5]) << 8);
+        accel_x = ((float)x) / 100.0;
+        accel_y = ((float)y) / 100.0;
+        accel_z = ((float)z) / 100.0;
+
+        uint8_t quat_reg = BNO055_QUATERNION_DATA_W_LSB_ADDR;
+        i2c_write_blocking(i2c_default, BNO055_ADDRESS, &quat_reg, 1, true);
+        i2c_read_blocking(i2c_default, BNO055_ADDRESS, quat, 8, false);
+        int16_t w;
+        w = x = y = z = 0;
+        w = ((int16_t)quat[0]) | (((int16_t)quat[1]) << 8);
+        x = ((int16_t)quat[2]) | (((int16_t)quat[3]) << 8);
+        y = ((int16_t)quat[4]) | (((int16_t)quat[5]) << 8);
+        z = ((int16_t)quat[6]) | (((int16_t)quat[7]) << 8);
+        abs_quaternion_w = ((float)w) / 16384.0; // 2^14 LSB
+        abs_quaternion_x = ((float)x) / 16384.0;
+        abs_quaternion_y = ((float)y) / 16384.0;
+        abs_quaternion_z = ((float)z) / 16384.0;
+
+        Eigen::Quaternion<float> q;
+        q.w() = abs_quaternion_w;
+        q.x() = abs_quaternion_x;
+        q.y() = abs_quaternion_y;
+        q.z() = abs_quaternion_z;
+        // q.normalize();
+        Eigen::Matrix3f rotation_matrix = q.toRotationMatrix();
+        Eigen::Vector3f lin_accel;
+        abs_lin_accel_x = accel_x* rotation_matrix(0, 0) + accel_y * rotation_matrix(0, 1) + accel_z* rotation_matrix(0, 2);
+        abs_lin_accel_y = accel_x * rotation_matrix(1, 0) + accel_y * rotation_matrix(1, 1) + accel_z * rotation_matrix(1, 2);
+        abs_lin_accel_z = -1.0f * (accel_x * rotation_matrix(2, 0) + accel_y * rotation_matrix(2, 1) + accel_z * rotation_matrix(2, 2));
+
+        printf("Acceleration Vector: %4.2f, %4.2f, %4.2f\n", accel_x, accel_y, accel_z);
+        printf("Abs Acceleration Vector: %4.2f, %4.2f, %4.2f\n", abs_lin_accel_x, abs_lin_accel_y, abs_lin_accel_z);
+        printf("Quaternion: %4.2f, %4.2f, %4.2f, %4.2f\n\n\n", abs_quaternion_w, abs_quaternion_x, abs_quaternion_y, abs_quaternion_z);
+        sleep_ms(1000);
+    }
+
+    return 0;
+}
+
+void get_calibration(uint8_t *sys, uint8_t *gyro, uint8_t *accel, uint8_t *mag) {
+    uint8_t buf[1] = {BNO055_CALIB_STAT_ADDR};
+    uint8_t cal_data = 0x00;
+    i2c_write_blocking(i2c_default, BNO055_ADDRESS, buf, 1, false);
+    i2c_read_blocking(i2c_default, BNO055_ADDRESS, &cal_data, 1, false);
+    if (sys != NULL) {
+        *sys = (cal_data >> 6) & 0x03;
+    }
+    if (gyro != NULL) {
+        *gyro = (cal_data >> 4) & 0x03;
+    }
+    if (accel != NULL) {
+        *accel = (cal_data >> 2) & 0x03;
+    }
+    if (mag != NULL) {
+        *mag = cal_data & 0x03;
+    }
+}
+
diff --git a/src/read_flash.c b/tools/read_flash.c
index 71d2870..91c75fb 100644
--- a/src/read_flash.c
+++ b/tools/read_flash.c
@@ -35,7 +35,7 @@ int main() {
     }
 
     printf("\nRead Data:\n");
-    printf("time (us) | state | dep pcnt | alt (m) | vel (m/s) | quat_w | quat_x | quat_y | quat_z | lin_ax | lin_ay | lin_az\n");
+    printf("time,state,board_temp,deploy_percent,altitude,velocity,lin_ax,lin_ay,lin_az,quat_w,quat_x,quat_y,quat_z\n");
     for (uint32_t i = 0; i < base_addr; i += PACKET_SIZE) {
         flash_read(spi_default, PICO_DEFAULT_SPI_CSN_PIN, i, entry, PACKET_SIZE);
         uint64_t now_us = (((uint64_t)entry[0] << 56) | ((uint64_t)entry[1] << 48) | \
@@ -43,29 +43,36 @@ int main() {
                           ((uint64_t)entry[4] << 24)  | ((uint64_t)entry[5] << 16) | \
                           ((uint64_t)entry[6] << 8)   | ((uint64_t)entry[7]));
 
-        uint8_t state = entry[8];
-        uint8_t deploy_percent = entry[9];
+        uint8_t state = entry[8] >> 4;
+        uint16_t temperature_data = ((uint16_t)(entry[8] & 0x0F) << 8) | ((uint16_t)entry[9]);
+        const float conversionFactor = 3.3f / (1 << 12);
+        float tempC = 27.0f - (((float)(temperature_data) * conversionFactor) - 0.706f) / 0.001721f;
 
-        uint32_t alt_bits = (entry[10] << 24) | (entry[11] << 16) | (entry[12] << 8) | (entry[13]);
+        uint8_t deploy_percent = entry[10];
+
+        float altitude = (float) ((int16_t) ((entry[11] << 8) | entry[12])) + (float) (entry[13] >> 4) * 0.0625;
         uint32_t vel_bits = (entry[14] << 24) | (entry[15] << 16) | (entry[16] << 8) | (entry[17]);
-        float altitude = *(float *)(&alt_bits);
         float velocity = *(float *)(&vel_bits);
 
-        int16_t w = ((int16_t)entry[18]) | (((int16_t)entry[19]) << 8);
-        int16_t x = ((int16_t)entry[20]) | (((int16_t)entry[21]) << 8);
-        int16_t y = ((int16_t)entry[22]) | (((int16_t)entry[23]) << 8);
-        int16_t z = ((int16_t)entry[24]) | (((int16_t)entry[25]) << 8);
-        float qw = ((float)w) / 16384.0; // 2^14 LSB
+        int16_t ax = ((int16_t)entry[18]) | (((int16_t)entry[19]) << 8);
+        int16_t ay = ((int16_t)entry[20]) | (((int16_t)entry[21]) << 8);
+        int16_t az = ((int16_t)entry[22]) | (((int16_t)entry[23]) << 8);
+        float lax = ((float)ax) / 100.0;
+        float lay = ((float)ay) / 100.0;
+        float laz = ((float)az) / 100.0;
+
+        int16_t w, x, y, z;
+        w = x = y = z = 0;
+        w = ((int16_t)entry[24]) | (((int16_t)entry[25]) << 8);
+        x = ((int16_t)entry[26]) | (((int16_t)entry[27]) << 8);
+        y = ((int16_t)entry[28]) | (((int16_t)entry[29]) << 8);
+        z = ((int16_t)entry[30]) | (((int16_t)entry[31]) << 8);
+        float qw = ((float)w) / 16384.0;
         float qx = ((float)x) / 16384.0;
         float qy = ((float)y) / 16384.0;
         float qz = ((float)z) / 16384.0;
-        int16_t ax = ((int16_t)entry[26]) | (((int16_t)entry[27]) << 8);
-        int16_t ay = ((int16_t)entry[28]) | (((int16_t)entry[29]) << 8);
-        int16_t az = ((int16_t)entry[30]) | (((int16_t)entry[31]) << 8);
-        float lax = ((float)x) / 100.0;
-        float lay = ((float)y) / 100.0;
-        float laz = ((float)z) / 100.0;
-        printf("%"PRIu64" | %c | %"PRIu8" | %4.2f | %4.2f | %4.2f | %4.2f| %4.2f | %4.2f | %4.2f | %4.2f |%4.2f\n", \
-                now_us, state, deploy_percent, altitude, velocity, qw, qx, qy, qz, lax, lay, laz);
+
+        printf("%"PRIu64",%"PRIu8",%04.2f,%"PRIu8",%04.2f,%04.2f,%04.2f,%04.2f,%04.2f,%04.2f,%04.2f,%04.2f,%04.2f\r\n", \
+                now_us, state, tempC, deploy_percent, altitude, velocity, lax, lay, laz, qw, qx, qy, qz);
     }
 }
diff --git a/src/servo_test.cpp b/tools/servo_test.cpp
index c5e8e6e..c5e8e6e 100644
--- a/src/servo_test.cpp
+++ b/tools/servo_test.cpp
author	Dawsyn Schraiber <[email protected]>	2024-06-13 14:30:58 -0400
committer	GitHub <[email protected]>	2024-06-13 14:30:58 -0400
commit	58b4bc754bbb9f5197119cd0c124e49c05acff46 (patch)
tree	8a65e23756374626e2c9cb997af9d8ed6f892390
parent	8fbd08fe29bbc2246a78b481b219c241f62ff420 (diff)
download	active-drag-system-58b4bc754bbb9f5197119cd0c124e49c05acff46.tar.gz active-drag-system-58b4bc754bbb9f5197119cd0c124e49c05acff46.tar.bz2 active-drag-system-58b4bc754bbb9f5197119cd0c124e49c05acff46.zip