Big Kahuna - IREC 2025 - Dawsyn's Final Commit (#16)HEADmain

# Dawsyn's Final Commit

This one is a little emotional as this is my final commit in this repository and as a member of Rocketry at Virginia Tech. This merges the changes seen in the branch known as 'big_kahuna' into main. This is the version of the ADS software as seen on [Roadkill](https://drive.google.com/file/d/120BvI-0ntliHo6i9UxcCn2pXAl-JsdP_/view?usp=drive_link) in the 2025 IREC competition. There are bound to be bugs, but I have found it useful to have the final competition version to be the one present on main at the end of every academic year. Hopefully this is useful to the next lead.

## Primary Changes
+ NEW I2C drivers to support sensors present on new ADS custom PCB
+ NEW logging library found in separate repository and pulled in as submodule ([pico-logger](https://github.com/rocketryvt/pico-logger))
     + No longer dependent on different flash chip from one used for code storage! Compile executable as RP2040 'copy-to-ram' type to increase flash read/write speeds!
+ NEW fixed-point libraries to allow for increased performance and sensor sampling speeds on RP2040 that lacks FPU
+ FreeRTOS Simultaneous Multi-processing (SMP) architecture for task handling and easier introduction / testing of new features
+ Serial monitor / command system with task performance monitoring commands
+ WORKING Kalman filter that takes altitude from barometer as measurement and z-axis acceleration from IMU as control to generate state vector containing filtered altitude and vertical velocity
+ NEW CFD equations from the Ben-ogrithm (to replace the Chen-ogrithm) that includes:
    + Apogee prediction model that takes current drag force, altitude, and vertical velocity
    + Current Drag Force equation based on current deployment and vertical velocity to use for Apogee Prediction model
    + Desired Drag force equation based on current altitude and vertical velocity to generate what drag force is needed to reach 10K ft
    + Deployment percentage equation based on current velocity and desired drag force to map to flap deployment percentage

7 files changed, 610 insertions, 351 deletions

diff --git a/tools/CMakeLists.txt b/tools/CMakeLists.txt
index 8806fb0..a5a7ba6 100644
--- a/tools/CMakeLists.txt
+++ b/tools/CMakeLists.txt
@@ -1,34 +1,42 @@
 if (COMPILE_TOOLS)
     add_executable(read_flash
-                   read_flash.c
-                   ../src/spi_flash.c
+                   read_flash.cpp
+                   ${PROJECT_SOURCE_DIR}/src/ms5607.cpp
+                   ${PROJECT_SOURCE_DIR}/src/adxl375.cpp
+                   ${PROJECT_SOURCE_DIR}/src/iim42653.cpp
+                   ${PROJECT_SOURCE_DIR}/src/mmc5983ma.cpp
+                   ${PROJECT_SOURCE_DIR}/src/log_format.cpp
+                   ${PROJECT_SOURCE_DIR}/src/heartbeat.cpp
+                   ${PROJECT_SOURCE_DIR}/src/serial.cpp
     )
 
     add_executable(servo_test
                    servo_test.cpp
-                   ../src/pwm.cpp
+                   ${PROJECT_SOURCE_DIR}/src/pwm.cpp
     )
 
     add_executable(alt_test
                    alt_test.cpp
-    )
-
-    add_executable(imu_calib
-                   imu_calib.cpp
+                   ${PROJECT_SOURCE_DIR}/src/ms5607.cpp
+                   ${PROJECT_SOURCE_DIR}/src/heartbeat.cpp
     )
 
     # pull in common dependencies
-    target_link_libraries(read_flash pico_stdlib hardware_spi)
-    target_include_directories(read_flash PUBLIC ../include)
+    target_link_libraries(read_flash pico_stdlib pico_logger pico_flash pico_rand pico_multicore pico_sync hardware_i2c hardware_adc hardware_timer FreeRTOS-Kernel FreeRTOS-Kernel-Heap4)
+    target_include_directories(read_flash PUBLIC ${PROJECT_SOURCE_DIR}/include)
+    target_compile_definitions(read_flash PRIVATE
+            USE_FREERTOS=1
+            DEBUG=1
+            PICO_STDIO_STACK_BUFFER_SIZE=64 # use a small printf on stack buffer
+    )
+    pico_set_binary_type(read_flash copy_to_ram)
 
     target_link_libraries(servo_test pico_stdlib hardware_pwm hardware_i2c)
-    target_include_directories(servo_test PUBLIC ../include)
+    target_include_directories(servo_test PUBLIC ${PROJECT_SOURCE_DIR}/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)
-
+    target_include_directories(alt_test PUBLIC ${PROJECT_SOURCE_DIR}/include)
+    
     pico_enable_stdio_usb(read_flash 1)
     pico_enable_stdio_uart(read_flash 0)
 
@@ -37,14 +45,10 @@ if (COMPILE_TOOLS)
 
     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
index a5c8849..9e92cdd 100644
--- a/tools/alt_test.cpp
+++ b/tools/alt_test.cpp
@@ -1,50 +1,47 @@
 #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
+#include "ms5607.hpp"
+#include "heartbeat.hpp"
+#include "rp2040_micro.h"
+
+#define DATA_RATE_HZ 200
 #define MAX_SCL 400000
-#define DATA_RATE_HZ 15
 
-float altitude = 0.0f;
-float get_altitude();
+bool data_callback(repeating_timer_t *rt);
+
+repeating_timer_t data_timer;
+
+MS5607 altimeter(i2c_default);
 
 int main() {
     stdio_init_all();
 
     i2c_init(i2c_default, MAX_SCL);
-    gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C);
+    gpio_init(PICO_DEFAULT_I2C_SCL_PIN);
+    gpio_init(PICO_DEFAULT_I2C_SDA_PIN);
     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);
+    gpio_set_function(PICO_DEFAULT_I2C_SDA_PIN, GPIO_FUNC_I2C);
 
-    uint8_t config[2] = {0};
+    altimeter.initialize();
+    heartbeat_initialize(PICO_DEFAULT_LED_PIN);
+    // gpio_init(PICO_DEFAULT_LED_PIN);
+    // gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
+    // gpio_put(PICO_DEFAULT_LED_PIN, 1);
 
-    // 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);
+    add_repeating_timer_us(-1000000 / DATA_RATE_HZ,  &data_callback, NULL, &data_timer);
 
     while (1) {
-        sleep_ms(1000);
-        altitude = get_altitude();
-        printf("Altitude: %4.2f\n", altitude);
+        sleep_ms(500);
+        printf("\n\nAltitude: %4.2f\nPressure: %4.2f\nTemperature: %4.2f\n", (float) altimeter.get_altitude() / ALTITUDE_SCALE_F, (float) altimeter.get_pressure() / PRESSURE_SCALE_F, (float) altimeter.get_temperature() / TEMPERATURE_SCALE_F);
     }
 }
 
-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;
+bool data_callback(repeating_timer_t *rt) {
+    altimeter.ms5607_start_sample();
+    return true;
 }
-
diff --git a/tools/clock_gen_test.cpp b/tools/clock_gen_test.cpp
new file mode 100644
index 0000000..6cfdcdd
--- /dev/null
+++ b/tools/clock_gen_test.cpp
@@ -0,0 +1,22 @@
+#include <stdio.h>
+
+#include "boards/pico_w.h"
+#include "pico/stdio.h"
+#include "hardware/gpio.h"
+#include "hardware/clocks.h"
+#include "pico/time.h"
+
+//Generate a 40kHz square wave with duty 0.5 on pin 21
+int main() {
+	stdio_init_all();
+	
+	//21   --> GPIO Selection, Pin 21 (Clock 0)
+	//0x6  --> Input selection, CLK_SYS as input (125MHz)
+	//3125 --> Clock Divider Int, 125MHz/40kHz = 3125
+	clock_gpio_init(21, 0x6, 3125);
+
+	while(1) {
+		printf("Clock may or may not be running?");
+		sleep_ms(1000);
+	}
+}
diff --git a/tools/imu_calib.cpp b/tools/imu_calib.cpp
deleted file mode 100644
index dc45f39..0000000
--- a/tools/imu_calib.cpp
+++ /dev/null
@@ -1,222 +0,0 @@
-#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/tools/read_flash.c b/tools/read_flash.c
deleted file mode 100644
index 91c75fb..0000000
--- a/tools/read_flash.c
+++ /dev/null
@@ -1,78 +0,0 @@
-#include <stdio.h>
-#include <inttypes.h>
-#include "boards/pico_w.h"
-#include "hardware/spi.h"
-#include "spi_flash.h"
-
-int main() {
-    stdio_init_all();
-    getchar();
-    // Enable SPI 0 at 1 MHz and connect to GPIOs
-    spi_init(spi_default, 1000 * 1000 * 60);
-    gpio_set_function(PICO_DEFAULT_SPI_RX_PIN, GPIO_FUNC_SPI);
-    gpio_set_function(PICO_DEFAULT_SPI_TX_PIN, GPIO_FUNC_SPI);
-    gpio_set_function(PICO_DEFAULT_SPI_SCK_PIN, GPIO_FUNC_SPI);
-
-    // Chip select is active-low, so we'll initialise it to a driven-high state
-    gpio_init(PICO_DEFAULT_SPI_CSN_PIN);
-    gpio_set_dir(PICO_DEFAULT_SPI_CSN_PIN, GPIO_OUT);
-    gpio_put(PICO_DEFAULT_SPI_CSN_PIN, 1);
-
-    uint8_t entry[PACKET_SIZE];
-
-    // flash_erase(spi_default, PICO_DEFAULT_SPI_CSN_PIN);
-    flash_read(spi_default, PICO_DEFAULT_SPI_CSN_PIN, base_addr, page_buffer, FLASH_PAGE_SIZE);
-    for (uint16_t i = 0; i < FLASH_PAGE_SIZE; i += PACKET_SIZE) {
-        if (page_buffer[i] == 0xFF) {
-            base_addr += i;
-            break;
-        }
-        if ((i + PACKET_SIZE) == FLASH_PAGE_SIZE) {
-            base_addr += FLASH_PAGE_SIZE;
-            flash_read(spi_default, PICO_DEFAULT_SPI_CSN_PIN, base_addr, page_buffer, FLASH_PAGE_SIZE);
-            i = 0;
-        }
-    }
-
-    printf("\nRead Data:\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) | \
-                          ((uint64_t)entry[2] << 40)  | ((uint64_t)entry[3] << 32) | \
-                          ((uint64_t)entry[4] << 24)  | ((uint64_t)entry[5] << 16) | \
-                          ((uint64_t)entry[6] << 8)   | ((uint64_t)entry[7]));
-
-        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;
-
-        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 velocity = *(float *)(&vel_bits);
-
-        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;
-
-        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/tools/read_flash.cpp b/tools/read_flash.cpp
new file mode 100644
index 0000000..b3305aa
--- /dev/null
+++ b/tools/read_flash.cpp
@@ -0,0 +1,538 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <inttypes.h>
+
+#include <cmath>    //TODO: Reimplement math properly using Eigen functions
+#include <sstream>  //TODO: This is for debug purposes, remove when implemented
+#include <Eigen/Dense>
+using namespace Eigen;  //TODO: Limit scope to necessary components once implemented
+
+#include "hardware/gpio.h"
+#include "hardware/i2c.h"
+#include "hardware/adc.h"
+#include <hardware/timer.h>
+#include "pico/rand.h"
+#include "pico/stdlib.h"
+#include "pico/stdio.h"
+#include "pico/multicore.h"
+#include "pico/time.h"
+#include <pico/error.h>
+#include <pico/types.h>
+
+/* Kernel includes. */
+#include "FreeRTOS.h"
+#include "FreeRTOSConfig.h"
+#include "portmacro.h"
+#include "projdefs.h"
+#include "serial.hpp"
+#include "task.h"
+#include "semphr.h"
+
+#include "heartbeat.hpp"
+#include "adxl375.hpp"
+#include "ms5607.hpp"
+#include "iim42653.hpp"
+#include "mmc5983ma.hpp"
+#include "serial.hpp"
+#include "pico_logger.h"
+#include "log_format.hpp"
+
+/* Priorities at which the tasks are created. */
+#define EVENT_HANDLER_PRIORITY      ( tskIDLE_PRIORITY + 4 )
+#define SENSOR_SAMPLE_PRIORITY      ( tskIDLE_PRIORITY + 3 )
+#define	HEARTBEAT_TASK_PRIORITY		( tskIDLE_PRIORITY + 2 )
+#define LOGGING_PRIORITY            ( tskIDLE_PRIORITY + 2 )
+#define	SERIAL_TASK_PRIORITY		( tskIDLE_PRIORITY + 1 )
+
+#define SENSOR_SAMPLE_RATE_HZ 500
+#define ORIENTATION_ESTIMATION_RATE_HZ 5
+
+#define MAX_SCL 400000
+
+static void populate_log_entry(log_entry_t * log_entry);
+static void sample_cmd_func();
+static void debug_cmd_func();
+static void circular_cmd_func();
+static void read_cmd_func();
+static void write_cmd_func();
+static void erase_cmd_func();
+static void orient_cmd_func();
+static void logging_task(void * unused_arg);
+
+void vApplicationTickHook(void) { /* optional */ }
+void vApplicationMallocFailedHook(void) { /* optional */ }
+void vApplicationStackOverflowHook(TaskHandle_t xTask, char *pcTaskName) { for( ;; ); }
+
+const char* executeable_name = "read_flash.uf2";
+const size_t num_user_cmds = 7;
+const command_t user_commands[] = { {.name = "sample",
+                                     .len = 6,
+                                     .function = &sample_cmd_func},
+                                    {.name = "debug",
+                                     .len = 5,
+                                     .function = &debug_cmd_func},
+                                    {.name = "circular",
+                                     .len = 8,
+                                     .function = &circular_cmd_func},
+                                    {.name = "read",
+                                     .len = 4,
+                                     .function = &read_cmd_func},
+                                    {.name = "write",
+                                     .len = 5,
+                                     .function = &write_cmd_func},
+                                    {.name = "erase",
+                                     .len = 5,
+                                     .function = &erase_cmd_func},
+                                    {.name = "orient",
+                                     .len = 6,
+                                     .function = &orient_cmd_func} };
+
+volatile bool serial_data_output = false;
+volatile bool use_circular_buffer = false;
+
+MS5607 alt(i2c_default);
+ADXL375 adxl375(i2c_default);
+IIM42653 iim42653(i2c_default);
+MMC5983MA mmc5983ma(i2c_default);
+
+Logger logger(PACKET_SIZE, LOG_BASE_ADDR, print_log_entry);
+
+volatile TaskHandle_t logging_handle = NULL;
+
+int main() {
+    stdio_init_all();
+
+    adc_init();
+    adc_set_temp_sensor_enabled(true);
+
+    gpio_init(PICO_DEFAULT_LED_PIN);
+    gpio_set_dir(PICO_DEFAULT_LED_PIN, GPIO_OUT);
+    gpio_put(PICO_DEFAULT_LED_PIN, 0);
+
+    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);
+
+    sleep_ms(2500);
+
+    info_cmd_func();
+    stdio_flush();
+
+    logger.initialize(true);
+
+    alt.initialize();
+    sleep_ms(500);
+    adxl375.initialize();
+    sleep_ms(500);
+    iim42653.initialize();
+    sleep_ms(500);
+    mmc5983ma.initialize();
+    sleep_ms(500);
+
+    xTaskCreate(heartbeat_task, "heartbeat", 256, NULL, HEARTBEAT_TASK_PRIORITY, NULL);
+    xTaskCreate(serial_task, "serial", 8192, NULL, SERIAL_TASK_PRIORITY, NULL);
+
+    vTaskStartScheduler();
+
+    while (1) {
+        tight_loop_contents();
+    }
+}
+
+static void logging_task(void * unused_arg) {
+    TickType_t xLastWakeTime;
+    const TickType_t xFrequency = pdMS_TO_TICKS(1000 / 10);
+
+    xLastWakeTime = xTaskGetTickCount();
+    printf("Time,Pressure,Altitude,Temperature,ax,ay,az,ax,ay,az,gx,gy,gz,ax,ay,az\n");
+    while (1) {
+        vTaskDelayUntil(&xLastWakeTime, xFrequency);
+//        printf("%" PRIu64 ",", time_us_64());
+//        printf("%4.2f,", ((float) alt.get_pressure()) / PRESSURE_SCALE_F);
+//        printf("%4.2f,", ((float) alt.get_altitude()) / ALTITUDE_SCALE_F);
+//        printf("%4.2f,", ((float) alt.get_temperature()) / TEMPERATURE_SCALE_F);
+//        printf("%4.2f,", adxl375.scale(adxl375.get_ax()));
+//        printf("%4.2f,", adxl375.scale(adxl375.get_ay()));
+//        printf("%4.2f,", adxl375.scale(adxl375.get_az()));
+//        printf("%4.2f,", iim42653.scale_accel(iim42653.get_ax()));
+//        printf("%4.2f,", iim42653.scale_accel(iim42653.get_ay()));
+//        printf("%4.2f,", iim42653.scale_accel(iim42653.get_az()));
+//        printf("%4.2f,", iim42653.scale_gyro(iim42653.get_gx()));
+//        printf("%4.2f,", iim42653.scale_gyro(iim42653.get_gy()));
+//        printf("%4.2f,", iim42653.scale_gyro(iim42653.get_gz()));
+//        printf("%" PRIi16 ",", mmc5983ma.get_ax());
+//        printf("%" PRIi16 ",", mmc5983ma.get_ay());
+//        printf("%" PRIi16 ",", mmc5983ma.get_az());
+//        printf("\r\n");
+        log_entry_t log_entry;
+        populate_log_entry(&log_entry);
+        printf("\nWriting the following entry!\n");
+        print_log_entry(reinterpret_cast<const uint8_t *>(&log_entry));
+        if (use_circular_buffer) {
+            logger.write_circular_buffer(reinterpret_cast<const uint8_t *>(&log_entry));
+        } else {
+            logger.write_memory(reinterpret_cast<const uint8_t *>(&log_entry), true);
+        }
+        stdio_flush();
+    }
+
+}
+
+//TODO: This task is getting increasingly complex and involved, it may be worth migrating to a class of its own
+static void pose_estimation_task(void * unused_arg) {
+    printf("--POSE: INITIALIZING DATA MEMBERS\n");
+    TickType_t xLastWakeTime;
+    const TickType_t xFrequency = pdMS_TO_TICKS(1000 / ORIENTATION_ESTIMATION_RATE_HZ);
+    xLastWakeTime = xTaskGetTickCount();
+    int orient_count = 0;
+
+    Quaternionf q_k(1, 0, 0, 0);     //Initialize to straight upright
+    Quaternionf q_k_est = q_k;
+    Matrix4f P_k = Matrix4f::Identity()*0.01f;  //TODO: Tune this initialization value
+    Matrix4f P_k_est = P_k;
+    
+    //TODO: Store covariance values somewhere reasonable instead of hardcoding them
+    Matrix3f gyro_covariance = Matrix3f::Identity()*0.05f*M_PI/180.0f;   //0.05deg/s RMSE @ 100HZ Bandwidth
+    Matrix<float, 6, 6> accel_mag_covariance;
+    accel_mag_covariance << 0.00065f, 0, 0, 0, 0, 0,
+                            0, 0.00065f, 0, 0, 0, 0,
+                            0, 0, 0.00070f, 0, 0, 0,
+                            0, 0, 0, 0.00120f, 0, 0,
+                            0, 0, 0, 0, 0.00120f, 0,
+                            0, 0, 0, 0, 0, 0.00120f;
+    //accel_mag_covariance *= 3;
+
+    //Stored intermediate equation matrices
+    Matrix4f gyro_skew;                     //Skew-symmetric matrix equivalent of gyroscope output
+    Matrix4f I_4 = Matrix4f::Identity();    //4x4 Identity matrix, used for F_k calculation
+    
+    Matrix4f F_k;                           //State Transition Matrix
+    Matrix4f H_k;                           //Observation Matrix
+    Matrix4f K_k;                           //Kalman Gain Matrix
+    Matrix<float, 4, 3> J_process_k;        //Jacobian for process (gyro) covariance transformation
+    Matrix<float, 4, 6> J_observation_k;    //Jacobian for observation (accel/mag) covariance transformation
+    Matrix4f process_noise_transformed;     //Matrix of transformed process noise
+    Matrix4f observation_noise_transformed; //Matrix of transformed observation noise
+    Matrix<float, 4, 1> z_k;                //Measurement vector
+    Matrix<float, 4, 1> z_k_est;            //Estimated measurement vector, used for Kalman filter calculation
+    
+    while (1) {
+        vTaskDelayUntil(&xLastWakeTime, xFrequency);
+        
+        //printf("-------- BEGINNING UPDATE COMPUTATION --------\n");
+        //Get all required sensor values and compute intermediate values
+        float imu_ax = -IIM42653::scale_accel(iim42653.get_ax());
+        float imu_ay = -IIM42653::scale_accel(iim42653.get_ay());
+        float imu_az = IIM42653::scale_accel(iim42653.get_az());
+        float imu_gx = -IIM42653::scale_gyro(iim42653.get_gx())/180.0f*M_PI;
+        float imu_gy = -IIM42653::scale_gyro(iim42653.get_gy())/180.0f*M_PI;
+        float imu_gz = IIM42653::scale_gyro(iim42653.get_gz())/180.0f*M_PI;
+        float mag_x = -MMC5983MA::scale_mag(mmc5983ma.get_ay());   //TODO: Not convinced mag axes are correct
+        float mag_y = MMC5983MA::scale_mag(mmc5983ma.get_ax());
+        float mag_z = -MMC5983MA::scale_mag(mmc5983ma.get_az());
+
+        //Normalize accelerometer and magnetometer measurements
+        float imu_a_mag = std::sqrt(std::pow(imu_ax, 2) + std::pow(imu_ay, 2) + std::pow(imu_az, 2));
+        float mag_mag = std::sqrt(std::pow(mag_x, 2) + std::pow(mag_y, 2) + std::pow(mag_z, 2));
+        imu_ax /= imu_a_mag;
+        imu_ay /= imu_a_mag;
+        imu_az /= imu_a_mag;
+        mag_x /= mag_mag;
+        mag_y /= mag_mag;
+        mag_z /= mag_mag;
+
+        printf("--- [I] INITIALIZATION | IMU Accel direct outputs (x, y, z): [%4.3f, %4.3f, %4.3f]\n", imu_ax, imu_ay, imu_az);
+        printf("--- [I] INITIALIZATION | IMU Gyro direct outputs (x, y, z): [%4.3f, %4.3f, %4.3f]\n", imu_gx, imu_gy, imu_gz);
+        printf("--- [I] INITIALIZATION | Magnetometer direct outputs (x, y, z): [%4.3f, %4.3f, %4.3f]\n", mag_x, mag_y, mag_z);
+
+        float mag_D = imu_ax*mag_x + imu_ay*mag_y + imu_az*mag_z;
+        float mag_N = std::sqrt(1.0f - std::pow(mag_D, 2));
+        printf("--- [I] INITIALIZATION | Magnetometer values (mag_mag, mag_D, mag_N): [%2.4f, %2.4f, %2.4f]\n", mag_mag, mag_D, mag_N);
+        
+        //UPDATE | Calculate Kalman gain
+        //TODO: This Jacobian calculation is FUGLY, and very computationally intensive
+        //   Separate out common math, vectorize, and put it in a function somewhere?
+        float pd_mD_ax = mag_x/(2*mag_D);
+        float pd_mD_ay = mag_y/(2*mag_D);
+        float pd_mD_az = mag_z/(2*mag_D);
+        float pd_mD_mx = imu_ax/(2*mag_D);
+        float pd_mD_my = imu_ay/(2*mag_D);
+        float pd_mD_mz = imu_az/(2*mag_D);
+        float pd_mN_ax = -mag_D/mag_N*pd_mD_ax;
+        float pd_mN_ay = -mag_D/mag_N*pd_mD_ay;
+        float pd_mN_az = -mag_D/mag_N*pd_mD_az;
+        float pd_mN_mx = -mag_D/mag_N*pd_mD_mx;
+        float pd_mN_my = -mag_D/mag_N*pd_mD_my;
+        float pd_mN_mz = -mag_D/mag_N*pd_mD_mz;
+        float common_num = (imu_ay*mag_z - imu_az*mag_y);
+        float common_denom = std::pow(mag_N, 2);
+
+        float J_o_4_1 = (-pd_mN_ax*common_num)/common_denom;
+        float J_o_4_2 = (mag_z*mag_N - pd_mN_ay*common_num)/common_denom;
+        float J_o_4_3 = (-mag_y*mag_N - pd_mN_az*common_num)/common_denom;
+        float J_o_4_4 = (-pd_mN_mx*common_num)/common_denom;
+        float J_o_4_5 = (-imu_az*mag_N - pd_mN_my*common_num)/common_denom;
+        float J_o_4_6 = (imu_ay*mag_N - pd_mN_mz*common_num)/common_denom;
+        J_observation_k << 1.0f, 0, 0, 0, 0, 0,
+                           0, 1.0f, 0, 0, 0, 0,
+                           0, 0, 1.0f, 0, 0, 0,
+                           J_o_4_1, J_o_4_2, J_o_4_3, J_o_4_4, J_o_4_5, J_o_4_6;
+        //printf("- [U] CALC KALMAN GAIN | [J_o_4_1 - J_o_4_6] after calculation, from J_observation_k: [%4.2f | %4.2f | %4.2f | %4.2f | %4.2f | %4.2f]\n", J_observation_k(3, 0), J_observation_k(3, 1), J_observation_k(3, 2), J_observation_k(3, 3), J_observation_k(3, 4), J_observation_k(3, 5));
+        
+        observation_noise_transformed = J_observation_k*accel_mag_covariance*J_observation_k.transpose();
+        //printf("- [U] CALC KALMAN GAIN | Diagonal of observation_noise_transformed after transformation: [%2.4f, %2.4f, %2.4f, %2.4f]\n", observation_noise_transformed(0, 0), observation_noise_transformed(1, 1), observation_noise_transformed(2, 2), observation_noise_transformed(3, 3));
+        
+        H_k << -q_k.y(), q_k.z(), -q_k.w(), q_k.x(),
+               q_k.x(), q_k.w(), q_k.z(), q_k.y(),
+               q_k.w(), -q_k.x(), -q_k.y(), q_k.z(),
+               q_k.z(), q_k.y(), q_k.x(), q_k.w();
+        H_k *= 2;
+        //printf("- [U] CALC KALMAN GAIN | First column of H_k after computation: [%2.4f, %2.4f, %2.4f, %2.4f]\n", H_k(0, 0), H_k(1, 0), H_k(2, 0), H_k(3, 0));
+        
+        K_k = P_k*H_k.transpose()*(H_k*P_k*H_k.transpose() + observation_noise_transformed).inverse();
+        //printf("- [U] CALC KALMAN GAIN | First column of K_k after computation: [%2.4f, %2.4f, %2.4f, %2.4f]\n", K_k(0, 0), K_k(1, 0), K_k(2, 0), K_k(3, 0));
+
+        //UPDATE | Calculate current state estimate based on accel/mag measurements
+        z_k << imu_ax, imu_ay, imu_az, ((imu_ay*mag_z - imu_az*mag_y)/mag_N);
+        z_k_est = H_k*q_k_est.coeffs();                 //TODO: Confirm quaternion multiplication, output and check
+        q_k = q_k_est.coeffs() + K_k*(z_k - z_k_est);   //TODO: Confirm quaternion arithmetic, output and check
+        q_k.normalize();    //Ensure state output is a proper rotation quaternion
+        //printf("- [U] CALC STATE ESTIMATE | See below block for direct output\n");
+
+        //UPDATE | Calculate state covariance based on accel/mag measurements
+        P_k = (I_4 - K_k*H_k)*P_k_est;
+        //printf("- [U] CALC STATE COVAR | First column of P_k after computation: [%2.4f, %2.4f, %2.4f, %2.4f]\n", P_k(0, 0), P_k(1, 1), P_k(2, 2), P_k(3, 3));
+
+        //printf("-------- COMPLETED UPDATE COMPUTATION! --------\n\n");
+        //if (orient_count == ORIENTATION_ESTIMATION_RATE_HZ) {
+            printf("~~> Estimate (q_k) Coefficients [w, x, y, z]: [%1.4f, %1.4f, %1.4f, %1.4f]\n", q_k.w(), q_k.x(), q_k.y(), q_k.z());
+            Vector3f testVect = q_k.toRotationMatrix().eulerAngles(2, 1, 0)*180.0f/M_PI;
+            printf("~~> Estimate (q_k) in Euler [Yaw, Pitch, Roll]: [%3.3f, %3.3f, %3.3f]\n", testVect[0], testVect[1], testVect[2]);
+            Vector3f testAccelVect(imu_ax, imu_ay, imu_az);
+            Vector3f testAccelVectRotated = q_k._transformVector(testAccelVect);
+            printf("~~> Estimate (q_k) rotated accel magnitude vector: [%2.4f, %2.4f, %2.4f]\n\n", testAccelVectRotated[0], testAccelVectRotated[1], testAccelVectRotated[2]);
+
+        //    orient_count = 0;
+        //}
+        //orient_count++;
+
+        //printf("-------- BEGINNING PREDICT COMPUTATION --------\n");
+
+        //Predict next state based on gyroscope measurements
+        gyro_skew << 0, -imu_gx, -imu_gy, -imu_gz,
+                     imu_gx, 0, imu_gz, -imu_gy,
+                     imu_gy, -imu_gz, 0, imu_gx,
+                     imu_gz, imu_gy, -imu_gx, 0;
+        //printf("- [P] PREDICT STATE | Top second element of gyro_skew after set: [%4.2f]\n", gyro_skew(0, 1));
+        
+        F_k = I_4 + (0.5f/ORIENTATION_ESTIMATION_RATE_HZ)*gyro_skew;
+        //printf("- [P] PREDICT STATE | First column of F_k after set: [%2.4f, %2.4f, %2.4f, %2.4f]\n", F_k(0, 0), F_k(1, 0), F_k(2, 0), F_k(3, 0));
+        
+        q_k_est = F_k*q_k.coeffs();
+        q_k_est.normalize();    //TODO: CONFIRM normalization necessary at intermediary steps
+        //printf("- [P] PREDICT STATE | State estimate (q_k_est) coefficients after predict step: [%1.4f, %1.4f, %1.4f, %1.4f]\n", q_k_est.w(), q_k_est.x(), q_k_est.y(), q_k_est.z());
+
+        //Predict next covariance based on gyroscope measurements + Jacobian-transformed measurement variance
+        J_process_k << q_k.x(), q_k.y(), q_k.z(),
+                       -1.0f*q_k.w(), q_k.z(), -1.0f*q_k.y(),
+                       -1.0f*q_k.z(), -1.0f*q_k.w(), q_k.x(),
+                       q_k.y(), -1.0f*q_k.x(), -1.0f*q_k.w();
+        //printf("- [P] PREDICT COVAR | Top left element of J_process_k after computation: [%4.2f]\n", J_process_k(0, 0));
+        
+        process_noise_transformed = std::pow(0.5f/ORIENTATION_ESTIMATION_RATE_HZ, 2) *
+                                    J_process_k*gyro_covariance*J_process_k.transpose();
+        //printf("- [P] PREDICT COVAR | Diagonal of process_noise_transformed after transformation: [%2.4f, %2.4f, %2.4f, %2.4f]\n", process_noise_transformed(0, 0), process_noise_transformed(1, 1), process_noise_transformed(2, 2), process_noise_transformed(3, 3));
+        
+        P_k_est = F_k*P_k*F_k.transpose() + process_noise_transformed;
+        //printf("- [P] PREDICT COVAR | Diagonal of P_k_est after prediction computation: [%2.4f, %2.4f, %2.4f, %2.4f]\n", P_k_est(0, 0), P_k_est(1, 1), P_k_est(2, 2), P_k_est(3, 3));
+        //printf("-------- PREDICT COMPUTATION COMPLETED! --------\n\n\n");
+    }
+}
+
+static void sample_cmd_func() {
+    static bool sampling = false;
+
+    if (sampling == false) {
+        vTaskSuspendAll();
+
+        xTaskCreate(MS5607::update_ms5607_task, "update_ms5607", 256, &alt, SENSOR_SAMPLE_PRIORITY, &(alt.update_task_handle));
+        xTaskCreate(ADXL375::update_adxl375_task, "update_adxl375", 256, &adxl375, SENSOR_SAMPLE_PRIORITY, &(adxl375.update_task_handle));
+        xTaskCreate(IIM42653::update_iim42653_task, "update_iim42653", 256, &iim42653, SENSOR_SAMPLE_PRIORITY, &(iim42653.update_task_handle));
+        xTaskCreate(MMC5983MA::update_mmc5983ma_task, "update_mmc5983ma", 256, &mmc5983ma, SENSOR_SAMPLE_PRIORITY, &(mmc5983ma.update_task_handle));
+
+        xTaskCreate(MS5607::ms5607_sample_handler, "ms5607_sample_handler", 256, &alt, EVENT_HANDLER_PRIORITY, &(alt.sample_handler_task_handle));
+
+        vTaskCoreAffinitySet( alt.update_task_handle, 0x01 );
+        vTaskCoreAffinitySet( adxl375.update_task_handle, 0x01 );
+        vTaskCoreAffinitySet( iim42653.update_task_handle, 0x01 );
+        vTaskCoreAffinitySet( mmc5983ma.update_task_handle, 0x01 );
+
+        vTaskCoreAffinitySet( alt.sample_handler_task_handle, 0x01 );
+        sampling = true;
+        xTaskResumeAll();
+    } else {
+        printf("Stopping sample!\n");
+        vTaskSuspendAll();
+
+        vTaskDelete(alt.update_task_handle);
+        vTaskDelete(adxl375.update_task_handle);
+        vTaskDelete(iim42653.update_task_handle);
+        vTaskDelete(mmc5983ma.update_task_handle);
+
+        vTaskDelete(alt.sample_handler_task_handle);
+
+        alt.update_task_handle = NULL;
+        adxl375.update_task_handle = NULL;
+        iim42653.update_task_handle = NULL;
+        mmc5983ma.update_task_handle = NULL;
+
+        alt.sample_handler_task_handle = NULL;
+
+        sampling = false;
+        xTaskResumeAll();
+    }
+}
+
+
+static void debug_cmd_func() {
+    if (logging_handle == NULL) {
+        vTaskSuspendAll();
+        xTaskCreate(logging_task, "logging", 256, NULL, LOGGING_PRIORITY, const_cast<TaskHandle_t *>(&logging_handle));
+        vTaskCoreAffinitySet(logging_handle, 0x02);
+        xTaskResumeAll();
+    } else {
+        vTaskSuspendAll();
+        vTaskDelete(logging_handle);
+        logging_handle = NULL;
+        xTaskResumeAll();
+    }
+}
+
+static void circular_cmd_func() {
+    if (logging_handle != NULL) {
+        vTaskSuspend(logging_handle);
+    }
+    if (!use_circular_buffer) {
+        logger.initialize_circular_buffer(PAD_BUFFER_SIZE);
+        use_circular_buffer = true;
+    } else {
+        logger.flush_circular_buffer(true);
+        use_circular_buffer = false;
+    }
+    if (logging_handle != NULL) {
+        vTaskResume(logging_handle);
+    }
+}
+
+static void read_cmd_func() {
+    if (logging_handle != NULL) {
+        vTaskSuspend(logging_handle);
+    }
+    if (use_circular_buffer) {
+        logger.read_circular_buffer();
+    } else {
+        logger.read_memory();
+    }
+    vTaskResume(logging_handle);
+}
+
+static void populate_log_entry(log_entry_t * log_entry) {
+    log_entry->time_us = time_us_64();
+
+    adc_select_input(4);
+    log_entry->temperature_chip = adc_read();
+    log_entry->state = PAD;
+    log_entry->deploy_percent = 80;
+    log_entry->pressure = alt.get_pressure();
+    log_entry->altitude = alt.get_altitude();
+    log_entry->temperature_alt = alt.get_temperature();
+
+    log_entry->ax = iim42653.get_ax();
+    log_entry->ay = iim42653.get_ay();
+    log_entry->az = iim42653.get_az();
+    log_entry->gx = iim42653.get_gx();
+    log_entry->gy = iim42653.get_gy();
+    log_entry->gz = iim42653.get_gz();
+
+    log_entry->mag_x = mmc5983ma.get_ax();
+    log_entry->mag_y = mmc5983ma.get_ay();
+    log_entry->mag_z = mmc5983ma.get_az();
+
+    log_entry->high_g_x = adxl375.get_ax();
+    log_entry->high_g_y = adxl375.get_ay();
+    log_entry->high_g_z = adxl375.get_az();
+
+    log_entry->data0 = get_rand_64();
+    log_entry->data1 = get_rand_64();
+    log_entry->data2 = get_rand_32();
+    log_entry->data3 = get_rand_32();
+}
+
+static void write_cmd_func() {
+    if (logging_handle != NULL) {
+        vTaskSuspend(logging_handle);
+    }
+    uint64_t start = time_us_64();
+    log_entry_t log_entry;
+    populate_log_entry(&log_entry);
+    printf("\nWriting the following entry!\n");
+    print_log_entry(reinterpret_cast<const uint8_t *>(&log_entry));
+    if (use_circular_buffer) {
+        logger.write_circular_buffer(reinterpret_cast<const uint8_t *>(&log_entry));
+    } else {
+        logger.write_memory(reinterpret_cast<const uint8_t *>(&log_entry), true);
+    }
+    uint64_t end = time_us_64();
+    printf("\nTook %" PRIu64 " us to write that entry!\n", (end - start));
+    if (logging_handle != NULL) {
+        vTaskResume(logging_handle);
+    }
+}
+
+static void erase_cmd_func() {
+    if (logging_handle != NULL) {
+        vTaskSuspend(logging_handle);
+    }
+    logger.erase_memory();
+    if (logging_handle != NULL) {
+        vTaskResume(logging_handle);
+    }
+}
+
+static void orient_cmd_func() {
+    /*
+    float pitch_rad = std::atan(imu_ay/std::sqrt(std::pow(imu_ax, 2) + std::pow(imu_az, 2)));
+    float roll_rad = std::atan(imu_ax/std::sqrt(std::pow(imu_ay, 2) + std::pow(imu_az, 2)));
+    float yaw_rad = std::atan2(-mag_y*std::cos(roll_rad) + mag_z*std::sin(roll_rad), 
+            mag_x*std::cos(pitch_rad) + mag_y*std::sin(pitch_rad)*std::sin(roll_rad) + mag_z*std::cos(roll_rad) * std::sin(pitch_rad));
+    */
+
+    static TaskHandle_t pose_estimation_handle = NULL;
+    static bool estimating = false;
+
+    if (!estimating) {
+        printf("================ BEGINNING STATE ESTIMATION ================\n");
+        vTaskSuspendAll();
+        
+        //TODO: Assign pose estimation unique priority?
+        xTaskCreate(pose_estimation_task, "pose_estimation", 1024, NULL, 
+                    SENSOR_SAMPLE_PRIORITY, &pose_estimation_handle);
+        vTaskCoreAffinitySet( pose_estimation_handle, 0x01 );
+
+        estimating = true;
+        xTaskResumeAll();
+    } else {
+        printf("================= ENDING STATE ESTIMATION ================\n");
+        vTaskSuspendAll();
+
+        vTaskDelete(pose_estimation_handle);
+        pose_estimation_handle = NULL;
+
+        estimating = false;
+        xTaskResumeAll();
+    }   
+}
+
diff --git a/tools/servo_test.cpp b/tools/servo_test.cpp
index c5e8e6e..9c26c25 100644
--- a/tools/servo_test.cpp
+++ b/tools/servo_test.cpp
@@ -3,16 +3,14 @@
 #include "pico/stdio.h"
 #include "pwm.hpp"
 
-#define MOSFET_PIN 1
-
 PWM pwm;
 
 int main() {
     stdio_init_all();
     // Initialize MOSFET
-    gpio_init(MOSFET_PIN);
-    gpio_set_dir(MOSFET_PIN, GPIO_OUT);
-    gpio_put(MOSFET_PIN, 1);
+    gpio_init(MICRO_DEFAULT_SERVO_ENABLE);
+    gpio_set_dir(MICRO_DEFAULT_SERVO_ENABLE, GPIO_OUT);
+    gpio_put(MICRO_DEFAULT_SERVO_ENABLE, 1);
     pwm.init();
     uint8_t duty_cycle = 13;
     while (1) {